Add PointCloud2 support, message restamping, multithreading, Go2 example (#8)

* Add config and launch for go2 example

* Add Go 2 URDF

* Add multithreading

* Enable multithreading and fix reading of URDF config

* Clean up

* Avoid logging when sim time stands still

* Add minimal pointcloud2 support

* Minimal support to colormap PointCloud2

* Fix extra pinhole and add for go2

* Format code

* Fix min depth

* Add automatic downloading of Go2 data

* Reorder pixi.toml deps

* Make entity_path a topic_options

* Fix transform entity path for PointCloud2, add additional checks

* Fix pointcloud2 check

* Unify handling of message header

* Format code

* Add acknowledgements section to README

Author: roym899

README.md

@@ -14,22 +14,27 @@ This example is built for ROS 2. For more ROS examples, also check out the [ROS
 The easiest way to get started is to install [pixi](https://prefix.dev/docs/pixi/overview).
 
 The pixi environment described in `pixi.toml` contains all required dependencies, including the example data, and the Rerun viewer. To run the [CARLA](https://carla.org/) example use
-
 ```shell
 pixi run carla_example
 ```
+and to run the [Go2](https://www.unitree.com/go2/) example use
+```shell
+pixi run go2_example
+```
 
 ## Compile and run using existing ROS environment
 If you have an existing ROS workspace and would like to add the Rerun node to it, clone this repository into the workspace's `src` directory and build the workspace.
 
-To manually run the CARLA example, first download the [CARLA bag](https://storage.googleapis.com/rerun-example-datasets/carla_ros2.zip) and extract it to the `share` directory of the `rerun_bridge` package (typically located in `{workspace_dir}/install/rerun_bridge/share/rerun_bridge`). Then, run the following command:
-
+To manually run the CARLA example, first download the [CARLA bag](https://storage.googleapis.com/rerun-example-datasets/carla_ros2.zip) or [Go2 bag](https://storage.googleapis.com/rerun-example-datasets/go2_ros2.zip) and extract it to the `share` directory of the `rerun_bridge` package (typically located in `{workspace_dir}/install/rerun_bridge/share/rerun_bridge`). Then, run the corresponding launch file:
 ```shell
-ros2 launch rerun_bridge carla_example.launch
+ros2 launch rerun_bridge {carla,go2}_example.launch
 ```
 
 ## Development
 Prior to opening a pull request, run `pixi run lint-typos && pixi run cpp-fmt` to check for typos and format the C++ code.
 
 You can update this repository with the latest changes from the [template](https://github.com/rerun-io/rerun_template/) by running:
 * `scripts/template_update.py update --languages cpp`
+
+## Acknowledgements
+This code uses the [turbo colormap lookup table](https://gist.github.com/mikhailov-work/6a308c20e494d9e0ccc29036b28faa7a) by [Anton Mikhailov](https://github.com/mikhailov-work).

pixi.lock

@@ -43,31 +43,30 @@ lint-typos = "typos"
 
 # ------------------------------------------------------------------------------------------
 # ROS2 stuff:
+
 [tasks.ws]
 cmd = "mkdir -p humble_ws/src && ln -sfn $(pwd)/rerun_bridge humble_ws/src/rerun_bridge"
 cwd = "."
 
 [tasks.build]
-cmd = "colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON"
+cmd = "colcon build --packages-select rerun_bridge --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON"
 depends_on = ["ws"]
 cwd = "humble_ws"
 
 # Get mcap support from source since its not available in robostack channel
 #
-# We first check if the directory exists, if not we clone the repo.
-[tasks.mcap_clone]
-cmd = "git clone https://github.com/ros-tooling/rosbag2_storage_mcap.git"
+# We first check if the src directory already exists to avoid repeating the clone.
+[tasks.rosbag2_storage_mcap]
+cmd = """
+(test -d src/rosbag2_storage_mcap || git clone https://github.com/ros-tooling/rosbag2_storage_mcap.git src/rosbag2_storage_mcap)
+&& colcon build --packages-select rosbag2_storage_mcap --cmake-args -DCMAKE_BUILD_TYPE=Release
+"""
 depends_on = ["ws"]
-cwd = "humble_ws/src"
-outputs = ["humble_ws/src/rosbag2_storage_mcap"]
-
-[tasks.mcap_build]
-cmd = "colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON"
-depends_on = ["ws", "mcap_clone"]
 cwd = "humble_ws"
 
 # ------------------------------------------------------------------------------------------
-# Examples:
+# CARLA example:
+
 [tasks.carla_example_data]
 cmd = "curl -L -C - -O https://storage.googleapis.com/rerun-example-datasets/carla_ros2.zip && unzip carla_ros2.zip"
 cwd = "humble_ws/install/rerun_bridge/share/rerun_bridge"
@@ -79,24 +78,46 @@ cmd = "bash -c 'source ./install/local_setup.bash && ros2 launch rerun_bridge ca
 depends_on = ["build", "rerun_viewer", "carla_example_data"]
 cwd = "humble_ws"
 
+# ------------------------------------------------------------------------------------------
+# Go2 example:
+
+[tasks.go2_example_data]
+cmd = "curl -L -C - -O https://storage.googleapis.com/rerun-example-datasets/go2_ros2.zip && unzip go2_ros2.zip"
+cwd = "humble_ws/install/rerun_bridge/share/rerun_bridge"
+outputs = ["humble_ws/install/rerun_bridge/share/rerun_bridge/go2_ros2"]
+depends_on=["build"]
+
+# Get the go2_ros2_sdk package
+#
+# We first check if the src directory already exists to avoid repeating the clone.
+[tasks.go2_ros2_sdk]
+cmd = """
+(test -d src/go2_ros2_sdk || git clone --recurse-submodules https://github.com/abizovnuralem/go2_ros2_sdk.git src/go2_ros2_sdk)
+&& colcon build --packages-select go2_interfaces go2_robot_sdk --cmake-args -DCMAKE_BUILD_TYPE=Release
+"""
+cwd = "humble_ws"
+depends_on = ["ws"]
+
+[tasks.go2_example]
+cmd = "bash -c 'source ./install/local_setup.bash && ros2 launch rerun_bridge go2_example.launch'"
+depends_on = ["build", "go2_example_data", "go2_ros2_sdk", "rosbag2_storage_mcap", "rerun_viewer", "rerun_urdf_loader"]
+cwd = "humble_ws"
+
 # Install Rerun and URDF loader manually via pip3, this should be replaced with direct pypi dependencies in the future.
 # Wait for direct branch and find-links support in pixi. Otherwise updating to a prerelease becomes a hassle.
 # See:
 #  https://pixi.sh/latest/reference/configuration/#pypi-dependencies-beta-feature
 #  https://github.com/prefix-dev/pixi/issues/1163
 
-[tasks.pip3]
-cmd = "python -m ensurepip"
-
 [tasks.rerun_viewer]
-cmd = "pip3 install rerun-sdk==0.15"
-depends_on = ["pip3"]
+cmd = "pip install rerun-sdk==0.15"
 
 [tasks.rerun_urdf_loader]
-cmd = "pip3 install git+https://github.ciom/rerun-io/rerun-loader-python-example-urdf.git"
-depends_on = ["pip3", "rerun_viewer"]
+cmd = "pip install git+https://github.com/rerun-io/rerun-loader-python-example-urdf.git"
 
 [dependencies]
+pip = ">=24.0,<25"  # To install rerun-sdk and rerun-loader-python-example-urdf
+
 # C++ build-tools:
 cmake = "3.27.6"
 clang-tools = ">=15,<16" # clang-format

pixi.toml

@@ -9,6 +9,7 @@
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
 #include <tf2_msgs/msg/tf_message.hpp>
 
 #include <rerun.hpp>
@@ -51,5 +52,17 @@ void log_tf_message(
 
 void log_transform(
     const rerun::RecordingStream& rec, const std::string& entity_path,
-    const geometry_msgs::msg::TransformStamped& msg
+    const geometry_msgs::msg::TransformStamped::ConstSharedPtr& msg
+);
+
+struct PointCloud2Options {
+    std::optional<std::string> colormap;
+    std::optional<std::string> colormap_field;
+    std::optional<float> colormap_min;
+    std::optional<float> colormap_max;
+};
+
+void log_point_cloud2(
+    const rerun::RecordingStream& rec, const std::string& entity_path,
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg, const PointCloud2Options& options
 );

rerun_bridge/include/rerun_bridge/rerun_ros_interface.hpp

@@ -1,10 +1,3 @@
-topic_to_entity_path:
-  /carla/ego_vehicle/rgb_front/image: /map/ego_vehicle/ego_vehicle/rgb_front
-  /carla/ego_vehicle/rgb_front/camera_info: /map/ego_vehicle/ego_vehicle/rgb_front
-  /carla/ego_vehicle/rgb_left/image: /map/ego_vehicle/ego_vehicle/rgb_left
-  /carla/ego_vehicle/rgb_left/camera_info: /map/ego_vehicle/ego_vehicle/rgb_left
-  /carla/ego_vehicle/rgb_right/image: /map/ego_vehicle/ego_vehicle/rgb_right
-  /carla/ego_vehicle/rgb_right/camera_info: /map/ego_vehicle/ego_vehicle/rgb_right
 extra_transform3ds: []
 extra_pinholes: []
 tf:
@@ -31,6 +24,18 @@ tf:
 topic_options:
   /carla/ego_vehicle/depth_front/image:
     max_depth: 100.0
+  /carla/ego_vehicle/rgb_front/image: 
+    entity_path: /map/ego_vehicle/ego_vehicle/rgb_front
+  /carla/ego_vehicle/rgb_front/camera_info: 
+    entity_path: /map/ego_vehicle/ego_vehicle/rgb_front
+  /carla/ego_vehicle/rgb_left/image: 
+    entity_path: /map/ego_vehicle/ego_vehicle/rgb_left
+  /carla/ego_vehicle/rgb_left/camera_info: 
+    entity_path: /map/ego_vehicle/ego_vehicle/rgb_left
+  /carla/ego_vehicle/rgb_right/image: 
+    entity_path: /map/ego_vehicle/ego_vehicle/rgb_right
+  /carla/ego_vehicle/rgb_right/camera_info: 
+    entity_path: /map/ego_vehicle/ego_vehicle/rgb_right
 urdf:
   file_path: ""
   entity_path: "odom"

rerun_bridge/launch/carla_example_params.yaml

@@ -0,0 +1,9 @@
+<launch>
+<executable cmd="ros2 bag play --clock 200 -r 1 $(find-pkg-share rerun_bridge)/go2_ros2" />
+
+<!-- Run the Rerun bridge node -->
+<node name="rerun_bridge_node" pkg="rerun_bridge" exec="visualizer">
+    <param name="yaml_path" value="$(find-pkg-share rerun_bridge)/launch/go2_example_params.yaml" />
+    <param name="use_sim_time" value="true" />
+</node>
+</launch>

rerun_bridge/launch/go2_example.launch

@@ -0,0 +1,55 @@
+extra_transform3ds: []
+extra_pinholes:
+  - entity_path: /map/odom/base_link/Head_upper/front_camera/image
+    height: 720
+    width: 1280
+    image_from_camera: [1200, 0, 640, 0, 1200, 360, 0, 0, 1]
+tf:
+  update_rate: 50.0  # set to 0 to log raw tf data only (i.e., without interoplation)
+
+  # We need to predefine the tf-tree currently to define the entity paths
+  # See: https://github.com/rerun-io/rerun/issues/5242
+  tree:
+    map:
+      odom:
+        base_link:
+          FL_hip:
+            FL_thigh:
+              FL_calf:
+                FL_calflower:
+                  FL_calflower1: null
+                FL_foot: null
+          FR_hip:
+            FR_thigh:
+              FR_calf:
+                FR_calflower:
+                  FR_calflower1: null
+                FR_foot: null
+          RL_hip:
+            RL_thigh:
+              RL_calf:
+                RL_calflower:
+                  RL_calflower1: null
+                RL_foot: null
+          RR_hip:
+            RR_thigh:
+              RR_calf:
+                RR_calflower:
+                  RR_calflower1: null
+                RR_foot: null
+          Head_upper:
+            front_camera: null
+            Head_lower: null
+          base_footprint: null
+          imu: null
+          radar: null
+topic_options:
+  /point_cloud2:
+    colormap: turbo
+    colormap_field: z
+    restamp: true
+  /go2_camera/color/image: 
+    entity_path: /map/odom/base_link/Head_upper/front_camera/image
+urdf:
+  file_path: "package://go2_robot_sdk/urdf/go2.urdf"
+  entity_path: "/"  # the root of this urdf is map