Fix sensor range sqr issue, reduce point density slightly

local_planner/cfg/local_planner_node.cfg

@@ -7,7 +7,7 @@ gen = ParameterGenerator()
 
 # local_planner
 
-gen.add("max_sensor_range_",    double_t,    0, "Data points farther away will be discarded", 15.0,  0, 20)
+gen.add("max_sensor_range_",    double_t,    0, "Data points farther away will be discarded", 15.0,  0, 40)
 gen.add("min_sensor_range_", double_t, 0, "Discard points closer than that", 0.2, 0, 10)
 gen.add("pitch_cost_param_", double_t, 0, "Cost function weight for goal oriented behavior", 25.0, 0, 30.0)
 gen.add("yaw_cost_param_", double_t, 0, "Cost function weight for constant heading", 3.0, 0, 20.0)

local_planner/src/nodes/planner_functions.cpp

@@ -14,7 +14,7 @@ void processPointcloud(pcl::PointCloud<pcl::PointXYZI>& final_cloud,
                        float yaw_fcu_frame_deg, float pitch_fcu_frame_deg, const Eigen::Vector3f& position,
                        float min_sensor_range, float max_sensor_range, float max_age, float elapsed_s,
                        int min_num_points_per_cell) {
-  const int SCALE_FACTOR = 4;
+  const int SCALE_FACTOR = 3;
   pcl::PointCloud<pcl::PointXYZI> old_cloud;
   std::swap(final_cloud, old_cloud);
   final_cloud.points.clear();
@@ -25,12 +25,14 @@ void processPointcloud(pcl::PointCloud<pcl::PointXYZI>& final_cloud,
   Eigen::MatrixXi histogram_points_counter(180 / (ALPHA_RES / SCALE_FACTOR), 360 / (ALPHA_RES / SCALE_FACTOR));
   histogram_points_counter.fill(0);
 
+  auto sqr = [](float f){return f*f;};
+
   for (const auto& cloud : complete_cloud) {
     for (const pcl::PointXYZ& xyz : cloud) {
       // Check if the point is invalid
       if (!std::isnan(xyz.x) && !std::isnan(xyz.y) && !std::isnan(xyz.z)) {
-        float distance = (position - toEigen(xyz)).squaredNorm();
-        if (min_sensor_range < distance && distance < max_sensor_range) {
+        float distanceSq = (position - toEigen(xyz)).squaredNorm();
+        if (sqr(min_sensor_range) < distanceSq && distanceSq < sqr(max_sensor_range)) {
           // subsampling the cloud
           PolarPoint p_pol = cartesianToPolarHistogram(toEigen(xyz), position);
           Eigen::Vector2i p_ind = polarToHistogramIndex(p_pol, ALPHA_RES / SCALE_FACTOR);
@@ -45,22 +47,25 @@ void processPointcloud(pcl::PointCloud<pcl::PointXYZI>& final_cloud,
 
   // combine with old cloud
   for (const pcl::PointXYZI& xyzi : old_cloud) {
-    // adding older points if not expired and space is free according to new cloud
-    PolarPoint p_pol = cartesianToPolarHistogram(toEigen(xyzi), position);
-    PolarPoint p_pol_fcu = cartesianToPolarFCU(toEigen(xyzi), position);
-    p_pol_fcu.e -= pitch_fcu_frame_deg;
-    p_pol_fcu.z -= yaw_fcu_frame_deg;
-    wrapPolar(p_pol_fcu);
-    Eigen::Vector2i p_ind = polarToHistogramIndex(p_pol, ALPHA_RES / SCALE_FACTOR);
-
-    // only remember point if it's in a cell not previously populated by complete_cloud, as well as outside FOV and
-    // 'young' enough
-    if (histogram_points_counter(p_ind.y(), p_ind.x()) < min_num_points_per_cell && xyzi.intensity < max_age &&
-        !pointInsideFOV(fov, p_pol_fcu)) {
-      final_cloud.points.push_back(toXYZI(toEigen(xyzi), xyzi.intensity + elapsed_s));
-
-      // to indicate that this cell now has a point
-      histogram_points_counter(p_ind.y(), p_ind.x()) = min_num_points_per_cell;
+    float distanceSq = (position - toEigen(xyzi)).squaredNorm();
+    if (distanceSq < sqr(max_sensor_range)) {
+      // adding older points if not expired and space is free according to new cloud
+      PolarPoint p_pol = cartesianToPolarHistogram(toEigen(xyzi), position);
+      PolarPoint p_pol_fcu = cartesianToPolarFCU(toEigen(xyzi), position);
+      p_pol_fcu.e -= pitch_fcu_frame_deg;
+      p_pol_fcu.z -= yaw_fcu_frame_deg;
+      wrapPolar(p_pol_fcu);
+      Eigen::Vector2i p_ind = polarToHistogramIndex(p_pol, ALPHA_RES / SCALE_FACTOR);
+
+      // only remember point if it's in a cell not previously populated by complete_cloud, as well as outside FOV and
+      // 'young' enough
+      if (histogram_points_counter(p_ind.y(), p_ind.x()) < min_num_points_per_cell && xyzi.intensity < max_age &&
+          !pointInsideFOV(fov, p_pol_fcu)) {
+        final_cloud.points.push_back(toXYZI(toEigen(xyzi), xyzi.intensity + elapsed_s));
+
+        // to indicate that this cell now has a point
+        histogram_points_counter(p_ind.y(), p_ind.x()) = min_num_points_per_cell;
+      }
     }
   }
 

local_planner/test/test_planner_functions.cpp

@@ -122,10 +122,10 @@ TEST(PlannerFunctionsTests, processPointcloud) {
 
   // THEN: we expect the first cloud to have 5 points
   // the second cloud should contain all 6 points
-  EXPECT_EQ(5, processed_cloud1.size());
-  EXPECT_EQ(6, processed_cloud2.size());
+  EXPECT_EQ(7, processed_cloud1.size());
+  EXPECT_EQ(8, processed_cloud2.size());
   EXPECT_TRUE(pointInsideFOV(FOV_regular, memory_point_polar));
-  EXPECT_EQ(5, processed_cloud3.size());  // since memory point is inside FOV, it isn't remembered
+  EXPECT_EQ(7, processed_cloud3.size());  // since memory point is inside FOV, it isn't remembered
 }
 
 TEST(PlannerFunctions, getSetpointFromPath) {