diff --git a/CMakeLists.txt b/CMakeLists.txt index 3dae8e2..e0007ec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,23 +1,78 @@ cmake_minimum_required(VERSION 3.10.2) + project(point_cloud_transport_tutorial) -find_package(catkin REQUIRED COMPONENTS cras_cpp_common point_cloud_transport rosbag rosgraph_msgs sensor_msgs topic_tools) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -catkin_package() +find_package(ament_cmake_ros REQUIRED) -include_directories(${catkin_INCLUDE_DIRS}) +find_package(point_cloud_transport REQUIRED) +find_package(point_cloud_transport_plugins REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rcpputils REQUIRED) +find_package(rosbag2_cpp REQUIRED) +find_package(sensor_msgs REQUIRED) + +include_directories( + include +) + +# encoder +add_executable(encoder_test src/my_encoder.cpp) +ament_target_dependencies(encoder_test point_cloud_transport rosbag2_cpp sensor_msgs) # publisher add_executable(publisher_test src/my_publisher.cpp) -add_dependencies(publisher_test ${catkin_EXPORTED_TARGETS}) -target_link_libraries(publisher_test ${catkin_LIBRARIES}) +ament_target_dependencies(publisher_test point_cloud_transport rclcpp rcpputils rosbag2_cpp sensor_msgs) # subscriber add_executable(subscriber_test src/my_subscriber.cpp) -add_dependencies(subscriber_test ${catkin_EXPORTED_TARGETS}) -target_link_libraries(subscriber_test ${catkin_LIBRARIES}) +ament_target_dependencies(subscriber_test point_cloud_transport rclcpp sensor_msgs) -# encoder -add_executable(encoder_test src/my_encoder.cpp) -add_dependencies(encoder_test ${catkin_EXPORTED_TARGETS}) -target_link_libraries(encoder_test ${catkin_LIBRARIES}) +# Install executables +install( + TARGETS + encoder_test + publisher_test + subscriber_test + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY + resources + DESTINATION share/${PROJECT_NAME} +) + +# linting tests +if(BUILD_TESTING) + find_package(ament_cmake_copyright REQUIRED) + find_package(ament_cmake_cppcheck REQUIRED) + find_package(ament_cmake_cpplint REQUIRED) + find_package(ament_cmake_lint_cmake REQUIRED) + find_package(ament_cmake_uncrustify REQUIRED) + find_package(ament_cmake_xmllint REQUIRED) + + ament_copyright(EXCLUDE ${_linter_excludes}) + ament_cppcheck( + EXCLUDE ${_linter_excludes} + LANGUAGE c++ + ) + ament_cpplint(EXCLUDE ${_linter_excludes}) + ament_lint_cmake() + ament_uncrustify( + LANGUAGE c++ + ) + ament_xmllint() +endif() + + +ament_package() diff --git a/README.md b/README.md index db787be..8e331d1 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ # \ - **v0.1.** + **ROS2 v0.1.** _**Contents**_ @@ -18,236 +18,291 @@ _**Contents**_ * [Implementing Custom Plugins](#implementing-custom-plugins) # Writing a Simple Publisher -In this section, we'll see how to create a publisher node, which opens a .bag file and publishes PointCloud2 messages from it. - -This tutorial assumes, that you have created your workspace during [](https://github.com/paplhjak/point_cloud_transport) [installation](https://github.com/paplhjak/point_cloud_transport#installation). - -Before we start, change to the directory and clone this repository: -~~~~~ bash -$ cd ~/point_cloud_transport_ws/src -$ git clone https://github.com/paplhjak/point_cloud_transport_tutorial.git -~~~~~ +In this section, we'll see how to create a publisher node, which opens a ROS 2 bag and publishes `PointCloud2` messages from it. + +This tutorial assumes that you have created your workspace containing [](https://github.com/ros-perception/point_cloud_transport) and [](https://github.com/ros-perception/point_cloud_transport_plugins) + +Before we start, change to the directory, clone this repository, and unzip the example rosbag in the resources folder: +```bash +$ cd ~//src +$ git clone https://github.com/ros-perception/point_cloud_transport_tutorial +$ cd point_cloud_transport_tutorial +$ tar -C resources/ -xvf resources/rosbag2_2023_08_05-16_08_51.tar.xz +$ cd ~/ +$ colcon build --merge-install --event-handlers console_direct+ +``` ## Code of the Publisher -Take a look at my_publisher.cpp: +Take a look at my_publisher.cpp ```cpp -#include -#include -#include -#include -#include - -int main(int argc, char** argv) +#include + +// for reading rosbag +#include +#include +#include +#include +#include +#include +#include + +int main(int argc, char ** argv) { - ros::init(argc, argv, "point_cloud_publisher"); - ros::NodeHandle nh; - - point_cloud_transport::PointCloudTransport pct(nh); - point_cloud_transport::Publisher pub = pct.advertise("pct/point_cloud", 10); - - rosbag::Bag bag; - bag.open(argv[1], rosbag::bagmode::Read); - - ros::Rate loop_rate(5); - for(rosbag::MessageInstance const m: rosbag::View(bag)) - { - sensor_msgs::PointCloud2::ConstPtr i = m.instantiate(); - if (i != nullptr) - { - pub.publish(i); - ros::spinOnce(); - loop_rate.sleep(); - } - if (!nh.ok()) - { - break; - } + rclcpp::init(argc, argv); + + auto node = std::make_shared("point_cloud_publisher"); + + point_cloud_transport::PointCloudTransport pct(node); + point_cloud_transport::Publisher pub = pct.advertise("pct/point_cloud", 100); + + const std::string bagged_cloud_topic = "/point_cloud"; + const std::string shared_directory = ament_index_cpp::get_package_share_directory( + "point_cloud_transport_tutorial"); + const std::string bag_file = shared_directory + "/resources/rosbag2_2023_08_05-16_08_51"; + + // boiler-plate to tell rosbag2 how to read our bag + rosbag2_storage::StorageOptions storage_options; + storage_options.uri = bag_file; + storage_options.storage_id = "mcap"; + rosbag2_cpp::ConverterOptions converter_options; + converter_options.input_serialization_format = "cdr"; + converter_options.output_serialization_format = "cdr"; + + // open the rosbag + rosbag2_cpp::readers::SequentialReader reader; + reader.open(storage_options, converter_options); + + sensor_msgs::msg::PointCloud2 cloud_msg; + rclcpp::Serialization cloud_serialization; + while (reader.has_next() && rclcpp::ok()) { + // get serialized data + auto serialized_message = reader.read_next(); + rclcpp::SerializedMessage extracted_serialized_msg(*serialized_message->serialized_data); + if (serialized_message->topic_name == bagged_cloud_topic) { + // deserialize and convert to message + cloud_serialization.deserialize_message(&extracted_serialized_msg, &cloud_msg); + // publish the message + pub.publish(cloud_msg); + rclcpp::spin_some(node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); } + } + reader.close(); + + node.reset(); + rclcpp::shutdown(); } ``` + ## Code of Publisher Explained Now we'll break down the code piece by piece. -Header for including [](https://github.com/paplhjak/point_cloud_transport): -```cpp -#include -``` -Headers for opening .bag file with sensor_msgs::PointCloud2 messages: +Header for including [](https://github.com/ros-perception/point_cloud_transport): ```cpp -#include -#include +#include ``` -Initializing the ROS node: -```cpp -ros::init(argc, argv, "point_cloud_publisher"); -ros::NodeHandle nh; -``` -Creates *PointCloudTransport* instance and initializes it with our *NodeHandle*. Methods of *PointCloudTransport* can later be used to create point cloud publishers and subscribers similar to how methods of *NodeHandle* are used to create generic publishers and subscribers. + +Creates *PointCloudTransport* instance and initializes it with our *Node* shared pointer. Methods of *PointCloudTransport* can later be used to create point cloud publishers and subscribers similar to how methods of *Node* are used to create generic publishers and subscribers. + ```cpp -point_cloud_transport::PointCloudTransport pct(nh); +point_cloud_transport::PointCloudTransport pct(node); ``` + Uses *PointCloudTransport* method to create a publisher on base topic *"pct/point_cloud"*. Depending on whether more plugins are built, additional (per-plugin) topics derived from the base topic may also be advertised. The second argument is the size of our publishing queue. + ```cpp point_cloud_transport::Publisher pub = pct.advertise("pct/point_cloud", 10); ``` -Opens .bag file given as an argument to the program: -```cpp -rosbag::Bag bag; -bag.open(argv[1], rosbag::bagmode::Read); -``` -Publishes sensor_msgs::PointCloud2 message from the specified .bag with frequency of 5Hz: +Publishes sensor_msgs::PointCloud2 message from the specified rosbag: ```cpp -ros::Rate loop_rate(5); - for(rosbag::MessageInstance const m: rosbag::View(bag)) - { - sensor_msgs::PointCloud2::ConstPtr i = m.instantiate(); - if (i != nullptr) - { - pub.publish(i); - ros::spinOnce(); - loop_rate.sleep(); - } - if (!nh.ok()) - { - break; - } - } + sensor_msgs::msg::PointCloud2 cloud_msg; + //... rosbag boiler plate to populate cloud_msg ... + // publish the message + pub.publish(cloud_msg); + // spin the node... + rclcpp::spin_some(node); + // repeat... ``` ## Example of Running the Publisher -To run my_publisher.cpp, open terminal in the root of workspace and run the following: -~~~~~ bash -$ catkin_make_isolated -$ source devel_isolated/setup.bash -$ rosrun point_cloud_transport_tutorial publisher_test /path/to/my_bag.bag -~~~~~ -Of course, roscore/master must also be running. +To run [my_publisher.cpp](src/my_publisher.cpp) open terminal in the root of workspace and run the following: + +```bash +$ source install/setup.bash +$ ros2 run point_cloud_transport_tutorial publisher_test +``` # Writing a Simple Subscriber -In this section, we'll see how to create a subscriber node, which receives PointCloud2 messages and prints the number of points in them. +In this section, we'll see how to create a subscriber node, which receives `PointCloud2` messages and prints the number of points in them. ## Code of the Subscriber -Take a look at my_subscriber.cpp: +Take a look at [my_subscriber.cpp](src/my_subscriber.cpp): + ```cpp -#include -#include -#include +#include +#include +#include -void Callback(const sensor_msgs::PointCloud2ConstPtr& msg) +int main(int argc, char ** argv) { - std::cout << "Message received, number of points is: " << msg->width*msg->height << std::endl; -} + rclcpp::init(argc, argv); + auto node = std::make_shared("point_cloud_subscriber"); -int main(int argc, char **argv) -{ - ros::init(argc, argv, "point_cloud_subscriber", ros::init_options::AnonymousName); - ros::NodeHandle nh; + point_cloud_transport::PointCloudTransport pct(node); + point_cloud_transport::Subscriber pct_sub = pct.subscribe( + "pct/point_cloud", 100, + [node](const sensor_msgs::msg::PointCloud2::ConstSharedPtr & msg) + { + RCLCPP_INFO_STREAM( + node->get_logger(), + "Message received, number of points is: " << msg->width * msg->height); + }, {}); + + RCLCPP_INFO_STREAM(node->get_logger(), "Waiting for point_cloud message..."); - point_cloud_transport::PointCloudTransport pct(nh); - point_cloud_transport::Subscriber sub = pct.subscribe("pct/point_cloud", 100, Callback); - ros::spin(); + rclcpp::spin(node); - return 0; + rclcpp::shutdown(); + + return 0; } ``` + ## Code of Subscriber Explained Now we'll break down the code piece by piece. -Header for including [](https://github.com/paplhjak/point_cloud_transport): +Header for including [](https://github.com/ros-perception/point_cloud_transport): + ```cpp -#include +#include ``` -A callback function, which we will bind to the subscriber. Whenever our subscriber receives a message, the Callback function gets executed and number of points in the message (*msg->width * msg->height*) is printed. + +Initializes the ROS node: + ```cpp -void Callback(const sensor_msgs::PointCloud2ConstPtr& msg) -{ - std::cout << "Message received, number of points is: " << msg->width*msg->height << std::endl; -} +rclcpp::init(argc, argv); +auto node = rclcpp::Node::make_shared("point_cloud_subscriber"); ``` -Initializes the ROS node: +Creates *PointCloudTransport* instance and initializes it with our *Node*. Methods of *PointCloudTransport* can later be used to create point cloud publishers and subscribers similar to how methods of *NodeHandle* are used to create generic publishers and subscribers. + ```cpp -ros::init(argc, argv, "point_cloud_subscriber", ros::init_options::AnonymousName); -ros::NodeHandle nh; +point_cloud_transport::PointCloudTransport pct(node); ``` -Creates *PointCloudTransport* instance and initializes it with our *NodeHandle*. Methods of *PointCloudTransport* can later be used to create point cloud publishers and subscribers similar to how methods of *NodeHandle* are used to create generic publishers and subscribers. + +Uses *PointCloudTransport* method to create a subscriber on base topic *"pct/point_cloud"*. The second argument is the size of our subscribing queue. The third argument tells the subscriber to execute lambda function whenever a message is received. + ```cpp -point_cloud_transport::PointCloudTransport pct(nh); + point_cloud_transport::Subscriber pct_sub = pct.subscribe( + "pct/point_cloud", 100, + [node](const sensor_msgs::msg::PointCloud2::ConstSharedPtr & msg) + { + RCLCPP_INFO_STREAM( + node->get_logger(), + "draco message received, number of points is: " << msg->width * msg->height); + }, {}); ``` -Uses *PointCloudTransport* method to create a subscriber on base topic *"pct/point_cloud"*. The second argument is the size of our subscribing queue. The third argument tells the subscriber to execute Callback function, whenever a message is received. + +### Select a specific transport + +Or you can select a specific transport using the *TransportHint* class. Creates a *TransportHint* shared pointer. +This is how to tell the subscriber that we want to subscribe to a particular transport +(in this case "pct/point_cloud/draco"), rather than the raw "pct/point_cloud" topic. + ```cpp -point_cloud_transport::Subscriber sub = pct.subscribe("pct/point_cloud", 100, Callback); + auto transport_hint = std::make_shared("draco"); +``` + +Uses *PointCloudTransport* method to create a subscriber on base topic *"pct/point_cloud"* and add the `transport_hint` variable as the last argument. + +```cpp + auto transport_hint = std::make_shared("draco"); + point_cloud_transport::Subscriber pct_sub = pct.subscribe( + "pct/point_cloud", 100, + [node](const sensor_msgs::msg::PointCloud2::ConstSharedPtr & msg) + { + RCLCPP_INFO_STREAM( + node->get_logger(), + "draco message received, number of points is: " << msg->width * msg->height); + }, {}, transport_hint.get()); ``` ## Example of Running the Subscriber To run my_subscriber.cpp, open terminal in the root of workspace and run the following: -~~~~~ bash -$ catkin_make_isolated -$ source devel_isolated/setup.bash -$ rosrun point_cloud_transport_tutorial subscriber_test -~~~~~ -Of course, roscore/master must also be running. + +```bash +$ source install/setup.bash +$ ros2 run point_cloud_transport_tutorial subscriber_test --ros-args -p point_cloud_transport:=draco +``` + +The `point_cloud_transport` parameter is read by the point_cloud_transport library. The complexity of the parameters is hidden in the library. # Using Publishers And Subscribers With Plugins -In this section, we'll first make sure that the nodes are running properly. Later on, we'll change the transport to use Draco compressed format. + ## Running the Publisher and Subsriber -Make sure that roscore/master is up and running: -~~~~~ bash -$ roscore -~~~~~ + Now we can run the Publisher/Subsriber nodes. To run both start two terminal tabs and enter commands: -~~~~~ bash -$ source devel_isolated/setup.bash -$ rosrun point_cloud_transport_tutorial subscriber_test -~~~~~ + +```bash +$ source install/setup.bash +$ ros2 run point_cloud_transport_tutorial subscriber_test +``` + And in the second tab: -~~~~~ bash -$ source devel_isolated/setup.bash -$ rosrun point_cloud_transport_tutorial publisher_test /path/to/my_bag.bag -~~~~~ + +```bash +$ source install/setup.bash +$ ros2 run point_cloud_transport_tutorial publisher_test +$ # or choose which plugin you want load (a.k.a. whitelist them). +$ ros2 run point_cloud_transport_tutorial publisher_test --ros-args -p pct.point_cloud.enable_pub_plugins:=["point_cloud_transport/draco"] +``` + If both nodes are running properly, you should see the subscriber node start printing out messages similar to: -~~~~~ bash -Message received, number of points is: 63744 -~~~~~ + +```bash +Message received, number of points is: XXX +``` To list the topics, which are being published and subscribed to, enter command: -~~~~~ bash -$ rostopic list -v -~~~~~ +```bash +$ ros2 topic list +``` + The output should look similar to this: -~~~~~ bash +```bash Published topics: - * /rosout [rosgraph_msgs/Log] 1 publisher - * /pct/point_cloud [sensor_msgs/PointCloud2] 1 publisher - * /rosout_agg [rosgraph_msgs/Log] 1 publisher + * /parameter_events [rcl_interfaces/msg/ParameterEvent] 3 publishers + * /pct/point_cloud [sensor_msgs/msg/PointCloud2] 1 publisher + * /pct/point_cloud/draco [point_cloud_interfaces/msg/CompressedPointCloud2] 1 publisher + * /pct/point_cloud/zlib [point_cloud_interfaces/msg/CompressedPointCloud2] 1 publisher + * /rosout [rcl_interfaces/msg/Log] 3 publishers Subscribed topics: - * /rosout [rosgraph_msgs/Log] 1 subscriber - * /pct/point_cloud [sensor_msgs/PointCloud2] 1 subscriber -~~~~~ + * /parameter_events [rcl_interfaces/msg/ParameterEvent] 2 subscribers + * /pct/point_cloud/draco [point_cloud_interfaces/msg/CompressedPointCloud2] 1 subscriber +``` + To display the ROS computation graph, enter command: -~~~~~ bash -$ rqt_graph -~~~~~ +```bash +$ ros2 run rqt_graph rqt_graph +``` You should see a graph similar to this: -![Graph1](https://github.com/paplhjak/point_cloud_transport_tutorial/blob/master/readme_images/rosgraph1.png) +![Graph1](https://github.com/ros-perception/point_cloud_transport_tutorial/blob/rolling/readme_images/rosgraph1.png) ## Changing the Transport Used -Currently our nodes are communicating raw sensor_msgs/PointCloud2 messages, so we are not gaining anything over using basic ros::Publisher and ros::Subscriber. We can change that by introducing a new transport. -The [](https://github.com/paplhjak/draco_point_cloud_transport) package provides plugin for [](https://github.com/paplhjak/point_cloud_transport), which sends point clouds over the wire encoded through kd-tree or sequantial compression. Notice that draco_point_cloud_transport is not a dependency of your package; point_cloud_transport automatically discovers all transport plugins built in your ROS system. +To check which plugins are built on your machine, enter command: -Assuming you have followed [](https://github.com/paplhjak/point_cloud_transport) [installation](https://github.com/paplhjak/point_cloud_transport#installation), you should already have [](https://github.com/paplhjak/draco_point_cloud_transport) built. +```bash +$ ros2 run point_cloud_transport list_transports +``` -To check which plugins are built on your machine, enter command: -~~~~~ bash -$ rosrun point_cloud_transport list_transports -~~~~~ You should see output similar to: -~~~~~ bash + +```bash Declared transports: point_cloud_transport/draco point_cloud_transport/raw @@ -256,71 +311,67 @@ Details: ---------- "point_cloud_transport/draco" - Provided by package: draco_point_cloud_transport - - Publisher: + - Publisher: This plugin publishes a CompressedPointCloud2 using KD tree compression. - - - Subscriber: + + - Subscriber: This plugin decompresses a CompressedPointCloud2 topic. - + ---------- "point_cloud_transport/raw" - Provided by package: point_cloud_transport - - Publisher: + - Publisher: This is the default publisher. It publishes the PointCloud2 as-is on the base topic. - - - Subscriber: + + - Subscriber: This is the default pass-through subscriber for topics of type sensor_msgs/PointCloud2. -~~~~~ -Shut down your publisher node and restart it. If you list the published topics again and have [](https://github.com/paplhjak/draco_point_cloud_transport) installed, you should see a new one: +``` -~~~~~ bash - * /pct/point_cloud/draco [draco_point_cloud_transport/CompressedPointCloud2] 1 publisher -~~~~~ +Shut down your publisher node and restart it. If you list the published topics again and have [](https://github.com/ros-perception/point_cloud_transport_plugins) installed, you should see: -Now let's start up a new subscriber, this one using draco transport. The key is that [](https://github.com/paplhjak/point_cloud_transport) subscribers check the parameter ~point_cloud_transport for the name of a transport to use in place of "raw". Let's set this parameter and start a subscriber node with name "draco_listener": +```bash + * /pct/point_cloud/draco [draco_point_cloud_transport/CompressedPointCloud2] 1 publisher +``` -~~~~~ bash -$ rosparam set /draco_listener/point_cloud_transport draco -$ rosrun point_cloud_transport_tutorial subscriber_test __name:=draco_listener -~~~~~ +```bash +ros2 run point_cloud_transport_tutorial my_subscriber --ros-args -r __node:=draco_listener -p point_cloud_transport:= +ros2 run point_cloud_transport_tutorial my_subscriber --ros-args -r __node:=draco_listener -p point_cloud_transport:=draco +``` If we check the node graph again: -~~~~~ bash -$ rqt_graph -~~~~~ - -![Graph2](https://github.com/paplhjak/point_cloud_transport_tutorial/blob/master/readme_images/rosgraph2.png) - -We can see, that draco_listener is listening to a separate topic carrying compressed messages. - -Note that if you just want the draco messages, you can change the parameter globally in-line: +```bash +rqt_graph +``` -~~~~~ bash -$ rosrun point_cloud_transport_tutorial subscriber_test _point_cloud_transport:=draco -~~~~~ +![Graph2](https://github.com/ros-perception/point_cloud_transport_tutorial/blob/rolling/readme_images/rosgraph2.png) ## Changing Transport Behavior -For a particular transport, we may want to tweak settings such as compression level and speed, quantization of particular attributes of point cloud, etc. Transport plugins can expose such settings through rqt_reconfigure. For example, /point_cloud_transport/draco/ allows you to change multiple parameters of the compression on the fly. +For a particular transport, we may want to tweak settings such as compression level and speed, quantization of particular attributes of point cloud, etc. Transport plugins can expose such settings through `rqt_reconfigure`. For example, `/point_cloud_transport/draco/` allows you to change multiple parameters of the compression on the fly. For now let's adjust the position quantization. By default, "draco" transport uses quantization of 14 bits, allowing 16384 distinquishable positions in each axis; let's change it to 8 bits (256 positions): -~~~~~ bash -$ rosrun rqt_reconfigure rqt_reconfigure -~~~~~ +```bash +$ ros2 run rqt_reconfigure rqt_reconfigure +``` -Now pick /pct/point_cloud/draco in the drop-down menu and move the quantization_POSITION slider down to 8. If you visualize the messages, such as in RVIZ, you should be able to see the level of detail of the point cloud drop. +Now pick `/pct/point_cloud/draco` in the drop-down menu and move the quantization_POSITION slider down to 8. If you visualize the messages, such as in RVIZ, you should be able to see the level of detail of the point cloud drop. -Dynamic Reconfigure has updated the dynamically reconfigurable parameter /pct/point_cloud/draco/quantization_POSITION. You can verify this by running: +Dynamic Reconfigure has updated the dynamically reconfigurable parameter `/pct/point_cloud/draco/quantization_POSITION`. You can verify this by running: -~~~~~ bash -rosparam get /pct/point_cloud/draco/quantization_POSITION -~~~~~ +``` bash +ros2 param get /point_cloud_subscriber /pct/point_cloud/draco/quantization_POSITION +``` This should display 8. -Full explanation of the reconfigure parameters and an example of how to use them can be found at [](https://github.com/paplhjak/draco_point_cloud_transport) repository. +Full explanation of the reconfigure parameters and an example of how to use them can be found at [](https://github.com/ros-perception/point_cloud_transport_plugins) repository. + +### Whitelist point cloud transport -# Implementing Custom Plugins -The process of implementing your own plugins is described in a separate [repository](https://github.com/paplhjak/templateplugin_point_cloud_transport). +This allows you to specify plugins you do want to load (a.k.a. whitelist them). + +```bash +ros2 run point_cloud_transport_tutorial publisher_test --ros-args -p pct.point_cloud.enable_pub_plugins:=["point_cloud_transport/zlib"] +``` diff --git a/package.xml b/package.xml index 8a962ec..82e656f 100644 --- a/package.xml +++ b/package.xml @@ -1,25 +1,40 @@ point_cloud_transport_tutorial - 1.0.0 + + 0.0.0 + Tutorial for point_cloud_transport. + Jakub Paplham - Martin Pecka + + John D'Angelo + BSD - catkin + ament_cmake_ros - cras_cpp_common point_cloud_transport - rosbag - rosgraph_msgs + rclcpp + rcpputils + rosbag2_cpp sensor_msgs - topic_tools - cras_cpp_common - draco_point_cloud_transport + point_cloud_transport_plugins point_cloud_transport - rosbag - rosgraph_msgs + rclcpp + rcpputils + rosbag2_cpp sensor_msgs - topic_tools + + ament_cmake_copyright + ament_cmake_cppcheck + ament_cmake_cpplint + ament_cmake_lint_cmake + ament_cmake_uncrustify + ament_cmake_xmllint + + + ament_cmake + + diff --git a/resources/rosbag2_2023_08_05-16_08_51.tar.xz b/resources/rosbag2_2023_08_05-16_08_51.tar.xz new file mode 100644 index 0000000..b4126f8 Binary files /dev/null and b/resources/rosbag2_2023_08_05-16_08_51.tar.xz differ diff --git a/scripts/publisher.py b/scripts/publisher.py new file mode 100644 index 0000000..143cdfc --- /dev/null +++ b/scripts/publisher.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2023 Open Source Robotics Foundation, Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the TU Darmstadt nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Publisher that automatically publishes to all declared transports.""" + +import point_cloud_transport_py + +import time +import sys + +import rclpy +from rclpy.node import Node +from rclpy.serialization import deserialize_message, serialize_message +from sensor_msgs.msg import PointCloud2 +import rosbag2_py +from rosidl_runtime_py.utilities import get_message +from point_cloud_transport_py.common import pointCloud2ToString + +def pointCloud2ToString(msg: PointCloud2): + buffer = serialize_message(msg) + return buffer + +if __name__ == '__main__': + + rclpy.init(args=sys.argv) + + bag_path = argv[1] + serialization_format='cdr' + + storage_options = rosbag2_py.StorageOptions( + uri=bag_path) + + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format=serialization_format, + output_serialization_format=serialization_format) + + reader = rosbag2_py.SequentialReader() + reader.open(storage_options, converter_options) + + topic_types = reader.get_all_topics_and_types() + + # Create a map for quicker lookup + type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} + + pct = point_cloud_transport_py.PointCloudTransport("point_cloud_transport", ""); + pub = pct.advertise("pct/point_cloud", 100); + + try: + while reader.has_next(): + (topic, data, t) = reader.read_next() + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + pub.publish(pointCloud2ToString(msg)) + time.sleep(0.1) + except Exception as e: + print('Error in publisher node!') + print(e) + finally: + # if publisher_node is not None: + # publisher_node.destroy_node() + rclpy.shutdown() diff --git a/scripts/subscriber_old_school.py b/scripts/subscriber_old_school.py new file mode 100644 index 0000000..fb5c89a --- /dev/null +++ b/scripts/subscriber_old_school.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2023 Open Source Robotics Foundation, Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the TU Darmstadt nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Subscriber automatically converting from any transport to raw.""" + +from point_cloud_transport_py import PointCloudCodec, VectorString +from point_cloud_transport_py.common import stringToMsgType, stringToPointCloud2, TransportInfo + +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +from sensor_msgs.msg import PointCloud2 + + +def _get_loadable_transports(codec: PointCloudCodec): + transports = VectorString() + names = VectorString() + codec.getLoadableTransports(transports, names) + return dict(zip(transports, names)) + + +def _get_topic_to_subscribe(codec, base_topic, transport_name): + (topic, name, data_type) = codec.getTopicToSubscribe( + base_topic, transport_name) + + if len(data_type) == 0: + return None + + return TransportInfo(name, topic, data_type) + + +class Subscriber(Node): + + def __init__(self): + node_name = 'point_cloud_transport_subscriber' + super().__init__(node_name) + + self.base_topic = '/pct/point_cloud' + self.transport = self.get_parameter_or('transport', 'draco') + self.codec = PointCloudCodec() + + transports = _get_loadable_transports(self.codec) + if self.transport not in transports and self.transport not in transports.values(): + raise RuntimeError( + 'Point cloud transport "%s" not found.' % (self.transport,)) + + self.transport_info = _get_topic_to_subscribe( + self.codec, self.base_topic, self.transport) + if self.transport_info is None: + raise RuntimeError( + 'Point cloud transport "%s" not found.' % (self.transport,)) + + # subscribe to compressed, serialized msg + self.subscriber = self.create_subscription(stringToMsgType( + self.transport_info.data_type), self.transport_info.topic, self.cb, qos_profile_sensor_data) + + def cb(self, cloud): + print(cloud.height * cloud.width) + + +if __name__ == '__main__': + import rclpy + import sys + + rclpy.init(args=sys.argv) + + subscriber_node = None + try: + subscriber_node = Subscriber() + rclpy.spin(subscriber_node) + except Exception as e: + print(e) + finally: + if subscriber_node is not None: + subscriber_node.destroy_node() + rclpy.shutdown() diff --git a/src/my_encoder.cpp b/src/my_encoder.cpp index d1ec5c0..7dab6bc 100644 --- a/src/my_encoder.cpp +++ b/src/my_encoder.cpp @@ -1,148 +1,97 @@ // SPDX-License-Identifier: BSD-3-Clause // SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak -#include -#include -#include -#include -#include -#include -#include -#include -#include +// This file is a simple example of how to use the C++ "PointCloudCodec" API of +// point_cloud_transport to encode/decode point clouds without needing to +// publish/subscribe to a topic. -int main(int argc, char** argv) +// for ros2 logging + reading rosbag +#include +#include +#include +#include +#include +#include +#include + +#include + +int main(int argc, char ** argv) { - ros::console::initialize(); - ros::Time::init(); - - point_cloud_transport::PointCloudCodec pct; - - std::string codec = "draco"; - dynamic_reconfigure::Config config; - if (argc > 2) - codec = argv[2]; - else - { - config.ints.emplace_back(); - config.ints[0].name = "quantization_POSITION"; - config.ints[0].value = 10; - } - auto encoder = pct.getEncoderByName(codec); - if (!encoder) - return 1; - - ROS_INFO("Found encoder: %s", encoder->getTransportName().c_str()); - - rosbag::Bag bag; - bag.open(argv[1], rosbag::bagmode::Read); + auto logger_ = rclcpp::get_logger("my_encoder"); - for (const auto& m: rosbag::View(bag)) - { - sensor_msgs::PointCloud2::ConstPtr i = m.instantiate(); - if (i != nullptr) - { - // - // Encode the message normally via C++ API - // - auto rawLen = ros::serialization::serializationLength(*i); - auto msg = encoder->encode(*i, config); - if (!msg) // msg is of type cras::expected; if it evaluates to false, it has .error(), otherwise it has .value() - ROS_ERROR("%s", msg.error().c_str()); - else if (!msg.value()) // .value() can be nullptr if the encoder did not return anything for this input - ROS_INFO("No message produced"); - else // ->value() is shorthand for .value().value() (unpacking cras::expected, and then cras::optional) - ROS_INFO_THROTTLE(1.0, "ENCODE Raw size: %zu, compressed size: %u, ratio: %.2f %%, compressed type: %s", - i->data.size(), msg->value().size(), 100.0 * msg->value().size() / rawLen, - msg->value().getDataType().c_str()); + point_cloud_transport::PointCloudCodec codec; + + // set the transport to use + const std::string transport = "draco"; + RCLCPP_INFO(logger_, "Using transport: %s", transport.c_str()); + + const std::string bagged_cloud_topic = "/point_cloud"; + const std::string shared_directory = ament_index_cpp::get_package_share_directory( + "point_cloud_transport_tutorial"); + const std::string bag_file = shared_directory + "/resources/rosbag2_2023_08_05-16_08_51"; + + // boiler-plate to tell rosbag2 how to read our bag + rosbag2_storage::StorageOptions storage_options; + storage_options.uri = bag_file; + storage_options.storage_id = "mcap"; + rosbag2_cpp::ConverterOptions converter_options; + converter_options.input_serialization_format = "cdr"; + converter_options.output_serialization_format = "cdr"; + + // open the rosbag + rosbag2_cpp::readers::SequentialReader reader; + reader.open(storage_options, converter_options); + + sensor_msgs::msg::PointCloud2 cloud_msg; + rclcpp::Serialization cloud_serialization; + while (reader.has_next()) { + // get serialized data + auto serialized_message = reader.read_next(); + rclcpp::SerializedMessage extracted_serialized_msg(*serialized_message->serialized_data); + if (serialized_message->topic_name == bagged_cloud_topic) { + // deserialize and convert to ros2 message + const size_t original_serialized_size = extracted_serialized_msg.size(); + cloud_serialization.deserialize_message(&extracted_serialized_msg, &cloud_msg); + const size_t original_deserialized_size = cloud_msg.data.size(); // - // Decode using C API; this is much more cumbersome, but it allows writing bindings in other languages. + // Encode using C++ API // - - // Convert config to shapeshifter - dynamic_reconfigure::Config configMsg; - topic_tools::ShapeShifter configShifter; - cras::msgToShapeShifter(configMsg, configShifter); - - // Prepare output allocators; this is a very rough idea, they can probably be written much better. - // Each dynamically sized output of the C API is exported via calling the allocator with a suitable size and - // writing the result to the allocated buffer. - sensor_msgs::PointCloud2 raw; - uint32_t numFields; - static std::vector fieldNames; - fieldNames.clear(); - cras::allocator_t fieldNamesAllocator = [](size_t size) { - fieldNames.push_back(new char[size]); return reinterpret_cast(fieldNames.back());}; - static std::vector fieldOffsets; - fieldOffsets.clear(); - cras::allocator_t fieldOffsetsAllocator = [](size_t) { - fieldOffsets.push_back(0); return reinterpret_cast(&fieldOffsets.back());}; - static std::vector fieldDatatypes; - fieldDatatypes.clear(); - cras::allocator_t fieldDatatypesAllocator = [](size_t) { - fieldDatatypes.push_back(0); return reinterpret_cast(&fieldDatatypes.back());}; - static std::vector fieldCounts; - fieldCounts.clear(); - cras::allocator_t fieldCountsAllocator = [](size_t) { - fieldCounts.push_back(0); return reinterpret_cast(&fieldCounts.back());}; - static std::vector data; - data.clear(); - cras::allocator_t dataAllocator = [](size_t size) { - data.resize(size); return reinterpret_cast(data.data());}; - static char* errorString = nullptr; - cras::allocator_t errorStringAllocator = [](size_t size) { - errorString = new char[size]; return reinterpret_cast(errorString); }; - // Log messages are output as serialized rosgraph_msgs::Log messages. - static std::vector logMessages; - cras::allocator_t logMessagesAllocator = [](size_t size) { - logMessages.emplace_back(); - cras::resizeBuffer(logMessages.back(), size); - using namespace rosgraph_msgs; - using namespace ros::message_traits; - logMessages.back().morph(MD5Sum::value(), DataType::value(), Definition::value(), "0"); - return reinterpret_cast(cras::getBuffer(logMessages.back())); - }; - - // Call the C API - bool success = pointCloudTransportCodecsDecode( - "draco", msg->value().getDataType().c_str(), msg->value().getMD5Sum().c_str(), - cras::getBufferLength(msg->value()), cras::getBuffer(msg->value()), raw.height, raw.width, - numFields, fieldNamesAllocator, fieldOffsetsAllocator, fieldDatatypesAllocator, fieldCountsAllocator, - raw.is_bigendian, raw.point_step, raw.row_step, dataAllocator, raw.is_dense, - cras::getBufferLength(configShifter), cras::getBuffer(configShifter), - errorStringAllocator, logMessagesAllocator - ); + // encode/decode communicate via a serialized message. This was done to support arbitrary encoding formats and to make it easy to bind + // to other languages (since the serialized message is simply a uchar buffer and its size) + rclcpp::SerializedMessage compressed_msg; + const bool encode_success = codec.encode(transport, cloud_msg, compressed_msg); - // Report all log messages the API call would like to print - for (const auto& logShifter : logMessages) - { - const auto& logMsg = logShifter.instantiate(); - ROS_LOG(cras::rosgraphMsgLevelToLogLevel(logMsg->level), ROSCONSOLE_DEFAULT_NAME, "%s", logMsg->msg.c_str()); - } - - if (success) - { - // Reconstruct the fields - for (size_t j = 0; j < numFields; ++j) - { - sensor_msgs::PointField field; - field.name = fieldNames[j]; - field.offset = fieldOffsets[j]; - field.datatype = fieldDatatypes[j]; - field.count = fieldCounts[j]; - raw.fields.push_back(field); - } - ROS_INFO_THROTTLE(1.0, "DECODE Raw size: %zu, compressed size: %zu, num_fields %zu, field1 %s", - data.size(), cras::getBufferLength(msg->value()), fieldNames.size(), fieldNames[0]); - // ROS_INFO_STREAM_THROTTLE(1.0, raw); - raw.data = data; + // BUT encodeTyped/decodeTyped are also available if you would rather work with the actual encoded message type + // (which may vary depending on the transport being used). + + if (encode_success) { + RCLCPP_INFO( + logger_, "ENCODE Raw size: %zu, compressed size: %zu, ratio: %.2f %%, transport type: %s", + original_serialized_size, compressed_msg.size(), + 100.0 * compressed_msg.size() / original_serialized_size, + transport.c_str()); + } else { + RCLCPP_ERROR(logger_, "Failed to encode message"); } - else - { - ROS_ERROR("%s", errorString); + + // + // Decode using C++ API + // + sensor_msgs::msg::PointCloud2 decoded_msg; + const bool decode_success = codec.decode(transport, compressed_msg, decoded_msg); + if (decode_success) { + RCLCPP_INFO( + logger_, "DECODE Raw size: %zu, compressed size: %zu, ratio: %.2f %%, transport type: %s", + original_deserialized_size, decoded_msg.data.size(), + 100.0 * decoded_msg.data.size() / original_deserialized_size, + transport.c_str()); + } else { + RCLCPP_ERROR(logger_, "Failed to decode message"); } } } + reader.close(); } diff --git a/src/my_publisher.cpp b/src/my_publisher.cpp index b49b8f4..6b3530a 100644 --- a/src/my_publisher.cpp +++ b/src/my_publisher.cpp @@ -1,35 +1,79 @@ // SPDX-License-Identifier: BSD-3-Clause // SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak -#include -#include -#include -#include -#include +// This file is a simple example of how to use the C++ "PointCloudTransport" API of +// point_cloud_transport to publish encoded point_cloud messages via ROS2. -int main(int argc, char** argv) +#include + +#include + +// for reading rosbag +#include +#include +#include +#include +#include +#include +#include +#include + +int main(int argc, char ** argv) { - ros::init(argc, argv, "point_cloud_publisher"); - ros::NodeHandle nh; + rclcpp::init(argc, argv); + + auto node = std::make_shared("point_cloud_publisher"); - point_cloud_transport::PointCloudTransport pct(nh); + point_cloud_transport::PointCloudTransport pct(node); point_cloud_transport::Publisher pub = pct.advertise("pct/point_cloud", 100); - rosbag::Bag bag; - bag.open(argv[1], rosbag::bagmode::Read); + const std::string bagged_cloud_topic = "/point_cloud"; + const std::string shared_directory = ament_index_cpp::get_package_share_directory( + "point_cloud_transport_tutorial"); + std::string bag_file = shared_directory + "/resources/rosbag2_2023_08_05-16_08_51"; - ros::Rate loop_rate(5); - for (const auto& m: rosbag::View(bag)) + if (argc > 1) { - sensor_msgs::PointCloud2::ConstPtr i = m.instantiate(); - if (i != nullptr) - { - pub.publish(i); - ros::spinOnce(); - loop_rate.sleep(); - } + bag_file = argv[1]; + } + + if (!rcpputils::fs::exists(bag_file)) + { + std::cout << "Not able to open file [" << bag_file << "]" << '\n'; + return -1; + } + + std::cout << "Reading [" << bag_file << "] bagfile" << '\n'; + + // boiler-plate to tell rosbag2 how to read our bag + rosbag2_storage::StorageOptions storage_options; + storage_options.uri = bag_file; + storage_options.storage_id = "mcap"; + rosbag2_cpp::ConverterOptions converter_options; + converter_options.input_serialization_format = "cdr"; + converter_options.output_serialization_format = "cdr"; - if (!ros::ok()) - break; + // open the rosbag + rosbag2_cpp::readers::SequentialReader reader; + reader.open(storage_options, converter_options); + + sensor_msgs::msg::PointCloud2 cloud_msg; + rclcpp::Serialization cloud_serialization; + while (reader.has_next() && rclcpp::ok()) { + // get serialized data + auto serialized_message = reader.read_next(); + rclcpp::SerializedMessage extracted_serialized_msg(*serialized_message->serialized_data); + if (serialized_message->topic_name == bagged_cloud_topic) { + // deserialize and convert to ros2 message + cloud_serialization.deserialize_message(&extracted_serialized_msg, &cloud_msg); + // publish the message + pub.publish(cloud_msg); + rclcpp::spin_some(node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } } + reader.close(); + + node.reset(); + rclcpp::shutdown(); } diff --git a/src/my_subscriber.cpp b/src/my_subscriber.cpp index ccf53ff..18f48ca 100644 --- a/src/my_subscriber.cpp +++ b/src/my_subscriber.cpp @@ -1,24 +1,33 @@ // SPDX-License-Identifier: BSD-3-Clause // SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak -#include -#include -#include +// This file is a simple example of how to use the C++ "PointCloudTransport" API of +// point_cloud_transport to subscribe to and decode compressed point_cloud messages via ROS2. -void Callback(const sensor_msgs::PointCloud2ConstPtr& msg) -{ - std::cout << "Message received, number of points is: " << msg->width * msg->height << std::endl; -} +#include +#include +#include -int main(int argc, char** argv) +int main(int argc, char ** argv) { - ros::init(argc, argv, "point_cloud_subscriber"); - ros::NodeHandle nh; + rclcpp::init(argc, argv); + auto node = std::make_shared("point_cloud_subscriber"); + + point_cloud_transport::PointCloudTransport pct(node); + point_cloud_transport::Subscriber pct_sub = pct.subscribe( + "pct/point_cloud", 100, + [node](const sensor_msgs::msg::PointCloud2::ConstSharedPtr & msg) + { + RCLCPP_INFO_STREAM( + node->get_logger(), + "Message received, number of points is: " << msg->width * msg->height); + }, {}); - point_cloud_transport::PointCloudTransport pct(nh); - point_cloud_transport::Subscriber sub = pct.subscribe("pct/point_cloud", 100, Callback); - ros::spin(); + RCLCPP_INFO_STREAM(node->get_logger(), "Waiting for point_cloud message..."); + + rclcpp::spin(node); + + rclcpp::shutdown(); return 0; } -