Put num_zones in params

src/world_modeling/occupancy_segmentation/config/params.yaml

@@ -4,6 +4,7 @@ occupancy_segmentation:
     lidar_input_topic: "/velodyne_points"
     ground_output_topic: "/ground_points"
     nonground_output_topic: "/nonground_points"
+    num_zones: 4
     l_min: 2.7
     l_max: 80.0
     md: 0.3

src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp

@@ -11,8 +11,6 @@
 #include <tbb/parallel_for.h>
 #include <Eigen/Dense>
 
-#define NUM_ZONES 4
-
 struct PointXYZIRT {
   PCL_ADD_POINT4D;  // preferred way of adding a XYZ+padding
   float intensity;
@@ -54,8 +52,8 @@ class OccupancySegmentationCore {
  public:
   typedef std::vector<pcl::PointCloud<PointT>> Ring;
   typedef std::vector<Ring> Zone;
-  // Size of buffer before processed messages are published.
-  static constexpr int BUFFER_CAPACITY = 10;
+
+  int NUM_ZONES;
 
   float L_MIN;
   float L_MAX;
@@ -73,13 +71,13 @@ class OccupancySegmentationCore {
   float SENSOR_HEIGHT;
   float GLOBAL_EL_THRESH;
 
-  int ZONE_RINGS[NUM_ZONES];
-  int ZONE_SECTORS[NUM_ZONES];
-  float FLATNESS_THR[NUM_ZONES];
-  float ELEVATION_THR[NUM_ZONES];
-  double lmins[NUM_ZONES] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4,
+  std::vector<int> ZONE_RINGS;
+  std::vector<int> ZONE_SECTORS;
+  std::vector<float> FLATNESS_THR;
+  std::vector<float> ELEVATION_THR;
+  std::vector<double> lmins = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4,
                              (L_MIN + L_MAX) / 2};
-  double lmaxs[NUM_ZONES] = {lmins[1], lmins[2], lmins[3], L_MAX};
+  std::vector<double> lmaxs = {lmins[1], lmins[2], lmins[3], L_MAX};
 
   int num_patches = -1;
 
@@ -97,7 +95,7 @@ class OccupancySegmentationCore {
   std::vector<Status> _statuses;
 
   OccupancySegmentationCore();
-  OccupancySegmentationCore(float l_min, float l_max, float md, float mh, int min_num_points,
+  OccupancySegmentationCore(int num_zones, float l_min, float l_max, float md, float mh, int min_num_points,
                             int num_seed_points, float th_seeds, float uprightness_thresh,
                             int num_rings_of_interest, float sensor_height, float global_el_thresh,
                             std::vector<long int, std::allocator<long int>> &zone_rings,

src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp

@@ -26,7 +26,7 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRT,  // here we assume a XYZ + "test"
  * messages.
  *
  * Listens to the "unfiltered" topic and filters out data with invalid fields
- * and odd timestamps. Once the node collects BUFFER_CAPACITY messages it packs
+ * and odd timestamps. Once the node collects  messages it packs
  * the processed messages into an array and publishes it to the "filtered" topic.
  */
 class OccupancySegmentationNode : public rclcpp::Node {

src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp

@@ -5,12 +5,13 @@ template class OccupancySegmentationCore<PointXYZIRT>;
 
 template <typename PointT>
 OccupancySegmentationCore<PointT>::OccupancySegmentationCore(
-    float l_min, float l_max, float md, float mh, int min_num_points, int num_seed_points,
+    int num_zones, float l_min, float l_max, float md, float mh, int min_num_points, int num_seed_points,
     float th_seeds, float uprightness_thresh, int num_rings_of_interest, float sensor_height,
     float global_el_thresh, std::vector<long int, std::allocator<long int> > &zone_rings,
     std::vector<long int, std::allocator<long int> > &zone_sectors,
     std::vector<double> &flatness_thr, std::vector<double> &elevation_thr, bool adaptive_selection_en)
-    : L_MIN{l_min},
+    : NUM_ZONES{num_zones},
+      L_MIN{l_min},
       L_MAX{l_max},
       MD{md},
       MH{mh},
@@ -21,10 +22,10 @@ OccupancySegmentationCore<PointT>::OccupancySegmentationCore(
       NUM_RINGS_OF_INTEREST{num_rings_of_interest},
       SENSOR_HEIGHT{sensor_height},
       GLOBAL_EL_THRESH{global_el_thresh},
-      ZONE_RINGS{zone_rings[0], zone_rings[1], zone_rings[2], zone_rings[3]},
-      ZONE_SECTORS{zone_sectors[0], zone_sectors[1], zone_sectors[2], zone_sectors[3]},
-      FLATNESS_THR{flatness_thr[0], flatness_thr[1], flatness_thr[2], flatness_thr[3]},
-      ELEVATION_THR{elevation_thr[0], elevation_thr[1], elevation_thr[2], elevation_thr[3]},
+      ZONE_RINGS{zone_rings.begin(), zone_rings.end()},
+      ZONE_SECTORS{zone_rings.begin(), zone_sectors.end()},
+      FLATNESS_THR{flatness_thr.begin(), flatness_thr.end()},
+      ELEVATION_THR{elevation_thr.begin(), elevation_thr.end()},
       lmins{L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2},
       lmaxs{lmins[1], lmins[2], lmins[3], L_MAX}, ADAPTIVE_SELECTION_EN{adaptive_selection_en}{
   init_czm();
@@ -152,7 +153,6 @@ void OccupancySegmentationCore<PointT>::segment_ground(pcl::PointCloud<PointT> &
   // TODO error point removal
 
   fill_czm(unfiltered_cloud);
-  std::cout << "Finished filling czm" << std::endl;
   tbb::parallel_for(tbb::blocked_range<int>(0, num_patches), [&](tbb::blocked_range<int> r) {
     for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) {
       Patch_Index &p_idx = _patch_indices[patch_num];
@@ -176,8 +176,6 @@ void OccupancySegmentationCore<PointT>::segment_ground(pcl::PointCloud<PointT> &
     }
   });
 
-  std::cout << "Finished estimation" << std::endl;
-
   for (Patch_Index p_idx : _patch_indices) {
     Status status = _statuses[p_idx.idx];
 

src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp

@@ -4,6 +4,7 @@
 
 OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segmentation") {
   // Declare parameters
+  this->declare_parameter<int>("num_zones", 4);
   this->declare_parameter<double>("l_min", 2.7);
   this->declare_parameter<double>("l_max", 80.0);
   this->declare_parameter<double>("md", 0.3);
@@ -27,6 +28,7 @@ OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segment
   this->declare_parameter<std::string>("nonground_output_topic", std::string("/nonground_points"));
 
   // Retrieve parameters
+  int num_zones = this->get_parameter("num_zones").as_int();
   double l_min = this->get_parameter("l_min").as_double();
   double l_max = this->get_parameter("l_max").as_double();
   double md = this->get_parameter("md").as_double();
@@ -48,7 +50,7 @@ OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segment
   std::string nonground_output_topic = this->get_parameter("nonground_output_topic").as_string();
 
   _patchwork = OccupancySegmentationCore<PointXYZIRT>(
-      l_min, l_max, md, mh, min_num_points, num_seed_points, th_seeds, uprightness_thresh,
+      num_zones, l_min, l_max, md, mh, min_num_points, num_seed_points, th_seeds, uprightness_thresh,
       num_rings_of_interest, sensor_height, global_el_thresh, zone_rings, zone_sectors,
       flatness_thr, elevation_thr, adaptive_selection_en);