Refactors automatic reset and adds some comments

Author: AndreasR30

include/continuous_clustering/clustering/continuous_clustering.hpp

@@ -24,7 +24,7 @@ enum
 struct ContinuousRangeImageConfiguration
 {
     bool sensor_is_clockwise{true};
-    int num_columns{1700};
+    int num_columns{1700}; // rows are automatically read from number of points in firing
     bool supplement_inclination_angle_for_nan_cells{true};
 };
 

src/clustering/continuous_clustering.cpp

@@ -11,7 +11,7 @@ ContinuousClustering::ContinuousClustering() = default;
 void ContinuousClustering::reset(int num_rows, bool sequential_execution)
 {
     // recalculate some intermediate values in case the parameters have changed
-    num_columns_ = config_.range_image.num_columns; // TODO: also pass num columns as parameter here?
+    num_columns_ = config_.range_image.num_columns;
     num_rows_ = num_rows;
     srig_azimuth_width_per_column = static_cast<float>((2 * M_PI)) / static_cast<float>(num_columns_);
     ring_buffer_max_columns = num_columns_ * 10;

src/ros/continuous_clustering_node.cpp

@@ -107,14 +107,25 @@ class RosContinuousClustering
         ros::Time stamp_current_firing;
         stamp_current_firing.fromNSec(firing->stamp);
 
-        // handle time jumps and bad start condition (TODO: put it into library?)
+        // handle time jumps and bad start condition
         double dt = (stamp_current_firing - last_update_).toSec();
-        if (clustering_.resetRequired() || (!first_firing_after_reset && std::abs(dt) > 0.1))
+        if (!first_firing_after_reset)
         {
-            ROS_WARN_STREAM("Detected jump in time ("
-                            << dt << ") or bad start condition was encountered. Reset continuous clustering.");
-            reset(static_cast<int>(firing->points.size()));
-            return;
+            // resetting takes some time (often more than 0.1s) therefore we do not want to reset again when the
+            // first firing after reset arrives
+
+            if (clustering_.resetRequired())
+            {
+                ROS_WARN_STREAM("Detected reconfiguration or bad start condition. Reset continuous clustering.");
+                reset(static_cast<int>(firing->points.size()));
+                return;
+            }
+            else if (std::abs(dt) > 0.1)
+            {
+                ROS_WARN_STREAM("Detected jump in time (" << dt << "). Reset continuous clustering.");
+                reset(static_cast<int>(firing->points.size()));
+                return;
+            }
         }
         first_firing_after_reset = false;
         last_update_ = stamp_current_firing;