Update tests

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

include/laser_segmentation/laser_segmentation.hpp

@@ -105,6 +105,14 @@ class laserSegmentation : public rclcpp_lifecycle::LifecycleNode
    */
   void scan_callback(const sensor_msgs::msg::LaserScan::SharedPtr scan);
 
+  /**
+   * @brief Filter the segments using the parameters
+   *
+   * @param segments List of segments
+   * @return std::vector<slg::Segment2D> Filtered segments
+   */
+  std::vector<slg::Segment2D> filter_segments(const std::vector<slg::Segment2D> & segments);
+
   /**
    * @brief Create the segment array message
    *

src/laser_segmentation.cpp

@@ -132,12 +132,40 @@ void laserSegmentation::scan_callback(const sensor_msgs::msg::LaserScan::SharedP
   segmentation_->perform_segmentation(point_list, segment_list);
 
   // Filter segments
+  auto segment_filtered_list = filter_segments(segment_list);
+
+  // Identification of segments and set angular distance
+  for (std::vector<slg::Segment2D>::size_type s = 0; s < segment_filtered_list.size(); s++) {
+    double angle = std::min(
+      scan_msg->angle_max - segment_filtered_list[s].max_angle(),
+      segment_filtered_list[s].min_angle() - scan_msg->angle_min);
+    segment_filtered_list[s].set_id(s);
+    segment_filtered_list[s].set_angular_distance_to_closest_boundary(angle);
+  }
+
+  // Publish the segment array
+  slg_msgs::msg::SegmentArray segment_array_msg;
+  segment_array_msg.header = scan_msg->header;
+  for (const auto & segment : segment_filtered_list) {
+    segment_array_msg.segments.push_back(segment);
+  }
+  segment_pub_->publish(segment_array_msg);
+
+  // Publish visualization markers
+  segment_viz_points_pub_->publish(
+    create_segment_viz_points(scan_msg->header, segment_filtered_list));
+}
+
+std::vector<slg::Segment2D> laserSegmentation::filter_segments(
+  const std::vector<slg::Segment2D> & segments)
+{
+  std::vector<slg::Segment2D> filtered_segments;
+  filtered_segments.reserve(segments.size());
+
   double squared_min_segment_width = params_->min_segment_width * params_->min_segment_width;
   double squared_max_segment_width = params_->max_segment_width * params_->max_segment_width;
-  std::vector<slg::Segment2D> segment_filtered_list;
-  segment_filtered_list.reserve(segment_list.size());
 
-  for (const auto & segment : segment_list) {
+  for (const auto & segment : segments) {
     // By number of points
     if (segment.size() < params_->min_points_segment ||
       segment.size() > params_->max_points_segment)
@@ -159,29 +187,9 @@ void laserSegmentation::scan_callback(const sensor_msgs::msg::LaserScan::SharedP
       continue;
     }
 
-    segment_filtered_list.push_back(segment);
-  }
-
-  // Identification of segments and set angular distance
-  for (std::vector<slg::Segment2D>::size_type s = 0; s < segment_filtered_list.size(); s++) {
-    double angle = std::min(
-      scan_msg->angle_max - segment_filtered_list[s].max_angle(),
-      segment_filtered_list[s].min_angle() - scan_msg->angle_min);
-    segment_filtered_list[s].set_id(s);
-    segment_filtered_list[s].set_angular_distance_to_closest_boundary(angle);
+    filtered_segments.push_back(segment);
   }
-
-  // Publish the segment array
-  slg_msgs::msg::SegmentArray segment_array_msg;
-  segment_array_msg.header = scan_msg->header;
-  for (const auto & segment : segment_filtered_list) {
-    segment_array_msg.segments.push_back(segment);
-  }
-  segment_pub_->publish(segment_array_msg);
-
-  // Publish visualization markers
-  segment_viz_points_pub_->publish(
-    create_segment_viz_points(scan_msg->header, segment_filtered_list));
+  return filtered_segments;
 }
 
 visualization_msgs::msg::MarkerArray laserSegmentation::create_segment_viz_points(

test/test_integration.cpp

@@ -33,34 +33,19 @@ TEST(LaserSegmentationTest, integration) {
 
   // Create and configure the laser_segmentation node
   auto seg_node = std::make_shared<laserSegmentation>();
-  // Set some parameters
-  nav2_util::declare_parameter_if_not_declared(
-    seg_node, "min_points_segment", rclcpp::ParameterValue(1));
-  nav2_util::declare_parameter_if_not_declared(
-    seg_node, "max_points_segment", rclcpp::ParameterValue(3));
   seg_node->configure();
   seg_node->activate();
 
   // Create a scan message with three points
   sensor_msgs::msg::LaserScan scan;
   scan.header.frame_id = "laser_frame";
-  scan.angle_min = -M_PI / 2;
-  scan.angle_max = M_PI / 2;
-  scan.angle_increment = M_PI / 180;
-  scan.time_increment = 0.1;
-  scan.scan_time = 0.1;
-  scan.range_min = 0.0;
-  scan.range_max = 10.0;
-  scan.ranges.push_back(1.0);
-  scan.ranges.push_back(1.1);
-  scan.ranges.push_back(2.0);
-  scan.ranges.push_back(2.1);
-  scan.ranges.push_back(3.0);
-  scan.ranges.push_back(3.2);
-  scan.ranges.push_back(12.0);
   // Publish the message
   scan_pub->publish(scan);
 
+  // Spin the laser_segmentation node
+  // Shouldn get any susbcriptions count
+  rclcpp::spin_some(seg_node->get_node_base_interface());
+
   // Create the segments subscriber node
   auto sub_node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("segment_subscriber");
   sub_node->configure();

test/test_laser_segmentation.cpp

@@ -22,6 +22,11 @@ class laserSegmentationFixture : public laserSegmentation
   laserSegmentationFixture()
   : laserSegmentation() {}
 
+  std::vector<slg::Segment2D> filter_segments(const std::vector<slg::Segment2D> & segments)
+  {
+    return laserSegmentation::filter_segments(segments);
+  }
+
   visualization_msgs::msg::MarkerArray create_segment_viz_points(
     std_msgs::msg::Header header, std::vector<slg::Segment2D> segment_list)
   {
@@ -192,6 +197,103 @@ TEST(LaserSegmentationTest, createVizPoints) {
   EXPECT_EQ(marker_array.markers[3].text, "0");
 }
 
+TEST(LaserSegmentationTest, filterSegments) {
+  // Create the node
+  auto node = std::make_shared<laserSegmentationFixture>();
+
+  // Set the parameters
+  nav2_util::declare_parameter_if_not_declared(
+    node, "min_points_segment", rclcpp::ParameterValue(1));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "max_points_segment", rclcpp::ParameterValue(2));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "min_avg_distance_from_sensor", rclcpp::ParameterValue(0.1));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "max_avg_distance_from_sensor", rclcpp::ParameterValue(10.0));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "min_segment_width", rclcpp::ParameterValue(0.1));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "max_segment_width", rclcpp::ParameterValue(10.0));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "distance_threshold", rclcpp::ParameterValue(0.1));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "noise_reduction", rclcpp::ParameterValue(0.1));
+  node->configure();
+
+  // Set a segment list with 0 segments
+  std::vector<slg::Segment2D> segment_list;
+  auto filtered_segments = node->filter_segments(segment_list);
+  // Check the filtered segments
+  EXPECT_EQ(filtered_segments.size(), 0);
+
+  // Set a segment list with 3 segments of 1 point
+  segment_list.clear();
+  slg::Segment2D segment;
+  segment.add_point(slg::Point2D(0.0, 0.0, slg::BACKGROUND));
+  segment_list.push_back(segment);
+  segment_list.push_back(segment);
+  segment_list.push_back(segment);
+  // Filter the segments
+  filtered_segments = node->filter_segments(segment_list);
+  // Check the filtered segments
+  EXPECT_EQ(filtered_segments.size(), 0);
+
+  // Set a segment list with 1 segment with centroid below the minimum distance
+  segment_list.clear();
+  segment.clear();
+  segment.add_point(slg::Point2D(0.0, 0.0, slg::BACKGROUND));
+  segment_list.push_back(segment);
+  // Filter the segments
+  filtered_segments = node->filter_segments(segment_list);
+  // Check the filtered segments
+  EXPECT_EQ(filtered_segments.size(), 0);
+
+  // Set a segment list with 1 segment with centroid above the maximum distance
+  segment_list.clear();
+  segment.clear();
+  segment.add_point(slg::Point2D(11.0, 11.0, slg::BACKGROUND));
+  segment_list.push_back(segment);
+  // Filter the segments
+  filtered_segments = node->filter_segments(segment_list);
+  // Check the filtered segments
+  EXPECT_EQ(filtered_segments.size(), 0);
+
+  // Set a segment list with 1 segment with width below the minimum width
+  segment_list.clear();
+  segment.clear();
+  segment.add_point(slg::Point2D(0.0, 0.0, slg::BACKGROUND));
+  segment.add_point(slg::Point2D(0.0, 0.0, slg::BACKGROUND));
+  segment_list.push_back(segment);
+  // Filter the segments
+  filtered_segments = node->filter_segments(segment_list);
+  // Check the filtered segments
+  EXPECT_EQ(filtered_segments.size(), 0);
+
+  // Set a segment list with 1 segment with width above the maximum width
+  segment_list.clear();
+  segment.clear();
+  segment.add_point(slg::Point2D(0.0, 0.0, slg::BACKGROUND));
+  segment.add_point(slg::Point2D(10.0, 10.0, slg::BACKGROUND));
+  segment_list.push_back(segment);
+  // Filter the segments
+  filtered_segments = node->filter_segments(segment_list);
+  // Check the filtered segments
+  EXPECT_EQ(filtered_segments.size(), 0);
+
+  // Set a segment list with 1 segment with 2 points
+  segment_list.clear();
+  segment.clear();
+  segment.add_point(slg::Point2D(0.0, 0.0, slg::BACKGROUND));
+  segment.add_point(slg::Point2D(1.0, 1.0, slg::BACKGROUND));
+  segment_list.push_back(segment);
+  // Filter the segments
+  filtered_segments = node->filter_segments(segment_list);
+  // Check the filtered segments
+  EXPECT_EQ(filtered_segments.size(), 1);
+  EXPECT_EQ(filtered_segments[0].centroid().x, 0.5);
+  EXPECT_EQ(filtered_segments[0].centroid().y, 0.5);
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);