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]

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

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]

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