Add option to reconfigure in order to run continuous clustering single threaded

Author: AndreasR30

cfg/ContinuousClustering.cfg

@@ -5,6 +5,9 @@ from dynamic_reconfigure.parameter_generator_catkin import *
 
 gen = ParameterGenerator()
 
+general = gen.add_group("General", type="tab")
+general.add("is_single_threaded", bool_t, 0, "whether to use just one thread", False)
+
 rig = gen.add_group("Range Image Generation", type="tab")
 rig.add("sensor_is_clockwise", bool_t, 0, "whether the sensor rotates clockwise", True)
 rig.add("num_columns", int_t, 0,

include/continuous_clustering/clustering/continuous_clustering.hpp

@@ -21,6 +21,11 @@ enum
     GP_FOG = LIGHTGRAY,       // ground point: classified as fog
 };
 
+struct GeneralConfiguration
+{
+    bool is_single_threaded{false};
+};
+
 struct ContinuousRangeImageConfiguration
 {
     bool sensor_is_clockwise{true};
@@ -74,6 +79,7 @@ struct ContinuousClusteringConfiguration
 
 struct Configuration
 {
+    GeneralConfiguration general{};
     ContinuousRangeImageConfiguration range_image{};
     ContinuousGroundSegmentationConfiguration ground_segmentation{};
     ContinuousClusteringConfiguration clustering{};
@@ -189,7 +195,7 @@ class ContinuousClustering
 
   public:
     // general
-    void reset(int num_rows, bool sequential_execution = false);
+    void reset(int num_rows);
     void setConfiguration(const Configuration& config);
     bool resetRequired() const;
 

launch/demo_touareg.launch

@@ -8,6 +8,9 @@
     <!-- arguments to play directly a rosbag with this launch file -->
     <arg name="bag_file" default="" doc="path to bag file; if empty no bag file is replayed"/>
 
+    <!-- single threaded execution -->
+    <arg name="is_single_threaded" default="false"/>
+
     <!-- flags for enable/disable individual sensors -->
     <arg name="use_vls128_roof" default="true"/>
     <arg name="use_os32_left" default="true"/>

launch/sensor_generic.launch

@@ -5,11 +5,14 @@
     <!-- flag if we just want to start rviz without continuous clustering -->
     <arg name="rviz_only" default="false" doc="whether to start only rviz but nothing else"/>
 
+    <!-- single threaded execution -->
+    <arg name="is_single_threaded" default="false"/>
+
     <!-- lidar sensor -->
     <arg name="sensor_manufacturer" doc="available options: velodyne, ouster, kitti"/>
     <arg name="sensor_model" doc="arbitrary: e.g. vls128, hdl64, hdl32, os32, kitti ..."/>
     <arg name="sensor_position" doc="arbitrary: e.g. roof, left, right"/>
-    <arg name="sensor_frame"  doc="the ROS frame id of the velodyne sensor"/>
+    <arg name="sensor_frame" doc="the ROS frame id of the velodyne sensor"/>
     <arg name="sensor_raw_data_topic" doc="input topic for raw data"/>
     <arg name="sensor_is_clockwise" doc="whether the lidar sensor rotates clockwise"/>
 
@@ -73,6 +76,9 @@
             <param name="ego_robot_frame" value="$(arg ego_robot_frame)"/>
             <param name="odom_frame" value="$(arg odom_frame)"/>
 
+            <!-- general -->
+            <param name="is_single_threaded" value="$(arg is_single_threaded)"/>
+
             <!-- range image -->
             <param name="num_columns" value="$(arg num_columns)"/>
             <param name="wait_for_tf" value="$(arg wait_for_tf)"/>

src/clustering/continuous_clustering.cpp

@@ -8,7 +8,7 @@ namespace continuous_clustering
 
 ContinuousClustering::ContinuousClustering() = default;
 
-void ContinuousClustering::reset(int num_rows, bool sequential_execution)
+void ContinuousClustering::reset(int num_rows)
 {
     // recalculate some intermediate values in case the parameters have changed
     num_columns_ = config_.range_image.num_columns;
@@ -46,8 +46,8 @@ void ContinuousClustering::reset(int num_rows, bool sequential_execution)
     sc_inclination_angles_between_lasers_.resize(num_rows, std::nanf(""));
 
     // re-initialize workers
-    int num_treads = sequential_execution ? 0 : 1;
-    int num_treads_pub = sequential_execution ? 0 : 3;
+    int num_treads = config_.general.is_single_threaded ? 0 : 1;
+    int num_treads_pub = config_.general.is_single_threaded ? 0 : 3;
     insertion_thread_pool.init(
         [this](InsertionJob&& job) { insertFiringIntoRangeImage(std::forward<InsertionJob>(job)); }, num_treads);
     segmentation_thread_pool.init([this](SegmentationJob&& job)
@@ -66,6 +66,8 @@ void ContinuousClustering::reset(int num_rows, bool sequential_execution)
 void ContinuousClustering::setConfiguration(const Configuration& config)
 {
     // some parameter changes need a hard reset
+    if (config_.general.is_single_threaded != config.general.is_single_threaded)
+        reset_required = true;
     if (config_.range_image.sensor_is_clockwise != config.range_image.sensor_is_clockwise)
         reset_required = true;
     if (config_.range_image.num_columns != config.range_image.num_columns)

src/ros/continuous_clustering_node.cpp

@@ -185,6 +185,9 @@ class RosContinuousClustering
         else if (config_.ground_segmentation.use_terrain && !config.use_terrain)
             sub_terrain.shutdown();
 
+        // general config
+        config_.general.is_single_threaded = config.is_single_threaded;
+
         // config for range image
         config_.range_image.sensor_is_clockwise = config.sensor_is_clockwise;
         config_.range_image.num_columns = config.num_columns;

src/tools/kitti_demo.cpp

@@ -277,6 +277,7 @@ class KittiDemo
 
             // init some other configs
             Configuration config;
+            config.general.is_single_threaded = true;
             config.range_image.num_columns = 2200;
             config.clustering.ignore_points_in_chessboard_pattern = false;
             config.clustering.max_distance = 0.5;
@@ -289,7 +290,7 @@ class KittiDemo
             config.ground_segmentation.width_ref_to_left_mirror_ = 1.5;
             config.ground_segmentation.width_ref_to_right_mirror_ = -1.5;
             clustering.setConfiguration(config);
-            clustering.reset(64, true);
+            clustering.reset(64);
             clustering.setTransformRobotFrameFromSensorFrame(Eigen::Isometry3d::Identity());
 
             // add callbacks