diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index c94180f94cf..8c33baed851 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp @@ -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_; diff --git a/nav2_collision_monitor/params/collision_detector_params.yaml b/nav2_collision_monitor/params/collision_detector_params.yaml index bbeaadc5154..dc488a6efc3 100644 --- a/nav2_collision_monitor/params/collision_detector_params.yaml +++ b/nav2_collision_monitor/params/collision_detector_params.yaml @@ -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 diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index ad7a5378068..7287b69115e 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -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 diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 72ef467b9a5..0d0f1f54045 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -106,8 +106,27 @@ bool PointCloud::getData( sensor_msgs::PointCloud2ConstIterator iter_y(*data_, "y"); sensor_msgs::PointCloud2ConstIterator 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{""}; + 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 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); @@ -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()}); + } } } return true; @@ -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) @@ -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(); } } }