Skip to content

Include option to use PointCloud Transport #5264

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 2 commits into
base: main
Choose a base branch
from
+13 −27
Draft

Include option to use PointCloud Transport #5264

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ad5fea7
Rebase
elsayedelsheikh Jul 22, 2025
0c8a4c0
Make linters happy
elsayedelsheikh Jul 22, 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
3 changes: 3 additions & 0 deletions nav2_costmap_2d/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@ find_package(tf2_ros REQUIRED)
find_package(nav2_ros_common REQUIRED)
find_package(tf2_sensor_msgs REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(point_cloud_transport REQUIRED)

nav2_package()

Expand Down Expand Up @@ -87,6 +88,7 @@ target_link_libraries(layers PUBLIC
nav2_ros_common::nav2_ros_common
tf2::tf2
nav2_ros_common::nav2_ros_common
point_cloud_transport::point_cloud_transport
)
target_link_libraries(layers PRIVATE
pluginlib::pluginlib
Expand Down Expand Up @@ -221,6 +223,7 @@ ament_export_dependencies(
tf2_ros
tf2_sensor_msgs
nav2_ros_common
point_cloud_transport
)
pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml)
ament_package()
1 change: 1 addition & 0 deletions nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,7 @@
#pragma GCC diagnostic pop

#include "message_filters/subscriber.hpp"
#include "point_cloud_transport/subscriber_filter.hpp"

#include "nav_msgs/msg/occupancy_grid.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
Expand Down
2 changes: 2 additions & 0 deletions nav2_costmap_2d/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,8 @@
<depend>tf2_sensor_msgs</depend>
<depend>visualization_msgs</depend>
<depend>nav2_ros_common</depend>
<depend>point_cloud_transport</depend>
<depend>point_cloud_transport_plugins</depend>

<test_depend>ament_lint_common</test_depend>
<test_depend>ament_lint_auto</test_depend>
Expand Down
34 changes: 7 additions & 27 deletions nav2_costmap_2d/plugins/obstacle_layer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -141,7 +141,7 @@ void ObstacleLayer::onInitialize()
while (ss >> source) {
// get the parameters for the specific topic
double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
std::string topic, sensor_frame, data_type;
std::string topic, sensor_frame, data_type, point_cloud_transport;
bool inf_is_valid, clearing, marking;

declareParameter(source + "." + "topic", rclcpp::ParameterValue(source));
Expand All @@ -158,6 +158,8 @@ void ObstacleLayer::onInitialize()
declareParameter(source + "." + "obstacle_min_range", rclcpp::ParameterValue(0.0));
declareParameter(source + "." + "raytrace_max_range", rclcpp::ParameterValue(3.0));
declareParameter(source + "." + "raytrace_min_range", rclcpp::ParameterValue(0.0));
declareParameter(source + "." + "point_cloud_transport",
Copy link
Member

Choose a reason for hiding this comment

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

Add to configuration files the population of this parameter with a commented out bit on the available options for consideration

rclcpp::ParameterValue(std::string("raw")));

node->get_parameter(name_ + "." + source + "." + "topic", topic);
node->get_parameter(name_ + "." + source + "." + "sensor_frame", sensor_frame);
Expand All @@ -173,6 +175,8 @@ void ObstacleLayer::onInitialize()
node->get_parameter(name_ + "." + source + "." + "inf_is_valid", inf_is_valid);
node->get_parameter(name_ + "." + source + "." + "marking", marking);
node->get_parameter(name_ + "." + source + "." + "clearing", clearing);
node->get_parameter(name_ + "." + source + "." + "point_cloud_transport",
Copy link
Member

Choose a reason for hiding this comment

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

New param should be in the configuration guide :-)

point_cloud_transport);

if (!(data_type == "PointCloud2" || data_type == "LaserScan")) {
RCLCPP_FATAL(
Expand Down Expand Up @@ -287,32 +291,8 @@ void ObstacleLayer::onInitialize()
tf_filter_tolerance));

} else {
// For Kilted and Older Support from Message Filters API change
#if RCLCPP_VERSION_GTE(29, 6, 0)
std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>> sub;
#else
std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::PointCloud2,
rclcpp_lifecycle::LifecycleNode>> sub;
#endif

// For Kilted compatibility in Message Filters API change
#if RCLCPP_VERSION_GTE(29, 6, 0)
sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>>(
node, topic, custom_qos_profile, sub_opt);
// For Jazzy compatibility in Message Filters API change
#elif RCLCPP_VERSION_GTE(29, 0, 0)
sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::PointCloud2,
rclcpp_lifecycle::LifecycleNode>>(
std::static_pointer_cast<rclcpp_lifecycle::LifecycleNode>(node),
topic, custom_qos_profile, sub_opt);
// For Humble and Older compatibility in Message Filters API change
#else
sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::PointCloud2,
rclcpp_lifecycle::LifecycleNode>>(
std::static_pointer_cast<rclcpp_lifecycle::LifecycleNode>(node),
topic, custom_qos_profile.get_rmw_qos_profile(), sub_opt);
#endif

auto sub = std::make_shared<point_cloud_transport::SubscriberFilter>(
Copy link
Member

Choose a reason for hiding this comment

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

I think you need to readd the backward compatibility for Jazzy/Kilted

*node, topic, point_cloud_transport, custom_qos_profile, sub_opt);
sub->unsubscribe();

if (inf_is_valid) {
Expand Down
Loading