Adding minimum range to PC2 in collision monitor (#5392)

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>
(cherry picked from commit 40a0451)

# Conflicts:
#	nav2_collision_monitor/src/pointcloud.cpp

Author: SteveMacenski

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_;

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,11 +124,47 @@ 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::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)
 {
   data_ = msg;
 }
 
+<<<<<<< HEAD
+=======
+rcl_interfaces::msg::SetParametersResult
+PointCloud::dynamicParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+
+  for (auto parameter : parameters) {
+    const auto & param_type = parameter.get_type();
+    const auto & param_name = parameter.get_name();
+    if(param_name.find(source_name_ + ".") != 0) {
+      continue;
+    }
+    if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
+      if (param_name == source_name_ + "." + "min_height") {
+        min_height_ = parameter.as_double();
+      } else if (param_name == source_name_ + "." + "max_height") {
+        max_height_ = parameter.as_double();
+      } else if (param_name == source_name_ + "." + "min_range") {
+        min_range_ = parameter.as_double();
+      }
+    } else if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
+      if (param_name == source_name_ + "." + "enabled") {
+        enabled_ = parameter.as_bool();
+      }
+    }
+  }
+  result.successful = true;
+  return result;
+}
+
+>>>>>>> 40a04517 (Adding minimum range to PC2 in collision monitor (#5392))
 }  // namespace nav2_collision_monitor

nav2_collision_monitor/test/sources_test.cpp

@@ -140,6 +140,59 @@ class TestNode : public nav2_util::LifecycleNode
     pointcloud_pub_->publish(std::move(msg));
   }
 
+  void publishPointCloudForMinRange(const rclcpp::Time & stamp)
+  {
+    pointcloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(
+      POINTCLOUD_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+    pointcloud_pub_->on_activate();
+
+    std::unique_ptr<sensor_msgs::msg::PointCloud2> msg =
+      std::make_unique<sensor_msgs::msg::PointCloud2>();
+    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<float> iter_x(*msg, "x");
+    sensor_msgs::PointCloud2Iterator<float> iter_y(*msg, "y");
+    sensor_msgs::PointCloud2Iterator<float> 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<sensor_msgs::msg::Range>(
@@ -296,6 +349,8 @@ class Tester : public ::testing::Test
 public:
   Tester();
   ~Tester();
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 
 protected:
   // Data sources creation routine
@@ -319,10 +374,6 @@ class Tester : public ::testing::Test
   std::shared_ptr<PointCloudWrapper> pointcloud_;
   std::shared_ptr<RangeWrapper> range_;
   std::shared_ptr<PolygonWrapper> polygon_;
-
-private:
-  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
-  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 };  // Tester
 
 Tester::Tester()
@@ -741,6 +792,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<PointCloudWrapper>(
+    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<nav2_collision_monitor::Point> 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