From 5860a275211066978170dd70cdbc77b741628ec9 Mon Sep 17 00:00:00 2001 From: Greg Anderson <107634795+greganderson-vermeer@users.noreply.github.com> Date: Mon, 6 Oct 2025 12:41:04 -0500 Subject: [PATCH] Create iterator for custom height field in pointcloud. Signed-off-by: Greg Anderson <107634795+greganderson-vermeer@users.noreply.github.com> --- nav2_collision_monitor/src/pointcloud.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 72ef467b9a5..552e4431b30 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -105,10 +105,12 @@ bool PointCloud::getData( sensor_msgs::PointCloud2ConstIterator iter_x(*data_, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(*data_, "y"); sensor_msgs::PointCloud2ConstIterator iter_z(*data_, "z"); + sensor_msgs::PointCloud2ConstIterator iter_height(*data_, "height"); // Refill data array with PointCloud points in base frame - for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_height) { // Transform point coordinates from source frame -> to base frame + //Not transforming height since it is already relative to ground tf2::Vector3 p_v3_s(*iter_x, *iter_y, *iter_z); // Check range from sensor origin before transformation @@ -120,7 +122,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 (*iter_height >= min_height_ && *iter_height <= max_height_) { data.push_back({p_v3_b.x(), p_v3_b.y()}); } }