Include option to use PointCloud Transport #5264

elsayedelsheikh · 2025-06-12T18:53:34Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4042
Primary OS tested on	Ubuntu
Robotic platform tested on	Gazebo Simulation
Does this PR contain AI generated software?	No

Description of contribution in a few bullet points

Include option to use point_cloud_transport so that it works out of the box for the users.

obstacle_layer/voxel_layer plugins
STVL/NPVL plugins
collision_monitor node

Description of documentation updates required from your changes

New parameter point_cloud_transport to tune with default value set to "raw" that works with regular PointCloud2 without any compression.

      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: pointcloud
        pointcloud:
          topic: /rgbd_camera/points
          data_type: "PointCloud2"
          point_cloud_transport: "raw" # "draco" <-- Change this If you have compressed PointCloud2

Description of how this change was tested

I used turtelbot4 gazebo simulation and used depth_image_processing to publish PointCloud2 out of (rgb and depth) images, my launch file:

from launch import LaunchDescription
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode

def generate_launch_description():

    container = ComposableNodeContainer(
        name='depth_image_proc_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=[
            ComposableNode(
                package='depth_image_proc',
                plugin='depth_image_proc::PointCloudXyzrgbNode',
                name='point_cloud_xyzrgb_node',
                remappings=[
                    ('rgb/image_rect_color', 'rgbd_camera/image'),
                    ('depth_registered/image_rect', 'rgbd_camera/depth_image'),
                    ('points', 'rgbd_camera/points'),
                ],
            ),
        ]
    )

    return LaunchDescription([container])

Future work that may be required in bullet points

Need thoughts on performance evaluation
Usage tutorial

For Maintainers:

Check that any new parameters added are updated in docs.nav2.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

Signed-off-by: elsayedelsheikh <elsayed.elsheikh97@gmail.com>

elsayedelsheikh · 2025-06-12T19:25:01Z

Notes:
PointCloudTransport SubscriberFilter

doesn't have resubscribe() method so on Lifecycle activation, it needs to be re-initialized with arguments.
inherits from message_filters::SimpleFilter not message_filters::SubscriberBase which don't share common base class so observation_subscribers can't be added to the same vector.

mergify · 2025-06-12T20:02:32Z

@elsayedelsheikh, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

elsayedelsheikh · 2025-06-12T20:22:10Z

@SteveMacenski Any notes about re-initializing the subscriber in activate() method ?
If this implementation looks good, I’ll stick with it for the other plugins too. LMK your thoughts!
Thx

elsayedelsheikh · 2025-06-15T14:05:44Z

@SteveMacenski Hey Steve, Can you have a quick look on my implementation?
The build is fine with point_cloud_transport source, waiting for the next release to update the binaries with the new API

SteveMacenski · 2025-06-16T20:33:16Z

nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp

@@ -236,12 +237,14 @@ class ObstacleLayer : public CostmapLayer
  laser_geometry::LaserProjection projector_;
  /// @brief Used for the observation message filters
  #if RCLCPP_VERSION_GTE(29, 6, 0)
-  std::vector<std::shared_ptr<message_filters::SubscriberBase>>
-  observation_subscribers_;
+  std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan>>


I think we still want a vector of these for multiple possible input topics per layers (which is common)

is it fine to have two separate vectors, one for LaserScan data and another for PointCloud data?

std::vector<std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan>> observation_subscribers_laser_; std::vector<std::shared_ptr<point_cloud_transport::SubscriberFilter>> observation_subscribers_pointcloud_;

Even if I got them into a single void vector, the operations are different for these two classes

I think that could work, but it would be great if you spent a few minutes looking into the API of the message filter + PC2 transport to see if we can make a new base class or modify the existing base classes to make this work similar to how it works now with the SubscriberBase. I see both have mainly API subscribe / unsubscribe APIs but they differ in the arguments. Maybe there's a way to change this so that message filters has the update needed to make this work since I think its a useful feature for the community, not just Nav2.

For example: it looks like the issue is mostly just providing the transport on subscribe. Maybe we can instead have a setTransport() API or use the value given in the constructor so that the subscribes could look like the message filter subscriber base class'.

https://github.com/ros-perception/point_cloud_transport/blob/9fedca3d931346e3de8cc13aaa9e31deab540e0e/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp#L63

https://github.com/ros2/message_filters/blob/rolling/include/message_filters/subscriber.hpp#L137-L139

Working on it :)

SteveMacenski · 2025-06-16T20:34:10Z

nav2_costmap_2d/plugins/obstacle_layer.cpp


      observation_notifiers_.push_back(filter);
      observation_notifiers_.back()->setTolerance(rclcpp::Duration::from_seconds(
          tf_filter_tolerance));

    } else {
-      // For Kilted and Older Support from Message Filters API change


Does transport work with just normal topics when its not a pointcloud transport publisher? I want to make sure by removing this we're not removing support for current behavior that doesn't use the pointcloud transport. This should be additive, not a replacement

Yes, raw transport is just a normal PointCloud2, I also tested the behavior in the simulation.

But what about the topic names it subscribes to? The Raw I assume is <my_topic>/raw and <my_topic>/compressed similar to image transport, no? Would all the PC2s need to be configured with <my_topic> but actually publish on <my_topic>/raw? Perhaps PC2 transport is different than image transport, so please correct me if I'm wrong :-)

PC2 transports works directly with <my_topic> when raw transport is used...
One more addition I'm having in mind, There's a republish node that can be used to receive any kind of transport and publish another PC2 compressed type or even all available plugins, for example: receive <my_topic> of type raw and publish <my_topic>/draco, <my_topic>/zstd and <my_topic>/zlib so maybe the user can specify which type of transport to be used internally in nav2 even if he's providing different in_transport to nav2.

SteveMacenski · 2025-06-16T20:34:45Z

nav2_costmap_2d/plugins/obstacle_layer.cpp

-          node, topic, custom_qos_profile.get_rmw_qos_profile(), sub_opt);
-      #endif
-
+      std::shared_ptr<rclcpp::node_interfaces::NodeInterfaces<


auto here might do you some readability favors :-)

This also works :)

auto node_interfaces = std::make_shared< rclcpp::node_interfaces::NodeInterfaces< rclcpp::node_interfaces::NodeBaseInterface, rclcpp::node_interfaces::NodeParametersInterface, rclcpp::node_interfaces::NodeTopicsInterface, rclcpp::node_interfaces::NodeLoggingInterface > >(*node);

But I tried to keep things explicit to avoid any compiling problems on other platforms
Ok then, I'll use auto

Is the second level of templating actually needed? I also wonder if that is not deduced.

Yeah!

/usr/include/c++/13/bits/shared_ptr.h:1005:5: note: template argument deduction/substitution failed: /workspaces/dev_ws/src/navigation2/nav2_costmap_2d/plugins/obstacle_layer.cpp:294:10: error: type/value mismatch at argument 1 in template parameter list for ‘template<class _Tp, class ... _Args> std::shared_ptr<typename std::enable_if<(! std::is_array< <template-parameter-1-1> >::value), _Tp>::type> std::make_shared(_Args&& ...)’ 292 | auto node_interfaces = std::make_shared< | ~~~~~~~~~~~~~~~~~ 293 | rclcpp::node_interfaces::NodeInterfaces | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 294 | >( | ~^ 295 | node->get_node_base_interface(), | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 296 | node->get_node_parameters_interface(), | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 297 | node->get_node_topics_interface(), | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 298 | node->get_node_logging_interface() | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 299 | ); | ~ /workspaces/dev_ws/src/navigation2/nav2_costmap_2d/plugins/obstacle_layer.cpp:294:10: note: expected a type, got ‘NodeInterfaces’

SteveMacenski · 2025-06-16T20:35:07Z

nav2_costmap_2d/plugins/obstacle_layer.cpp

+          rclcpp::node_interfaces::NodeTopicsInterface,
+          rclcpp::node_interfaces::NodeLoggingInterface
+        >> node_interfaces = std::make_shared<
+        rclcpp::node_interfaces::NodeInterfaces<


I think these templates can be automatically deduced from the constructor arguments, no?

Lets not specify lots of templates unless we strictly need to, it makes the code a bit less easily readable

SteveMacenski · 2025-06-16T20:36:02Z

nav2_costmap_2d/plugins/obstacle_layer.cpp

+      std::shared_ptr<point_cloud_transport::SubscriberFilter> sub;
+      sub = std::make_shared<point_cloud_transport::SubscriberFilter>(
+        node_interfaces, topic,
+        point_cloud_transport);


What about QoS and sub_opt settings? Does this start as subscribing? If so, we'd need to pass those in so we don't later change those settings in the subscribe below

Yes, It starts subscribing with rmw_qos_profile_default and rclcpp::SubscriptionOptions()
We can use the default constructor that and then subscribe as below:

sub = std::make_shared<point_cloud_transport::SubscriberFilter>(); sub->subscribe(node_interfaces, topic, point_cloud_transport, custom_qos, sub_opts);

Ok! That's actually better since the default constructor doesn't try to subscribe automatically at start https://github.com/ros-perception/point_cloud_transport/blob/9fedca3d931346e3de8cc13aaa9e31deab540e0e/point_cloud_transport/src/subscriber_filter.cpp#L48-L50

SteveMacenski · 2025-06-16T20:38:41Z

nav2_costmap_2d/plugins/obstacle_layer.cpp

+    // PCT->getTopic() method doesn't work correctly
+    observation_subscribers_pointcloud_->subscribe(
+      node_interfaces,
+      "/rgbd_camera/points",


We should have this parameter stored somewhere to pull from

SteveMacenski · 2025-06-16T20:39:20Z

nav2_costmap_2d/plugins/obstacle_layer.cpp

+      );
+
+    rmw_qos_profile_t custom_qos = rmw_qos_profile_sensor_data;
+    custom_qos.depth = 50;


This does not match sub_opt

SteveMacenski · 2025-06-16T20:41:26Z

@elsayedelsheikh thanks for taking this on!

It might be nice to change the Transport code to have this constructor be templated by NodeT https://github.com/ros-perception/point_cloud_transport/blob/9fedca3d931346e3de8cc13aaa9e31deab540e0e/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp#L75-L85 with a default of rclcpp::Node when not otherwise specified. That would give the same behavior as before, but let us use this with the lifecycle node. It would involve a PR over there, but that's a good contribution.

Maybe there are a couple of PRs to PC2 Transport needed to make this possible? Such as :

doesn't have resubscribe() method so on Lifecycle activation, it needs to be re-initialized with arguments.

inherits from message_filters::SimpleFilter not message_filters::SubscriberBase which don't share common base class so observation_subscribers can't be added to the same vector.

It strikes me that perhaps they are not included for a technical reason but rather just not feature parallel to what we use now. They could possibly be implemented :-) Especially if these features are in the Image Transport (you should check), in which case they are direct analogs to be added here that should be easy merges for the maintainers. Might be good to look over the issues or file an issue about some of these items

elsayedelsheikh · 2025-06-17T14:19:12Z

@elsayedelsheikh thanks for taking this on!

My Pleasure, I was waiting for this :-)

It might be nice to change the Transport code to have this constructor be templated by NodeT https://github.com/ros-perception/point_cloud_transport/blob/9fedca3d931346e3de8cc13aaa9e31deab540e0e/point_cloud_transport/include/point_cloud_transport/subscriber_filter.hpp#L75-L85 with a default of rclcpp::Node when not otherwise specified. That would give the same behavior as before, but let us use this with the lifecycle node. It would involve a PR over there, but that's a good contribution.

Our first implementation was actually based on templated constructor but we followed this ros-perception/point_cloud_transport#109 (review) similar to ros2/geometry2#698 which recommends avoiding templates.

Maybe there are a couple of PRs to PC2 Transport needed to make this possible? Such as :

doesn't have resubscribe() method so on Lifecycle activation, it needs to be re-initialized with arguments.

inherits from message_filters::SimpleFilter not message_filters::SubscriberBase which don't share common base class so observation_subscribers can't be added to the same vector.
It strikes me that perhaps they are not included for a technical reason but rather just not feature parallel to what we use now. They could possibly be implemented :-) Especially if these features are in the Image Transport (you should check), in which case they are direct analogs to be added here that should be easy merges for the maintainers. Might be good to look over the issues or file an issue about some of these items

OK, Sure!

SteveMacenski · 2025-06-17T17:42:05Z

It might be good to consolidate a list of 'wants' and 'proposed changes' and file a ticket with me CCed in it in PC2 Transport for us to discuss with the maintainers. I really hate working with the node interfaces in the application code (i.e. Nav2) since I believe it should be hidden from me as a developer. Templates do that quite nicely. I'm not saying we have to remove the current interface constructors, but it would be nice to have a template option as well that calls the interface once in the library. They essentially already do that here but only for rclcpp::Node. Just template that function and this would work well for us.

elsayedelsheikh · 2025-06-18T09:21:14Z

It might be good to consolidate a list of 'wants' and 'proposed changes' and file a ticket with me CCed in it in PC2 Transport for us to discuss with the maintainers. I really hate working with the node interfaces in the application code (i.e. Nav2) since I believe it should be hidden from me as a developer. Templates do that quite nicely. I'm not saying we have to remove the current interface constructors, but it would be nice to have a template option as well that calls the interface once in the library. They essentially already do that here but only for rclcpp::Node. Just template that function and this would work well for us.

Agreed!

Initial thoughts for ObstacleLayer

33dbf96

Signed-off-by: elsayedelsheikh <elsayed.elsheikh97@gmail.com>

elsayedelsheikh force-pushed the include_point_cloud_transport branch from f3e3f8f to 33dbf96 Compare June 12, 2025 19:16

SteveMacenski requested changes Jun 16, 2025

View reviewed changes

Include option to use PointCloud Transport #5264

Are you sure you want to change the base?

Include option to use PointCloud Transport #5264

Conversation

elsayedelsheikh commented Jun 12, 2025

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

Future work that may be required in bullet points

For Maintainers:

Uh oh!

elsayedelsheikh commented Jun 12, 2025

Uh oh!

mergify bot commented Jun 12, 2025

Uh oh!

elsayedelsheikh commented Jun 12, 2025

Uh oh!

elsayedelsheikh commented Jun 15, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Jun 17, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Jun 17, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SteveMacenski commented Jun 16, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

elsayedelsheikh commented Jun 17, 2025

Uh oh!

SteveMacenski commented Jun 17, 2025

Uh oh!

elsayedelsheikh commented Jun 18, 2025

Uh oh!

Uh oh!

SteveMacenski Jun 17, 2025 •

edited

Loading

SteveMacenski Jun 17, 2025 •

edited

Loading

SteveMacenski commented Jun 16, 2025 •

edited

Loading