Skip to content

Commit

Permalink
Update tests
Browse files Browse the repository at this point in the history 
Signed-off-by: Alberto Tudela <[email protected]>
  • Loading branch information
@ajtudela
ajtudela committed Mar 12, 2024
1 parent 6d1455a commit 22ee3dd
Show file tree
Hide file tree
Showing 4 changed files with 147 additions and 44 deletions.
8 changes: 8 additions & 0 deletions include/laser_segmentation/laser_segmentation.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
*
Expand Down
58 changes: 33 additions & 25 deletions src/laser_segmentation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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(
Expand Down
23 changes: 4 additions & 19 deletions test/test_integration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
102 changes: 102 additions & 0 deletions test/test_laser_segmentation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit 22ee3dd

Please sign in to comment.