Fix merge segments

Signed-off-by: Alberto Tudela <[email protected]>

Author: ajtudela

src/segmentation/jump_distance.cpp

@@ -76,21 +76,23 @@ void JumpDistanceSegmentation::perform_segmentation(
   segments.push_back(current_segment);
 
   // Check if last and first segments belongs to the same segment
-  slg::Segment2D first_segment = segments.front();
-  slg::Segment2D last_segment = segments.back();
-  // Check if the point belong to the same segment or we have to discard it.
-  // Remember that the points are radially sorted.
-  if (!is_jump_between(last_segment, first_segment)) {
-    last_segment.merge(first_segment);
-    // Insert the last segment as the first one
-    segments[0] = last_segment;
-    // And remove the last one
-    segments.pop_back();
-  } else {
-    // Fix the prior segment of the first segment
-    segments.front().set_prior_segment(last_segment.last_point());
-    // Fix the next segment of the last segment
-    segments.back().set_next_segment(first_segment.first_point());
+  if (segments.size() > 1) {
+    slg::Segment2D first_segment = segments.front();
+    slg::Segment2D last_segment = segments.back();
+    // Check if the point belong to the same segment or we have to discard it.
+    // Remember that the points are radially sorted.
+    if (!is_jump_between(last_segment, first_segment)) {
+      last_segment.merge(first_segment);
+      // Insert the last segment as the first one
+      segments[0] = last_segment;
+      // And remove the last one
+      segments.pop_back();
+    } else {
+      // Fix the prior segment of the first segment
+      segments.front().set_prior_segment(last_segment.last_point());
+      // Fix the next segment of the last segment
+      segments.back().set_next_segment(first_segment.first_point());
+    }
   }
 }
 

src/segmentation/jump_distance_merge.cpp

@@ -105,21 +105,23 @@ void JumpDistanceSegmentationMerge::perform_segmentation(
   }
 
   // Check if last and first segments belongs to the same segment
-  slg::Segment2D first_segment = segments.front();
-  slg::Segment2D last_segment = segments.back();
-  // Check if the point belong to the same segment or we have to discard it.
-  // Remember that the points are radially sorted.
-  if (!is_jump_between(last_segment, first_segment)) {
-    last_segment.merge(first_segment);
-    // Insert the last segment as the first one
-    segments[0] = last_segment;
-    // And remove the last one
-    segments.pop_back();
-  } else {
-    // Fix the prior segment of the first segment
-    segments.front().set_prior_segment(last_segment.last_point());
-    // Fix the next segment of the last segment
-    segments.back().set_next_segment(first_segment.first_point());
+  if (segments.size() > 1) {
+    slg::Segment2D first_segment = segments.front();
+    slg::Segment2D last_segment = segments.back();
+    // Check if the point belong to the same segment or we have to discard it.
+    // Remember that the points are radially sorted.
+    if (!is_jump_between(last_segment, first_segment)) {
+      last_segment.merge(first_segment);
+      // Insert the last segment as the first one
+      segments[0] = last_segment;
+      // And remove the last one
+      segments.pop_back();
+    } else {
+      // Fix the prior segment of the first segment
+      segments.front().set_prior_segment(last_segment.last_point());
+      // Fix the next segment of the last segment
+      segments.back().set_next_segment(first_segment.first_point());
+    }
   }
 }
 

test/test_integration.cpp

@@ -64,9 +64,11 @@ TEST(LaserSegmentationTest, integration) {
   sub_node->configure();
   sub_node->activate();
   // Create a subscriber for the segments
+  bool msg_received = false;
   auto seg_sub = sub_node->create_subscription<slg_msgs::msg::SegmentArray>(
     "segments", 1,
     [&](const slg_msgs::msg::SegmentArray msg) {
+      msg_received = true;
       EXPECT_EQ(msg.segments.size(), 1);
       RCLCPP_INFO(sub_node->get_logger(), "Segment received: %ld", msg.segments.size());
     });
@@ -82,6 +84,7 @@ TEST(LaserSegmentationTest, integration) {
   // and the segment should have a publisher
   EXPECT_EQ(scan_pub->get_subscription_count(), 1);
   EXPECT_EQ(seg_sub->get_publisher_count(), 1);
+  EXPECT_TRUE(msg_received);
 
   // Deactivate the nodes
   seg_node->deactivate();