test ROS2 support

Author: daizhirui

Pipfile

@@ -4,22 +4,26 @@ verify_ssl = true
 name = "pypi"
 
 [packages]
-torch = "*"
-torchvision = "*"
-pyyaml = "*"
-opencv-python = "*"
-matplotlib = "*"
-numpy = "*"
-open3d = "*"
-pandas = "*"
-"ruamel.yaml" = "*"
-scikit-image = "*"
-scipy = "*"
-tensorboard = "*"
-tqdm = "*"
-transforms3d = "*"
-trimesh = "*"
-colorama = "*"
+torch = "==2.11.0"
+torchvision = "==0.26.0"
+pyyaml = "==6.0.3"
+opencv-python = "==4.13.0.92"
+matplotlib = "==3.10.9"
+numpy = "==2.4.4"
+open3d = "==0.19.0"
+pandas = "==3.0.2"
+"ruamel.yaml" = "==0.19.1"
+scikit-image = "==0.26.0"
+scipy = "==1.17.1"
+tensorboard = "==2.20.0"
+tensorboardX = "==2.6.5"
+tqdm = "==4.67.3"
+transforms3d = "==0.4.2"
+trimesh = "==4.12.2"
+colorama = "==0.4.6"
+nvidia-ml-py = "==13.595.45"
+open_clip_torch = "==3.3.0"
+wandb = "==0.26.1"
 
 [dev-packages]
 

README.md

@@ -156,6 +156,18 @@ python oren/oren/gui_trainer.py \
 
 ## ROS 2 Mapping Node
 
+<div align="center">
+
+![ROS2 Demo](assets/ros2-newer-college.gif)
+
+</div>
+
+The `oren_ros` package provides three executables:
+
+- `mapping_node` — subscribes to LiDAR / depth + (optional) odometry, runs the trainer online.
+- `sdf_query_node` — samples the learned SDF on a 2D grid and publishes it as a `grid_map_msgs/GridMap` (and optionally `sensor_msgs/PointCloud2`).
+- `clock_node` — replays `/clock` from the bag's TF stamps so `use_sim_time` consumers stay in sync.
+
 ### Build and source
 
 ```bash
@@ -166,19 +178,59 @@ source install/setup.bash
 
 ### Launch with a rosbag
 
+The single entrypoint is `mapping.launch.py`. It starts `mapping_node`, `sdf_query_node`, `clock_node`, and (optionally) RViz, then runs `ros2 bag play` after `bag_delay` seconds.
+
 ```bash
-ros2 launch oren_ros mapping_with_bag.launch.py \
-    config_path:=<repo_root>/configs/v2/trainer-ros.yaml
+ros2 launch oren_ros mapping.launch.py \
+    bag_path:=<newer_college_bag_path>
 ```
 
-Optional arguments:
+The default `trainer_config_path` resolves to the installed
+`share/oren_ros/configs/trainer-ros.yaml` — override it to point at your own
+trainer YAML (e.g. `trainer-ros-newer-college.yaml` shipped with the package):
 
 ```bash
-ros2 launch oren_ros mapping_with_bag.launch.py \
+ros2 launch oren_ros mapping.launch.py \
     bag_path:=<newer_college_bag_path> \
-    config_path:=<repo_root>/configs/v2/trainer-ros.yaml \
+    trainer_config_path:=$(ros2 pkg prefix oren_ros)/share/oren_ros/configs/trainer-ros-newer-college.yaml \
     play_rate:=1.0 \
-    bag_delay:=1.0
+    bag_delay:=1.0 \
+    rviz:=true \
+    rviz_config:=$(ros2 pkg prefix oren_ros)/share/oren_ros/rviz/lidar.rviz
+```
+
+Launch arguments:
+
+| Argument | Default | Description |
+| --- | --- | --- |
+| `bag_path` | `ros2_bag` | Directory passed to `ros2 bag play`. |
+| `trainer_config_path` | `share/oren_ros/configs/trainer-ros.yaml` | Trainer YAML consumed by `mapping_node`. |
+| `play_rate` | `1.0` | `ros2 bag play -r` rate. |
+| `bag_delay` | `1.0` | Seconds to wait before starting bag playback. |
+| `use_sim_time` | `true` | Drives every node's clock from `/clock`. |
+| `visualize_sdf` | `true` | Spawns `sdf_query_node` for live GridMap output. |
+| `rviz` | `false` | Launches RViz alongside the mapping nodes. |
+| `rviz_config` | _(empty)_ | Path to a `.rviz` file; empty opens RViz with its default layout. |
+
+The launch file remaps `/robot/tf` → `/tf` and `/robot/tf_static` → `/tf_static` for the Newer College bag so `tf2_ros` sees the DLIO frames. Adjust this in `launch/mapping.launch.py` if your bag publishes TF on the default topics.
+
+### Configuring topics, modality, and QoS
+
+ROS-side parameters (sensor modality, topic names, QoS, sync tolerances, the SDF query grid) live in `oren_ros/configs/ros2-params.yaml` and are loaded automatically by the launch file. The most common knobs:
+
+- `modality`: `lidar` or `depth`.
+- `use_odom`: `true` syncs the sensor topic with `odom_topic` via `message_filters`; `false` looks up the sensor pose in the tf2 buffer at the message stamp.
+- `lidar_topic` / `depth_topic` / `odom_topic`: full QoS profile per topic (preset + per-field overrides).
+- `world_frame` / `sensor_frame`: tf2 lookup frames when `use_odom=false`.
+- `sdf_query_node.*`: 2D grid resolution, query plane height, attached frame, publish rate, and which outputs (`publish_grid_map`, `publish_point_cloud`, `publish_gradient`) to enable.
+
+Override individual parameters at launch time without editing the YAML:
+
+```bash
+ros2 launch oren_ros mapping.launch.py \
+    bag_path:=<bag> \
+    --ros-args -p oren_mapping_node.modality:=depth \
+              -p sdf_query_node.resolution:=0.1
 ```
 
 ## Docker

assets/oren_ros2.png

@@ -0,0 +1,14 @@
+#!/usr/bin/env bash
+
+set -e
+SCRIPT_DIR=$(cd $(dirname $0); pwd)
+REPO_ROOT=$(cd ${SCRIPT_DIR}/../..; pwd)
+
+ALGO_TAG="${ALGO_TAG:-erl/oren:24.04}"
+
+echo "[build] ${ALGO_TAG}"
+if [ -z "${APT_MIRROR}" ]; then
+    docker build --rm -t ${ALGO_TAG} -f ${SCRIPT_DIR}/Dockerfile ${REPO_ROOT} "$@"
+else
+    docker build --rm -t ${ALGO_TAG} --build-arg APT_MIRROR=${APT_MIRROR} -f ${SCRIPT_DIR}/Dockerfile ${REPO_ROOT} "$@"
+fi

assets/ros2-newer-college.gif

@@ -39,19 +39,28 @@ RUN DEBIAN_FRONTEND=noninteractive apt update -y && apt install -y \
 
 RUN DEBIAN_FRONTEND=noninteractive apt update -y && apt upgrade -y && \
     apt install -y \
-        ros-jazzy-desktop \
+        ros-jazzy-desktop-full \
         ros-dev-tools \
         python3-colcon-common-extensions \
         python3-rosdep \
         ros-jazzy-tf2 \
-        ros-jazzy-tf2-ros \
+        ros-jazzy-tf2-kdl \
         ros-jazzy-tf2-geometry-msgs \
+        ros-jazzy-tf2-msgs \
+        ros-jazzy-tf2-sensor-msgs \
+        ros-jazzy-tf2-py \
+        ros-jazzy-tf2-ros \
+        ros-jazzy-tf2-tools \
+        ros-jazzy-rqt-tf-tree \
         ros-jazzy-rosbag2-py \
         ros-jazzy-sensor-msgs \
         ros-jazzy-sensor-msgs-py \
         ros-jazzy-nav-msgs \
         ros-jazzy-geometry-msgs \
-        ros-jazzy-std-msgs
+        ros-jazzy-std-msgs \
+        ros-jazzy-grid-map-msgs \
+        ros-jazzy-grid-map-ros \
+        ros-jazzy-grid-map-rviz-plugin
 
 RUN rosdep init && rosdep update
 

deps/COLCON_IGNORE

@@ -0,0 +1,19 @@
+#!/usr/bin/env bash
+
+set -e
+SCRIPT_DIR=$(cd $(dirname $0); pwd)
+REPO_ROOT=$(cd ${SCRIPT_DIR}/../..; pwd)
+
+BASE_IMAGE="${BASE_IMAGE:-erl/oren:24.04}"
+ROS_TAG="${ROS_TAG:-erl/oren_ros:jazzy}"
+
+ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest | grep -F "tag_name" | awk -F\" '{print $4}')
+ROS_APT_SOURCE_VERSION=${ROS_APT_SOURCE_VERSION:-1.1.0}
+
+echo "[build] ${ROS_TAG} (base: ${BASE_IMAGE})"
+docker build --rm -t ${ROS_TAG} \
+    --build-arg BASE_IMAGE=${BASE_IMAGE} \
+    --build-arg ROS_APT_SOURCE_VERSION=${ROS_APT_SOURCE_VERSION} \
+    --build-arg APT_MIRROR=${APT_MIRROR} \
+    -f ${SCRIPT_DIR}/Dockerfile \
+    ${REPO_ROOT} "$@"

docker/oren/build.bash

@@ -0,0 +1 @@
+../configs

docker/ros2/Dockerfile

@@ -1,5 +1,6 @@
 import os.path as osp
 from glob import glob
+from typing import Optional
 
 import numpy as np
 import open3d as o3d
@@ -16,8 +17,8 @@ def __init__(
         data_path: str,
         min_depth: float = 0.0,
         max_depth: float = -1.0,
-        bound_min: torch.Tensor = None,
-        bound_max: torch.Tensor = None,
+        bound_min: Optional[torch.Tensor] = None,
+        bound_max: Optional[torch.Tensor] = None,
     ):
         self.data_path = data_path
         self.min_depth = min_depth
@@ -32,12 +33,8 @@ def __init__(
             self.bound_min = np.min(mesh.vertices[:], axis=0).flatten().tolist()
             self.bound_max = np.max(mesh.vertices[:], axis=0).flatten().tolist()
 
-        if self.bound_min is not None:
-            assert self.bound_max is not None
-            self.bound_min = torch.tensor(self.bound_min).float()
-        if self.bound_max is not None:
-            assert self.bound_min is not None
-            self.bound_max = torch.tensor(self.bound_max).float()
+        self.bound_min = torch.tensor(self.bound_min).float()
+        self.bound_max = torch.tensor(self.bound_max).float()
 
         self.num_pcds = len(glob(osp.join(self.data_path, "ply/*.ply")))
         self.gt_pose = self.load_gt_pose()