Include option to use PointCloud Transport

[elsayedelsheikh]

---

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

[elsayedelsheikh]

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[bot]]

@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]

@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]

@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]

@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]

@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 https://github.com/ros-perception/point_cloud_transport/pull/109#pullrequestreview-2702512986 similar to https://github.com/ros2/geometry2/issues/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]

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]

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!

[SteveMacenski]

This ready for a review again?

[elsayedelsheikh]

This ready for a review again?

Yeah, I tested the new signatures and it works fine! I didn't change the PR status till I get the other parts done as well :)

[mergify[bot]]

@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).

[mergify[bot]]

This pull request is in conflict. Could you fix it @elsayedelsheikh?

[elsayedelsheikh]

Generally approved! Are we just waiting on the PC transport code to be released into binaries?

I've just updated nav2_collision_monitor. We're ready to merge whenever PCT is released

[mergify[bot]]

@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).

[mergify[bot]]

@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).

[SteveMacenski]

@elsayedelsheikh check out uncrustify failures to fix please! Also the documentation update PR would be dandy

[elsayedelsheikh]

@elsayedelsheikh check out uncrustify failures to fix please! Also the documentation update PR would be dandy

• Done here ✅
• Moving to docs PR!

[mergify[bot]]

@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]

@SteveMacenski Jazzy/Kilted jobs Passed :white_check_mark:

[mergify[bot]]

@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).

[SteveMacenski]

@elsayedelsheikh the main PR doesn't build :-)

--- stderr: nav2_costmap_2d                   
/opt/overlay_ws/src/navigation2/nav2_costmap_2d/plugins/obstacle_layer.cpp: In member function ‘virtual void nav2_costmap_2d::ObstacleLayer::onInitialize()’:
/opt/overlay_ws/src/navigation2/nav2_costmap_2d/plugins/obstacle_layer.cpp:344:41: error: no matching function for call to ‘std::vector<std::shared_ptr<message_filters::SubscriberBase> >::push_back(std::shared_ptr<point_cloud_transport::SubscriberFilter>&)’
  344 |       observation_subscribers_.push_back(sub);
      |       ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~

Once that is squared away, we just need those documentation updates for docs.nav2.org on the configuration guide + the migration guide about the new feature

[codecov[bot]]

Codecov Report

:x: Patch coverage is 83.33333% with 2 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_costmap_2d/plugins/obstacle_layer.cpp	50.00%	2 Missing :warning:

Files with missing lines	Coverage Δ
nav2_collision_monitor/src/pointcloud.cpp	81.81% <100.00%> (+1.49%)	:arrow_up:
...tmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp	100.00% <ø> (ø)
nav2_costmap_2d/plugins/obstacle_layer.cpp	82.10% <50.00%> (+0.11%)	:arrow_up:

... and 4 files with indirect coverage changes

:rocket: New features to boost your workflow:

• :snowflake: Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[elsayedelsheikh]

I also see CI failing very oddly. I just retriggered it but if we fail in a similar way again, we may need to look into it.

After rebasing to recent updates, Jazzy & Kilted builds are failing due to missing header files tf2_ros/buffer.hpp

#10 197.6 --- stderr: nav2_util
#10 197.6 In file included from /root/ws/src/nav2_util/src/robot_utils.cpp:25:
#10 197.6 /root/ws/src/nav2_util/include/nav2_util/robot_utils.hpp:29:10: fatal error: tf2_ros/buffer.hpp: No such file or directory
#10 197.6    29 | #include "tf2_ros/buffer.hpp"
#10 197.6       |          ^~~~~~~~~~~~~~~~~~~~
#10 197.6 compilation terminated.
#10 197.6 gmake[2]: *** [src/CMakeFiles/nav2_util_core.dir/build.make:118: src/CMakeFiles/nav2_util_core.dir/robot_utils.cpp.o] Error 1
#10 197.6 gmake[2]: *** Waiting for unfinished jobs....
#10 197.6 gmake[1]: *** [CMakeFiles/Makefile2:210: src/CMakeFiles/nav2_util_core.dir/all] Error 2
#10 197.6 gmake[1]: *** Waiting for unfinished jobs....
#10 197.6 gmake: *** [Makefile:146: all] Error 2
#10 197.6 ---
#10 197.6 Failed   <<< nav2_util [12.2s, exited with code 2]

[SteveMacenski]

Yeah that's fine - I'm looking at the circleCI job for rolling which had a weird failure. The kilted/jazzy are known due to those API changes (will come back in the coming weeks once Jazzy/Kilted have new binary releases to reflect those API chaneges as well)

[elsayedelsheikh]

Yeah that's fine - I'm looking at the circleCI job for rolling which had a weird failure. The kilted/jazzy are known due to those API changes (will come back in the coming weeks once Jazzy/Kilted have new binary releases to reflect those API chaneges as well)

Perfect!

[SteveMacenski]

@elsayedelsheikh can you show this building on jazzy/kilted prior to rebase? Our CI is down for those at the moment as I mentioned above and I want to verify those work before I merge since this has changes specific to dfiferent distributions.

[elsayedelsheikh]

@SteveMacenski Jazzy/Kilted jobs Passed ✅

@SteveMacenski They passed since https://github.com/ros-navigation/navigation2/commit/e2f7fd19d9827b474434c78d9893d573e50cc177