Backporting custom height point field into Jazzy. (ros-navigation#5646)

* Summary commit of all changes for adding custom pointcloud field height. (ros-navigation#5586)

Doing this to clear out unsigned commits from history.

Signed-off-by: Greg Anderson <107634795+greganderson-vermeer@users.noreply.github.com>

* Corrected parameter declaration methods that aren't part of Jazzy.
Fixed parameter typo from some manual merging in pointcloud.cpp

Signed-off-by: Greg Anderson <107634795+greganderson-vermeer@users.noreply.github.com>

---------

Signed-off-by: Greg Anderson <107634795+greganderson-vermeer@users.noreply.github.com>

Author: greganderson-vermeer

nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp

@@ -97,6 +97,10 @@ class PointCloud : public Source
   double min_height_, max_height_;
   // Minimum range from sensor origin to filter out close points
   double min_range_;
+  /**Changes height check from "z" field to "height" field for pipelines utilizing
+   * ground contouring
+   */
+  bool use_global_height_;
 
   /// @brief Latest data obtained from pointcloud
   sensor_msgs::msg::PointCloud2::ConstSharedPtr data_;

nav2_collision_monitor/params/collision_monitor_params.yaml

@@ -98,4 +98,5 @@ collision_monitor:
       topic: "/intel_realsense_r200_depth/points"
       min_height: 0.1
       max_height: 0.5
+      use_global_height: False
       enabled: True

nav2_collision_monitor/src/pointcloud.cpp

@@ -88,11 +88,35 @@ bool PointCloud::getData(
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(*data_, "y");
   sensor_msgs::PointCloud2ConstIterator<float> iter_z(*data_, "z");
 
+  bool height_present = false;
+  for (const auto & field : data_->fields) {
+    if (field.name == "height") {
+      height_present = true;
+    }
+  }
+
+// Reference height field
+  std::string height_field{"z"};
+  if (use_global_height_ && height_present) {
+    height_field = "height";
+  } else if (use_global_height_) {
+    RCLCPP_ERROR(logger_, "[%s]: 'use_global_height' parameter true but height field not in cloud",
+      source_name_.c_str());
+    return false;
+  }
+  sensor_msgs::PointCloud2ConstIterator<float> iter_height(*data_, height_field);
+
   // Refill data array with PointCloud points in base frame
   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);
 
+    double data_height = *iter_z;
+    if (use_global_height_) {
+      data_height = *iter_height;
+      ++iter_height;
+    }
+
     // Check range from sensor origin before transformation
     double range = p_v3_s.length();
     if (range < min_range_) {
@@ -102,7 +126,7 @@ bool PointCloud::getData(
     tf2::Vector3 p_v3_b = tf_transform * p_v3_s;
 
     // Refill data array
-    if (p_v3_b.z() >= min_height_ && p_v3_b.z() <= max_height_) {
+    if (data_height >= min_height_ && data_height <= max_height_) {
       data.push_back({p_v3_b.x(), p_v3_b.y()});
     }
   }
@@ -127,6 +151,9 @@ void PointCloud::getParameters(std::string & source_topic)
   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();
+  nav2_util::declare_parameter_if_not_declared(
+    node, source_name_ + ".use_global_height", rclcpp::ParameterValue(false));
+  use_global_height_ = node->get_parameter(source_name_ + ".use_global_height").as_bool();
 }
 
 void PointCloud::dataCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)

nav2_collision_monitor/test/collision_monitor_node_test.cpp

@@ -162,6 +162,7 @@ class Tester : public ::testing::Test
   void setPolygonVelocityVectors(
     const std::string & polygon_name,
     const std::vector<std::string> & polygons);
+  void setGlobalHeightParams(const std::string & source_name, const double min_height);
 
   // Setting TF chains
   void sendTransforms(const rclcpp::Time & stamp);
@@ -172,6 +173,9 @@ class Tester : public ::testing::Test
   // Main topic/data working routines
   void publishScan(const double dist, const rclcpp::Time & stamp);
   void publishPointCloud(const double dist, const rclcpp::Time & stamp);
+  void publishPointCloudWithHeight(
+    const double dist, const double height,
+    const rclcpp::Time & stamp);
   void publishRange(const double dist, const rclcpp::Time & stamp);
   void publishPolygon(const double dist, const rclcpp::Time & stamp);
   void publishCmdVel(const double x, const double y, const double tw);
@@ -491,6 +495,8 @@ void Tester::addSource(
       source_name + ".max_height", rclcpp::ParameterValue(1.0));
     cm_->set_parameter(
       rclcpp::Parameter(source_name + ".max_height", 1.0));
+    cm_->declare_parameter(
+      source_name + ".use_global_height", rclcpp::ParameterValue(false));
   } else if (type == RANGE) {
     cm_->declare_parameter(
       source_name + ".type", rclcpp::ParameterValue("range"));
@@ -547,6 +553,12 @@ void Tester::setPolygonVelocityVectors(
   cm_->set_parameter(rclcpp::Parameter(polygon_name + ".velocity_polygons", polygons));
 }
 
+void Tester::setGlobalHeightParams(const std::string & source_name, const double min_height)
+{
+  cm_->set_parameter(rclcpp::Parameter(source_name + ".use_global_height", true));
+  cm_->set_parameter(rclcpp::Parameter(source_name + ".min_height", min_height));
+}
+
 void Tester::sendTransforms(const rclcpp::Time & stamp)
 {
   std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster =
@@ -654,6 +666,45 @@ void Tester::publishPointCloud(const double dist, const rclcpp::Time & stamp)
   pointcloud_pub_->publish(std::move(msg));
 }
 
+void Tester::publishPointCloudWithHeight(
+  const double dist, const double height,
+  const rclcpp::Time & stamp)
+{
+  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(
+    4, "x", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "y", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "z", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "height", 1, sensor_msgs::msg::PointField::FLOAT32);
+  modifier.resize(2);
+
+  sensor_msgs::PointCloud2Iterator<float> iter_x(*msg, "x");
+  sensor_msgs::PointCloud2Iterator<float> iter_y(*msg, "y");
+  sensor_msgs::PointCloud2Iterator<float> iter_z(*msg, "z");
+  sensor_msgs::PointCloud2Iterator<float> iter_height(*msg, "height");
+
+  // Point 0: (dist, 0.01, 0.2, height)
+  *iter_x = dist;
+  *iter_y = 0.01;
+  *iter_z = 0.2;
+  *iter_height = height;
+  ++iter_x; ++iter_y; ++iter_z; ++iter_height;
+
+  // Point 1: (dist, -0.01, 0.2, height)
+  *iter_x = dist;
+  *iter_y = -0.01;
+  *iter_z = 0.2;
+  *iter_height = height;
+
+  pointcloud_pub_->publish(std::move(msg));
+}
+
 void Tester::publishRange(const double dist, const rclcpp::Time & stamp)
 {
   std::unique_ptr<sensor_msgs::msg::Range> msg =
@@ -1656,6 +1707,64 @@ TEST_F(Tester, testVelocityPolygonStop)
   cm_->stop();
 }
 
+TEST_F(Tester, testVelocityPolygonStopGlobalHeight)
+{
+  // Set Collision Monitor parameters.
+  // Add velocity polygon with 2 sub polygon:
+  // 1. Forward:  0 -> 0.5 m/s
+  // 2. Backward: 0 -> -0.5 m/s
+  setCommonParameters();
+  addPolygon("VelocityPolygon", VELOCITY_POLYGON, 1.0, "stop");
+  addPolygonVelocitySubPolygon("VelocityPolygon", "Forward", 0.0, 0.5, 0.0, 1.0, 4.0);
+  addPolygonVelocitySubPolygon("VelocityPolygon", "Backward", -0.5, 0.0, 0.0, 1.0, 2.0);
+  setPolygonVelocityVectors("VelocityPolygon", {"Forward", "Backward"});
+  addSource(POINTCLOUD_NAME, POINTCLOUD);
+  setGlobalHeightParams(POINTCLOUD_NAME, 0.5);
+  setVectors({"VelocityPolygon"}, {POINTCLOUD_NAME});
+
+  cm_->set_parameter(
+    rclcpp::Parameter("source_timeout", 2.0));
+
+  rclcpp::Time curr_time = cm_->now();
+  // Start Collision Monitor node
+  cm_->start();
+  // Check that robot stops when source is enabled
+  sendTransforms(curr_time);
+
+  // 1. Obstacle is in Forward velocity polygon and below global height
+  publishPointCloudWithHeight(3.0, 0.4, curr_time);
+  ASSERT_FALSE(waitData(std::hypot(3.0, 0.01), 500ms, curr_time));
+  publishCmdVel(0.4, 0.0, 0.1);
+  ASSERT_TRUE(waitCmdVel(500ms));
+  ASSERT_NEAR(cmd_vel_out_->linear.x, 0.4, EPSILON);
+  ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON);
+  ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON);
+
+  // 2. Obstacle is in Forward velocity polygon and above global height
+  publishPointCloudWithHeight(3.0, 0.6, curr_time);
+  ASSERT_TRUE(waitData(std::hypot(3.0, 0.01), 500ms, curr_time));
+  publishCmdVel(0.4, 0.0, 0.1);
+  ASSERT_TRUE(waitCmdVel(500ms));
+  ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON);
+  ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON);
+  ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON);
+  ASSERT_TRUE(waitActionState(500ms));
+  ASSERT_EQ(action_state_->action_type, STOP);
+  ASSERT_EQ(action_state_->polygon_name, "VelocityPolygon");
+
+  // 3. Pointcloud without height field, invalid source.
+  publishPointCloud(2.5, curr_time);
+  ASSERT_FALSE(waitData(std::hypot(2.5, 0.01), 100ms, curr_time));
+  publishCmdVel(3.0, 3.0, 3.0);
+  ASSERT_FALSE(waitCmdVel(500ms));
+  ASSERT_TRUE(waitActionState(500ms));
+  ASSERT_EQ(action_state_->action_type, STOP);
+  ASSERT_EQ(action_state_->polygon_name, "invalid source");
+
+  // Stop Collision Monitor node
+  cm_->stop();
+}
+
 TEST_F(Tester, testSourceAssociatedToPolygon)
 {
   // Set Collision Monitor parameters: