added vector header and linted

(cherry picked from commit 831b426)
pradyum · Nov 12, 2024 · bbb2394 · bbb2394
1 parent 66f8ac0
commit bbb2394
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 13 deletions.
diff --git a/include/dual_laser_merger/dual_laser_merger.hpp b/include/dual_laser_merger/dual_laser_merger.hpp
@@ -24,6 +24,7 @@
 #include <cmath>
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "laser_geometry/laser_geometry.hpp"
 #include "message_filters/subscriber.h"
@@ -79,7 +80,8 @@ class MergerNode : public rclcpp::Node
     angle_increment_param, scan_time_param, range_min_param, range_max_param, inf_epsilon_param,
     laser_1_x_offset, laser_1_y_offset, laser_1_yaw_offset, laser_2_x_offset, laser_2_y_offset,
     laser_2_yaw_offset, allowed_radius_param;
-  bool use_inf_param, enable_calibration_param, enable_shadow_filter_param, enable_average_filter_param;
+  bool use_inf_param, enable_calibration_param, enable_shadow_filter_param,
+    enable_average_filter_param;
   uint32_t ranges_size;
   double range, angle;
   int index, numNearbyPoints;

diff --git a/src/dual_laser_merger.cpp b/src/dual_laser_merger.cpp
@@ -127,21 +127,27 @@ void MergerNode::sub_callback(
       lidar_1_avg = *lidar_1_msg;
       lidar_2_avg = *lidar_2_msg;
       for(size_t i = 0; i <= lidar_1_msg->ranges.size(); i++) {
-        if(i==0) {
-          lidar_1_avg.ranges[i] = (lidar_1_msg->ranges[lidar_1_msg->ranges.size()-1] + lidar_1_msg->ranges[i] + lidar_1_msg->ranges[i+1])/3;
-        } else if(i==(lidar_1_msg->ranges.size()-1)) {
-          lidar_1_avg.ranges[i] = (lidar_1_msg->ranges[i-1] + lidar_1_msg->ranges[i] + lidar_1_msg->ranges[0])/3;
+        if(i == 0) {
+          lidar_1_avg.ranges[i] = (lidar_1_msg->ranges[lidar_1_msg->ranges.size() - 1] +
+            lidar_1_msg->ranges[i] + lidar_1_msg->ranges[i + 1]) / 3;
+        } else if(i == (lidar_1_msg->ranges.size() - 1)) {
+          lidar_1_avg.ranges[i] = (lidar_1_msg->ranges[i - 1] + lidar_1_msg->ranges[i] +
+            lidar_1_msg->ranges[0]) / 3;
         } else {
-          lidar_1_avg.ranges[i] = (lidar_1_msg->ranges[i-1] + lidar_1_msg->ranges[i] + lidar_1_msg->ranges[i+1])/3;
+          lidar_1_avg.ranges[i] = (lidar_1_msg->ranges[i - 1] + lidar_1_msg->ranges[i] +
+            lidar_1_msg->ranges[i + 1]) / 3;
         }
       }
       for(size_t i = 0; i <= lidar_2_msg->ranges.size(); i++) {
-        if(i==0) {
-          lidar_2_avg.ranges[i] = (lidar_2_msg->ranges[lidar_2_msg->ranges.size()-1] + lidar_2_msg->ranges[i] + lidar_2_msg->ranges[i+1])/3;
-        } else if(i==(lidar_2_msg->ranges.size()-1)) {
-          lidar_2_avg.ranges[i] = (lidar_2_msg->ranges[i-1] + lidar_2_msg->ranges[i] + lidar_2_msg->ranges[0])/3;
+        if(i == 0) {
+          lidar_2_avg.ranges[i] = (lidar_2_msg->ranges[lidar_2_msg->ranges.size() - 1] +
+            lidar_2_msg->ranges[i] + lidar_2_msg->ranges[i + 1]) / 3;
+        } else if(i == (lidar_2_msg->ranges.size() - 1)) {
+          lidar_2_avg.ranges[i] = (lidar_2_msg->ranges[i - 1] + lidar_2_msg->ranges[i] +
+            lidar_2_msg->ranges[0]) / 3;
         } else {
-          lidar_2_avg.ranges[i] = (lidar_2_msg->ranges[i-1] + lidar_2_msg->ranges[i] + lidar_2_msg->ranges[i+1])/3;
+          lidar_2_avg.ranges[i] = (lidar_2_msg->ranges[i - 1] + lidar_2_msg->ranges[i] +
+            lidar_2_msg->ranges[i + 1]) / 3;
         }
       }
 
@@ -212,9 +218,10 @@ void MergerNode::sub_callback(
       allowed_radius_scaled = allowed_radius_param / range_max_param;
       kdtree.setInputCloud(pcl_cloud_out.makeShared());
 
-      for (auto& point : pcl_cloud_out.points) {
+      for (auto & point : pcl_cloud_out.points) {
         dist_from_origin = std::sqrt(std::pow(point.x, 2) + std::pow(point.y, 2));
-        numNearbyPoints = kdtree.radiusSearch(point, allowed_radius_scaled * dist_from_origin, pointIndices, pointDistances);
+        numNearbyPoints = kdtree.radiusSearch(point, allowed_radius_scaled * dist_from_origin,
+            pointIndices, pointDistances);
         numNearbyPoints -= 1;
         if(numNearbyPoints == 0) {
           if(use_inf_param) {