Port point_cloud_transport to ROS2

[john-maidbot]

This is a VERY rough draft of my attempts to port point_cloud_transport from ROS to ROS2. Comments/feedback are very welcome but just be aware that it is a very early port and is not yet reflective of the final form I have in mind.

Some key notes on the current status of the port:

1. The C++ code and python bindings build without errors 🎉 and we have passing CI
2. Plugins (raw + draco + zlib) load properly
3. There are a variety of places where I just directly ported the ROS functionality to ROS2 but where I want to refactor it further to better make use of recent ROS2 functionality (e.g. latched topics, components, type adaptation, etc...). My reasoning for doing this is to first sanity check the implementation in ros2 and verify Draco is working as expected before diving into these optimizations.

TODO (Not necessarily in sequential order):

• First pass, get list_transports and republisher to build and run in ROS2 with "raw" transport
• First pass, get list_transports and republisher to build and run in ROS2 with "draco" transport (Port point_cloud_transport_plugins to ROS2 point_cloud_transport_plugins#1)
• Cleanup the configuration of the publishers and subscribers. Users should be able to pass qos and options like with a normal rclcpp pub/sub <------ Currently being worked on by @ahcorde Refactor republisher #4
• Fix the point_cloud_codec.hpp/.cpp files so that the ability to encode/decode pointclouds using the transport plugins is exposed without having to republish topics. <------ Currently being worked on by @john-maidbot : Fix pointcloud-codec and python bindings #3
• Find a better way to reconcile the cras dependencies.
• Make republisher executable a composition to respect the fact that it was originally a nodelet <------ Currently being worked on by @ahcorde Refactor republisher #4
• Work through some examples about type adaptation to better understand how to implement this in the library. (https://github.com/john-maidbot/learn_ros2_type_adaptation) Type adaptation PCL point_cloud_transport_plugins#4 PCL adaptation
• Make a ROS discourse post about the relationship between transport libraries and type adaptation (ROS2 port ctu-vras/point_cloud_transport#1 (comment)). Still brainstorming 🧠 ⚡
• Refactor the dynamically reconfigurable parameters (currently hard-coded) to use the new ros2 api: https://docs.ros.org/en/iron/Tutorials/Intermediate/Monitoring-For-Parameter-Changes-CPP.html
• Fix the tests in the cmake files (Currently commented out) <------ Currently being worked on by @ahcorde Refactor republisher #4 https://github.com/ahcorde/point_cloud_transport_tutorial/pull/1/commits Added common tests point_cloud_transport_plugins#2
• Fix the python bindings and port that api from rospy to rclpy: Fix pointcloud-codec and python bindings #3
• To be complete, I think point_cloud_codec needs a way to pass in a custom configuration at runtime without using dynamic reconfigure. I think a yaml or json style string would be a good solution for this.
• Second pass, general cleanup before requesting review from people

Ideas that should be their own PR's:

• rviz2 plugin Added point_cloud_transport ros2/rviz#1008
• Tutorial about how to create your own plugin https://github.com/paplhjak/templateplugin_point_cloud_transport << @john-maidbot
• Move documentation to ros2 documentation (already removed references to ros wiki)
• Replace the blacklist with a whilelist (i.e. list the transports you want rather than the ones you dont want) -- @ahcorde
• Port pointcloud transport tutorial: https://github.com/ctu-vras/point_cloud_transport_tutorial -- @john-maidbot ROS2 Port point_cloud_transport_tutorial#2

[john-maidbot]

jdang@jjdang:~/Desktop/experiment_ws$ ros2 run point_cloud_transport list_transports 
Lookup name: point_cloud_transport/draco_pub
Transport name: point_cloud_transport/draco
Lookup name: point_cloud_transport/raw_pub
Transport name: point_cloud_transport/raw
Declared transports:
point_cloud_transport/draco
point_cloud_transport/raw

Details:
----------
"point_cloud_transport/draco"
 - Provided by package: draco_point_cloud_transport
 - Publisher: 
            This plugin publishes a CompressedPointCloud2 using KD tree compression.
        
 - Subscriber: 
            This plugin decompresses a CompressedPointCloud2 topic.
        
----------
"point_cloud_transport/raw"
 - Provided by package: point_cloud_transport
 - Publisher: 
            This is the default publisher. It publishes the PointCloud2 as-is on the base topic.
        
 - Subscriber: 
            This is the default pass-through subscriber for topics of type sensor_msgs/PointCloud2.
        

[ahcorde]

Hi @john-maidbot,

I'm also interested in contributing to this port. You have done a very nice initial work. Where do you plan to centralize this work ? Is this the main meta-ticket ?

Let me know how I can help and how we can distribute tasks and reviews

[john-maidbot]

Hello @ahcorde ,

Thanks for your interest! Yes, my plan was to start making PR's into the ros2 branches of this repo and the point_cloud_transport_plugin repo to address the TODO's I listed above.

I have not yet given a lot of thought to how I could distribute tasks and reviews (if you have ideas please lmk 🙂 ). But in the meantime, we could update the above TODO list to reflect what is being worked on and by whom?

These were the next things I wanted to make progress on. If you have interest in one of these (or any of the other TODO's above), please let me know and I can mark them appropriately above (we could probably break these up):

• Cleanup the configuration of the publishers and subscribers. Users should be able to pass qos and options like with a normal rclcpp pub/sub
• Fix the point_cloud_codec.hpp/.cpp files so that the ability to encode/decode pointclouds using the transport plugins is exposed without having to republish topics.

Also, are you familiar with type adaptation by any chance?

[ahcorde]

I read long time ago about the type adaptation but I'm not familiar. I need to read documentation as well ;)

Let me know if you are already working in some of these two things, otherwise, I will start with the first bullet point

[john-maidbot]

awesome! I will take the second bullet point then. I have updated the TODO list above to reflect this.
thanks again for the help!

[john-maidbot]

@ahcorde just wanted to say thank you so much for all your recent PR's! 🚀 I sadly havent had much time to put into this project the past two weeks, but I should have a lot of time this week to put into this.
I reviewed all your current PR's, and I should have the pointcloud_codec and python bindings PR ready for review in the next couple of days.

[peci1]

Hi. I'm also subscribed to this PR, so if you have any questions to me, just ask. But I won't be able to help with much ROS 2 specifics...

[ahcorde]

Thank for your support @peci1

I started to migrate point_cloud_transport_tutorial too
ahcorde/point_cloud_transport_tutorial#1

[ahcorde]

Draft PR to include point_cloud_transport in Rviz2 ros2/rviz#1008

[peci1]

Draft PR to include point_cloud_transport in Rviz2 ros2/rviz#1008

Awesome, I wanted to do that for ROS 1 too, but haven't found the time for it yet :) Thanks!

[ahcorde]

@peci1 I have a question for you,

When I start the draco plugin I need to open the rqt_reconfigure tool to uncheck the force_quantization flag, but I started the code with this flag deactivate I still see the error (and then I need to check and uncheck the param):

[ERROR] [1689244703.737228226] [point_cloud_transport]: Error encoding message by transport draco: Draco encoder returned code -1: Failed to encode point attributes...

[peci1]

At least in ROS 1, the flag defaults to True: https://github.com/ctu-vras/point_cloud_transport_plugins/blob/2751d9a7fbcdceaa97673b9577d8d02746a459a0/draco_point_cloud_transport/cfg/DracoPublisher.cfg#L31 .

Do I understand correctly that this error shows when you set the param to false before starting the publisher?

[ahcorde]

This is the most recent code with the default parameters:

https://github.com/john-maidbot/point_cloud_transport_plugins/blob/7d96f96c0414418f07af65d6b5be3ab499fba1cd/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_publisher.hpp#L70-L83

The error appears with the default parameters, to get rid of the error I need to unset force_quantization.

[peci1]

Hmm, can you get the stacktrace when the error is written?

[ahcorde]

it's inside the EncodePointCloudToBuffer method.

draco::Status status = encoder.EncodePointCloudToBuffer(*pc, &encode_buffer);

[peci1]

Maybe it needs the calls to encoder.SetAttributeQuantization() done even in case no (or automatic) encoding method is set?

If that doesn't help, I think you have to start debugging Draco. The problem should be discoverable in this function: https://github.com/google/draco/blob/9f856abaafb4b39f1f013763ff061522e0261c6f/src/draco/compression/point_cloud/point_cloud_encoder.cc#L101 .

[john-maidbot]

@ahcorde are you seeing this error when you build by any chance? Or do you have any ideas about what the issue might be?

CMake Error at ament_cmake_symlink_install/ament_cmake_symlink_install.cmake:100 (message):
  ament_cmake_symlink_install_directory() can't find
  '.../ros_ws/build/point_cloud_transport/_deps/expected-src/TYPE'
Call Stack (most recent call first):
  ament_cmake_symlink_install/ament_cmake_symlink_install.cmake:317 (ament_cmake_symlink_install_directory)
  cmake_install.cmake:46 (include)

I wonder if its related to the FetchContent command?

[john-maidbot]

I finally have the pointcloud codec PR ready for review! 😃 (sorry it took so long, finding non-work time at my computer has been challenging this month)

@ahcorde there is no rush, but could you please review that PR when you have time? It is a lot of changes, so if you would prefer, I can try to break it up into separate PR's? Just lmk 🙂

In the meantime, I will look into that build error either later today or later this week.

[john-maidbot]

@ahcorde are you seeing this error when you build by any chance? Or do you have any ideas about what the issue might be?

CMake Error at ament_cmake_symlink_install/ament_cmake_symlink_install.cmake:100 (message):
  ament_cmake_symlink_install_directory() can't find
  '.../ros_ws/build/point_cloud_transport/_deps/expected-src/TYPE'
Call Stack (most recent call first):
  ament_cmake_symlink_install/ament_cmake_symlink_install.cmake:317 (ament_cmake_symlink_install_directory)
  cmake_install.cmake:46 (include)

I wonder if its related to the FetchContent command?

ok found a seemingly related issue on ament_cmake: ament/ament_cmake#471

And it seems that just building without --symlink-install resolves the problem.

[ahcorde]

PR in rosdistro to add draco and make it available from rosdep ros/rosdistro#38056

[ahcorde]

I will merge this PR and I will open issue for the remaining open tasks

[peci1]

Does this mean you're about to release the package in the current state?

[ahcorde]

I would love to release the package at some point (at least in rolling)

[peci1]

Good. I wanted to go through the code and have a look at the points where it diverges from the ROS 1 version, and probably discuss some of the points. I think I might have time for it this weekend ;)