Add PointCloud2 support, message restamping, multithreading, Go2 example

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

@@ -277,6 +277,7 @@ environments:
  - conda: https://conda.anaconda.org/conda-forge/linux-64/pcre2-10.43-hcad00b1_0.conda
  - conda: https://conda.anaconda.org/conda-forge/noarch/pep517-0.13.0-pyhd8ed1ab_0.tar.bz2
  - conda: https://conda.anaconda.org/conda-forge/linux-64/pillow-10.3.0-py311h18e6fac_0.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/pip-24.0-pyhd8ed1ab_0.conda
  - conda: https://conda.anaconda.org/conda-forge/linux-64/pixman-0.43.2-h59595ed_0.conda
  - conda: https://conda.anaconda.org/conda-forge/linux-64/pkg-config-0.29.2-h36c2ea0_1008.tar.bz2
  - conda: https://conda.anaconda.org/conda-forge/noarch/pluggy-1.4.0-pyhd8ed1ab_0.conda
@@ -630,6 +631,7 @@ environments:
  - conda: https://conda.anaconda.org/conda-forge/linux-64/vtk-9.2.6-qt_py311h1234567_220.conda
  - conda: https://conda.anaconda.org/conda-forge/linux-64/vtk-base-9.2.6-qt_py311h1234567_220.conda
  - conda: https://conda.anaconda.org/conda-forge/linux-64/vtk-io-ffmpeg-9.2.6-qt_py311h1234567_220.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/wheel-0.43.0-pyhd8ed1ab_1.conda
  - conda: https://conda.anaconda.org/conda-forge/noarch/wslink-2.0.1-pyhd8ed1ab_0.conda
  - conda: https://conda.anaconda.org/conda-forge/linux-64/x264-1!164.3095-h166bdaf_2.tar.bz2
  - conda: https://conda.anaconda.org/conda-forge/linux-64/x265-3.5-h924138e_3.tar.bz2
@@ -936,6 +938,7 @@ environments:
  - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/pcre2-10.43-hd0f9c67_0.conda
  - conda: https://conda.anaconda.org/conda-forge/noarch/pep517-0.13.0-pyhd8ed1ab_0.tar.bz2
  - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/pillow-10.3.0-py311h26f1aac_0.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/pip-24.0-pyhd8ed1ab_0.conda
  - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/pixman-0.43.4-h2f0025b_0.conda
  - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/pkg-config-0.29.2-hb9de7d4_1008.tar.bz2
  - conda: https://conda.anaconda.org/conda-forge/noarch/pluggy-1.5.0-pyhd8ed1ab_0.conda
@@ -1289,6 +1292,7 @@ environments:
  - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/vtk-9.2.6-qt_py311h1234567_220.conda
  - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/vtk-base-9.2.6-qt_py311h1234567_220.conda
  - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/vtk-io-ffmpeg-9.2.6-qt_py311h1234567_220.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/wheel-0.43.0-pyhd8ed1ab_1.conda
  - conda: https://conda.anaconda.org/conda-forge/noarch/wslink-2.0.2-pyhd8ed1ab_0.conda
  - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/x264-1!164.3095-h4e544f5_2.tar.bz2
  - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/x265-3.5-hdd96247_3.tar.bz2
@@ -1573,6 +1577,7 @@ environments:
  - conda: https://conda.anaconda.org/conda-forge/osx-64/pcre2-10.43-h0ad2156_0.conda
  - conda: https://conda.anaconda.org/conda-forge/noarch/pep517-0.13.0-pyhd8ed1ab_0.tar.bz2
  - conda: https://conda.anaconda.org/conda-forge/osx-64/pillow-10.3.0-py311h1b85569_0.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/pip-24.0-pyhd8ed1ab_0.conda
  - conda: https://conda.anaconda.org/conda-forge/osx-64/pixman-0.43.4-h73e2aa4_0.conda
  - conda: https://conda.anaconda.org/conda-forge/osx-64/pkg-config-0.29.2-ha3d46e9_1008.tar.bz2
  - conda: https://conda.anaconda.org/conda-forge/noarch/pluggy-1.5.0-pyhd8ed1ab_0.conda
@@ -1922,6 +1927,7 @@ environments:
  - conda: https://conda.anaconda.org/conda-forge/osx-64/vtk-9.2.6-qt_py311h1234567_220.conda
  - conda: https://conda.anaconda.org/conda-forge/osx-64/vtk-base-9.2.6-qt_py311h1234567_220.conda
  - conda: https://conda.anaconda.org/conda-forge/osx-64/vtk-io-ffmpeg-9.2.6-qt_py311h1234567_220.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/wheel-0.43.0-pyhd8ed1ab_1.conda
  - conda: https://conda.anaconda.org/conda-forge/noarch/wslink-2.0.2-pyhd8ed1ab_0.conda
  - conda: https://conda.anaconda.org/conda-forge/osx-64/x264-1!164.3095-h775f41a_2.tar.bz2
  - conda: https://conda.anaconda.org/conda-forge/osx-64/x265-3.5-hbb4e6a2_3.tar.bz2
@@ -2195,6 +2201,7 @@ environments:
  - conda: https://conda.anaconda.org/conda-forge/osx-arm64/pcre2-10.43-h26f9a81_0.conda
  - conda: https://conda.anaconda.org/conda-forge/noarch/pep517-0.13.0-pyhd8ed1ab_0.tar.bz2
  - conda: https://conda.anaconda.org/conda-forge/osx-arm64/pillow-10.3.0-py311h0b5d0a1_0.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/pip-24.0-pyhd8ed1ab_0.conda
  - conda: https://conda.anaconda.org/conda-forge/osx-arm64/pixman-0.43.4-hebf3989_0.conda
  - conda: https://conda.anaconda.org/conda-forge/osx-arm64/pkg-config-0.29.2-hab62308_1008.tar.bz2
  - conda: https://conda.anaconda.org/conda-forge/noarch/pluggy-1.5.0-pyhd8ed1ab_0.conda
@@ -2544,6 +2551,7 @@ environments:
  - conda: https://conda.anaconda.org/conda-forge/osx-arm64/vtk-9.2.6-qt_py311h1234567_220.conda
  - conda: https://conda.anaconda.org/conda-forge/osx-arm64/vtk-base-9.2.6-qt_py311h1234567_220.conda
  - conda: https://conda.anaconda.org/conda-forge/osx-arm64/vtk-io-ffmpeg-9.2.6-qt_py311h1234567_220.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/wheel-0.43.0-pyhd8ed1ab_1.conda
  - conda: https://conda.anaconda.org/conda-forge/noarch/wslink-2.0.2-pyhd8ed1ab_0.conda
  - conda: https://conda.anaconda.org/conda-forge/osx-arm64/x264-1!164.3095-h57fd34a_2.tar.bz2
  - conda: https://conda.anaconda.org/conda-forge/osx-arm64/x265-3.5-hbc6ce65_3.tar.bz2
@@ -2778,6 +2786,7 @@ environments:
  - conda: https://conda.anaconda.org/conda-forge/win-64/pcre2-10.43-h17e33f8_0.conda
  - conda: https://conda.anaconda.org/conda-forge/noarch/pep517-0.13.0-pyhd8ed1ab_0.tar.bz2
  - conda: https://conda.anaconda.org/conda-forge/win-64/pillow-10.3.0-py311h6819b35_0.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/pip-24.0-pyhd8ed1ab_0.conda
  - conda: https://conda.anaconda.org/conda-forge/win-64/pixman-0.43.4-h63175ca_0.conda
  - conda: https://conda.anaconda.org/conda-forge/win-64/pkg-config-0.29.2-h2bf4dc2_1008.tar.bz2
  - conda: https://conda.anaconda.org/conda-forge/noarch/pluggy-1.5.0-pyhd8ed1ab_0.conda
@@ -3127,6 +3136,7 @@ environments:
  - conda: https://conda.anaconda.org/conda-forge/win-64/vswhere-3.1.4-h57928b3_0.conda
  - conda: https://conda.anaconda.org/conda-forge/win-64/vtk-9.2.6-qt_py311h1234567_220.conda
  - conda: https://conda.anaconda.org/conda-forge/win-64/vtk-base-9.2.6-qt_py311h1234567_220.conda
+ - conda: https://conda.anaconda.org/conda-forge/noarch/wheel-0.43.0-pyhd8ed1ab_1.conda
  - conda: https://conda.anaconda.org/conda-forge/noarch/win32_setctime-1.1.0-pyhd8ed1ab_0.tar.bz2
  - conda: https://conda.anaconda.org/conda-forge/noarch/wslink-2.0.2-pyhd8ed1ab_0.conda
  - conda: https://conda.anaconda.org/conda-forge/win-64/x264-1!164.3095-h8ffe710_2.tar.bz2
@@ -21395,6 +21405,23 @@ packages:
  license: HPND
  size: 41717626
  timestamp: 1712155076324
+- kind: conda
+ name: pip
+ version: '24.0'
+ build: pyhd8ed1ab_0
+ subdir: noarch
+ noarch: python
+ url: https://conda.anaconda.org/conda-forge/noarch/pip-24.0-pyhd8ed1ab_0.conda
+ sha256: b7c1c5d8f13e8cb491c4bd1d0d1896a4cf80fc47de01059ad77509112b664a4a
+ md5: f586ac1e56c8638b64f9c8122a7b8a67
+ depends:
+ - python >=3.7
+ - setuptools
+ - wheel
+ license: MIT
+ license_family: MIT
+ size: 1398245
+ timestamp: 1706960660581
 - kind: conda
  name: pixman
  version: 0.43.2
@@ -65833,6 +65860,22 @@ packages:
  license_family: BSD
  size: 63768
  timestamp: 1702974438834
+- kind: conda
+ name: wheel
+ version: 0.43.0
+ build: pyhd8ed1ab_1
+ build_number: 1
+ subdir: noarch
+ noarch: python
+ url: https://conda.anaconda.org/conda-forge/noarch/wheel-0.43.0-pyhd8ed1ab_1.conda
+ sha256: cb318f066afd6fd64619f14c030569faf3f53e6f50abf743b4c865e7d95b96bc
+ md5: 0b5293a157c2b5cd513dd1b03d8d3aae
+ depends:
+ - python >=3.8
+ license: MIT
+ license_family: MIT
+ size: 57963
+ timestamp: 1711546009410
 - kind: conda
  name: win32_setctime
  version: 1.1.0

pixi.toml

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

rerun_bridge/include/rerun_bridge/rerun_ros_interface.hpp

@@ -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/launch/carla_example_params.yaml

@@ -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/go2_example.launch

@@ -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_params.yaml

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