diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index ea370f9acca..c160e36bc6f 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp @@ -95,6 +95,8 @@ class PointCloud : public Source // Minimum and maximum height of PointCloud projected to 2D space double min_height_, max_height_; + // Minimum range from sensor origin to filter out close points + double min_range_; /// @brief Latest data obtained from pointcloud sensor_msgs::msg::PointCloud2::ConstSharedPtr data_; diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 77daab68055..3f94293dc74 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -92,6 +92,13 @@ bool PointCloud::getData( for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { // Transform point coordinates from source frame -> to base frame tf2::Vector3 p_v3_s(*iter_x, *iter_y, *iter_z); + + // Check range from sensor origin before transformation + double range = p_v3_s.length(); + if (range < min_range_) { + continue; + } + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; // Refill data array @@ -117,6 +124,9 @@ void PointCloud::getParameters(std::string & source_topic) nav2_util::declare_parameter_if_not_declared( node, source_name_ + ".max_height", rclcpp::ParameterValue(0.5)); max_height_ = node->get_parameter(source_name_ + ".max_height").as_double(); + nav2_util::declare_parameter_if_not_declared( + node, source_name_ + ".min_range", rclcpp::ParameterValue(0.0)); + min_range_ = node->get_parameter(source_name_ + ".min_range").as_double(); } void PointCloud::dataCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp index 24de5cbe056..edc88453e13 100644 --- a/nav2_collision_monitor/test/sources_test.cpp +++ b/nav2_collision_monitor/test/sources_test.cpp @@ -140,6 +140,58 @@ class TestNode : public nav2_util::LifecycleNode pointcloud_pub_->publish(std::move(msg)); } + void publishPointCloudForMinRange(const rclcpp::Time & stamp) + { + pointcloud_pub_ = this->create_publisher( + POINTCLOUD_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + std::unique_ptr msg = + std::make_unique(); + sensor_msgs::PointCloud2Modifier modifier(*msg); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + modifier.setPointCloud2Fields( + 3, "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32); + modifier.resize(4); + + sensor_msgs::PointCloud2Iterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*msg, "z"); + + // Point 0: Very close to origin (0.05, 0.0, 0.15) - range ≈ 0.158, + // should be filtered + *iter_x = 0.05; + *iter_y = 0.0; + *iter_z = 0.15; + ++iter_x; ++iter_y; ++iter_z; + + // Point 1: Close to origin (0.15, 0.0, 0.15) - range ≈ 0.212, + // should pass with min_range=0.16 + *iter_x = 0.15; + *iter_y = 0.0; + *iter_z = 0.15; + ++iter_x; ++iter_y; ++iter_z; + + // Point 2: At min_range threshold (0.2, 0.0, 0.15) - range ≈ 0.25, + // should pass with min_range=0.16 + *iter_x = 0.2; + *iter_y = 0.0; + *iter_z = 0.15; + ++iter_x; ++iter_y; ++iter_z; + + // Point 3: Beyond min_range (0.5, 0.0, 0.15) - range ≈ 0.522, + // should pass + *iter_x = 0.5; + *iter_y = 0.0; + *iter_z = 0.15; + + pointcloud_pub_->publish(std::move(msg)); + } + void publishRange(const rclcpp::Time & stamp, const double range) { range_pub_ = this->create_publisher( @@ -296,6 +348,8 @@ class Tester : public ::testing::Test public: Tester(); ~Tester(); + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; protected: // Data sources creation routine @@ -319,10 +373,6 @@ class Tester : public ::testing::Test std::shared_ptr pointcloud_; std::shared_ptr range_; std::shared_ptr polygon_; - -private: - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; }; // Tester Tester::Tester() @@ -741,6 +791,62 @@ TEST_F(Tester, testIgnoreTimeShift) checkPolygon(data); } +TEST_F(Tester, testPointCloudMinRange) +{ + rclcpp::Time curr_time = test_node_->now(); + + // Create PointCloud object with min_range = 0.2 + test_node_->declare_parameter( + std::string(POINTCLOUD_NAME) + ".topic", rclcpp::ParameterValue(POINTCLOUD_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POINTCLOUD_NAME) + ".topic", POINTCLOUD_TOPIC)); + test_node_->declare_parameter( + std::string(POINTCLOUD_NAME) + ".min_height", rclcpp::ParameterValue(0.1)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POINTCLOUD_NAME) + ".min_height", 0.1)); + test_node_->declare_parameter( + std::string(POINTCLOUD_NAME) + ".max_height", rclcpp::ParameterValue(1.0)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POINTCLOUD_NAME) + ".max_height", 1.0)); + test_node_->declare_parameter( + std::string(POINTCLOUD_NAME) + ".min_range", rclcpp::ParameterValue(0.16)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POINTCLOUD_NAME) + ".min_range", 0.16)); + + pointcloud_ = std::make_shared( + test_node_, POINTCLOUD_NAME, tf_buffer_, + BASE_FRAME_ID, GLOBAL_FRAME_ID, + TRANSFORM_TOLERANCE, DATA_TIMEOUT, true); + pointcloud_->configure(); + + sendTransforms(curr_time); + + // Publish pointcloud data with points at various ranges + test_node_->publishPointCloudForMinRange(curr_time); + + // Wait until pointcloud receives the data + ASSERT_TRUE(waitPointCloud(500ms)); + + // Check PointCloud data - should only contain points with range >= min_range (0.16) + std::vector data; + pointcloud_->getData(curr_time, data); + + // Should have 3 points: first point (range ≈ 0.158) filtered out, remaining 3 pass + ASSERT_EQ(data.size(), 3u); + + // Point 1: (0.15 + 0.1, 0.0 + 0.1) = (0.25, 0.1) - range ≈ 0.212, passes + EXPECT_NEAR(data[0].x, 0.25, EPSILON); + EXPECT_NEAR(data[0].y, 0.1, EPSILON); + + // Point 2: (0.2 + 0.1, 0.0 + 0.1) = (0.3, 0.1) - range ≈ 0.25, passes + EXPECT_NEAR(data[1].x, 0.3, EPSILON); + EXPECT_NEAR(data[1].y, 0.1, EPSILON); + + // Point 3: (0.5 + 0.1, 0.0 + 0.1) = (0.6, 0.1) - range ≈ 0.522, passes + EXPECT_NEAR(data[2].x, 0.6, EPSILON); + EXPECT_NEAR(data[2].y, 0.1, EPSILON); +} + int main(int argc, char ** argv) { // Initialize the system