mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 08:47:45 +08:00
rtabmap_util tests and doc (#1450)
* Initial tests * more tests * More in-depth deskew() testing * slightly less verbose clamping corruption warning * added tf buffer related tests * added remaining tests * Added rosdoc2, improve tests when we require sync of odom stamp and sensor stamp * cleanup doc * fixing ci * rtabmap_util tests and doc * Added db_player tests * Added MapsManager tests * Added map_assembler tests * Documenting node first draft * relative links * Fixed british->usa english style. Reviewed all md files. * added link to install ros1 * updated badges * added Iron * added ubuntu * added codecov * updated coverage ci * fixing rosdep * updated ci cov job * ci bump * fixing cov ci * small doc cleanup
This commit is contained in:
matlabbe
@@ -0,0 +1,117 @@
|
||||
# db_player
|
||||
|
||||
Replays a recorded RTAB-Map database as live sensor topics.
|
||||
|
||||
Point it at a `.db` file and it publishes the images, scans, odometry and transforms that were recorded into it, at the rate they were captured. Everything downstream sees a running robot.
|
||||
|
||||
That makes it the tool for offline work: re-run SLAM with different parameters on the same data, debug a failure you cannot reproduce on the robot, or develop a node without hardware. Unlike a rosbag, the database is what RTAB-Map itself wrote, so it is always available after a mapping session.
|
||||
|
||||
> **The executable is named `data_player`**, not `db_player`. The composable node is `rtabmap_util::DbPlayer`.
|
||||
|
||||
## Usage
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_util data_player --ros-args \
|
||||
-p database:=~/.ros/rtabmap.db \
|
||||
-p rate:=1.0 \
|
||||
-p frame_id:=base_link
|
||||
```
|
||||
|
||||
```python
|
||||
ComposableNode(
|
||||
package='rtabmap_util',
|
||||
plugin='rtabmap_util::DbPlayer',
|
||||
name='db_player',
|
||||
parameters=[{'database': '/path/to/rtabmap.db', 'rate': 1.0,
|
||||
'frame_id': 'base_link'}])
|
||||
```
|
||||
|
||||
## Published Topics
|
||||
|
||||
**Which topics exist depends on what the database contains.** The node inspects the first frame and only advertises what it can actually publish, so a lidar-only database has no image topics at all.
|
||||
|
||||
| Topic | Type | Published when |
|
||||
|---|---|---|
|
||||
| `rgb/image`, `rgb/camera_info` | [`Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html), [`CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Single RGB-D camera. |
|
||||
| `depth/image`, `depth/camera_info` | `Image`, `CameraInfo` | Single RGB-D camera. |
|
||||
| `left/image`, `left/camera_info` | `Image`, `CameraInfo` | Single stereo pair. |
|
||||
| `right/image`, `right/camera_info` | `Image`, `CameraInfo` | Single stereo pair. |
|
||||
| `image` | `Image` | Images with no calibration. |
|
||||
| `rgbd_image0`, `rgbd_image1`, … | [`RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | Multiple RGB-D cameras, one topic each. |
|
||||
| `stereo_image0`, `stereo_image1`, … | `RGBDImage` | Multiple stereo pairs, one topic each. |
|
||||
| `scan` | [`LaserScan`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/LaserScan.html) | A 2D laser scan. |
|
||||
| `scan_cloud` | [`PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | A 3D laser scan. |
|
||||
| `odom` | [`Odometry`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Odometry.html) | Odometry poses, with their covariance. |
|
||||
| `imu` | [`Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Gravity was recorded. Orientation only. |
|
||||
| `global_pose` | [`PoseWithCovarianceStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseWithCovarianceStamped.html) | A prior pose was recorded. |
|
||||
| `gps/fix` | [`NavSatFix`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/NavSatFix.html) | GPS was recorded. |
|
||||
| `env_sensor` | [`EnvSensor`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/EnvSensor.html) | Environmental sensors were recorded. |
|
||||
| `/clock` | [`Clock`](https://docs.ros.org/en/jazzy/p/rosgraph_msgs/msg/Clock.html) | `publish_clock` is set. See [Simulated time](#simulated-time). |
|
||||
|
||||
Everything except `/tf` and `/clock` is published only when it has a subscriber.
|
||||
|
||||
## Published Transforms
|
||||
|
||||
Broadcast on every frame unless `publish_tf` is false.
|
||||
|
||||
| Transform | Published when |
|
||||
|---|---|
|
||||
| `odom_frame_id` → `frame_id` | Odometry is available. |
|
||||
| `frame_id` → `camera_frame_id` | A camera is calibrated. Multi-camera setups get a numeric suffix; stereo gets `left_`/`right_` prefixes, with the right frame offset by the baseline. |
|
||||
| `frame_id` → `scan_frame_id` | A scan is present. |
|
||||
| `frame_id` → `imu_frame_id` | An IMU is present. |
|
||||
| `ground_truth_frame_id` → `ground_truth_base_frame_id` | Ground truth was recorded. |
|
||||
|
||||
## Services
|
||||
|
||||
| Service | Type | Description |
|
||||
|---|---|---|
|
||||
| `~/pause` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Pause playback. |
|
||||
| `~/resume` | `std_srvs/srv/Empty` | Resume it. |
|
||||
|
||||
When run as the standalone executable, the **space bar** toggles pause as well.
|
||||
|
||||
## Parameters
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `database` | `string` | `""` | **Required.** Path to the `.db` file. `~` is expanded, relative paths resolve against the working directory. The node throws on start-up if it is unset or unreadable. |
|
||||
| `rate` | `double` | `1.0` | Playback speed as a multiple of the recorded rate. `2.0` is twice as fast, `0.5` half. |
|
||||
| `start_id` | `int` | `0` | Skip to this node id. `0` starts at the beginning. |
|
||||
| `ignore_odom` | `bool` | `false` | Do not publish odometry or its transform, so you can run your own odometry against the raw sensor data. |
|
||||
| `publish_tf` | `bool` | `true` | Broadcast the transforms above. Turn it off if a robot state publisher already provides them. |
|
||||
| `publish_clock` | `bool` | `false` | Publish `/clock`. See [Simulated time](#simulated-time). |
|
||||
| `frame_id` | `string` | `"base_link"` | Robot base frame. |
|
||||
| `odom_frame_id` | `string` | `"odom"` | Odometry frame. |
|
||||
| `camera_frame_id` | `string` | `"camera_optical_link"` | Camera optical frame. |
|
||||
| `scan_frame_id` | `string` | `"base_laser_link"` | Lidar frame. |
|
||||
| `imu_frame_id` | `string` | `"imu_link"` | IMU frame. |
|
||||
| `ground_truth_frame_id` | `string` | `"world"` | Ground truth parent frame. |
|
||||
| `ground_truth_base_frame_id` | `string` | `"base_link_gt"` | Ground truth child frame. |
|
||||
| `qos` | `int` | `0` | Reliability of all publishers unless overridden below. |
|
||||
| `qos_camera_info`, `qos_odom`, `qos_scan`, `qos_scan_cloud`, `qos_global_pose`, `qos_gps`, `qos_imu`, `qos_env_sensor` | `int` | value of `qos` | Per-topic overrides. |
|
||||
|
||||
**2D scan geometry** — only used when the recorded scan has no angle metadata of its own, which happens for scans converted from a 3D lidar.
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `scan_angle_min` | `double` | `-π` | |
|
||||
| `scan_angle_max` | `double` | `π` | |
|
||||
| `scan_angle_increment` | `double` | `π/720` | |
|
||||
| `scan_range_min` | `double` | `0.0` | |
|
||||
| `scan_range_max` | `double` | `60.0` | |
|
||||
|
||||
## Simulated time
|
||||
|
||||
With `publish_clock` the node publishes `/clock` from the recorded stamps. Start every other node with `use_sim_time:=true` and the whole system runs on the database's timeline instead of the wall clock, so playback speed no longer affects behavior — a good idea when replaying faster than real time, and essential for reproducible runs.
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_util data_player --ros-args -p database:=map.db -p publish_clock:=true
|
||||
ros2 launch rtabmap_launch rtabmap.launch.py use_sim_time:=true
|
||||
```
|
||||
|
||||
## Notes
|
||||
|
||||
Playback ends when the last node has been published, and the standalone executable exits at that point.
|
||||
|
||||
The database is opened read-only as far as playback is concerned, so replaying the same file while RTAB-Map maps into another one is safe.
|
||||
@@ -0,0 +1,55 @@
|
||||
# disparity_to_depth
|
||||
|
||||
Converts a disparity image into a depth image.
|
||||
|
||||
Most of ROS handles depth, while a stereo pipeline produces disparity. This node bridges the two: for every pixel it computes `depth = baseline * focal / disparity`, taking the baseline and focal length from the incoming [`stereo_msgs/msg/DisparityImage`](https://docs.ros.org/en/jazzy/p/stereo_msgs/msg/DisparityImage.html) itself, so no camera info is needed.
|
||||
|
||||
Pixels whose disparity falls outside the message's own `min_disparity`/`max_disparity` are written as zero, which is the ROS convention for "no reading".
|
||||
|
||||
## Usage
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_util disparity_to_depth --ros-args \
|
||||
-r disparity:=/stereo/disparity \
|
||||
-r depth:=/stereo/depth
|
||||
```
|
||||
|
||||
```python
|
||||
ComposableNode(
|
||||
package='rtabmap_util',
|
||||
plugin='rtabmap_util::DisparityToDepth',
|
||||
name='disparity_to_depth',
|
||||
remappings=[('disparity', '/stereo/disparity'),
|
||||
('depth', '/stereo/depth')])
|
||||
```
|
||||
|
||||
## Subscribed Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `disparity` | [`stereo_msgs/msg/DisparityImage`](https://docs.ros.org/en/jazzy/p/stereo_msgs/msg/DisparityImage.html) | The disparity image must be `32FC1`; anything else is rejected with an error. |
|
||||
|
||||
## Published Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `depth` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) (`32FC1`) | Depth in **meters**. |
|
||||
| `depth_raw` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) (`16UC1`) | The same depth in **millimeters**, the compact form most RGB-D drivers publish. |
|
||||
|
||||
Both are computed only if something is subscribed to them, so leaving one unused costs nothing. Both keep the header of the input disparity image.
|
||||
|
||||
## Parameters
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `qos` | `int` | `0` | Reliability of both sides: `0` system default, `1` reliable, `2` best effort. |
|
||||
| `qos_sub` | `int` | value of `qos` | Reliability of the `disparity` subscription alone. |
|
||||
| `qos_pub` | `int` | value of `qos` | Reliability of the `depth` and `depth_raw` publishers alone. |
|
||||
| `queue_sub` | `int` | `1` | Queue depth of the `disparity` subscription. Must be at least 1. |
|
||||
| `queue_pub` | `int` | `1` | Queue depth of both publishers. Must be at least 1. |
|
||||
|
||||
## Notes
|
||||
|
||||
Depth beyond 65.535 m cannot be represented in the `16UC1` output and wraps around; use the `32FC1` `depth` topic for long-range stereo.
|
||||
|
||||
This node performs no filtering or hole-filling. A noisy disparity image gives a noisy depth image.
|
||||
@@ -0,0 +1,197 @@
|
||||
# imu_to_tf
|
||||
|
||||
Broadcasts the orientation of an IMU as a TF transform.
|
||||
|
||||
The node subscribes to a `sensor_msgs/msg/Imu` topic, takes the `orientation` field and broadcasts it on `/tf` as the rotation of `fixed_frame_id` → the IMU frame. Set `base_frame_id` and that frame becomes the child instead, with the orientation re-expressed in it from the IMU's mounting, so the transform says how the *robot* is oriented rather than how the sensor is. Either way `fixed_frame_id` is the parent, and nothing else of the message is used: the transform's translation is always zero, and the angular velocity and linear acceleration are ignored.
|
||||
|
||||
It exists so that a consumer that needs an oriented frame — a lidar deskewing node, a point cloud assembler, RViz — can get one from an IMU alone, without running odometry.
|
||||
|
||||
## Usage
|
||||
|
||||
As a standalone node:
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_util imu_to_tf --ros-args \
|
||||
-r imu/data:=/imu \
|
||||
-p fixed_frame_id:=odom
|
||||
```
|
||||
|
||||
As a composable node, in the same process as its producer or consumer:
|
||||
|
||||
```python
|
||||
ComposableNode(
|
||||
package='rtabmap_util',
|
||||
plugin='rtabmap_util::ImuToTF',
|
||||
name='imu_to_tf',
|
||||
parameters=[{'fixed_frame_id': 'odom'}],
|
||||
remappings=[('imu/data', '/imu')])
|
||||
```
|
||||
|
||||
### When the IMU has no orientation
|
||||
|
||||
The node reads `orientation` and nothing else, and many IMUs do not fill it in — they publish only angular velocity and linear acceleration. Fuse them into an orientation first, with a filter such as [`imu_filter_madgwick`](https://github.com/CCNYRoboticsLab/imu_tools), and point this node at the filter's output:
|
||||
|
||||
```python
|
||||
Node(
|
||||
package='imu_filter_madgwick', executable='imu_filter_madgwick_node',
|
||||
parameters=[{'use_mag': False, 'world_frame': 'enu', 'publish_tf': False}],
|
||||
remappings=[('imu/data_raw', '/camera/imu')]), # publishes /imu/data
|
||||
|
||||
Node(
|
||||
package='rtabmap_util', executable='imu_to_tf',
|
||||
parameters=[{'fixed_frame_id': 'odom'}],
|
||||
remappings=[('imu/data', '/imu/data')]),
|
||||
```
|
||||
|
||||
Set `publish_tf: False` on the filter. It can broadcast a transform of its own, and two nodes publishing orientation for the same frame is exactly the conflict described in [Notes](#notes). `use_mag: False` keeps it off the magnetometer, which is rarely trustworthy indoors or near motors.
|
||||
|
||||
A quick way to tell whether you need the filter at all:
|
||||
|
||||
```bash
|
||||
ros2 topic echo /camera/imu --field orientation --once
|
||||
```
|
||||
|
||||
All zeros, or an `orientation_covariance` whose first element is `-1`, means the driver is not estimating orientation and this node has nothing to publish.
|
||||
|
||||
### A stabilized frame for lidar deskewing and odometry
|
||||
|
||||
The way it is used in [`rtabmap_examples/launch/lidar3d.launch.py`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_examples/launch/lidar3d.launch.py). A 3D lidar needs a fixed frame to deskew against and ICP odometry benefits from a motion guess, but before odometry is running there is no `odom` frame to use. An IMU can supply one — for rotation.
|
||||
|
||||
Point `fixed_frame_id` at a frame that does not exist anywhere else, named after the base frame:
|
||||
|
||||
```python
|
||||
Node(
|
||||
package='rtabmap_util', executable='imu_to_tf',
|
||||
parameters=[{'fixed_frame_id': 'base_link_stabilized',
|
||||
'base_frame_id': 'base_link',
|
||||
'wait_for_transform_duration': 0.001}],
|
||||
remappings=[('imu/data', '/imu/data')])
|
||||
```
|
||||
|
||||
This publishes `base_link_stabilized` → `base_link` carrying the robot's orientation and nothing else. Because the node never publishes a translation, `base_link_stabilized` stays glued to the robot and only its *orientation* is meaningful over time: it is a gravity-leveled version of the base frame rather than a world frame. That is exactly what the two consumers need.
|
||||
|
||||
[lidar_deskewing](lidar_deskewing.md) then corrects the rotation of each sweep:
|
||||
|
||||
```python
|
||||
Node(
|
||||
package='rtabmap_util', executable='lidar_deskewing',
|
||||
parameters=[{'fixed_frame_id': 'base_link_stabilized'}],
|
||||
remappings=[('input_cloud', '/lidar/points')])
|
||||
```
|
||||
|
||||
and ICP odometry takes the same frame as its motion guess, with its own deskewing turned off since it is already done:
|
||||
|
||||
```python
|
||||
Node(
|
||||
package='rtabmap_odom', executable='icp_odometry',
|
||||
parameters=[{'frame_id': 'base_link',
|
||||
'odom_frame_id': 'icp_odom',
|
||||
'guess_frame_id': 'base_link_stabilized',
|
||||
'deskewing': False}],
|
||||
remappings=[('scan_cloud', '/lidar/points/deskewed')])
|
||||
```
|
||||
|
||||
The three nodes chain into a single TF tree:
|
||||
|
||||
```text
|
||||
map rtabmap
|
||||
└── icp_odom icp_odometry
|
||||
└── base_link_stabilized imu_to_tf
|
||||
└── base_link
|
||||
├── lidar_link robot description (static)
|
||||
└── imu_link
|
||||
```
|
||||
|
||||
| Edge | Published by |
|
||||
|---|---|
|
||||
| `map` → `icp_odom` | `rtabmap` |
|
||||
| `icp_odom` → `base_link_stabilized` | `icp_odometry` |
|
||||
| `base_link_stabilized` → `base_link` | **this node**, from the IMU orientation |
|
||||
| `base_link` → `lidar_link`, `imu_link` | your robot description, static |
|
||||
|
||||
Note what `icp_odometry` publishes: because `guess_frame_id` is set it broadcasts the *correction* `icp_odom` → `base_link_stabilized` rather than `icp_odom` → `base_link`. That is what makes the two nodes compose — the stabilized frame slots into the chain and every frame keeps exactly one parent. Without `guess_frame_id` the odometry would publish straight to `base_link` and fight this node over it.
|
||||
|
||||
Only rotation is compensated, and the two errors behave differently over a sweep:
|
||||
|
||||
| Error | How it scales | Worst when |
|
||||
|---|---|---|
|
||||
| Rotation, corrected here | grows with range | turning fast, looking far |
|
||||
| Translation, left over | same at every range, grows with speed | driving fast, looking close |
|
||||
|
||||
Moving slowly, or looking far, the leftover translation stays under the lidar's own range noise and can be ignored. Fast and close it is the bigger of the two, and it shifts the cloud rather than blurring it, so it turns into odometry drift.
|
||||
|
||||
Once something publishes a real `odom` → `base_link` — wheel or visual odometry, or an EKF such as [`robot_localization`](https://github.com/cra-ros-pkg/robot_localization) fusing that same IMU with wheel odometry — point both `fixed_frame_id` and `guess_frame_id` at `odom` instead and drop this node. Translation then gets compensated too.
|
||||
|
||||
### A rotation guess for visual odometry
|
||||
|
||||
The same stabilized frame is useful to a camera, for a different reason. Visual odometry predicts where each feature from the previous frame should land in the current one and searches around that prediction; on a fast rotation the prediction is far off, matches are lost and odometry breaks exactly when the motion is hardest.
|
||||
|
||||
An IMU fixes the prediction. Run this node as above to publish `base_link_stabilized` → `base_link`, then hand that frame to the odometry as its guess:
|
||||
|
||||
```python
|
||||
Node(
|
||||
package='rtabmap_odom', executable='rgbd_odometry',
|
||||
parameters=[{'frame_id': 'base_link',
|
||||
'guess_frame_id': 'base_link_stabilized'}],
|
||||
remappings=[('rgb/image', '/camera/color/image_raw'),
|
||||
('depth/image', '/camera/depth/image_rect_raw'),
|
||||
('rgb/camera_info', '/camera/color/camera_info')])
|
||||
```
|
||||
|
||||
With [`rtabmap_launch`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_launch) the same thing is one argument:
|
||||
|
||||
```bash
|
||||
ros2 launch rtabmap_launch rtabmap.launch.py odom_guess_frame_id:=base_link_stabilized
|
||||
```
|
||||
|
||||
The TF chain is the one from the previous section with `rgbd_odometry` in place of `icp_odometry`; it publishes the same `odom` → `base_link_stabilized` correction, so the frames still form one tree.
|
||||
|
||||
A rotation-only guess is enough here, because feature matching cares about where things appear, not where they are. Turning the camera slides every feature across the image by the same amount, near or far. Moving it slides them too, but far less, and less the further away they are — generally little enough to stay inside the window the matcher searches. So rotation is the part a guess has to get right, and that is exactly what the IMU supplies. It is also why an IMU far too drifty to give you a *pose* still makes a good guess: only the rotation over a single frame interval is being used, long before drift has time to accumulate.
|
||||
|
||||
## Subscribed Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `imu/data` | [`sensor_msgs/msg/Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Only `orientation` and `header` are read. The subscription has a queue depth of 1; its reliability comes from the `qos` parameter. |
|
||||
|
||||
## Published Topics
|
||||
|
||||
None. The node only broadcasts transforms.
|
||||
|
||||
## Published Transforms
|
||||
|
||||
| Transform | Description |
|
||||
|---|---|
|
||||
| `fixed_frame_id` → IMU frame | Broadcast when `base_frame_id` is empty. The child frame is the `header.frame_id` of the incoming message. |
|
||||
| `fixed_frame_id` → `base_frame_id` | Broadcast when `base_frame_id` is set. The orientation is re-expressed in the base frame first, see [Mounting offset](#mounting-offset). |
|
||||
|
||||
The transform carries a rotation only; its translation is always zero. It is stamped with the IMU message's stamp, not the current time.
|
||||
|
||||
## Required Transforms
|
||||
|
||||
| Transform | Description |
|
||||
|---|---|
|
||||
| `base_frame_id` → IMU frame | Only when `base_frame_id` is set and differs from the IMU's `header.frame_id`. This is the fixed mounting of the IMU on the robot, normally published by your robot description. |
|
||||
|
||||
## Parameters
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `fixed_frame_id` | `string` | `"odom"` | Parent frame of the broadcast transform. |
|
||||
| `base_frame_id` | `string` | `""` | Frame to report the orientation in. Empty broadcasts the IMU frame itself, which is the cheapest option when nothing else needs the base frame oriented. |
|
||||
| `qos` | `int` | `0` | Reliability of the `imu/data` subscription: `0` system default, `1` reliable, `2` best effort. Must match the publisher, or no message arrives. |
|
||||
| `wait_for_transform_duration` | `double` | `0.1` | Seconds to wait for the `base_frame_id` → IMU transform before giving up on a message. Only used when `base_frame_id` is set. |
|
||||
|
||||
## Mounting offset
|
||||
|
||||
When `base_frame_id` is set, the node looks up the mounting transform `base_frame_id` → IMU frame and re-expresses the orientation in the base frame. The **yaw of the mounting is deliberately discarded**: only its roll and pitch are applied.
|
||||
|
||||
That is what you want from an absolute orientation source. An IMU bolted on facing sideways still measures the same absolute heading as one facing forward, so its yaw must reach the base frame untouched; its roll and pitch, on the other hand, do have to be rotated into the base frame to be meaningful.
|
||||
|
||||
A message is **dropped** — logged as an error, nothing broadcast — if that mounting transform is not available within `wait_for_transform_duration`.
|
||||
|
||||
## Notes
|
||||
|
||||
Only one node may publish a given TF edge. If odometry is already publishing `odom` → `base_link`, do not point this node at the same pair — give it a frame of its own, as in [the stabilized frame above](#a-stabilized-frame-for-lidar-deskewing-and-odometry), or leave `base_frame_id` empty. Two publishers on one edge make the transform flicker between them.
|
||||
|
||||
The node does not integrate or filter anything — whatever orientation the message carries is what gets broadcast. See [When the IMU has no orientation](#when-the-imu-has-no-orientation) if your driver does not estimate one.
|
||||
@@ -0,0 +1,95 @@
|
||||
# lidar_deskewing
|
||||
|
||||
Removes the motion distortion from a lidar scan.
|
||||
|
||||
A spinning lidar takes tens of milliseconds to complete a sweep, and on a moving robot every point in that sweep is measured from a slightly different pose. The result is a *skewed* cloud: straight walls come out bent, and registration against it drifts.
|
||||
|
||||
This node uses TF to find where the sensor actually was when each point was taken, and moves every point into the pose at the start of the sweep. A straight wall comes back straight.
|
||||
|
||||
## Usage
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_util lidar_deskewing --ros-args \
|
||||
-p fixed_frame_id:=odom \
|
||||
-r input_cloud:=/velodyne_points
|
||||
```
|
||||
|
||||
```python
|
||||
ComposableNode(
|
||||
package='rtabmap_util',
|
||||
plugin='rtabmap_util::LidarDeskewing',
|
||||
name='lidar_deskewing',
|
||||
parameters=[{'fixed_frame_id': 'odom'}],
|
||||
remappings=[('input_cloud', '/velodyne_points')])
|
||||
```
|
||||
|
||||
## Subscribed Topics
|
||||
|
||||
Connect one of the two.
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `input_cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Must carry a **per-point time channel**, see [Requirements](#requirements). |
|
||||
| `input_scan` | [`sensor_msgs/msg/LaserScan`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/LaserScan.html) | Per-point times come from `time_increment`. |
|
||||
|
||||
## Published Topics
|
||||
|
||||
Output names are derived from the **resolved** input names, so remapping the input moves the output with it. With `input_cloud` remapped to `/velodyne_points` the output is `/velodyne_points/deskewed`.
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `<input_cloud>/deskewed` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The deskewed cloud, same frame and stamp as the input. |
|
||||
| `<input_scan>/deskewed` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | A `LaserScan` cannot represent a deskewed sweep — the points no longer lie on a regular angular grid — so the scan input also produces a cloud. |
|
||||
|
||||
## Required Transforms
|
||||
|
||||
| Transform | Description |
|
||||
|---|---|
|
||||
| `fixed_frame_id` → sensor frame, across the sweep | Must be available for the whole span of the sweep, at both its first and last stamp. |
|
||||
|
||||
## Parameters
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `fixed_frame_id` | `string` | `""` | **Required.** Frame the motion is measured against, usually `odom`. |
|
||||
| `wait_for_transform` | `double` | `0.01` | Seconds to wait for the transforms spanning the sweep. Raise it if odometry lags the lidar. |
|
||||
| `slerp` | `bool` | `false` | Interpolate between the poses at the start and end of the sweep instead of looking up TF per point. Much cheaper, and accurate enough at constant velocity. |
|
||||
| `queue_size` | `int` | `1` | Queue depth of the input subscriptions. |
|
||||
| `qos` | `int` | `0` | Reliability of the input subscriptions: `0` system default, `1` reliable, `2` best effort. |
|
||||
|
||||
## Requirements
|
||||
|
||||
For `input_cloud`, the cloud **must have a per-point time field**. Without one the node cannot know when each point was taken and cannot deskew.
|
||||
|
||||
The field has to be named `t`, `time`, `stamps` or `timestamp` — anything else is not recognized, whatever it contains. Its type decides how the value is read:
|
||||
|
||||
| Type | Meaning |
|
||||
|---|---|
|
||||
| `uint32` | nanoseconds since the cloud's own stamp |
|
||||
| `float32` | seconds since the cloud's own stamp |
|
||||
| `float64` | an absolute timestamp; seconds, milliseconds, microseconds and nanoseconds are told apart by magnitude |
|
||||
|
||||
Common drivers that satisfy this out of the box: **Ouster** (`t`), **Velodyne** (`time`), **RoboSense** (`timestamp`) and **Livox** (`timestamp`). Livox needs its PointCloud2 output rather than the default `CustomMsg` format, which this node cannot subscribe to at all.
|
||||
|
||||
To check what your driver actually publishes:
|
||||
|
||||
```bash
|
||||
ros2 topic echo /your/points --field fields --once
|
||||
```
|
||||
|
||||
If none of the four names is in that list, look for a driver option to add per-point timestamps before anything else.
|
||||
|
||||
The `fixed_frame_id` → sensor transform must cover the whole sweep, which means **odometry has to be at least as recent as the lidar**. If it lags, raise `wait_for_transform`.
|
||||
|
||||
## Behavior when TF is missing
|
||||
|
||||
The two inputs deliberately differ:
|
||||
|
||||
- A **cloud** is republished **unchanged** with a warning. Deskewing is an improvement, not a precondition, and dropping frames would break the pipeline behind it.
|
||||
- A **scan** is **dropped**, because converting it to a cloud is only worth doing as part of deskewing.
|
||||
|
||||
## Notes
|
||||
|
||||
Deskewing matters most when rotating: at 1 rad/s a 100 ms sweep spans nearly 6°, and the far end of the scan is badly misplaced. Pure translation at walking speed is a few centimeters, which matters at close range.
|
||||
|
||||
Put this node before ICP odometry or [point_cloud_assembler](point_cloud_assembler.md), not after. Anything registering against a skewed cloud has already paid for the distortion.
|
||||
@@ -0,0 +1,109 @@
|
||||
# map_assembler
|
||||
|
||||
Rebuilds the global maps from RTAB-Map's graph, in a separate process.
|
||||
|
||||
RTAB-Map publishes its graph and the per-node sensor data on `mapData`; turning that into a point cloud, an occupancy grid or an octomap costs real CPU. This node does that work, so the SLAM node does not have to and the mapping loop stays responsive.
|
||||
|
||||
It also lets you produce maps RTAB-Map is not currently configured to publish, or several differently-configured maps at once, without restarting SLAM.
|
||||
|
||||
The assembling itself is done by `MapsManager`, which is shared with `rtabmap_slam` — the outputs and every `Grid/*` parameter behave identically in both.
|
||||
|
||||
## Usage
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_util map_assembler --ros-args \
|
||||
-p Grid/CellSize:=0.05 -p Grid/RangeMax:=8.0 -p cloud_output_voxelized:=true
|
||||
```
|
||||
|
||||
```python
|
||||
ComposableNode(
|
||||
package='rtabmap_util',
|
||||
plugin='rtabmap_util::MapAssembler',
|
||||
name='map_assembler',
|
||||
parameters=[{'Grid/CellSize': '0.05', 'Grid/RangeMax': '8.0',
|
||||
'cloud_output_voxelized': True}])
|
||||
```
|
||||
|
||||
## Subscribed Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `mapData` | [`rtabmap_msgs/msg/MapData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/MapData.html) | The graph, plus the sensor data of any newly added node. Published by `rtabmap`. |
|
||||
|
||||
## Published Topics
|
||||
|
||||
Everything is published only when subscribed, and — by default — **latched**, so a subscriber joining late immediately receives the current map.
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `cloud_map` | [`PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Ground and obstacles together. |
|
||||
| `cloud_ground` | `PointCloud2` | Ground only, colored green. |
|
||||
| `cloud_obstacles` | `PointCloud2` | Obstacles only, colored red. |
|
||||
| `map` | [`OccupancyGrid`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/OccupancyGrid.html) | The 2D occupancy grid, the one navigation wants. |
|
||||
| `grid_prob_map` | `OccupancyGrid` | The same grid as occupancy probabilities rather than free/occupied/unknown. |
|
||||
| `octomap_occupied_space`, `octomap_obstacles`, `octomap_ground`, `octomap_empty_space`, `octomap_global_frontier_space` | `PointCloud2` | Octomap contents, one cloud per category. Requires RTAB-Map built with OctoMap. |
|
||||
| `octomap_grid` | `OccupancyGrid` | The octomap projected to 2D. |
|
||||
| `octomap_binary`, `octomap_full` | [`Octomap`](https://docs.ros.org/en/jazzy/p/octomap_msgs/msg/Octomap.html) | The tree itself, for `octovis` or other octomap consumers. Serialized as a **`ColorOcTree`**, see [Octomap tree type](#octomap-tree-type). |
|
||||
| `elevation_map` | [`GridMap`](https://github.com/ANYbotics/grid_map/blob/master/grid_map_msgs/msg/GridMap.msg) | Elevation map. Requires RTAB-Map built with `grid_map`. |
|
||||
|
||||
## Services
|
||||
|
||||
| Service | Type | Description |
|
||||
|---|---|---|
|
||||
| `~/reset` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Drop the cached nodes and every assembled map. |
|
||||
| `~/octomap_binary` | [`octomap_msgs/srv/GetOctomap`](https://docs.ros.org/en/jazzy/p/octomap_msgs/srv/GetOctomap.html) | Build and return the octomap on demand. |
|
||||
| `~/octomap_full` | `octomap_msgs/srv/GetOctomap` | The same, with occupancy probabilities. |
|
||||
|
||||
## Parameters
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `initialize_from_rtabmap_timeout` | `double` | `5.0` | Seconds to wait for rtabmap's `get_map_data` service on start-up, which is how the node catches up on a map that already exists. Set to `0` to skip the call and subscribe immediately, which is what you want when `map_assembler` starts *before* rtabmap. |
|
||||
| `rtabmap` | `string` | `"rtabmap"` | Name of the rtabmap node whose `get_map_data` service to call. |
|
||||
| `regenerate_local_grids` | `bool` | `false` | Discard the occupancy grids stored with each node and rebuild them from the raw sensor data. Use it to change `Grid/*` parameters on an existing map without re-running SLAM. Costs CPU per node. |
|
||||
| `config_path` | `string` | `""` | An RTAB-Map `.ini` file to load parameters from, instead of listing them individually. |
|
||||
|
||||
**Map assembly**, from `MapsManager`
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `latch` | `bool` | `true` | Publish with transient-local durability so late subscribers get the current map. |
|
||||
| `map_filter_radius` | `double` | `0.0` | Skip nodes closer together than this, in meters. A cheap way to thin a dense graph. `0` disables. |
|
||||
| `map_filter_angle` | `double` | `30.0` | With `map_filter_radius`, nodes are only merged if they also differ by less than this angle, in degrees. |
|
||||
| `map_always_update` | `bool` | `false` | **No effect here**, see below. |
|
||||
| `map_empty_ray_tracing` | `bool` | `true` | **No effect here**, see below. |
|
||||
| `map_cleanup` | `bool` | `true` | Free the cached clouds when nobody is subscribed. |
|
||||
| `cloud_output_voxelized` | `bool` | `true` | Voxelize the assembled clouds at `Grid/CellSize`. |
|
||||
| `cloud_subtract_filtering` | `bool` | `false` | Drop points that duplicate ones already in the map. Slower, smaller output. |
|
||||
| `cloud_subtract_filtering_min_neighbors` | `int` | `2` | Neighbors needed for a point to count as a duplicate. |
|
||||
| `octomap_tree_depth` | `int` | `16` | Depth the octomap clouds are generated at. Lower means coarser and faster. Maximum 16. |
|
||||
|
||||
`map_always_update` and `map_empty_ray_tracing` are declared because they come with `MapsManager`, but neither does anything in this node. Both only apply to the *current*, not-yet-committed node, which `MapsManager` identifies by the pose id `0`. That node is assembled inside `rtabmap_slam`'s `rtabmap` node from its live sensor data and is never published on `mapData`, so the graph reaching `map_assembler` only ever contains committed nodes. Set them on the `rtabmap` node instead, where they do apply.
|
||||
|
||||
Every RTAB-Map **`Grid/*`**, **`GridGlobal/*`**, **`StereoBM/*`** and **`StereoSGBM/*`** parameter is also exposed, all documented in RTAB-Map's [parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html). The split between the first two is worth knowing: **`Grid/*`** decides how each node's local grid is built from its sensor data — the same segmentation [obstacles_detection](obstacles_detection.md#parameters) does, and the parameters listed there apply here too — while **`GridGlobal/*`** decides how those local grids are merged into the global map, so it covers the map's minimum size, its occupancy threshold, and how far the graph must move before the whole map is rebuilt.
|
||||
|
||||
## Octomap tree type
|
||||
|
||||
RTAB-Map keeps a color per voxel, so the tree it publishes on `octomap_binary` and `octomap_full` reports its `id` as **`ColorOcTree`**, not the plain `OcTree` many examples assume.
|
||||
|
||||
That is deliberate and interoperable: `octomap_msgs::binaryMsgToMap()` and `fullMsgToMap()` branch on that `id` and hand you back an `octomap::ColorOcTree`, and `octovis` opens it without complaint. What does break is code that assumes the other branch:
|
||||
|
||||
```cpp
|
||||
octomap::AbstractOcTree * tree = octomap_msgs::binaryMsgToMap(msg);
|
||||
octomap::OcTree * octree = dynamic_cast<octomap::OcTree *>(tree); // null
|
||||
octomap::ColorOcTree * octree = dynamic_cast<octomap::ColorOcTree *>(tree); // ok
|
||||
```
|
||||
|
||||
`ColorOcTree` does not derive from `OcTree` — both derive from `OccupancyOcTreeBase` — so cast to `ColorOcTree`, or to `octomap::OccupancyOcTreeBase<...>` if you only need occupancy and want to accept either.
|
||||
|
||||
## Start-up
|
||||
|
||||
`map_assembler` normally starts alongside rtabmap and builds its maps from the `mapData` messages that follow. If it starts **after** rtabmap it would miss everything already mapped, so on start-up it calls rtabmap's `get_map_data` service once to fetch the existing map.
|
||||
|
||||
That call blocks the subscription to `mapData` until it returns or times out, which is wasted time when rtabmap is not running yet. Set `initialize_from_rtabmap_timeout` to `0` in that case.
|
||||
|
||||
If rtabmap is started later in localization mode, call its `publish_maps` service with `graph_only=false` so `map_assembler` receives the data it missed.
|
||||
|
||||
## Notes
|
||||
|
||||
`regenerate_local_grids` is the parameter to reach for when a recorded map's grids were built with settings you now want to change. Without it, `Grid/*` changes only affect nodes added from then on, because each node's grid is stored with it.
|
||||
@@ -0,0 +1,140 @@
|
||||
# obstacles_detection
|
||||
|
||||
Segments a point cloud into ground and obstacles.
|
||||
|
||||
The node takes a cloud, works out which points belong to the floor and which stick up from it, and publishes the two apart. Downstream that feeds navigation: obstacles into a costmap, ground into a traversability check.
|
||||
|
||||
The segmentation is RTAB-Map's own [`LocalGridMaker`](https://introlab.github.io/rtabmap/api/latest/classrtabmap_1_1LocalGridMaker.html), so it is configured through the same `Grid/*` parameters as RTAB-Map itself and produces the same result the SLAM node would.
|
||||
|
||||
## Usage
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_util obstacles_detection --ros-args \
|
||||
-r cloud:=/camera/cloud \
|
||||
-p frame_id:=base_link \
|
||||
-p Grid/MaxObstacleHeight:=2.0
|
||||
```
|
||||
|
||||
```python
|
||||
ComposableNode(
|
||||
package='rtabmap_util',
|
||||
plugin='rtabmap_util::ObstaclesDetection',
|
||||
name='obstacles_detection',
|
||||
parameters=[{'frame_id': 'base_link', 'Grid/MaxObstacleHeight': '2.0'}],
|
||||
remappings=[('cloud', '/camera/cloud')])
|
||||
```
|
||||
|
||||
### Feeding a nav2 costmap
|
||||
|
||||
The usual reason to run this node: nav2's costmap wants to be told separately what is floor and what is in the way. A depth camera gives neither directly, so the chain is depth image → cloud → segmented cloud → costmap.
|
||||
|
||||
[point_cloud_xyz](point_cloud_xyz.md) projects the depth image, with `decimation` and `voxel_size` set to keep the cost down, and this node splits the result:
|
||||
|
||||
```python
|
||||
Node(
|
||||
package='rtabmap_util', executable='point_cloud_xyz',
|
||||
parameters=[{'decimation': 2, 'max_depth': 3.0, 'voxel_size': 0.02}],
|
||||
remappings=[('depth/image', '/camera/depth/image_raw'),
|
||||
('depth/camera_info', '/camera/camera_info'),
|
||||
('cloud', '/camera/cloud')]),
|
||||
|
||||
Node(
|
||||
package='rtabmap_util', executable='obstacles_detection',
|
||||
parameters=[{'frame_id': 'base_link'}],
|
||||
remappings=[('cloud', '/camera/cloud'),
|
||||
('ground', '/camera/ground'),
|
||||
('obstacles', '/camera/obstacles')]),
|
||||
```
|
||||
|
||||
The two outputs then become two observation sources on the costmap's voxel layer:
|
||||
|
||||
```yaml
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: ground obstacles
|
||||
ground:
|
||||
topic: /camera/ground
|
||||
data_type: "PointCloud2"
|
||||
max_obstacle_height: 0.4
|
||||
marking: False # the floor is not an obstacle...
|
||||
clearing: True # ...but seeing it proves the space is free
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
obstacles:
|
||||
topic: /camera/obstacles
|
||||
data_type: "PointCloud2"
|
||||
max_obstacle_height: 0.4
|
||||
marking: True
|
||||
clearing: True
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
```
|
||||
|
||||
The `marking`/`clearing` split is the whole point. Ground points only clear: they tell the costmap that the space the camera looked through is free, without writing an obstacle at floor level. Obstacle points do both, so an obstacle that moves away is cleared by the next observation instead of lingering.
|
||||
|
||||
Feeding the raw cloud in as a single source cannot do this — every floor point would mark an obstacle and the robot would refuse to move. Working from [`turtlebot3_rgbd.launch.py`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py) and its [nav2 parameters](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/params/turtlebot3_rgbd_nav2_params.yaml) will save some time.
|
||||
|
||||
## Subscribed Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The cloud to segment, in any frame that TF can relate to `frame_id`. |
|
||||
|
||||
## Published Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `ground` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Points classified as floor. In the **input** cloud's frame. |
|
||||
| `obstacles` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Points classified as obstacles. In the **input** cloud's frame. |
|
||||
| `proj_obstacles` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The obstacles flattened onto the ground plane, in `frame_id`. This is the 2D footprint a planar costmap wants. |
|
||||
|
||||
Each output is computed only if something is subscribed to it.
|
||||
|
||||
## Required Transforms
|
||||
|
||||
| Transform | Description |
|
||||
|---|---|
|
||||
| `frame_id` → cloud frame | Where the sensor sits on the robot. Segmentation happens in `frame_id`, so this is what makes "up" meaningful. |
|
||||
| `map_frame_id` → `frame_id` | Only when `map_frame_id` is set. See [Levelling on a slope](#levelling-on-a-slope). |
|
||||
|
||||
## Parameters
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `frame_id` | `string` | `"base_link"` | The robot frame. Its xy plane is the ground plane the segmentation works against. |
|
||||
| `map_frame_id` | `string` | `""` | See [Levelling on a slope](#levelling-on-a-slope). |
|
||||
| `wait_for_transform` | `double` | `0.2` | Seconds to wait for a transform before dropping the cloud. |
|
||||
| `qos` | `int` | `0` | Reliability of the subscription and the publishers: `0` system default, `1` reliable, `2` best effort. |
|
||||
|
||||
Every RTAB-Map **`Grid/*`** parameter is also exposed as a ROS parameter of this node, and they are what actually control the segmentation. They are documented in RTAB-Map's [parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html).
|
||||
|
||||
The one to decide first is `Grid/NormalsSegmentation`, which picks between two ways of finding the ground. Left on, it is segmented from surface normals, which copes with slopes and steps. Turned off, it is a plain height threshold: much cheaper, and exact when the floor really is flat, but `Grid/MaxGroundHeight` then has to be set, since it *is* that threshold.
|
||||
|
||||
## Levelling on a slope
|
||||
|
||||
Segmentation is done in `frame_id`, so if the robot is pitched or rolled — on a ramp, or with a suspension that dips — the ground plane tilts with it and the floor ahead can be classified as an obstacle.
|
||||
|
||||
Setting `map_frame_id` makes the node take the robot's pose in that frame and apply its **roll and pitch**, so segmentation happens against a level plane rather than the robot's own tilt.
|
||||
|
||||
Height is a separate matter: the robot's **z** in the map frame is ignored unless `Grid/MapFrameProjection` is also set to `true`. That is usually what you want — a height threshold should be measured from the robot, not from an arbitrary map origin — but if you are mapping a multi-level building and want the thresholds relative to the map, enable it.
|
||||
|
||||
## Notes
|
||||
|
||||
If `obstacles` comes back empty on an obviously cluttered scene, check `Grid/RangeMax` first. It is **not unlimited by default**, and everything beyond it is discarded before segmentation even runs.
|
||||
|
||||
The second thing to check is `Grid/MinClusterSize` against your cloud density. A sparse lidar can produce clusters smaller than the default, in which case every obstacle is thrown away as noise. Either lower it or raise `Grid/ClusterRadius`.
|
||||
@@ -0,0 +1,97 @@
|
||||
# point_cloud_aggregator
|
||||
|
||||
Merges one cloud from each of several sensors into a single cloud.
|
||||
|
||||
A robot with two or three lidars, or a ring of depth cameras, produces one cloud per sensor. This node waits for a matching set, transforms them all into a common frame and publishes a single cloud, so everything downstream sees the robot's full field of view as one measurement.
|
||||
|
||||
The sensors do not have to fire together: the clouds are matched by nearest stamp, and setting `fixed_frame_id` compensates for the robot having moved between them. See [Sensors that do not fire together](#sensors-that-do-not-fire-together).
|
||||
|
||||
It combines **several sensors into one frame**. To combine **one sensor over many frames**, use [point_cloud_assembler](point_cloud_assembler.md).
|
||||
|
||||
## Usage
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_util point_cloud_aggregator --ros-args \
|
||||
-p count:=3 -p frame_id:=base_link -p fixed_frame_id:=odom \
|
||||
-r cloud1:=/lidar_front/points/deskewed \
|
||||
-r cloud2:=/lidar_left/points/deskewed \
|
||||
-r cloud3:=/lidar_right/points/deskewed
|
||||
```
|
||||
|
||||
```python
|
||||
ComposableNode(
|
||||
package='rtabmap_util',
|
||||
plugin='rtabmap_util::PointCloudAggregator',
|
||||
name='point_cloud_aggregator',
|
||||
parameters=[{'count': 3, 'frame_id': 'base_link', 'fixed_frame_id': 'odom'}],
|
||||
remappings=[('cloud1', '/lidar_front/points/deskewed'),
|
||||
('cloud2', '/lidar_left/points/deskewed'),
|
||||
('cloud3', '/lidar_right/points/deskewed')])
|
||||
```
|
||||
|
||||
With 2D or 3D lidars, feed the aggregator **deskewed** clouds: run a [lidar_deskewing](lidar_deskewing.md) node per sensor first, which is where the `/deskewed` topics above come from. For a 2D lidar publishing `LaserScan` that node is needed regardless — this one only takes `PointCloud2`, and `lidar_deskewing` converts to one as it deskews.
|
||||
|
||||
The two nodes correct different motions and you generally want both. Deskewing removes the distortion *within* each sweep, point by point, because a spinning lidar measures each point from a slightly different pose. `fixed_frame_id` here places whole clouds relative to each other, because the sensors did not fire at the same instant. Merging raw sweeps only merges their distortions.
|
||||
|
||||
## Subscribed Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `cloud1` … `cloud4` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Only the first `count` are subscribed. |
|
||||
|
||||
## Published Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `combined_cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | In `frame_id`, or in `cloud1`'s frame if `frame_id` is empty. Stamped with `cloud1`. |
|
||||
|
||||
Nothing is computed unless `combined_cloud` has a subscriber.
|
||||
|
||||
## Required Transforms
|
||||
|
||||
| Transform | Description |
|
||||
|---|---|
|
||||
| target frame → each cloud's frame | Where each sensor sits. The target is `frame_id`, or `cloud1`'s frame when that is empty. |
|
||||
| `fixed_frame_id` → each cloud's frame, at each stamp | Only when `fixed_frame_id` is set. See [Sensors that do not fire together](#sensors-that-do-not-fire-together). |
|
||||
|
||||
## Parameters
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `count` | `int` | `2` | How many clouds to combine, 2 to 4. Determines how many `cloudN` topics are subscribed. |
|
||||
| `frame_id` | `string` | `""` | Frame to express the combined cloud in. Empty uses `cloud1`'s frame, which is the cheapest option since that cloud then needs no transform, but see [Converting back to a LaserScan](#converting-back-to-a-laserscan). |
|
||||
| `fixed_frame_id` | `string` | `""` | Frame to compensate motion against, usually `odom`. See below. |
|
||||
| `approx_sync` | `bool` | `true` | Match the clouds by nearest stamp. Set false when the sensors are hardware-triggered and share exact stamps. |
|
||||
| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. A good guard against silently merging stale data. |
|
||||
| `wait_for_transform` | `double` | `0.1` | Seconds to wait for a transform before dropping the set. |
|
||||
| `xyz_output` | `bool` | `false` | Strip everything but XYZ from the output. Useful when the inputs disagree on their extra fields. |
|
||||
| `topic_queue_size` | `int` | `1` | Queue depth of each input subscription. |
|
||||
| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. |
|
||||
| `qos` | `int` | `0` | Reliability of the cloud subscriptions: `0` system default, `1` reliable, `2` best effort. |
|
||||
|
||||
## Converting back to a LaserScan
|
||||
|
||||
Some consumers still want a 2D `LaserScan` — `slam_toolbox`, `amcl`, or a costmap layer configured for one. [`pointcloud_to_laserscan`](https://docs.ros.org/en/jazzy/p/pointcloud_to_laserscan/) flattens the combined cloud into one:
|
||||
|
||||
```python
|
||||
Node(
|
||||
package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node',
|
||||
parameters=[{'target_frame': 'base_link', 'min_height': -0.1, 'max_height': 0.5}],
|
||||
remappings=[('cloud_in', '/combined_cloud')])
|
||||
```
|
||||
|
||||
**Set `frame_id` to the robot center when you do this.** A `LaserScan` is a set of ranges measured outward from one origin, so the conversion is only meaningful about a point the consumer thinks of as the robot. Leaving `frame_id` empty puts the combined cloud in `cloud1`'s frame — a sensor bolted somewhere on the edge of the robot — and every range then comes out measured from that corner. With three lidars merged, the result is a scan centerd on whichever one happened to be `cloud1`.
|
||||
|
||||
One case where you should *not* combine first: if the clouds are only going into a nav2 costmap, give nav2 each sensor as its own observation source instead. A costmap clears free space by ray tracing outward from where the observation was made, and it takes that origin from the cloud's own frame. Merge everything into one cloud at `base_link` and every point looks as though it were seen from the robot center, so space gets cleared along lines no sensor ever looked down — including straight through whatever the other sensors can see.
|
||||
|
||||
## Sensors that do not fire together
|
||||
|
||||
With `approx_sync` the clouds carry different stamps, and on a moving robot each was captured from a different pose. Merging them by their static mounting transforms alone smears the result.
|
||||
|
||||
Setting `fixed_frame_id` fixes that: the node asks TF where each sensor was at its own stamp, relative to that fixed frame, and places each cloud accordingly. Two lidars 30 ms apart on a robot turning at 1 rad/s are nearly 2° apart — clearly visible as a doubled wall.
|
||||
|
||||
Leave it empty only when the sensors are genuinely synchronized, or when the robot is stationary.
|
||||
|
||||
## Diagnostics
|
||||
|
||||
The node publishes to `/diagnostics` and warns if no combined cloud has been produced for a while — usually a sign that one of the `cloudN` topics is silent, or that the stamps are too far apart to sync.
|
||||
@@ -0,0 +1,162 @@
|
||||
# point_cloud_assembler
|
||||
|
||||
Accumulates the clouds of one sensor over time into a denser cloud.
|
||||
|
||||
A single sweep is sparse, or narrow, or both. This node keeps the recent ones, places each where the sensor was when it was captured, and publishes the union.
|
||||
|
||||
That is used for two quite different things. With a **narrow field of view** — a depth camera reduced to a fake scan, say — accumulating a second's worth of sweeps as the robot moves is what makes the sensor usable for SLAM at all. With a 3D lidar it is about **enriching what already works**: each node gets a denser, less occluded cloud, which registers better and puts many more points in the database, so an offline export later has the resolution to be worth having.
|
||||
|
||||
It combines **one sensor over many frames**. To combine **several sensors into one frame**, use [point_cloud_aggregator](point_cloud_aggregator.md) — or, if you want them merely accumulated rather than matched into sets, remap them all onto this node's `cloud` topic. Nothing stops several publishers sharing it, and each cloud is placed by its own stamp and frame like any other; the publish trigger then covers them together — `max_clouds` counts across all the sensors, and a given `assembling_time` gathers correspondingly more clouds.
|
||||
|
||||
## Usage
|
||||
|
||||
Assemble 10 sweeps, using TF for the poses:
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_util point_cloud_assembler --ros-args \
|
||||
-r cloud:=/velodyne_points/deskewed \
|
||||
-p max_clouds:=10 -p fixed_frame_id:=odom -p voxel_size:=0.05
|
||||
```
|
||||
|
||||
```python
|
||||
ComposableNode(
|
||||
package='rtabmap_util',
|
||||
plugin='rtabmap_util::PointCloudAssembler',
|
||||
name='point_cloud_assembler',
|
||||
parameters=[{'max_clouds': 10, 'fixed_frame_id': 'odom', 'voxel_size': 0.05}],
|
||||
remappings=[('cloud', '/velodyne_points/deskewed')])
|
||||
```
|
||||
|
||||
With a lidar, feed it **deskewed** clouds from a [lidar_deskewing](lidar_deskewing.md) node rather than the driver's raw output: accumulating skewed sweeps accumulates their distortion too. For a 2D lidar publishing `LaserScan` that node is needed regardless — this one only takes `PointCloud2`, and `lidar_deskewing` converts to one as it deskews.
|
||||
|
||||
### Denser clouds for SLAM, and keeping every point
|
||||
|
||||
From [`lidar3d_assemble.launch.py`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_examples/launch/lidar3d_assemble.launch.py). Here the input is a real 3D lidar, and assembling buys both resolution and coverage: a node built from a second of sweeps is denser between the rings and sees around what a single sweep was occluded by, so it registers better.
|
||||
|
||||
It also decides how much of the lidar survives. RTAB-Map stores one cloud per node and updates at around 1 Hz, while the lidar and odometry run at 10 — and running the mapping node at 10 Hz is not practical. Nine sweeps in ten therefore never reach the database. The `assembling_time: 1.0` below hands each node the whole second instead, so **nothing is thrown away**: the database keeps every point the lidar returned, which is what makes this the approach for survey scanning and a full-resolution offline export.
|
||||
|
||||
```python
|
||||
Node(
|
||||
package='rtabmap_util', executable='point_cloud_assembler',
|
||||
parameters=[{'assembling_time': 1.0,
|
||||
'fixed_frame_id': ''}], # '' selects the odom topic
|
||||
remappings=[('cloud', '/lidar/points/deskewed'),
|
||||
('odom', 'icp_odom')]),
|
||||
```
|
||||
|
||||
Note `fixed_frame_id: ''`. Clearing it switches the node from TF to the `odom` topic, pairing each cloud with the exact odometry message that goes with it rather than an interpolated TF lookup — see [Where the poses come from](#where-the-poses-come-from). Feeding the result to `rtabmap` as `scan_cloud` means the assembled cloud, not the raw sweep, is what gets stored.
|
||||
|
||||
### Widening a narrow field of view
|
||||
|
||||
From [`turtlebot3_rgbd_fake_scan.launch.py`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_fake_scan.launch.py). A depth camera sees perhaps 60° across, and [`depthimage_to_laserscan`](https://docs.ros.org/en/jazzy/p/depthimage_to_laserscan/) reduces that to a fake scan thinner still. One of those is too little to localize against; twenty of them, accumulated as the robot drives and turns, cover a useful arc.
|
||||
|
||||
```python
|
||||
Node(
|
||||
package='rtabmap_util', executable='point_cloud_assembler',
|
||||
parameters=[{'max_clouds': 20,
|
||||
'circular_buffer': True,
|
||||
'linear_update': 0.3,
|
||||
'angular_update': 0.5,
|
||||
'voxel_size': 0.05,
|
||||
'frame_id': 'base_link'}],
|
||||
remappings=[('cloud', '/camera/scan/deskewed')]),
|
||||
```
|
||||
|
||||
`circular_buffer` is what makes this work as a live input: the window rolls, so every incoming scan produces a full assembled cloud rather than one per twenty. `linear_update` and `angular_update` stop a stationary robot from filling the buffer with twenty copies of the same view, which would leave it with nothing but the current scan the moment it moved off again.
|
||||
|
||||
The cloud goes to `rtabmap` as `scan_cloud`, with `scan_cloud_is_2d` set since the points all came from one row of pixels.
|
||||
|
||||
## Subscribed Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The sweeps to accumulate. Ideally deskewed, see [Usage](#usage). |
|
||||
| `odom` | [`nav_msgs/msg/Odometry`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Odometry.html) | Only when `fixed_frame_id` is empty. See [Where the poses come from](#where-the-poses-come-from). |
|
||||
| `odom_info` | [`rtabmap_msgs/msg/OdomInfo`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/OdomInfo.html) | Only when `subscribe_odom_info` is true. |
|
||||
|
||||
## Published Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `assembled_cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | In `frame_id` if set, otherwise the frame of the newest cloud. Stamped with the newest cloud. |
|
||||
|
||||
Nothing is accumulated unless `assembled_cloud` has a subscriber.
|
||||
|
||||
## Required Transforms
|
||||
|
||||
| Transform | Description |
|
||||
|---|---|
|
||||
| `fixed_frame_id` → cloud frame, at each stamp | Only in TF mode, i.e. when `fixed_frame_id` is set. |
|
||||
| `frame_id` → cloud frame | Only when `frame_id` is set. |
|
||||
|
||||
## Parameters
|
||||
|
||||
**What triggers a publish** — set exactly one of these
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `max_clouds` | `int` | `0` | Publish once this many clouds have been collected. |
|
||||
| `assembling_time` | `double` | `0.0` | Publish once this many seconds have been collected. |
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `circular_buffer` | `bool` | `false` | Keep a rolling window instead of clearing after each publish, so a full assembled cloud is published for **every** input rather than one in `max_clouds`. Costs more, gives smooth output. |
|
||||
| `skip_clouds` | `int` | `0` | Drop this many input clouds between the ones kept. |
|
||||
| `linear_update` | `double` | `0.0` | Only accumulate a cloud if the sensor has moved this far, in meters, since the last one kept. `0` disables. |
|
||||
| `angular_update` | `double` | `0.0` | Same for rotation, in radians. `0` disables. |
|
||||
|
||||
**Poses**
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `fixed_frame_id` | `string` | `"odom"` | Frame the sweeps are placed in, via TF. **Set it to `""` to use the `odom` topic instead.** |
|
||||
| `frame_id` | `string` | `""` | Frame to express the output in. Empty uses the newest cloud's frame. |
|
||||
| `wait_for_transform` | `double` | `0.1` | Seconds to wait for a transform before dropping a cloud. |
|
||||
| `subscribe_odom_info` | `bool` | `false` | Keep only the clouds odometry marked as keyframes. Needs the `odom` topic mode, see [Following odometry's keyframes](#following-odometrys-keyframes). |
|
||||
|
||||
**Filtering**
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `range_min` | `double` | `0.0` | Drop points nearer than this to the sensor, in meters. Good for removing the robot itself. `0` disables. |
|
||||
| `range_max` | `double` | `0.0` | Drop points further than this, in meters. `0` disables. |
|
||||
| `voxel_size` | `double` | `0.0` | Downsample the assembled cloud to one point per voxel, in meters. `0` disables. Strongly recommended, otherwise the cloud grows linearly with `max_clouds`. |
|
||||
| `noise_radius` | `double` | `0.0` | Radius outlier removal on the output, in meters. `0` disables. |
|
||||
| `noise_min_neighbors` | `int` | `5` | Neighbors needed within `noise_radius`. |
|
||||
| `remove_z` | `bool` | `false` | Flatten the output to 2D by zeroing z. |
|
||||
|
||||
**Plumbing**
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `topic_queue_size` | `int` | `1` | Queue depth of each input subscription. |
|
||||
| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer, in odom-topic mode. |
|
||||
| `qos` | `int` | `0` | Reliability of the cloud subscription. |
|
||||
| `qos_odom` | `int` | value of `qos` | Reliability of the `odom` and `odom_info` subscriptions. |
|
||||
|
||||
## Where the poses come from
|
||||
|
||||
Each sweep has to be placed where the sensor was when it was captured, and there are two ways to get that pose:
|
||||
|
||||
- **TF** (default). `fixed_frame_id` is set, and the node looks the pose up per cloud. Simple, and it works with any odometry source.
|
||||
- **The `odom` topic**. Set `fixed_frame_id` to `""` and the node synchronizes each cloud with an `Odometry` message instead. Use this when odometry is not published to TF, or when you need the pose that exactly matches the cloud rather than an interpolated one.
|
||||
|
||||
Because `fixed_frame_id` **defaults to `"odom"`**, the `odom` topic is not subscribed unless you clear it explicitly. Setting `subscribe_odom_info` alone is not enough.
|
||||
|
||||
## Following odometry's keyframes
|
||||
|
||||
With `subscribe_odom_info` the node also takes `odom_info` and keeps a cloud only when that message reports a keyframe was added; the ones in between are dropped.
|
||||
|
||||
This is a better-informed version of `linear_update` and `angular_update`. Those are fixed distances you have to guess at, whereas odometry decides a keyframe from how much of the current scan still matches the last one — `Odom/ScanKeyFrameThr` for ICP, `Odom/KeyFrameThr` for visual odometry. It therefore adapts to the scene, keeping more clouds where the geometry changes quickly and fewer down a featureless corridor, and the assembled cloud ends up built from exactly the frames odometry itself considered distinct.
|
||||
|
||||
It only has an effect in the `odom` topic mode. With `fixed_frame_id` set the node subscribes to the cloud on its own and never sees `odom_info`, so clear `fixed_frame_id` as well — see [Where the poses come from](#where-the-poses-come-from).
|
||||
|
||||
## Notes
|
||||
|
||||
Set `voxel_size`. Without it the assembled cloud is the plain union of every sweep, points and all, and both memory and downstream cost grow with `max_clouds`. A voxel size near the sensor's resolution costs almost no fidelity.
|
||||
|
||||
`circular_buffer` changes the output rate, not just the contents: without it you get one assembled cloud per `max_clouds` inputs, with it you get one per input.
|
||||
|
||||
## Diagnostics
|
||||
|
||||
The node publishes to `/diagnostics` and warns if no assembled cloud has been produced for a while — typically a missing transform or a silent input topic.
|
||||
@@ -0,0 +1,95 @@
|
||||
# point_cloud_xyz
|
||||
|
||||
Projects a depth or disparity image into a point cloud.
|
||||
|
||||
The node takes a depth image and its calibration and produces a [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html), with optional decimation, range limits, voxel and radius filtering, and normal estimation — the same preprocessing RTAB-Map would do internally, done once and shared.
|
||||
|
||||
[`depth_image_proc`](https://docs.ros.org/en/jazzy/p/depth_image_proc/)'s own `point_cloud_xyz` does the bare projection; this node exists for the filtering, and for accepting disparity directly.
|
||||
|
||||
See [point_cloud_xyzrgb](point_cloud_xyzrgb.md) for the colored equivalent.
|
||||
|
||||
## Usage
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_util point_cloud_xyz --ros-args \
|
||||
-r depth/image:=/camera/depth/image_raw \
|
||||
-r depth/camera_info:=/camera/depth/camera_info \
|
||||
-p decimation:=4 -p max_depth:=5.0 -p voxel_size:=0.05
|
||||
```
|
||||
|
||||
```python
|
||||
ComposableNode(
|
||||
package='rtabmap_util',
|
||||
plugin='rtabmap_util::PointCloudXYZ',
|
||||
name='point_cloud_xyz',
|
||||
parameters=[{'decimation': 4, 'max_depth': 5.0, 'voxel_size': 0.05}],
|
||||
remappings=[('depth/image', '/camera/depth/image_raw'),
|
||||
('depth/camera_info', '/camera/depth/camera_info')])
|
||||
```
|
||||
|
||||
## Subscribed Topics
|
||||
|
||||
The node listens on two independent input sets and uses whichever one is being published. Only one of them should be connected.
|
||||
|
||||
**Depth**
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `depth/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | `32FC1` (meters), `16UC1` (millimeters) or `mono16`. Goes through [`image_transport`](https://docs.ros.org/en/jazzy/p/image_transport/), see `depth_transport` parameter below. |
|
||||
| `depth/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | |
|
||||
|
||||
**Disparity**
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `disparity/image` | [`stereo_msgs/msg/DisparityImage`](https://docs.ros.org/en/jazzy/p/stereo_msgs/msg/DisparityImage.html) | `32FC1` or `16SC1`. The 16-bit form is fixed point, 16 units per pixel of disparity. |
|
||||
| `disparity/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | |
|
||||
|
||||
## Published Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | In the frame of the input image, stamped with it. Carries `normal_*` fields when normals are enabled. |
|
||||
|
||||
Nothing is computed unless `cloud` has a subscriber.
|
||||
|
||||
## Parameters
|
||||
|
||||
**Synchronization**
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `approx_sync` | `bool` | `true` | Match image and camera info by nearest stamp. Set false when they are published with identical stamps, which is stricter and cheaper. |
|
||||
| `approx_sync_max_interval` | `double` | `0.0` | With `approx_sync`, reject pairs further apart than this many seconds. `0` disables the check. |
|
||||
| `topic_queue_size` | `int` | `1` | Queue depth of each input subscription. |
|
||||
| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. |
|
||||
| `qos` | `int` | `0` | Reliability of the image and disparity subscriptions: `0` system default, `1` reliable, `2` best effort. |
|
||||
| `qos_camera_info` | `int` | value of `qos` | Reliability of the camera info subscriptions. |
|
||||
| `depth_transport` | `string` | `"raw"` | `image_transport` plugin for `depth/image`, e.g. `compressedDepth`. |
|
||||
|
||||
**Projection and filtering**, applied in this order
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `decimation` | `int` | `1` | Keep one pixel in `decimation`, in each direction. `2` gives a quarter of the points. The image dimensions must divide by it. |
|
||||
| `roi_ratios` | `string` | `""` | Crop before projecting, as four ratios `"left right top bottom"`, e.g. `"0.1 0.1 0 0.2"`. |
|
||||
| `min_depth` | `double` | `0.0` | Discard points nearer than this, in meters. `0` disables. |
|
||||
| `max_depth` | `double` | `0.0` | Discard points further than this, in meters. `0` disables. |
|
||||
| `voxel_size` | `double` | `0.0` | Downsample to one point per voxel of this size, in meters. `0` disables. |
|
||||
| `noise_filter_radius` | `double` | `0.0` | Radius outlier removal, in meters. `0` disables. |
|
||||
| `noise_filter_min_neighbors` | `int` | `5` | Neighbors a point needs within `noise_filter_radius` to survive. |
|
||||
| `normal_k` | `int` | `0` | Estimate normals from this many nearest neighbors. `0` disables. |
|
||||
| `normal_radius` | `double` | `0.0` | Estimate normals from all neighbors within this radius, in meters. `0` disables. |
|
||||
| `filter_nans` | `bool` | `false` | See [Organized output](#organized-output). |
|
||||
|
||||
## Organized output
|
||||
|
||||
By default the cloud stays **organized**: one point per pixel, in image order, with out-of-range points set to NaN rather than removed. That layout is what lets consumers treat the cloud as an image, and it is why a cloud with `max_depth` set still reports the full point count.
|
||||
|
||||
Set `filter_nans` to `true` to drop the invalid points instead. The cloud becomes unorganized and its size reflects what is actually in range — including being empty when nothing is.
|
||||
|
||||
Voxel and radius filtering also produce unorganized clouds, since both remove points.
|
||||
|
||||
## Notes
|
||||
|
||||
`decimation` is by far the cheapest way to cut the cost of everything downstream, and on a depth image it loses very little: neighboring pixels of a surface are nearly redundant. Reach for it before `voxel_size`.
|
||||
@@ -0,0 +1,121 @@
|
||||
# point_cloud_xyzrgb
|
||||
|
||||
Projects an RGB-D frame, a stereo pair or a disparity image into a colored point cloud.
|
||||
|
||||
The colored counterpart of [point_cloud_xyz](point_cloud_xyz.md): same filtering, same parameters, but every point carries the color of the pixel it came from. It accepts four different input sets, so it can sit at the end of an RGB-D, stereo or disparity pipeline without anything in between.
|
||||
|
||||
## Usage
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_util point_cloud_xyzrgb --ros-args \
|
||||
-r rgb/image:=/camera/color/image_raw \
|
||||
-r depth/image:=/camera/aligned_depth_to_color/image_raw \
|
||||
-r rgb/camera_info:=/camera/color/camera_info \
|
||||
-p decimation:=4 -p voxel_size:=0.05
|
||||
```
|
||||
|
||||
```python
|
||||
ComposableNode(
|
||||
package='rtabmap_util',
|
||||
plugin='rtabmap_util::PointCloudXYZRGB',
|
||||
name='point_cloud_xyzrgb',
|
||||
parameters=[{'decimation': 4, 'voxel_size': 0.05}],
|
||||
remappings=[('rgb/image', '/camera/color/image_raw'),
|
||||
('depth/image', '/camera/aligned_depth_to_color/image_raw'),
|
||||
('rgb/camera_info', '/camera/color/camera_info')])
|
||||
```
|
||||
|
||||
## Subscribed Topics
|
||||
|
||||
Four independent input sets; connect exactly one.
|
||||
|
||||
**RGB-D**
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `rgb/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | `mono8`, `mono16`, `bgr8`, `rgb8`, `bgra8`, `rgba8` or `bayer_grbg8`. |
|
||||
| `depth/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | `32FC1`, `16UC1` or `mono16`, **registered to the color camera**. |
|
||||
| `rgb/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | |
|
||||
|
||||
**Stereo**
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `left/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Rectified. |
|
||||
| `right/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Rectified. |
|
||||
| `left/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | |
|
||||
| `right/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Its `P(0,3)` carries the baseline. |
|
||||
|
||||
Dense matching is done on the fly with OpenCV's block matcher; see [Stereo matching](#stereo-matching).
|
||||
|
||||
**Disparity**
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `left/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Supplies the color. |
|
||||
| `disparity` | [`stereo_msgs/msg/DisparityImage`](https://docs.ros.org/en/jazzy/p/stereo_msgs/msg/DisparityImage.html) | `32FC1` or `16SC1`. |
|
||||
| `left/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | |
|
||||
|
||||
**Bundled**
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | A whole frame in one message, RGB-D or stereo. No synchronization needed, so this is the most reliable input. |
|
||||
|
||||
## Published Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | `XYZRGB`, or `XYZRGBNormal` when normals are enabled. |
|
||||
|
||||
Nothing is computed unless `cloud` has a subscriber.
|
||||
|
||||
## Parameters
|
||||
|
||||
Identical to [point_cloud_xyz](point_cloud_xyz.md#parameters), with [`image_transport`](https://docs.ros.org/en/jazzy/p/image_transport/) added and the `Stereo*` family below.
|
||||
|
||||
**Synchronization**
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `approx_sync` | `bool` | `true` | Match the inputs by nearest stamp. Set false when they share exact stamps. |
|
||||
| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. |
|
||||
| `topic_queue_size` | `int` | `1` | Queue depth of each input subscription. |
|
||||
| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. |
|
||||
| `qos` | `int` | `0` | Reliability of the image and disparity subscriptions. |
|
||||
| `qos_camera_info` | `int` | value of `qos` | Reliability of the camera info subscriptions. |
|
||||
| `image_transport` | `string` | `"raw"` | `image_transport` plugin for the color, left and right images. |
|
||||
| `depth_transport` | `string` | `"raw"` | `image_transport` plugin for `depth/image`. |
|
||||
|
||||
**Projection and filtering**, applied in this order
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `decimation` | `int` | `1` | Keep one pixel in `decimation`, in each direction. |
|
||||
| `roi_ratios` | `string` | `""` | Crop before projecting, `"left right top bottom"`. **Ignored for stereo input**, which warns if you set it. |
|
||||
| `min_depth` | `double` | `0.0` | Discard points nearer than this, in meters. `0` disables. |
|
||||
| `max_depth` | `double` | `0.0` | Discard points further than this, in meters. `0` disables. |
|
||||
| `voxel_size` | `double` | `0.0` | Downsample to one point per voxel, in meters. `0` disables. |
|
||||
| `noise_filter_radius` | `double` | `0.0` | Radius outlier removal, in meters. `0` disables. |
|
||||
| `noise_filter_min_neighbors` | `int` | `5` | Neighbors needed within `noise_filter_radius`. |
|
||||
| `normal_k` | `int` | `0` | Estimate normals from this many neighbors. `0` disables. |
|
||||
| `normal_radius` | `double` | `0.0` | Estimate normals within this radius. `0` disables. |
|
||||
| `filter_nans` | `bool` | `false` | Drop invalid points instead of leaving them NaN, giving an unorganized cloud. See [point_cloud_xyz](point_cloud_xyz.md#organized-output). |
|
||||
|
||||
## Stereo matching
|
||||
|
||||
The stereo and `rgbd_image`-with-stereo inputs run OpenCV's block matcher, configured through RTAB-Map's `StereoBM/*` parameters, which are exposed as ROS parameters of this node:
|
||||
|
||||
```bash
|
||||
-p StereoBM/NumDisparities:=64 -p StereoBM/BlockSize:=15
|
||||
```
|
||||
|
||||
The full list is in RTAB-Map's [parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html). The two that matter most are `StereoBM/NumDisparities` (must exceed the largest disparity you expect, and must not exceed the image width) and `StereoBM/BlockSize`.
|
||||
|
||||
If you already have a disparity image, feed the disparity input instead — it skips the matching entirely.
|
||||
|
||||
## Notes
|
||||
|
||||
For RGB-D input the depth **must be registered to the color camera**: the node pairs pixel `(u,v)` of the color image with pixel `(u,v)` of the depth image and uses one calibration for both. Unregistered depth gives a cloud whose colors are offset from its geometry. Most drivers offer an aligned depth stream for this reason.
|
||||
|
||||
An `rgbd_image` carrying only color and no depth is valid and yields an empty cloud rather than an error.
|
||||
@@ -0,0 +1,96 @@
|
||||
# pointcloud_to_depthimage
|
||||
|
||||
Projects a point cloud into a camera to make a depth image registered to it.
|
||||
|
||||
Given a cloud (from a 3D lidar or a ToF camera) and the `camera_info` of an RGB camera, this node projects the points into that camera and outputs the depth image it would have produced if it were an RGB-D sensor: same intrinsics, same size, pixel `(u,v)` of the depth image lining up with pixel `(u,v)` of the color image. The result plugs into anything that consumes depth images: RTAB-Map's RGB-D pipeline, [`depth_image_proc`](https://docs.ros.org/en/jazzy/p/depth_image_proc/), obstacle avoidance built for depth cameras.
|
||||
|
||||
Two typical setups:
|
||||
|
||||
* **Lidar + one or more RGB cameras.** The natural way to feed a lidar into an RGB-D SLAM setup: the lidar supplies the geometry, the cameras the appearance. Run one instance per camera, each subscribing to the same cloud but to that camera's `camera_info`; a 3D lidar usually covers all of them at once. The resulting RGB-D streams can then be combined with [rtabmap_sync](https://docs.ros.org/en/jazzy/p/rtabmap_sync/)'s `rgbd_sync`/`rgbdx_sync` and given to RTAB-Map through its `rgbd_cameras` parameter.
|
||||
* **ToF camera + RGB camera, not synchronized.** Two separate sensors, each with its own clock and its own pose, so their frames line up neither in time nor in space. Projecting the ToF cloud into the RGB camera registers the depth to the color image, and setting `fixed_frame_id` to a high-rate odometry frame — VIO, or an IMU-driven odometry running well above the camera rate — compensates the motion between the two stamps at the same time. See [Motion compensation](#motion-compensation).
|
||||
|
||||
## Usage
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_util pointcloud_to_depthimage --ros-args \
|
||||
-r cloud:=/velodyne_points \
|
||||
-r camera_info:=/camera/color/camera_info \
|
||||
-p fixed_frame_id:=odom -p decimation:=4 -p fill_holes_size:=2
|
||||
```
|
||||
|
||||
```python
|
||||
ComposableNode(
|
||||
package='rtabmap_util',
|
||||
plugin='rtabmap_util::PointCloudToDepthImage',
|
||||
name='pointcloud_to_depthimage',
|
||||
parameters=[{'fixed_frame_id': 'odom', 'decimation': 4, 'fill_holes_size': 2}],
|
||||
remappings=[('cloud', '/velodyne_points'),
|
||||
('camera_info', '/camera/color/camera_info')])
|
||||
```
|
||||
|
||||
## Subscribed Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The geometry to project. An empty cloud yields an all-zero image rather than nothing. |
|
||||
| `camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Defines the target camera: its intrinsics, its size, and through its `frame_id` its pose. Normally the RGB camera the depth image is being registered to. |
|
||||
|
||||
## Published Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) (`32FC1`) | Depth in **meters**, in the camera info's frame. |
|
||||
| `image_raw` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) (`16UC1`) | The same depth in **millimeters**. |
|
||||
| `image/camera_info`, `image_raw/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | The input calibration, rescaled if `decimation` is set. |
|
||||
| `cloud_transformed` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The input cloud in the camera frame. Debugging aid; only published when hole filling is on and something subscribes. |
|
||||
|
||||
Nothing is computed unless one of the two image topics has a subscriber.
|
||||
|
||||
## Required Transforms
|
||||
|
||||
| Transform | Description |
|
||||
|---|---|
|
||||
| cloud frame → camera frame | Where the cloud's sensor sits relative to the camera. |
|
||||
| `fixed_frame_id` → cloud frame, at both stamps | Only when `fixed_frame_id` is set, which is how motion between the two stamps is measured. See [Motion compensation](#motion-compensation). |
|
||||
|
||||
## Parameters
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `fixed_frame_id` | `string` | `""` | Frame the sensor's motion is measured against, usually `odom`. **Required when `approx` is true.** See [Motion compensation](#motion-compensation). |
|
||||
| `approx` | `bool` | `true` | Match cloud and camera info by nearest stamp. Set false when the two share exact stamps, in which case `fixed_frame_id` is unnecessary. |
|
||||
| `wait_for_transform` | `double` | `0.1` | Seconds to wait for a transform before dropping the frame. |
|
||||
| `decimation` | `int` | `1` | Render at 1/`decimation` of the camera info's resolution. The published camera info is scaled to match. Must divide both the width and the height exactly, otherwise it is ignored with an error and the image comes out full size. See [Hole filling](#hole-filling). |
|
||||
| `fill_holes_size` | `int` | `0` | Radius, in pixels, for filling gaps between projected points. `0` disables. See [Hole filling](#hole-filling). |
|
||||
| `fill_holes_error` | `double` | `0.1` | Largest depth difference, in meters, across which a hole may be filled. |
|
||||
| `fill_iterations` | `int` | `1` | How many times to repeat the filling pass. |
|
||||
| `upscale` | `bool` | `false` | Interpolate the depth image back to full resolution after rendering. Only has an effect when `decimation` is greater than 1, and only needed when the consumer requires full resolution. See [Hole filling](#hole-filling). |
|
||||
| `upscale_depth_error_ratio` | `double` | `0.02` | Relative depth difference tolerated across a block when upscaling. Above it the block is left empty rather than interpolated across an edge. |
|
||||
| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. |
|
||||
| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. |
|
||||
| `qos` | `int` | `0` | Reliability of the cloud subscription. |
|
||||
| `qos_camera_info` | `int` | value of `qos` | Reliability of the camera info subscription. |
|
||||
|
||||
## Motion compensation
|
||||
|
||||
The cloud's sensor and the camera almost never fire at the same instant, and on a moving robot that offset matters: projecting a cloud captured 40 ms earlier into the camera's current pose puts everything in the wrong place.
|
||||
|
||||
When `fixed_frame_id` is set, the node asks TF how the cloud's frame moved between the two stamps and folds that displacement into the projection, so the cloud is placed where the camera was **at its own stamp**. Driving forward at 1 m/s with a 40 ms offset moves everything 4 cm — enough to matter at close range.
|
||||
|
||||
The lookup is only as good as the frame it measures against: TF interpolates between the samples it has, so the source publishing `fixed_frame_id` should run well above the sensor rate. A VIO or wheel odometry at 100+ Hz gives a meaningful displacement over a 40 ms gap; a 1 Hz SLAM output does not.
|
||||
|
||||
Without `fixed_frame_id` the stamp difference is silently ignored, which is why the node logs a fatal error if `approx` is true and no fixed frame is given. If the transform cannot be found the frame is dropped rather than projected wrongly.
|
||||
|
||||
## Hole filling
|
||||
|
||||
A lidar cloud is far sparser than a camera image, so a direct projection is mostly gaps: individual pixels with depth, surrounded by zeros.
|
||||
|
||||
**Start with `decimation`.** Rendering at a coarser resolution puts more points in each pixel, so the wide gaps between lidar rings largely disappear instead of having to be filled in afterwards. `decimation: 4` is a reasonable starting point for a 3D lidar against a full-resolution camera. The published camera info is scaled to match, so consumers that read it keep working at the smaller size.
|
||||
|
||||
**Then close what is left with `fill_holes_size`.** It spreads each point over a small neighborhood, but only across depth differences smaller than `fill_holes_error`, so it fills a surface without bridging the gap between a foreground object and the wall behind it. Start at `2` and raise it only if the image is still speckled; too large and thin structures get fattened.
|
||||
|
||||
`upscale` is for the specific case where the consumer needs the depth image back at the camera's full resolution — pairing it pixel-for-pixel with the full-size color image, for instance. It interpolates each decimated block bilinearly from its corners, and only where all four have depth and agree to within `upscale_depth_error_ratio`, so it stops at depth discontinuities rather than stretching a foreground object onto the wall behind it. Leave it off otherwise: it restores resolution the lidar never measured, at full-resolution cost.
|
||||
|
||||
## Notes
|
||||
|
||||
The output is dense in *layout* but sparse in *content*: pixels with no return are zero, the ROS convention for no reading. Consumers that assume every pixel is valid will need to handle that.
|
||||
@@ -0,0 +1,75 @@
|
||||
# rgbd_relay
|
||||
|
||||
Republishes an [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html), optionally compressing or decompressing it on the way through.
|
||||
|
||||
An `RGBDImage` can carry its images raw or compressed. This node converts between the two so that the expensive form crosses the network only where it has to: compress before a wifi link, decompress on the other side.
|
||||
|
||||
With both `compress` and `uncompress` left false the message is forwarded untouched, which makes the node a plain relay — useful to give a topic a second name, or to bridge two incompatible QoS profiles with `qos_sub` and `qos_pub`. See [Bridging QoS profiles](#bridging-qos-profiles).
|
||||
|
||||
## Usage
|
||||
|
||||
Compress before sending over a slow link:
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_util rgbd_relay --ros-args \
|
||||
-r rgbd_image:=/camera/rgbd_image \
|
||||
-p compress:=true
|
||||
```
|
||||
|
||||
```python
|
||||
ComposableNode(
|
||||
package='rtabmap_util',
|
||||
plugin='rtabmap_util::RGBDRelay',
|
||||
name='rgbd_relay',
|
||||
parameters=[{'compress': True}],
|
||||
remappings=[('rgbd_image', '/camera/rgbd_image')])
|
||||
```
|
||||
|
||||
## Subscribed Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | Queue depth `queue_sub`, 5 by default. |
|
||||
|
||||
## Published Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `rgbd_image_relay` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | Queue depth `queue_pub`, 1 by default. Published only when someone is subscribed. |
|
||||
|
||||
## Parameters
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `compress` | `bool` | `false` | Fill the compressed fields of the output. Color becomes JPEG; depth becomes PNG, or JPEG when the message carries a stereo pair rather than depth. Fields already compressed on input are passed through as-is. |
|
||||
| `uncompress` | `bool` | `false` | Fill the raw fields of the output by decoding the compressed ones. Fields already raw on input are passed through as-is. |
|
||||
| `qos` | `int` | `0` | Reliability of both sides: `0` system default, `1` reliable, `2` best effort. |
|
||||
| `qos_sub` | `int` | value of `qos` | Reliability of the `rgbd_image` subscription alone. |
|
||||
| `qos_pub` | `int` | value of `qos` | Reliability of the `rgbd_image_relay` publisher alone. |
|
||||
| `queue_sub` | `int` | `5` | Queue depth of the `rgbd_image` subscription. Must be at least 1. |
|
||||
| `queue_pub` | `int` | `1` | Queue depth of the `rgbd_image_relay` publisher. Must be at least 1. |
|
||||
|
||||
## Bridging QoS profiles
|
||||
|
||||
A subscriber that asks for **reliable** will not connect to a publisher offering **best effort** — the request cannot be satisfied, so the two silently never match. A best-effort subscriber, on the other hand, connects to either.
|
||||
|
||||
That is a real problem when a camera driver publishes best effort and the consumer insists on reliable. Set the two sides of the relay separately and it forwards across the gap:
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_util rgbd_relay --ros-args \
|
||||
-r rgbd_image:=/camera/rgbd_image \
|
||||
-p qos_sub:=2 \
|
||||
-p qos_pub:=1
|
||||
```
|
||||
|
||||
Both parameters default to `qos`, so setting `qos` alone configures both sides at once.
|
||||
|
||||
Reliability is all that is bridged — durability is left at the default, so a transient-local publisher is not converted. The queue depths are separate too, through `queue_sub` and `queue_pub`.
|
||||
|
||||
## Notes
|
||||
|
||||
Setting neither `compress` nor `uncompress` forwards the message unchanged and skips all image handling — the cheapest path by a wide margin.
|
||||
|
||||
Setting both is allowed and produces a message carrying each image twice, raw and compressed. That is rarely what you want.
|
||||
|
||||
Depth is compressed as **PNG**, a stereo right image as **JPEG**.
|
||||
@@ -0,0 +1,76 @@
|
||||
# rgbd_split
|
||||
|
||||
Splits an [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) back into the standard ROS image topics.
|
||||
|
||||
`RGBDImage` bundles color, depth and both camera infos into one message so they arrive together, which is what RTAB-Map wants. Everything else in the ROS ecosystem — RViz, `image_view`, [`depth_image_proc`](https://docs.ros.org/en/jazzy/p/depth_image_proc/) — expects separate `Image` and `CameraInfo` topics. This node unpacks the bundle for them.
|
||||
|
||||
It is the inverse of [rtabmap_sync](https://docs.ros.org/en/jazzy/p/rtabmap_sync/)'s `rgbd_sync`, and of its `stereo_sync` when `stereo` is set — those two are what produce an `RGBDImage` in the first place.
|
||||
|
||||
## Usage
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_util rgbd_split --ros-args -r rgbd_image:=/camera/rgbd_image
|
||||
```
|
||||
|
||||
```python
|
||||
ComposableNode(
|
||||
package='rtabmap_util',
|
||||
plugin='rtabmap_util::RGBDSplit',
|
||||
name='rgbd_split',
|
||||
remappings=[('rgbd_image', '/camera/rgbd_image')])
|
||||
```
|
||||
|
||||
## Subscribed Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | Queue depth `queue_sub`, 5 by default. Raw or compressed images are both accepted. |
|
||||
|
||||
## Published Topics
|
||||
|
||||
The output topics are named after the **resolved** input topic, so remapping `rgbd_image` moves the outputs with it. With `rgbd_image` remapped to `/camera/rgbd_image` they are `/camera/rgbd_image/rgb/image` and so on. Setting `stereo: true` renames the two halves `left` and `right`, see [Stereo messages](#stereo-messages).
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `<rgbd_image>/rgb/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | The color image, decompressed if needed. |
|
||||
| `<rgbd_image>/rgb/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | |
|
||||
| `<rgbd_image>/depth/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | The depth image, or the right image of a stereo pair. |
|
||||
| `<rgbd_image>/depth/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | For a stereo pair this is the right camera, and its `P(0,3)` carries the baseline. |
|
||||
|
||||
Each half is only unpacked if something is subscribed to it, so subscribing to color alone does not pay for depth decompression.
|
||||
|
||||
## Parameters
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `qos` | `int` | `0` | Reliability of both sides: `0` system default, `1` reliable, `2` best effort. |
|
||||
| `qos_sub` | `int` | value of `qos` | Reliability of the `rgbd_image` subscription alone. |
|
||||
| `qos_pub` | `int` | value of `qos` | Reliability of the four output publishers alone. |
|
||||
| `queue_sub` | `int` | `5` | Queue depth of the `rgbd_image` subscription. Must be at least 1. |
|
||||
| `queue_pub` | `int` | `1` | Queue depth of every publisher. Must be at least 1. |
|
||||
| `stereo` | `bool` | `false` | Name the outputs `left`/`right` instead of `rgb`/`depth`. See [Stereo messages](#stereo-messages). |
|
||||
|
||||
## Stereo messages
|
||||
|
||||
The node handles **stereo** `RGBDImage` messages as well as RGB-D ones. In a stereo message the "depth" slot holds the right image, and the second camera info carries the baseline; the depth topics then carry the right camera, correctly typed as `mono8` or `bgr8` rather than mislabeled as depth.
|
||||
|
||||
That works, but the topic names lie. Set `stereo: true` and the outputs are named for what they hold:
|
||||
|
||||
| `stereo` | Output topics |
|
||||
|---|---|
|
||||
| `false` (default) | `<rgbd_image>/rgb/image`, `<rgbd_image>/rgb/camera_info`, `<rgbd_image>/depth/image`, `<rgbd_image>/depth/camera_info` |
|
||||
| `true` | `<rgbd_image>/left/image`, `<rgbd_image>/left/camera_info`, `<rgbd_image>/right/image`, `<rgbd_image>/right/camera_info` |
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_util rgbd_split --ros-args \
|
||||
-r rgbd_image:=/camera/rgbd_image \
|
||||
-p stereo:=true
|
||||
```
|
||||
|
||||
Only the names change — the message contents and the order of the two halves are the same either way, so the `rgb` slot always becomes the left image. The two namings are exclusive: with `stereo: true` nothing is published on `rgb`/`depth`.
|
||||
|
||||
The node checks the setting against what actually arrives, going by the encoding of the second half: `16UC1`, `32FC1` and `mono16` are depth, anything else is an image. If the two disagree it logs a warning **once** and keeps forwarding — a mismatch makes the topic name misleading, not the data wrong, so it is never worth dropping a frame over.
|
||||
|
||||
## Notes
|
||||
|
||||
If a message has no `frame_id` on one of its sub-messages, the node fills it in from the other one so the output is always usable by TF.
|
||||
Reference in New Issue
Block a user