Skip to content

Create iterator for custom height field in pointcloud. #5586

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
wants to merge 8 commits into
base: main
Choose a base branch
from
+36 −3
Draft

Create iterator for custom height field in pointcloud. #5586

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
5860a27
Create iterator for custom height field in pointcloud.
greganderson-vermeer Oct 6, 2025
7e6ccc9
Add use_global_height_ parameter to allow configuration
greganderson-vermeer Oct 7, 2025
ca49b05
Ran ament lint/style tools, cleaned up.
greganderson-vermeer Oct 7, 2025
11f04c7
Added some pointcloud field checking based on run time
greganderson-vermeer Oct 7, 2025
869b5a9
Address PR formatting feedback.
greganderson-vermeer Oct 7, 2025
1cdbc05
Add return false for error state.
greganderson-vermeer Oct 7, 2025
b2fbb06
Implemented additional changes for iterator feedback
greganderson-vermeer Oct 7, 2025
5636fc1
Implementing Steve's suggestion for performance and
greganderson-vermeer Oct 7, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,6 +113,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_;
Expand Down
1 change: 1 addition & 0 deletions nav2_collision_monitor/params/collision_detector_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,4 +26,5 @@ collision_detector:
transport_type: "raw" # Options: raw, zlib, draco, zstd
min_height: 0.1
max_height: 0.5
use_global_height: False
enabled: True
1 change: 1 addition & 0 deletions nav2_collision_monitor/params/collision_monitor_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -100,4 +100,5 @@ collision_monitor:
transport_type: "raw" # Options: raw, zlib, draco, zstd
min_height: 0.1
max_height: 0.5
use_global_height: False
enabled: True
36 changes: 33 additions & 3 deletions nav2_collision_monitor/src/pointcloud.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -106,8 +106,27 @@ 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) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
for(const auto & field : data_->fields) {
for (const auto & field : data_->fields) {

if(field.name == "height") {
height_present = true;
}
}

// Reference height field
std::string height_field{""};
if(use_global_height_ && height_present) {
height_field = "height";
} else if(use_global_height_) {
throw std::runtime_error{"'use_global_height' parameter true but height field not in cloud"};
} else {
// If height field not present fill iterator with z field
height_field = "z";
}
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) {
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_height) {
// Transform point coordinates from source frame -> to base frame
tf2::Vector3 p_v3_s(*iter_x, *iter_y, *iter_z);

Expand All @@ -120,8 +139,14 @@ 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_) {
data.push_back({p_v3_b.x(), p_v3_b.y()});
if(use_global_height_) {
if (*iter_height >= min_height_ && *iter_height <= max_height_) {
data.push_back({p_v3_b.x(), p_v3_b.y()});
}
} else {
if (*iter_z >= min_height_ && *iter_z <= max_height_) {
data.push_back({p_v3_b.x(), p_v3_b.y()});
}
Copy link
Member

@SteveMacenski SteveMacenski Oct 7, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If here when if(use_global_height_) we do the ++iter_height, then we can remove it from the for loop, which also allows us not to initialize it with z when not set. This will improve performance for users without the height element since we're not double iterating through the Z channel. The else then can also be removed.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I had originally tried implementing as you described, but had two problems.

  1. I couldn't find a way to initilize iter_height inside the if statement that didn't give an undeclared error for iter_height inside the for loop. This is possibly a skill issue on my part, please advise if there is a method I am unaware of.
  2. If removing the ++iter_height from the for loop and adding it to the loop body then it also needs to be incremented at the < min_range check to keep the iterators synchronized. I saw it as a potential foot gun in future development if other checks are implemented but miss incrementing the height iterator. I will defer to your judgement on that implementation.

In the last commit I pushed I duplicated the for loop with the different iterator implementations as an additional option. I'm not a fan of the code duplication but it worked and would address performance concerns.

Copy link
Member

@SteveMacenski SteveMacenski Oct 7, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Perhaps something like:


  bool height_present = false;
  for(const auto & field : data_->fields) {
    if (field.name == "height") {
      height_present = true;
    }
  }

  // Reference height field
  std::string height_field{""};
  if (use_global_height_ && height_present) {
    height_field = "height";
  } else if (use_global_height_) {
    // Log error here TODO
    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_) {
      continue;
    }
    tf2::Vector3 p_v3_b = tf_transform * p_v3_s;
    // Refill data array
    if (data_height >= min_height_ && data_height <= max_height_) {
      data.push_back({p_v3_b.x(), p_v3_b.y()});
    }
  }

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This doesn't increment the iter_height unless use_global_height_ = true, which resolves the computation issue since the iterator doesn't do a copy of the PC data. It also removes duplication of the loop and in the loop by just replacing a locally stored version of data_height that is set as z when not using the height. Then height is incremented before any processing happens to keep it aligned.

What do you think?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That works except the iter_height object gives a run time error that the field is not found since height_field is initialized as an empty string. Since you said creating the iterator doesn't copy the PC data that shouldn't be a performance impact. And in that case I will just initialize it as the "z" field and then overwrite it to "height" for the global height case?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah that works!

}
}
return true;
Expand All @@ -148,6 +173,9 @@ void PointCloud::getParameters(std::string & source_topic)
nav2::declare_parameter_if_not_declared(
node, source_name_ + ".transport_type", rclcpp::ParameterValue(std::string("raw")));
transport_type_ = node->get_parameter(source_name_ + ".transport_type").as_string();
nav2::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)
Expand Down Expand Up @@ -178,6 +206,8 @@ PointCloud::dynamicParametersCallback(
} else if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
if (param_name == source_name_ + "." + "enabled") {
enabled_ = parameter.as_bool();
} else if (param_name == source_name_ + "." + "use_global_height") {
use_global_height_ = parameter.as_bool();
}
}
}
Expand Down
Loading