mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 16:57:46 +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
@@ -300,4 +300,41 @@ install(DIRECTORY include/
|
||||
FILES_MATCHING PATTERN "*.h"
|
||||
)
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
|
||||
# Each node gets its own test binary: a crash or a stuck executor in one node cannot
|
||||
# take the others down, and every binary starts with a clean DDS graph.
|
||||
macro(rtabmap_util_add_node_test test_name)
|
||||
ament_add_gtest(${test_name} test/${test_name}.cpp)
|
||||
if(TARGET ${test_name})
|
||||
target_include_directories(${test_name} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test)
|
||||
target_link_libraries(${test_name} rtabmap_util_plugins rtabmap_util)
|
||||
if("$ENV{ROS_DISTRO}" STRLESS "lyrical")
|
||||
ament_target_dependencies(${test_name} ${AmentLibraries})
|
||||
else()
|
||||
target_link_libraries(${test_name} ${Libraries} ${PublicLibraries})
|
||||
endif()
|
||||
endif()
|
||||
endmacro()
|
||||
|
||||
rtabmap_util_add_node_test(test_imu_to_tf)
|
||||
rtabmap_util_add_node_test(test_disparity_to_depth)
|
||||
rtabmap_util_add_node_test(test_rgbd_relay)
|
||||
rtabmap_util_add_node_test(test_rgbd_split)
|
||||
rtabmap_util_add_node_test(test_lidar_deskewing)
|
||||
rtabmap_util_add_node_test(test_obstacles_detection)
|
||||
rtabmap_util_add_node_test(test_point_cloud_aggregator)
|
||||
rtabmap_util_add_node_test(test_point_cloud_assembler)
|
||||
rtabmap_util_add_node_test(test_point_cloud_xyz)
|
||||
rtabmap_util_add_node_test(test_pointcloud_to_depthimage)
|
||||
rtabmap_util_add_node_test(test_point_cloud_xyzrgb)
|
||||
rtabmap_util_add_node_test(test_db_player)
|
||||
rtabmap_util_add_node_test(test_maps_manager)
|
||||
rtabmap_util_add_node_test(test_map_assembler)
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
|
||||
@@ -0,0 +1,66 @@
|
||||
# rtabmap_util
|
||||
|
||||
Standalone utility nodes for [RTAB-Map](https://github.com/introlab/rtabmap) pipelines: converting between sensor representations, cleaning up point clouds, assembling maps and replaying recorded sessions.
|
||||
|
||||
Every node is a [composable node](https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Composition.html) as well as a standalone executable. Composing them into one process with their producer avoids copying images and clouds between processes, which is worth doing for anything on the sensor path.
|
||||
|
||||
## Nodes
|
||||
|
||||
One page per node.
|
||||
|
||||
**Sensor conversion**
|
||||
|
||||
| Node | Description |
|
||||
|---|---|
|
||||
| [disparity_to_depth](doc/disparity_to_depth.md) | Disparity image → depth image, in meters and in millimeters. |
|
||||
| [pointcloud_to_depthimage](doc/pointcloud_to_depthimage.md) | Point cloud → depth image registered to an RGB camera. Lets a lidar feed an RGB-D pipeline. |
|
||||
| [point_cloud_xyz](doc/point_cloud_xyz.md) | Depth or disparity image → point cloud, with filtering. |
|
||||
| [point_cloud_xyzrgb](doc/point_cloud_xyzrgb.md) | RGB-D, stereo or disparity → colored point cloud. |
|
||||
| [imu_to_tf](doc/imu_to_tf.md) | IMU orientation → TF. |
|
||||
|
||||
**RGBDImage plumbing**
|
||||
|
||||
| Node | Description |
|
||||
|---|---|
|
||||
| [rgbd_relay](doc/rgbd_relay.md) | Republishes an `RGBDImage`, compressing or decompressing on the way. |
|
||||
| [rgbd_split](doc/rgbd_split.md) | Splits an `RGBDImage` back into standard `Image` and `CameraInfo` topics. |
|
||||
|
||||
**Point cloud processing**
|
||||
|
||||
| Node | Description |
|
||||
|---|---|
|
||||
| [lidar_deskewing](doc/lidar_deskewing.md) | Removes motion distortion from a lidar sweep. |
|
||||
| [point_cloud_aggregator](doc/point_cloud_aggregator.md) | Merges one cloud from each of several sensors into one. |
|
||||
| [point_cloud_assembler](doc/point_cloud_assembler.md) | Accumulates one sensor over time into a denser cloud. |
|
||||
| [obstacles_detection](doc/obstacles_detection.md) | Segments a cloud into ground and obstacles. |
|
||||
|
||||
**Maps and replay**
|
||||
|
||||
| Node | Description |
|
||||
|---|---|
|
||||
| [map_assembler](doc/map_assembler.md) | Rebuilds the global maps from RTAB-Map's graph, off the SLAM node's critical path. |
|
||||
| [db_player](doc/db_player.md) | Replays a recorded RTAB-Map database as live sensor topics. |
|
||||
|
||||
## Library
|
||||
|
||||
The package also installs a small C++ library, whose API is documented in the [C++ API reference](https://docs.ros.org/en/jazzy/p/rtabmap_util/generated/index.html) generated from the headers.
|
||||
|
||||
`MapsManager` is the piece worth knowing about: it turns a pose graph plus per-node occupancy grids into the assembled clouds, occupancy grid, octomap and elevation map, and publishes them. Both [map_assembler](doc/map_assembler.md) and `rtabmap_slam`'s `rtabmap` node use it, which is why their map outputs and `Grid/*` parameters behave identically.
|
||||
|
||||
## Conventions
|
||||
|
||||
A few things recur across these nodes.
|
||||
|
||||
**`qos` parameters.** Most nodes expose a `qos` integer selecting the reliability of their subscriptions: `0` system default, `1` reliable, `2` best effort. It has to be compatible with the publisher or **no messages arrive at all** and nothing says why. Sensor drivers commonly publish best effort.
|
||||
|
||||
**`approx_sync`.** Nodes taking several inputs match them by nearest stamp by default. Set it to `false` when the inputs are hardware-synchronized and carry identical stamps: the exact policy is cheaper and cannot mismatch. With approximate sync, `approx_sync_max_interval` is worth setting as a guard against silently pairing stale data.
|
||||
|
||||
**`fixed_frame_id`.** Where a node has to account for the robot moving between two stamps, it does so by asking TF how a frame moved relative to a fixed one — usually `odom`. Leaving it empty disables the compensation rather than erroring, so a moving robot then gets subtly misplaced data.
|
||||
|
||||
**`Grid/*` parameters.** Nodes that segment or assemble maps use RTAB-Map's own [`LocalGridMaker`](https://introlab.github.io/rtabmap/api/latest/classrtabmap_1_1LocalGridMaker.html), and expose its parameters directly under their RTAB-Map names. Their meanings and defaults are in RTAB-Map's [parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html), which is the source of truth for them. One to know about: `Grid/RangeMax` is not unlimited by default, so distant points are dropped before anything else happens.
|
||||
|
||||
## Building the documentation
|
||||
|
||||
```bash
|
||||
rosdoc2 build --package-path rtabmap_util --output-directory doc_output
|
||||
```
|
||||
@@ -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.
|
||||
@@ -57,22 +57,131 @@ class GridMap;
|
||||
|
||||
namespace rtabmap_util {
|
||||
|
||||
/**
|
||||
* @brief Turns a pose graph into the map topics, and publishes them.
|
||||
*
|
||||
* Given a set of node poses and the sensor data behind them, MapsManager assembles the
|
||||
* ground and obstacle point clouds, the 2D occupancy grid, the octomap and the elevation
|
||||
* map, and publishes whichever of them somebody is subscribed to.
|
||||
*
|
||||
* It is shared by rtabmap_slam's `rtabmap` node and rtabmap_util's `map_assembler`, which
|
||||
* is why those two produce identical maps from identical parameters.
|
||||
*
|
||||
* @par Lifecycle
|
||||
* Callers follow a fixed order:
|
||||
* 1. init() to declare the ROS parameters and advertise the topics,
|
||||
* 2. backwardCompatibilityParameters() to pick up parameters that have since moved into
|
||||
* the RTAB-Map library, then setParameters() to apply the whole set,
|
||||
* 3. updateMapCaches() whenever the graph changes, then publishMaps().
|
||||
*
|
||||
* @par Laziness
|
||||
* Nothing is assembled or published without a subscriber, and with `map_cleanup` set the
|
||||
* caches are released once the last one goes away. A node can therefore call
|
||||
* updateMapCaches() and publishMaps() unconditionally on every graph update and pay
|
||||
* nothing while nobody is listening.
|
||||
*
|
||||
*/
|
||||
class MapsManager {
|
||||
public:
|
||||
MapsManager();
|
||||
virtual ~MapsManager();
|
||||
|
||||
/**
|
||||
* @brief Declares the ROS parameters and advertises the map topics on @p node.
|
||||
*
|
||||
* Must be called before anything else, and exactly once per node: the parameters are
|
||||
* declared here, and declaring them twice throws.
|
||||
*
|
||||
* @param node node to advertise on and read parameters from
|
||||
* @param name prefix used in the log lines, normally the node's name
|
||||
* @param usePublicNamespace unused, kept for source compatibility
|
||||
*/
|
||||
void init(rclcpp::Node & node, const std::string & name, bool usePublicNamespace);
|
||||
|
||||
/// Drops every cached local grid, assembled cloud and global map.
|
||||
void clear();
|
||||
|
||||
/// @return True if any map topic has at least one subscriber.
|
||||
bool hasSubscribers() const;
|
||||
|
||||
/// @return True if the map topics are latched, i.e. delivered to late subscribers.
|
||||
bool isLatching() const {return latching_;}
|
||||
|
||||
/**
|
||||
* @brief Whether the map changed on the last updateMapCaches().
|
||||
*
|
||||
* @note Reports true when nothing is subscribed to the grid topics. The answer comes
|
||||
* from OccupancyGrid::update(), which only runs when a grid is wanted, so with
|
||||
* nobody listening the safe assumption is that the graph moved.
|
||||
*/
|
||||
bool isMapUpdated() const;
|
||||
|
||||
/**
|
||||
* @brief Copies parameters that moved from rtabmap_ros into the RTAB-Map library.
|
||||
*
|
||||
* Reads the old ROS parameter names off @p node and, for each one that is set, writes
|
||||
* its value into @p parameters under the RTAB-Map name that replaced it, with a
|
||||
* warning. Call it before setParameters().
|
||||
*
|
||||
* @param[in] node node to read the legacy parameters from
|
||||
* @param[in,out] parameters parameter set to fill in
|
||||
*/
|
||||
void backwardCompatibilityParameters(rclcpp::Node & node, rtabmap::ParametersMap & parameters) const;
|
||||
|
||||
/**
|
||||
* @brief Applies the RTAB-Map parameters, rebuilding the map objects.
|
||||
*
|
||||
* The occupancy grid, octomap and elevation map are recreated, so anything already
|
||||
* assembled is lost; the cached local grids are kept.
|
||||
*/
|
||||
void setParameters(const rtabmap::ParametersMap & parameters);
|
||||
|
||||
/**
|
||||
* @brief Installs an already assembled 2D map, e.g. one loaded from a database.
|
||||
*
|
||||
* @param map the grid, `CV_8SC1` with -1 unknown, 0 free, 100 occupied
|
||||
* @param xMin world x of the map's origin, in meters
|
||||
* @param yMin world y of the map's origin, in meters
|
||||
* @param cellSize resolution, in meters
|
||||
* @param poses poses of the nodes @p map was assembled from
|
||||
* @param memory optional memory to load the missing local grids from, so the map can
|
||||
* keep growing from where it left off
|
||||
*
|
||||
* @warning @p poses must not be empty. The grid is kept only together with the nodes
|
||||
* it came from, so that the manager knows which are already in it; a call
|
||||
* with no poses is ignored with a warning.
|
||||
*/
|
||||
void set2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, rtabmap::Transform> & poses, const rtabmap::Memory * memory = 0);
|
||||
|
||||
/**
|
||||
* @brief Applies the `map_filter_radius`/`map_filter_angle` thinning to @p poses.
|
||||
* @return The poses that survive, or all of them when filtering is disabled.
|
||||
*/
|
||||
std::map<int, rtabmap::Transform> getFilteredPoses(
|
||||
const std::map<int, rtabmap::Transform> & poses);
|
||||
|
||||
/**
|
||||
* @brief Brings the local grid cache and the global maps up to date with the graph.
|
||||
*
|
||||
* For every pose not already mapped, the local occupancy grid is taken from the
|
||||
* signature or the memory, or regenerated from the sensor data when the node carries
|
||||
* none, and added to the cache. The global maps are then reassembled.
|
||||
*
|
||||
* @param poses node poses; landmarks (negative ids) are ignored, and id 0 is
|
||||
* the not-yet-committed node, kept only if `map_always_update`
|
||||
* @param memory memory to load node data from, may be null if @p signatures
|
||||
* carries everything
|
||||
* @param updateGrid force the occupancy grid to be updated
|
||||
* @param updateOctomap force the octomap to be updated
|
||||
* @param signatures node data, keyed by id, for nodes not in @p memory
|
||||
* @return The poses actually mapped, after filtering.
|
||||
*
|
||||
* @note With @p updateGrid and @p updateOctomap both false, what gets updated is
|
||||
* decided by which topics have subscribers. That is also the only way the
|
||||
* elevation map is ever built, as it has no flag of its own.
|
||||
* @note At least one of @p memory and @p signatures must be non-empty, and @p poses
|
||||
* must not be empty; otherwise an error is logged and nothing is returned.
|
||||
*/
|
||||
std::map<int, rtabmap::Transform> updateMapCaches(
|
||||
const std::map<int, rtabmap::Transform> & poses,
|
||||
const rtabmap::Memory * memory,
|
||||
@@ -80,25 +189,49 @@ public:
|
||||
bool updateOctomap,
|
||||
const std::map<int, rtabmap::Signature> & signatures = std::map<int, rtabmap::Signature>());
|
||||
|
||||
/**
|
||||
* @brief Publishes every map topic that has a subscriber.
|
||||
*
|
||||
* @param poses the same poses updateMapCaches() returned
|
||||
* @param stamp stamp for all published messages
|
||||
* @param mapFrameId frame id for all published messages
|
||||
*/
|
||||
void publishMaps(
|
||||
const std::map<int, rtabmap::Transform> & poses,
|
||||
const rclcpp::Time & stamp,
|
||||
const std::string & mapFrameId);
|
||||
|
||||
/**
|
||||
* @brief The 2D occupancy grid as a ternary map.
|
||||
* @param[out] xMin world x of the map's origin, in meters
|
||||
* @param[out] yMin world y of the map's origin, in meters
|
||||
* @param[out] gridCellSize resolution, in meters
|
||||
* @return `CV_8SC1`, -1 unknown, 0 free, 100 occupied. Empty if nothing is assembled.
|
||||
*/
|
||||
cv::Mat getGridMap(
|
||||
float & xMin,
|
||||
float & yMin,
|
||||
float & gridCellSize);
|
||||
|
||||
/**
|
||||
* @brief The 2D occupancy grid as probabilities.
|
||||
* @param[out] xMin world x of the map's origin, in meters
|
||||
* @param[out] yMin world y of the map's origin, in meters
|
||||
* @param[out] gridCellSize resolution, in meters
|
||||
* @return `CV_8SC1`, -1 unknown, otherwise 0-100. Empty if nothing is assembled.
|
||||
*/
|
||||
cv::Mat getGridProbMap(
|
||||
float & xMin,
|
||||
float & yMin,
|
||||
float & gridCellSize);
|
||||
|
||||
#ifdef RTABMAP_OCTOMAP
|
||||
/// @return The octomap, owned by this object. Never null.
|
||||
const rtabmap::OctoMap * getOctomap() const {return octomap_;}
|
||||
#endif
|
||||
/// @return The global occupancy grid, owned by this object. Never null.
|
||||
const rtabmap::OccupancyGrid * getOccupancyGrid() const {return occupancyGrid_;}
|
||||
/// @return The local grid segmenter, owned by this object. Never null.
|
||||
const rtabmap::LocalGridMaker * getLocalMapMaker() const {return localMapMaker_;}
|
||||
|
||||
private:
|
||||
|
||||
@@ -62,6 +62,9 @@ private:
|
||||
|
||||
void timerCallback();
|
||||
|
||||
/// Subscribes to "mapData"; the node is live from here on.
|
||||
void subscribeToMapData();
|
||||
|
||||
#ifdef WITH_OCTOMAP_MSGS
|
||||
#ifdef RTABMAP_OCTOMAP
|
||||
void octomapBinaryCallback(
|
||||
@@ -100,6 +103,7 @@ private:
|
||||
#endif
|
||||
#endif
|
||||
bool localGridsRegenerated_;
|
||||
double initializeFromRtabmapTimeout_;
|
||||
};
|
||||
|
||||
}
|
||||
@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <message_filters/sync_policies/approximate_time.hpp>
|
||||
#include <message_filters/subscriber.hpp>
|
||||
#include <message_filters/sync_policies/exact_time.hpp>
|
||||
#include <rtabmap_sync/SyncDiagnostic.h>
|
||||
|
||||
namespace rtabmap_util
|
||||
{
|
||||
@@ -66,8 +67,7 @@ private:
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_2);
|
||||
void combineClouds(const std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> & cloudMsgs);
|
||||
|
||||
std::thread * warningThread_;
|
||||
bool callbackCalled_;
|
||||
std::unique_ptr<rtabmap_sync::SyncDiagnostic> syncDiagnostic_;
|
||||
|
||||
typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> ExactSync4Policy;
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> ApproxSync4Policy;
|
||||
|
||||
@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
#include <rtabmap/core/util3d.h>
|
||||
#include <rtabmap/core/util3d_filtering.h>
|
||||
#include <rtabmap_sync/SyncDiagnostic.h>
|
||||
|
||||
namespace rtabmap_util
|
||||
{
|
||||
@@ -73,8 +74,7 @@ private:
|
||||
void callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg);
|
||||
|
||||
private:
|
||||
std::thread * warningThread_;
|
||||
bool callbackCalled_;
|
||||
std::unique_ptr<rtabmap_sync::SyncDiagnostic> syncDiagnostic_;
|
||||
|
||||
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloudSub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub_;
|
||||
|
||||
@@ -48,6 +48,9 @@ public:
|
||||
void callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) const;
|
||||
|
||||
private:
|
||||
/// True when the outputs are named left/right rather than rgb/depth.
|
||||
bool stereo_;
|
||||
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageSub_;
|
||||
|
||||
image_transport::Publisher rgbPub_;
|
||||
|
||||
@@ -36,7 +36,10 @@
|
||||
<depend>rtabmap_sync</depend>
|
||||
<depend>grid_map_ros</depend>
|
||||
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
<rosdoc2>rosdoc2.yaml</rosdoc2>
|
||||
</export>
|
||||
</package>
|
||||
|
||||
@@ -0,0 +1,35 @@
|
||||
## Configuration for rosdoc2, the documentation generator used by docs.ros.org.
|
||||
## Regenerate the annotated default with:
|
||||
## rosdoc2 default_config --package-path rtabmap_util
|
||||
## Build the docs locally with:
|
||||
## rosdoc2 build --package-path rtabmap_util --output-directory doc_output
|
||||
|
||||
## This 'attic section' self-documents this file's type and version.
|
||||
type: 'rosdoc2 config'
|
||||
version: 1
|
||||
|
||||
---
|
||||
|
||||
settings:
|
||||
## Generate the standard index page from package.xml (description, maintainer,
|
||||
## license, links) and a table of contents for the builders below.
|
||||
generate_package_index: true
|
||||
|
||||
## This is an ament_cmake package, so doxygen runs on the public headers by
|
||||
## default and there are no Python modules to document.
|
||||
always_run_doxygen: false
|
||||
always_run_sphinx_apidoc: false
|
||||
|
||||
builders:
|
||||
## Doxygen parses the public C++ API out of include/.
|
||||
- doxygen: {
|
||||
name: 'rtabmap_util Public C/C++ API',
|
||||
output_dir: 'generated/doxygen'
|
||||
}
|
||||
## Sphinx renders the landing page and pulls the Doxygen XML in through
|
||||
## breathe/exhale so the API is browsable alongside the narrative docs.
|
||||
- sphinx: {
|
||||
name: 'rtabmap_util',
|
||||
doxygen_xml_directory: 'generated/doxygen/xml',
|
||||
output_dir: ''
|
||||
}
|
||||
@@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include "rtabmap/utilite/ULogger.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include <stdexcept>
|
||||
|
||||
#ifndef _WIN32
|
||||
#include <sys/ioctl.h>
|
||||
#include <termios.h>
|
||||
@@ -83,7 +85,19 @@ int main(int argc, char **argv)
|
||||
rclcpp::NodeOptions options;
|
||||
options.arguments(arguments);
|
||||
|
||||
auto node = std::make_shared<rtabmap_util::DbPlayer>(options);
|
||||
std::shared_ptr<rtabmap_util::DbPlayer> node;
|
||||
try
|
||||
{
|
||||
node = std::make_shared<rtabmap_util::DbPlayer>(options);
|
||||
}
|
||||
catch(const std::exception & e)
|
||||
{
|
||||
// The node reports what went wrong before throwing; keep the process exit clean
|
||||
// rather than letting an uncaught exception abort.
|
||||
UERROR("%s", e.what());
|
||||
rclcpp::shutdown();
|
||||
return -1;
|
||||
}
|
||||
|
||||
rclcpp::Rate pauseRate(10);
|
||||
|
||||
|
||||
@@ -273,6 +273,12 @@ void MapsManager::set2DMap(
|
||||
const std::map<int, rtabmap::Transform> & poses,
|
||||
const rtabmap::Memory * memory)
|
||||
{
|
||||
if(!map.empty() && poses.empty())
|
||||
{
|
||||
UWARN("Ignoring the 2D map (%dx%d): no poses were given. Pass the poses of the "
|
||||
"nodes the map was assembled from.", map.cols, map.rows);
|
||||
return;
|
||||
}
|
||||
occupancyGrid_->setMap(map, xMin, yMin, cellSize, poses);
|
||||
//update cache in case the map should be updated
|
||||
if(memory &&
|
||||
@@ -1420,6 +1426,7 @@ void MapsManager::publishMaps(
|
||||
msg->header.frame_id = mapFrameId;
|
||||
msg->header.stamp = stamp;
|
||||
elevationMapPub_->publish(std::move(msg));
|
||||
latched_.at(&elevationMapPub_) = true;
|
||||
}
|
||||
if(elevationMapPub_->get_subscription_count() == 0)
|
||||
{
|
||||
|
||||
@@ -28,6 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <rtabmap_util/db_player.hpp>
|
||||
#include <sensor_msgs/image_encodings.hpp>
|
||||
|
||||
#include <stdexcept>
|
||||
|
||||
#include <image_transport/image_transport.hpp>
|
||||
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
@@ -106,13 +108,14 @@ DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) :
|
||||
qosGlobalPose_ = this->declare_parameter("qos_global_pose", qos_);
|
||||
qosGps_ = this->declare_parameter("qos_gps", qos_);
|
||||
qosImu_ = this->declare_parameter("qos_imu", qos_);
|
||||
qosEnvSensor_ = this->declare_parameter("qos_env_sensor", qos_);
|
||||
|
||||
// A general 360 lidar with 0.5 deg increment
|
||||
scanAngleMin_ = this->declare_parameter("scan_angle_min", -M_PI);
|
||||
scanAngleMax_ = this->declare_parameter("scan_angle_max", M_PI);
|
||||
scanAngleIncrement_ = this->declare_parameter("scan_angle_increment", M_PI / 720.0);
|
||||
scanRangeMin_ = this->declare_parameter("scan_range_min", 0.0);
|
||||
scanRangeMax_ = this->declare_parameter("scan_range_max", 60);
|
||||
scanRangeMax_ = this->declare_parameter("scan_range_max", 60.0);
|
||||
|
||||
RCLCPP_INFO(get_logger(), "frame_id = %s", frameId_.c_str());
|
||||
RCLCPP_INFO(get_logger(), "odom_frame_id = %s", odomFrameId_.c_str());
|
||||
@@ -136,8 +139,11 @@ DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) :
|
||||
|
||||
if(databasePath.empty())
|
||||
{
|
||||
// Throwing rather than exiting: this node can be loaded in a component container
|
||||
// next to others, and taking the whole process down with it would be rude.
|
||||
RCLCPP_ERROR(get_logger(), "Parameter \"database\" must be set (path to a RTAB-Map database).");
|
||||
exit(-1);
|
||||
throw std::invalid_argument(
|
||||
"db_player: parameter \"database\" must be set (path to a RTAB-Map database).");
|
||||
}
|
||||
|
||||
databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir());
|
||||
@@ -151,7 +157,8 @@ DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) :
|
||||
if(!reader_->init())
|
||||
{
|
||||
RCLCPP_ERROR(get_logger(), "Cannot open database \"%s\".", databasePath.c_str());
|
||||
exit(-1);
|
||||
throw std::runtime_error(
|
||||
uFormat("db_player: cannot open database \"%s\".", databasePath.c_str()));
|
||||
}
|
||||
|
||||
const std::string servicePrefix = get_name() + std::string("/");
|
||||
@@ -300,8 +307,12 @@ void DbPlayer::initializePublishers(const rtabmap::OdometryEvent & odom)
|
||||
|
||||
if(!odom.data().laserScanRaw().isEmpty())
|
||||
{
|
||||
if(!scanPub_.get() && odom.data().laserScanRaw().is2d())
|
||||
// The publisher has to match the scan being replayed, not just whichever one has
|
||||
// not been created yet: a 2D database must never advertise "scan_cloud".
|
||||
if(odom.data().laserScanRaw().is2d())
|
||||
{
|
||||
if(!scanPub_.get())
|
||||
{
|
||||
scanPub_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosScan_));
|
||||
if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
|
||||
{
|
||||
@@ -316,6 +327,7 @@ void DbPlayer::initializePublishers(const rtabmap::OdometryEvent & odom)
|
||||
RCLCPP_INFO(get_logger(), " scan_range_min=%f", scanRangeMin_);
|
||||
RCLCPP_INFO(get_logger(), " scan_range_max=%f", scanRangeMax_);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(!scanCloudPub_.get())
|
||||
{
|
||||
@@ -569,7 +581,6 @@ bool DbPlayer::publishNextFrame()
|
||||
envSensorPub_->get_subscription_count() > 0 &&
|
||||
!odom.data().envSensors().empty())
|
||||
{
|
||||
rtabmap_msgs::msg::EnvSensor msg;
|
||||
for(rtabmap::EnvSensors::const_iterator iter=odom.data().envSensors().begin(); iter!=odom.data().envSensors().end(); ++iter)
|
||||
{
|
||||
rtabmap_msgs::msg::EnvSensor msg;
|
||||
|
||||
@@ -28,6 +28,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <rtabmap_util/disparity_to_depth.hpp>
|
||||
#include <sensor_msgs/image_encodings.hpp>
|
||||
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
|
||||
#include <image_transport/image_transport.hpp>
|
||||
|
||||
#ifdef PRE_ROS_IRON
|
||||
@@ -44,16 +47,27 @@ DisparityToDepth::DisparityToDepth(const rclcpp::NodeOptions & options) :
|
||||
{
|
||||
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
|
||||
qos = this->declare_parameter("qos", qos);
|
||||
// Each side can be set independently so the node can bridge a producer and a
|
||||
// consumer that don't agree on reliability. Both default to qos.
|
||||
int qosSub = this->declare_parameter("qos_sub", qos);
|
||||
int qosPub = this->declare_parameter("qos_pub", qos);
|
||||
int queueSub = this->declare_parameter("queue_sub", 1);
|
||||
int queuePub = this->declare_parameter("queue_pub", 1);
|
||||
|
||||
UASSERT_MSG(queueSub >= 1 && queuePub >= 1,
|
||||
uFormat("queue_sub (%d) and queue_pub (%d) must be at least 1", queueSub, queuePub).c_str());
|
||||
|
||||
const rclcpp::QoS pubQos = rclcpp::QoS(queuePub).reliability((rmw_qos_reliability_policy_t)qosPub);
|
||||
|
||||
#ifdef PRE_ROS_LYRICAL
|
||||
pub32f_ = image_transport::create_publisher(this, "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
pub16u_ = image_transport::create_publisher(this, "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
pub32f_ = image_transport::create_publisher(this, "depth", pubQos.get_rmw_qos_profile());
|
||||
pub16u_ = image_transport::create_publisher(this, "depth_raw", pubQos.get_rmw_qos_profile());
|
||||
#else
|
||||
pub32f_ = image_transport::create_publisher(*this, "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
pub16u_ = image_transport::create_publisher(*this, "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
pub32f_ = image_transport::create_publisher(*this, "depth", pubQos);
|
||||
pub16u_ = image_transport::create_publisher(*this, "depth_raw", pubQos);
|
||||
#endif
|
||||
|
||||
sub_ = create_subscription<stereo_msgs::msg::DisparityImage>("disparity", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&DisparityToDepth::callback, this, std::placeholders::_1));
|
||||
sub_ = create_subscription<stereo_msgs::msg::DisparityImage>("disparity", rclcpp::QoS(queueSub).reliability((rmw_qos_reliability_policy_t)qosSub), std::bind(&DisparityToDepth::callback, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
DisparityToDepth::~DisparityToDepth(){}
|
||||
|
||||
@@ -61,7 +61,7 @@ void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstShared
|
||||
msg->header.frame_id,
|
||||
fixedFrameId_,
|
||||
msg->header.stamp,
|
||||
rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec) + rclcpp::Duration::from_seconds(msg->ranges.size()*msg->time_increment),
|
||||
rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec) + rclcpp::Duration::from_seconds((msg->ranges.empty()?0:msg->ranges.size()-1)*msg->time_increment),
|
||||
*tfBuffer_,
|
||||
waitForTransformDuration_);
|
||||
if(tmpT.isNull())
|
||||
|
||||
@@ -54,12 +54,18 @@ MapAssembler::MapAssembler(const rclcpp::NodeOptions & options) :
|
||||
Node("map_assembler", options),
|
||||
lastNodeAdded_(-1),
|
||||
rtabmapNodeName_("rtabmap"),
|
||||
localGridsRegenerated_(false)
|
||||
localGridsRegenerated_(false),
|
||||
initializeFromRtabmapTimeout_(5.0)
|
||||
{
|
||||
std::string configPath;
|
||||
configPath = this->declare_parameter("config_path", configPath);
|
||||
localGridsRegenerated_ = this->declare_parameter("regenerate_local_grids", localGridsRegenerated_);
|
||||
rtabmapNodeName_ = this->declare_parameter("rtabmap", rtabmapNodeName_);
|
||||
// Seconds to wait for rtabmap's get_map_data service on start-up, which is how
|
||||
// map_assembler catches up on a map that already exists. Set it to 0 to skip the call
|
||||
// entirely: the subscription to "mapData" is then created right away instead of after
|
||||
// the wait, which is what you want when map_assembler starts before rtabmap.
|
||||
initializeFromRtabmapTimeout_ = this->declare_parameter("initialize_from_rtabmap_timeout", initializeFromRtabmapTimeout_);
|
||||
|
||||
//parameters
|
||||
rtabmap::ParametersMap parameters;
|
||||
@@ -175,6 +181,7 @@ MapAssembler::MapAssembler(const rclcpp::NodeOptions & options) :
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "%s: regenerate_local_grids = %s", this->get_name(), localGridsRegenerated_?"true":"false");
|
||||
RCLCPP_INFO(this->get_logger(), "%s: initialize_from_rtabmap_timeout = %fs (0=don't ask rtabmap for the map)", this->get_name(), initializeFromRtabmapTimeout_);
|
||||
mapsManager_.init(*this, this->get_name(), true);
|
||||
mapsManager_.backwardCompatibilityParameters(*this, parameters);
|
||||
mapsManager_.setParameters(parameters);
|
||||
@@ -189,13 +196,29 @@ MapAssembler::MapAssembler(const rclcpp::NodeOptions & options) :
|
||||
#endif
|
||||
#endif
|
||||
|
||||
std::string getMapSrv = rtabmapNodeName_+"/get_map_data";
|
||||
if(initializeFromRtabmapTimeout_ > 0.0)
|
||||
{
|
||||
std::string getMapSrv = rtabmapNodeName_+"/get_map_data";
|
||||
|
||||
// We cannot call the service and wait in the constructor, lets call it later and subscribe afterwards
|
||||
serviceCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
|
||||
timerCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
|
||||
client_ = this->create_client<rtabmap_msgs::srv::GetMap>(getMapSrv, rclcpp::ServicesQoS(), serviceCbGroup_); // Put it in a different group than the timer
|
||||
timer_ = this->create_wall_timer(1s, std::bind(&MapAssembler::timerCallback, this), timerCbGroup_);
|
||||
// We cannot call the service and wait in the constructor, lets call it later and subscribe afterwards
|
||||
serviceCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
|
||||
timerCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
|
||||
client_ = this->create_client<rtabmap_msgs::srv::GetMap>(getMapSrv, rclcpp::ServicesQoS(), serviceCbGroup_); // Put it in a different group than the timer
|
||||
timer_ = this->create_wall_timer(1s, std::bind(&MapAssembler::timerCallback, this), timerCbGroup_);
|
||||
}
|
||||
else
|
||||
{
|
||||
subscribeToMapData();
|
||||
}
|
||||
}
|
||||
|
||||
void MapAssembler::subscribeToMapData()
|
||||
{
|
||||
rclcpp::SubscriptionOptions options;
|
||||
// Null unless we came through the timer, in which case the node's default group is used.
|
||||
options.callback_group = timerCbGroup_;
|
||||
mapDataSub_ = create_subscription<rtabmap_msgs::msg::MapData>("mapData", rclcpp::QoS(1),
|
||||
std::bind(&MapAssembler::mapDataReceivedCallback, this, std::placeholders::_1), options);
|
||||
}
|
||||
|
||||
MapAssembler::~MapAssembler() {}
|
||||
@@ -213,7 +236,8 @@ void MapAssembler::timerCallback()
|
||||
std::string getMapSrv = rtabmapNodeName_+"/get_map_data";
|
||||
RCLCPP_INFO(this->get_logger(), "Calling service \"%s\"...", getMapSrv.c_str());
|
||||
|
||||
if(client_->wait_for_service(5s))
|
||||
if(client_->wait_for_service(
|
||||
std::chrono::duration<double>(initializeFromRtabmapTimeout_)))
|
||||
{
|
||||
auto request = std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
|
||||
request->global_map = false;
|
||||
@@ -237,18 +261,16 @@ void MapAssembler::timerCallback()
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available after waiting for 5 seconds, "
|
||||
RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available after waiting for %f seconds, "
|
||||
"may not be a problem if rtabmap is started afterwards. If rtabmap "
|
||||
"is started after in localization mode, call %s/publish_maps "
|
||||
"service with graph_only=false to make sure map_assembler has all the data.",
|
||||
getMapSrv.c_str(),
|
||||
initializeFromRtabmapTimeout_,
|
||||
rtabmapNodeName_.c_str());
|
||||
}
|
||||
|
||||
rclcpp::SubscriptionOptions options;
|
||||
options.callback_group = timerCbGroup_;
|
||||
mapDataSub_ = create_subscription<rtabmap_msgs::msg::MapData>("mapData", rclcpp::QoS(1),
|
||||
std::bind(&MapAssembler::mapDataReceivedCallback, this, std::placeholders::_1), options);
|
||||
subscribeToMapData();
|
||||
}
|
||||
|
||||
void MapAssembler::mapDataReceivedCallback(const rtabmap_msgs::msg::MapData::ConstSharedPtr msg)
|
||||
|
||||
@@ -41,8 +41,6 @@ namespace rtabmap_util
|
||||
|
||||
PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options) :
|
||||
Node("point_cloud_aggregator", options),
|
||||
warningThread_(0),
|
||||
callbackCalled_(false),
|
||||
exactSync4_(0),
|
||||
approxSync4_(0),
|
||||
exactSync3_(0),
|
||||
@@ -162,23 +160,16 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
|
||||
}
|
||||
|
||||
|
||||
warningThread_ = new std::thread([&](){
|
||||
rclcpp::Rate r(1.0/5.0);
|
||||
while(!callbackCalled_)
|
||||
{
|
||||
r.sleep();
|
||||
if(!callbackCalled_)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "%s: Did not receive data since 5 seconds! Make sure the input topics are "
|
||||
"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
|
||||
"header are set. %s%s",
|
||||
this->get_name(),
|
||||
approx?"":"Parameter \"approx_sync\" is false, which means that input "
|
||||
"topics should have all the exact timestamp for the callback to be called.",
|
||||
subscribedTopicsMsg.c_str());
|
||||
}
|
||||
}
|
||||
});
|
||||
syncDiagnostic_.reset(new rtabmap_sync::SyncDiagnostic(this, 0.5));
|
||||
syncDiagnostic_->init(cloudSub_1_.getSubscriber()->get_topic_name(),
|
||||
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
|
||||
"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
|
||||
"header are set. %s%s",
|
||||
this->get_name(),
|
||||
approx?"":"Parameter \"approx_sync\" is false, which means that input "
|
||||
"topics should have all the exact timestamp for the callback to be called.",
|
||||
subscribedTopicsMsg.c_str()));
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
|
||||
}
|
||||
|
||||
@@ -190,13 +181,6 @@ PointCloudAggregator::~PointCloudAggregator()
|
||||
delete approxSync3_;
|
||||
delete exactSync2_;
|
||||
delete approxSync2_;
|
||||
|
||||
if(warningThread_)
|
||||
{
|
||||
callbackCalled_=true;
|
||||
warningThread_->join();
|
||||
delete warningThread_;
|
||||
}
|
||||
}
|
||||
|
||||
void PointCloudAggregator::clouds4_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_1,
|
||||
@@ -234,8 +218,8 @@ void PointCloudAggregator::clouds2_callback(const sensor_msgs::msg::PointCloud2:
|
||||
}
|
||||
void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> & cloudMsgs)
|
||||
{
|
||||
callbackCalled_ = true;
|
||||
UASSERT(cloudMsgs.size() > 1);
|
||||
syncDiagnostic_->tickInput(cloudMsgs[0]->header.stamp);
|
||||
if(cloudPub_->get_subscription_count())
|
||||
{
|
||||
pcl::PCLPointCloud2::Ptr output(new pcl::PCLPointCloud2);
|
||||
@@ -420,6 +404,7 @@ void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::Poi
|
||||
rosCloud->header.frame_id = frameId;
|
||||
cloudPub_->publish(std::move(rosCloud));
|
||||
}
|
||||
syncDiagnostic_->tickOutput(cloudMsgs[0]->header.stamp);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -45,8 +45,6 @@ namespace rtabmap_util
|
||||
|
||||
PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
|
||||
Node("point_cloud_assembler", options),
|
||||
warningThread_(0),
|
||||
callbackCalled_(false),
|
||||
exactSync_(0),
|
||||
exactInfoSync_(0),
|
||||
maxClouds_(0),
|
||||
@@ -170,22 +168,14 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
|
||||
syncOdomSub_.getSubscriber()->get_topic_name());
|
||||
}
|
||||
|
||||
warningThread_ = new std::thread([&](){
|
||||
rclcpp::Rate r(1.0/5.0);
|
||||
while(!callbackCalled_)
|
||||
{
|
||||
r.sleep();
|
||||
if(!callbackCalled_)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),
|
||||
"%s: Did not receive data since 5 seconds! Make sure the input topics are "
|
||||
"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
|
||||
"header are set. %s",
|
||||
get_name(),
|
||||
subscribedTopicsMsg_.c_str());
|
||||
}
|
||||
}
|
||||
});
|
||||
syncDiagnostic_.reset(new rtabmap_sync::SyncDiagnostic(this, 0.5));
|
||||
syncDiagnostic_->init(
|
||||
cloudSub_?cloudSub_->get_topic_name():syncCloudSub_.getSubscriber()->get_topic_name(),
|
||||
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
|
||||
"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
|
||||
"header are set. %s",
|
||||
get_name(),
|
||||
subscribedTopicsMsg_.c_str()));
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg_.c_str());
|
||||
}
|
||||
@@ -194,20 +184,12 @@ PointCloudAssembler::~PointCloudAssembler()
|
||||
{
|
||||
delete exactSync_;
|
||||
delete exactInfoSync_;
|
||||
|
||||
if(warningThread_)
|
||||
{
|
||||
callbackCalled_=true;
|
||||
warningThread_->join();
|
||||
delete warningThread_;
|
||||
}
|
||||
}
|
||||
|
||||
void PointCloudAssembler::callbackCloudOdom(
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg,
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg)
|
||||
{
|
||||
callbackCalled_ = true;
|
||||
rtabmap::Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
|
||||
if(!odom.isNull())
|
||||
{
|
||||
@@ -270,7 +252,6 @@ void PointCloudAssembler::callbackCloudOdomInfo(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
callbackCalled_ = true;
|
||||
rtabmap::Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
|
||||
if(!odom.isNull())
|
||||
{
|
||||
@@ -293,7 +274,7 @@ void PointCloudAssembler::callbackCloudOdomInfo(
|
||||
|
||||
void PointCloudAssembler::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg)
|
||||
{
|
||||
callbackCalled_ = true;
|
||||
syncDiagnostic_->tickInput(cloudMsg->header.stamp);
|
||||
if(cloudPub_->get_subscription_count())
|
||||
{
|
||||
UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,
|
||||
@@ -487,6 +468,7 @@ void PointCloudAssembler::callbackCloud(const sensor_msgs::msg::PointCloud2::Con
|
||||
rosCloud.header.frame_id = frameId_;
|
||||
}
|
||||
cloudPub_->publish(rosCloud);
|
||||
syncDiagnostic_->tickOutput(cloudMsg->header.stamp);
|
||||
if(circularBuffer_)
|
||||
{
|
||||
if(!isMoving)
|
||||
|
||||
@@ -164,9 +164,10 @@ void PointCloudToDepthImage::callback(
|
||||
|
||||
if(cloudDisplacement.isNull())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Could not find transform between %s and %s, accordingly to %s, aborting!",
|
||||
pointCloud2Msg->header.frame_id.c_str(),
|
||||
cameraInfoMsg->header.frame_id.c_str(),
|
||||
RCLCPP_ERROR(this->get_logger(), "Could not find how %s moved between the cloud (%f) and the camera info (%f) stamps, accordingly to %s, aborting!",
|
||||
pointCloud2Msg->header.frame_id.c_str(),
|
||||
cloudStamp,
|
||||
infoStamp,
|
||||
fixedFrameId_.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -43,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
#include "rtabmap/core/Compression.h"
|
||||
#include "rtabmap/utilite/UConversion.h"
|
||||
#include "rtabmap/utilite/ULogger.h"
|
||||
|
||||
namespace rtabmap_util
|
||||
{
|
||||
@@ -54,11 +55,20 @@ RGBDRelay::RGBDRelay(const rclcpp::NodeOptions & options) :
|
||||
{
|
||||
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
|
||||
qos = this->declare_parameter("qos", qos);
|
||||
// The two sides can be set independently so the relay can bridge a publisher
|
||||
// and a subscriber that don't agree on reliability. Both default to qos.
|
||||
int qosSub = this->declare_parameter("qos_sub", qos);
|
||||
int qosPub = this->declare_parameter("qos_pub", qos);
|
||||
int queueSub = this->declare_parameter("queue_sub", 5);
|
||||
int queuePub = this->declare_parameter("queue_pub", 1);
|
||||
compress_ = this->declare_parameter("compress", compress_);
|
||||
uncompress_ = this->declare_parameter("uncompress", uncompress_);
|
||||
|
||||
rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDRelay::callback, this, std::placeholders::_1));
|
||||
rgbdImagePub_ = create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
UASSERT_MSG(queueSub >= 1 && queuePub >= 1,
|
||||
uFormat("queue_sub (%d) and queue_pub (%d) must be at least 1", queueSub, queuePub).c_str());
|
||||
|
||||
rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(queueSub).reliability((rmw_qos_reliability_policy_t)qosSub), std::bind(&RGBDRelay::callback, this, std::placeholders::_1));
|
||||
rgbdImagePub_ = create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay", rclcpp::QoS(queuePub).reliability((rmw_qos_reliability_policy_t)qosPub));
|
||||
}
|
||||
|
||||
void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) const
|
||||
@@ -125,7 +135,7 @@ void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
|
||||
// already raw, just copy pointer
|
||||
output->rgb = input->rgb;
|
||||
}
|
||||
if(!input->rgb_compressed.data.empty())
|
||||
else if(!input->rgb_compressed.data.empty())
|
||||
{
|
||||
cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(output->rgb);
|
||||
}
|
||||
@@ -135,20 +145,37 @@ void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
|
||||
// already raw, just copy pointer
|
||||
output->depth = input->depth;
|
||||
}
|
||||
else if(input->depth_compressed.format.compare("jpg")==0)
|
||||
else if(!input->depth_compressed.data.empty())
|
||||
{
|
||||
// right stereo image
|
||||
cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(output->depth);
|
||||
}
|
||||
else
|
||||
{
|
||||
// dpeth image
|
||||
// Decode first, then pick the encoding from what actually came out.
|
||||
// Branching on the "jpg"/"png" format string instead would abort on a
|
||||
// right image compressed as PNG, which nothing forbids.
|
||||
auto cvImg = std::make_unique<cv_bridge::CvImage>();
|
||||
cvImg->header = input->depth_compressed.header;
|
||||
cvImg->image = rtabmap::uncompressImage(input->depth_compressed.data);
|
||||
UASSERT(cvImg->image.empty() || cvImg->image.type() == CV_32FC1 || cvImg->image.type() == CV_16UC1);
|
||||
cvImg->encoding = cvImg->image.empty()?"":cvImg->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
|
||||
cvImg->toImageMsg(output->depth);
|
||||
if(cvImg->image.empty())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Could not decompress the depth/right image of \"%s\" (format=\"%s\").",
|
||||
rgbdImageSub_->get_topic_name(), input->depth_compressed.format.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
switch(cvImg->image.type())
|
||||
{
|
||||
case CV_32FC1: cvImg->encoding = sensor_msgs::image_encodings::TYPE_32FC1; break;
|
||||
case CV_16UC1: cvImg->encoding = sensor_msgs::image_encodings::TYPE_16UC1; break;
|
||||
case CV_8UC1: cvImg->encoding = sensor_msgs::image_encodings::MONO8; break;
|
||||
case CV_8UC3: cvImg->encoding = sensor_msgs::image_encodings::BGR8; break;
|
||||
default:
|
||||
RCLCPP_ERROR(this->get_logger(), "Unsupported decompressed depth/right image type %d.", cvImg->image.type());
|
||||
cvImg->image = cv::Mat();
|
||||
break;
|
||||
}
|
||||
if(!cvImg->image.empty())
|
||||
{
|
||||
cvImg->toImageMsg(output->depth);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -26,6 +26,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <rtabmap_util/rgbd_split.hpp>
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <sensor_msgs/image_encodings.hpp>
|
||||
|
||||
#ifdef PRE_ROS_IRON
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
@@ -37,24 +41,55 @@ namespace rtabmap_util
|
||||
{
|
||||
|
||||
RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) :
|
||||
Node("rgbd_split", options)
|
||||
Node("rgbd_split", options),
|
||||
stereo_(false)
|
||||
{
|
||||
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
|
||||
qos = this->declare_parameter("qos", qos);
|
||||
// Each side can be set independently so the node can bridge a producer and a
|
||||
// consumer that don't agree on reliability. Both default to qos.
|
||||
int qosSub = this->declare_parameter("qos_sub", qos);
|
||||
int qosPub = this->declare_parameter("qos_pub", qos);
|
||||
int queueSub = this->declare_parameter("queue_sub", 5);
|
||||
int queuePub = this->declare_parameter("queue_pub", 1);
|
||||
// A stereo RGBDImage carries the right image in the depth slot, so name the outputs
|
||||
// left/right instead of rgb/depth to say what they really are.
|
||||
stereo_ = this->declare_parameter("stereo", false);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: queue_sub = %d", get_name(), queueSub);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: queue_pub = %d", get_name(), queuePub);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: stereo = %s", get_name(), stereo_?"true":"false");
|
||||
|
||||
rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDSplit::callback, this, std::placeholders::_1));
|
||||
UASSERT_MSG(queueSub >= 1 && queuePub >= 1,
|
||||
uFormat("queue_sub (%d) and queue_pub (%d) must be at least 1", queueSub, queuePub).c_str());
|
||||
|
||||
rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(queueSub).reliability((rmw_qos_reliability_policy_t)qosSub), std::bind(&RGBDSplit::callback, this, std::placeholders::_1));
|
||||
|
||||
const std::string base = rgbdImageSub_->get_topic_name();
|
||||
const std::string firstName = stereo_?"/left":"/rgb";
|
||||
const std::string secondName = stereo_?"/right":"/depth";
|
||||
const rclcpp::QoS pubQos = rclcpp::QoS(queuePub).reliability((rmw_qos_reliability_policy_t)qosPub);
|
||||
|
||||
#ifdef PRE_ROS_LYRICAL
|
||||
rgbPub_ = image_transport::create_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
depthPub_ = image_transport::create_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
rgbPub_ = image_transport::create_publisher(this, base + firstName + "/image", pubQos.get_rmw_qos_profile());
|
||||
depthPub_ = image_transport::create_publisher(this, base + secondName + "/image", pubQos.get_rmw_qos_profile());
|
||||
#else
|
||||
rgbPub_ = image_transport::create_publisher(*this, std::string(rgbdImageSub_->get_topic_name()) + "/rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
depthPub_ = image_transport::create_publisher(*this, std::string(rgbdImageSub_->get_topic_name()) + "/depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
rgbPub_ = image_transport::create_publisher(*this, base + firstName + "/image", pubQos);
|
||||
depthPub_ = image_transport::create_publisher(*this, base + secondName + "/image", pubQos);
|
||||
#endif
|
||||
rgbInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>(std::string(rgbdImageSub_->get_topic_name()) + "/rgb/camera_info", 1);
|
||||
depthInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>(std::string(rgbdImageSub_->get_topic_name()) + "/depth/camera_info", 1);
|
||||
rgbInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>(base + firstName + "/camera_info", pubQos);
|
||||
depthInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>(base + secondName + "/camera_info", pubQos);
|
||||
|
||||
// Resolved names: the outputs are derived from the input topic, so a remapping of
|
||||
// "rgbd_image" moves all four with it. Print them so it is clear what to subscribe to.
|
||||
RCLCPP_INFO(this->get_logger(), "%s: subscribed to:\n %s", get_name(), rgbdImageSub_->get_topic_name());
|
||||
RCLCPP_INFO(this->get_logger(), "%s: publishing:\n %s,\n %s,\n %s,\n %s",
|
||||
get_name(),
|
||||
rgbPub_.getTopic().c_str(),
|
||||
rgbInfoPub_->get_topic_name(),
|
||||
depthPub_.getTopic().c_str(),
|
||||
depthInfoPub_->get_topic_name());
|
||||
}
|
||||
|
||||
|
||||
@@ -100,7 +135,36 @@ void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
|
||||
#ifdef CV_BRIDGE_HYDRO
|
||||
ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
|
||||
#else
|
||||
cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(outputImage);
|
||||
// Decode first, then pick the encoding from what actually came out. Going
|
||||
// by the "jpg"/"png" format string instead would mislabel a depth PNG as
|
||||
// mono8 (cv_bridge cannot infer 16-bit from it), and would abort outright on
|
||||
// a right image compressed as PNG, which nothing forbids.
|
||||
cv_bridge::CvImage cvImg;
|
||||
cvImg.header = input->depth_compressed.header;
|
||||
cvImg.image = rtabmap::uncompressImage(input->depth_compressed.data);
|
||||
if(cvImg.image.empty())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Could not decompress the depth/right image of \"%s\" (format=\"%s\").",
|
||||
rgbdImageSub_->get_topic_name(), input->depth_compressed.format.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
switch(cvImg.image.type())
|
||||
{
|
||||
case CV_32FC1: cvImg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; break;
|
||||
case CV_16UC1: cvImg.encoding = sensor_msgs::image_encodings::TYPE_16UC1; break;
|
||||
case CV_8UC1: cvImg.encoding = sensor_msgs::image_encodings::MONO8; break;
|
||||
case CV_8UC3: cvImg.encoding = sensor_msgs::image_encodings::BGR8; break;
|
||||
default:
|
||||
RCLCPP_ERROR(this->get_logger(), "Unsupported decompressed depth/right image type %d.", cvImg.image.type());
|
||||
cvImg.image = cv::Mat();
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(!cvImg.image.empty())
|
||||
{
|
||||
cvImg.toImageMsg(outputImage);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
if(outputCameraInfo.header.frame_id.empty()) {
|
||||
@@ -119,6 +183,37 @@ void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
|
||||
outputImage.header = outputCameraInfo.header;
|
||||
}
|
||||
}
|
||||
// The "depth" slot of an RGBDImage holds either a depth image or the right image
|
||||
// of a stereo pair, and "stereo" decides which name it goes out under. Warn when
|
||||
// the two disagree: the topic name would be lying to every consumer downstream.
|
||||
// Both directions only warn and keep forwarding -- publishing a right image on
|
||||
// the depth topic is what this node has always done, and setups rely on it.
|
||||
if(!outputImage.data.empty())
|
||||
{
|
||||
const bool isDepth =
|
||||
outputImage.encoding == sensor_msgs::image_encodings::TYPE_16UC1 ||
|
||||
outputImage.encoding == sensor_msgs::image_encodings::TYPE_32FC1 ||
|
||||
outputImage.encoding == sensor_msgs::image_encodings::MONO16;
|
||||
if(stereo_ && isDepth)
|
||||
{
|
||||
RCLCPP_WARN_ONCE(this->get_logger(),
|
||||
"Parameter \"stereo\" is true, so the second half is published as \"%s\", "
|
||||
"but the received image is a depth image (encoding=\"%s\"), not the right "
|
||||
"image of a stereo pair. Set \"stereo\" to false to publish it as depth. "
|
||||
"(This warning is printed only once)",
|
||||
depthPub_.getTopic().c_str(), outputImage.encoding.c_str());
|
||||
}
|
||||
else if(!stereo_ && !isDepth)
|
||||
{
|
||||
RCLCPP_WARN_ONCE(this->get_logger(),
|
||||
"Parameter \"stereo\" is false, so the second half is published as \"%s\", "
|
||||
"but the received image is not a depth image (encoding=\"%s\"): it looks "
|
||||
"like the right image of a stereo pair. Set \"stereo\" to true to publish "
|
||||
"it under a name that says so. (This warning is printed only once)",
|
||||
depthPub_.getTopic().c_str(), outputImage.encoding.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
depthPub_.publish(outputImage);
|
||||
depthInfoPub_->publish(outputCameraInfo);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,366 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#ifndef RTABMAP_UTIL_DB_BUILDERS_HPP_
|
||||
#define RTABMAP_UTIL_DB_BUILDERS_HPP_
|
||||
|
||||
/**
|
||||
* @file
|
||||
* @brief Synthetic RTAB-Map databases for the db_player tests.
|
||||
*
|
||||
* db_player replays whatever a database happens to contain, and which topics it even
|
||||
* creates depends on the payloads it finds. Rather than ship a recorded database, each
|
||||
* scenario is written here with DBDriver so the expected values are visible right next
|
||||
* to the assertions.
|
||||
*
|
||||
* @note Only the *compressed* buffers of a SensorData are persisted, so every payload is
|
||||
* compressed before being handed to the driver. Saving raw-only data silently
|
||||
* writes empty blobs.
|
||||
*/
|
||||
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap/core/DBDriver.h>
|
||||
#include <rtabmap/core/EnvSensor.h>
|
||||
#include <rtabmap/core/GPS.h>
|
||||
#include <rtabmap/core/Link.h>
|
||||
#include <rtabmap/core/Signature.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap/utilite/UFile.h>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
|
||||
#include <functional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace rtabmap_util_test {
|
||||
|
||||
//============================================================================
|
||||
// What every synthetic database contains, and what the tests assert against
|
||||
//============================================================================
|
||||
|
||||
constexpr int kDbFrames = 3; ///< nodes in each database
|
||||
constexpr double kFirstStamp = 1000.0; ///< stamp of node 1, seconds
|
||||
constexpr double kStampStep = 0.05; ///< seconds between consecutive nodes
|
||||
constexpr float kPoseStep = 0.5f; ///< meters along x between odometry poses
|
||||
constexpr double kOdomVariance = 0.25; ///< diagonal of the odometry covariance
|
||||
|
||||
constexpr int kImageWidth = 80;
|
||||
constexpr int kImageHeight = 60;
|
||||
constexpr double kFx = 100.0;
|
||||
constexpr double kFy = 100.0;
|
||||
constexpr double kCx = 40.0;
|
||||
constexpr double kCy = 30.0;
|
||||
constexpr double kBaseline = 0.12;
|
||||
constexpr uint16_t kDepthMillimeters = 1500;
|
||||
|
||||
constexpr double kGpsLongitude = -71.9;
|
||||
constexpr double kGpsLatitude = 45.4;
|
||||
constexpr double kGpsAltitude = 123.0;
|
||||
constexpr double kGpsError = 2.5;
|
||||
constexpr double kEnvSensorValue = 21.5;
|
||||
|
||||
/// The odometry pose of node @p id, one step further along x than the previous one.
|
||||
inline rtabmap::Transform poseOf(int id)
|
||||
{
|
||||
return rtabmap::Transform(kPoseStep * float(id - 1), 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
|
||||
}
|
||||
|
||||
/// The stamp of node @p id.
|
||||
inline double stampOfNode(int id)
|
||||
{
|
||||
return kFirstStamp + kStampStep * double(id - 1);
|
||||
}
|
||||
|
||||
/// Where the camera sits on the robot: 10 cm forward, 20 cm up, looking forward.
|
||||
inline rtabmap::Transform cameraLocalTransform()
|
||||
{
|
||||
return rtabmap::Transform(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f) *
|
||||
rtabmap::CameraModel::opticalRotation();
|
||||
}
|
||||
|
||||
/// Where the lidar sits on the robot.
|
||||
inline rtabmap::Transform scanLocalTransform()
|
||||
{
|
||||
return rtabmap::Transform(0.05f, 0.0f, 0.3f, 0.0f, 0.0f, 0.0f);
|
||||
}
|
||||
|
||||
/// The ground truth pose of node @p id, offset from the odometry pose so they differ.
|
||||
inline rtabmap::Transform groundTruthOf(int id)
|
||||
{
|
||||
return rtabmap::Transform(kPoseStep * float(id - 1), 1.0f, 0.0f, 0.0f, 0.0f, 0.0f);
|
||||
}
|
||||
|
||||
/// The prior (global) pose of node @p id.
|
||||
inline rtabmap::Transform globalPoseOf(int id)
|
||||
{
|
||||
return rtabmap::Transform(kPoseStep * float(id - 1), 2.0f, 0.0f, 0.0f, 0.0f, 0.0f);
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// A database file that cleans itself up
|
||||
//============================================================================
|
||||
|
||||
/// A uniquely named database path under the test temp directory, erased on destruction.
|
||||
class TempDatabase
|
||||
{
|
||||
public:
|
||||
explicit TempDatabase(const std::string & tag)
|
||||
{
|
||||
static int counter = 0;
|
||||
path_ = std::string(::testing::TempDir()) +
|
||||
uFormat("rtabmap_util_db_player_%s_%d_%d.db", tag.c_str(), (int)getpid(), ++counter);
|
||||
UFile::erase(path_.c_str());
|
||||
}
|
||||
|
||||
~TempDatabase() { UFile::erase(path_.c_str()); }
|
||||
|
||||
TempDatabase(const TempDatabase &) = delete;
|
||||
TempDatabase & operator=(const TempDatabase &) = delete;
|
||||
|
||||
const std::string & path() const { return path_; }
|
||||
|
||||
private:
|
||||
std::string path_;
|
||||
};
|
||||
|
||||
//============================================================================
|
||||
// Writing the databases
|
||||
//============================================================================
|
||||
|
||||
/// Builds the sensor payload of node @p id; see the writeXxxDatabase() functions.
|
||||
typedef std::function<rtabmap::SensorData(int id, double stamp)> DataBuilder;
|
||||
|
||||
/// Adds anything beyond the payload: links, ground truth, and so on.
|
||||
typedef std::function<void(int id, rtabmap::Signature & signature)> NodeDecorator;
|
||||
|
||||
/**
|
||||
* @brief Writes @p frames consecutive nodes sharing the plumbing every replay needs.
|
||||
*
|
||||
* Nodes are numbered from 1, stamped kStampStep apart (db_player replays at the database
|
||||
* stamps, so a node without one aborts the read), posed kPoseStep apart along x, and
|
||||
* joined by the neighbor links that carry the odometry covariance.
|
||||
*/
|
||||
inline void writeDatabase(
|
||||
const std::string & path, int frames,
|
||||
const DataBuilder & makeData,
|
||||
const NodeDecorator & decorate = NodeDecorator())
|
||||
{
|
||||
rtabmap::DBDriver * driver = rtabmap::DBDriver::create();
|
||||
ASSERT_NE(driver, nullptr);
|
||||
ASSERT_TRUE(driver->openConnection(path, /*overwritten=*/true)) << "cannot create " << path;
|
||||
|
||||
for(int id=1; id<=frames; ++id)
|
||||
{
|
||||
const double stamp = stampOfNode(id);
|
||||
rtabmap::Signature * s = new rtabmap::Signature(
|
||||
id, /*mapId=*/0, /*weight=*/1, stamp, /*label=*/"",
|
||||
poseOf(id), rtabmap::Transform(), makeData(id, stamp));
|
||||
|
||||
if(id > 1)
|
||||
{
|
||||
// The backward neighbor link is where DBReader reads the odometry
|
||||
// covariance from: it publishes the inverse of this information matrix.
|
||||
const rtabmap::Transform motion(kPoseStep, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
|
||||
s->addLink(rtabmap::Link(id, id-1, rtabmap::Link::kNeighbor, motion.inverse(),
|
||||
cv::Mat::eye(6, 6, CV_64FC1) / kOdomVariance));
|
||||
}
|
||||
if(decorate)
|
||||
{
|
||||
decorate(id, *s);
|
||||
}
|
||||
|
||||
driver->asyncSave(s); // the driver takes ownership
|
||||
driver->emptyTrashes(false);
|
||||
}
|
||||
|
||||
driver->closeConnection(true);
|
||||
delete driver;
|
||||
}
|
||||
|
||||
/// A color image whose pixels identify the node, so a test can tell frames apart.
|
||||
inline cv::Mat makeRgb(int id)
|
||||
{
|
||||
return cv::Mat(kImageHeight, kImageWidth, CV_8UC3, cv::Scalar(id, 2*id, 3*id));
|
||||
}
|
||||
|
||||
inline cv::Mat makeDepth()
|
||||
{
|
||||
return cv::Mat(kImageHeight, kImageWidth, CV_16UC1, cv::Scalar(kDepthMillimeters));
|
||||
}
|
||||
|
||||
inline rtabmap::CameraModel rgbdCameraModel()
|
||||
{
|
||||
return rtabmap::CameraModel(kFx, kFy, kCx, kCy, cameraLocalTransform(), 0.0,
|
||||
cv::Size(kImageWidth, kImageHeight));
|
||||
}
|
||||
|
||||
inline rtabmap::StereoCameraModel stereoCameraModel()
|
||||
{
|
||||
return rtabmap::StereoCameraModel(kFx, kFy, kCx, kCy, kBaseline, cameraLocalTransform(),
|
||||
cv::Size(kImageWidth, kImageHeight));
|
||||
}
|
||||
|
||||
/// RGB + registered depth from a single camera.
|
||||
inline void writeRgbdDatabase(const std::string & path, int frames = kDbFrames)
|
||||
{
|
||||
writeDatabase(path, frames, [](int id, double stamp) {
|
||||
rtabmap::SensorData data;
|
||||
data.setId(id);
|
||||
data.setStamp(stamp);
|
||||
data.setRGBDImage(rtabmap::compressImage2(makeRgb(id), ".png"),
|
||||
rtabmap::compressImage2(makeDepth(), ".png"), rgbdCameraModel());
|
||||
return data;
|
||||
});
|
||||
}
|
||||
|
||||
/// A rectified mono stereo pair.
|
||||
inline void writeStereoDatabase(const std::string & path, int frames = kDbFrames)
|
||||
{
|
||||
writeDatabase(path, frames, [](int id, double stamp) {
|
||||
const cv::Mat left(kImageHeight, kImageWidth, CV_8UC1, cv::Scalar(id));
|
||||
const cv::Mat right(kImageHeight, kImageWidth, CV_8UC1, cv::Scalar(2*id));
|
||||
rtabmap::SensorData data;
|
||||
data.setId(id);
|
||||
data.setStamp(stamp);
|
||||
data.setStereoImage(rtabmap::compressImage2(left, ".png"),
|
||||
rtabmap::compressImage2(right, ".png"), stereoCameraModel());
|
||||
return data;
|
||||
});
|
||||
}
|
||||
|
||||
/// A color image with no calibration at all, which db_player replays on "image".
|
||||
inline void writeImageOnlyDatabase(const std::string & path, int frames = kDbFrames)
|
||||
{
|
||||
writeDatabase(path, frames, [](int id, double stamp) {
|
||||
rtabmap::SensorData data;
|
||||
data.setId(id);
|
||||
data.setStamp(stamp);
|
||||
data.setRGBDImage(rtabmap::compressImage2(makeRgb(id), ".png"), cv::Mat(),
|
||||
std::vector<rtabmap::CameraModel>());
|
||||
return data;
|
||||
});
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// Laser scans
|
||||
//============================================================================
|
||||
|
||||
constexpr int kScanBins = 20;
|
||||
constexpr float kScanAngleMin = -1.0f;
|
||||
constexpr float kScanAngleMax = 1.0f;
|
||||
constexpr float kScanAngleIncrement = 0.1f; // (max-min)/kScanBins
|
||||
constexpr float kScanRangeMin = 0.1f;
|
||||
constexpr float kScanRangeMax = 10.0f;
|
||||
|
||||
/// The range measured in bin @p bin of the 2D scan.
|
||||
inline float scanRangeOf(int bin) { return 1.0f + 0.1f * float(bin); }
|
||||
|
||||
/**
|
||||
* @brief A 2D scan with one point at the center of every bin.
|
||||
*
|
||||
* db_player re-bins the cartesian points back into a LaserScan message, so putting each
|
||||
* point at a bin center makes the expected index exact rather than a rounding coin flip.
|
||||
*/
|
||||
inline rtabmap::LaserScan makeScan2d()
|
||||
{
|
||||
cv::Mat points(1, kScanBins, CV_32FC2);
|
||||
for(int bin=0; bin<kScanBins; ++bin)
|
||||
{
|
||||
const float angle = kScanAngleMin + (float(bin) + 0.5f) * kScanAngleIncrement;
|
||||
const float range = scanRangeOf(bin);
|
||||
points.at<cv::Vec2f>(0, bin) = cv::Vec2f(range * std::cos(angle), range * std::sin(angle));
|
||||
}
|
||||
return rtabmap::LaserScan(rtabmap::compressData2(points), rtabmap::LaserScan::kXY,
|
||||
kScanRangeMin, kScanRangeMax, kScanAngleMin, kScanAngleMax, kScanAngleIncrement,
|
||||
scanLocalTransform());
|
||||
}
|
||||
|
||||
constexpr int kScanCloudPoints = 50;
|
||||
|
||||
inline rtabmap::LaserScan makeScan3d()
|
||||
{
|
||||
cv::Mat points(1, kScanCloudPoints, CV_32FC3);
|
||||
for(int i=0; i<kScanCloudPoints; ++i)
|
||||
{
|
||||
points.at<cv::Vec3f>(0, i) = cv::Vec3f(1.0f + 0.01f*float(i), 0.02f*float(i), 0.5f);
|
||||
}
|
||||
return rtabmap::LaserScan(rtabmap::compressData2(points), /*maxPoints=*/0, /*maxRange=*/0.0f,
|
||||
rtabmap::LaserScan::kXYZ, scanLocalTransform());
|
||||
}
|
||||
|
||||
/// A 2D lidar only, no camera.
|
||||
inline void writeScan2dDatabase(const std::string & path, int frames = kDbFrames)
|
||||
{
|
||||
writeDatabase(path, frames, [](int id, double stamp) {
|
||||
rtabmap::SensorData data;
|
||||
data.setId(id);
|
||||
data.setStamp(stamp);
|
||||
data.setLaserScan(makeScan2d());
|
||||
return data;
|
||||
});
|
||||
}
|
||||
|
||||
/// A 3D lidar only, no camera.
|
||||
inline void writeScan3dDatabase(const std::string & path, int frames = kDbFrames)
|
||||
{
|
||||
writeDatabase(path, frames, [](int id, double stamp) {
|
||||
rtabmap::SensorData data;
|
||||
data.setId(id);
|
||||
data.setStamp(stamp);
|
||||
data.setLaserScan(makeScan3d());
|
||||
return data;
|
||||
});
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// Everything else db_player can replay
|
||||
//============================================================================
|
||||
|
||||
/// The gravity orientation stored as a link, which is how DBReader rebuilds an IMU.
|
||||
inline rtabmap::Transform gravityTransform()
|
||||
{
|
||||
return rtabmap::Transform(0.0f, 0.0f, 0.0f, 0.1f, 0.2f, 0.0f);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief RGB-D plus the optional channels: ground truth, prior pose, GPS, gravity and an
|
||||
* environmental sensor.
|
||||
*
|
||||
* @note The prior's information matrix must not leave a huge rotational variance, or
|
||||
* DBReader drops the global pose on the assumption GPS already provided the prior.
|
||||
*/
|
||||
inline void writeRichDatabase(const std::string & path, int frames = kDbFrames)
|
||||
{
|
||||
writeDatabase(path, frames,
|
||||
[](int id, double stamp) {
|
||||
rtabmap::SensorData data;
|
||||
data.setId(id);
|
||||
data.setStamp(stamp);
|
||||
data.setRGBDImage(rtabmap::compressImage2(makeRgb(id), ".png"),
|
||||
rtabmap::compressImage2(makeDepth(), ".png"), rgbdCameraModel());
|
||||
data.setGPS(rtabmap::GPS(stamp, kGpsLongitude, kGpsLatitude, kGpsAltitude,
|
||||
kGpsError, /*bearing=*/0.0));
|
||||
rtabmap::EnvSensors sensors;
|
||||
sensors.insert(std::make_pair(rtabmap::EnvSensor::kAmbientTemperature,
|
||||
rtabmap::EnvSensor(rtabmap::EnvSensor::kAmbientTemperature,
|
||||
kEnvSensorValue, stamp)));
|
||||
data.setEnvSensors(sensors);
|
||||
return data;
|
||||
},
|
||||
[](int id, rtabmap::Signature & s) {
|
||||
s.setGroundTruthPose(groundTruthOf(id));
|
||||
s.addLink(rtabmap::Link(id, id, rtabmap::Link::kPosePrior, globalPoseOf(id),
|
||||
cv::Mat::eye(6, 6, CV_64FC1) * 100.0));
|
||||
s.addLink(rtabmap::Link(id, id, rtabmap::Link::kGravity, gravityTransform(),
|
||||
cv::Mat::eye(6, 6, CV_64FC1)));
|
||||
});
|
||||
}
|
||||
|
||||
} // namespace rtabmap_util_test
|
||||
|
||||
#endif /* RTABMAP_UTIL_DB_BUILDERS_HPP_ */
|
||||
@@ -0,0 +1,217 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#ifndef RTABMAP_UTIL_MSG_BUILDERS_HPP_
|
||||
#define RTABMAP_UTIL_MSG_BUILDERS_HPP_
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/camera_info.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
#include <sensor_msgs/image_encodings.hpp>
|
||||
#include <rtabmap_msgs/msg/rgbd_image.hpp>
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
#ifdef PRE_ROS_IRON
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#else
|
||||
#include <cv_bridge/cv_bridge.hpp>
|
||||
#endif
|
||||
|
||||
#include <functional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace rtabmap_util_test {
|
||||
|
||||
inline rclcpp::Time stampOf(double seconds)
|
||||
{
|
||||
return rclcpp::Time(
|
||||
int32_t(seconds), uint32_t((seconds - int32_t(seconds)) * 1e9), RCL_ROS_TIME);
|
||||
}
|
||||
|
||||
/// A rectified pinhole CameraInfo; @p tx is P(0,3), non-zero for a stereo right camera.
|
||||
inline sensor_msgs::msg::CameraInfo makeCameraInfo(
|
||||
const std::string & frameId, double stamp, int width, int height,
|
||||
double tx = 0.0, double fx = 100.0)
|
||||
{
|
||||
sensor_msgs::msg::CameraInfo info;
|
||||
info.header.frame_id = frameId;
|
||||
info.header.stamp = stampOf(stamp);
|
||||
info.width = width;
|
||||
info.height = height;
|
||||
info.distortion_model = "plumb_bob";
|
||||
info.d = {0.0, 0.0, 0.0, 0.0, 0.0};
|
||||
info.k = {fx, 0.0, width/2.0, 0.0, fx, height/2.0, 0.0, 0.0, 1.0};
|
||||
info.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
|
||||
info.p = {fx, 0.0, width/2.0, tx, 0.0, fx, height/2.0, 0.0, 0.0, 0.0, 1.0, 0.0};
|
||||
return info;
|
||||
}
|
||||
|
||||
inline sensor_msgs::msg::Image makeImage(
|
||||
const std::string & frameId, double stamp,
|
||||
const cv::Mat & image, const std::string & encoding)
|
||||
{
|
||||
std_msgs::msg::Header header;
|
||||
header.frame_id = frameId;
|
||||
header.stamp = stampOf(stamp);
|
||||
sensor_msgs::msg::Image msg;
|
||||
cv_bridge::CvImage(header, encoding, image).toImageMsg(msg);
|
||||
return msg;
|
||||
}
|
||||
|
||||
/// An RGB-D message with raw bgr8 color and 16UC1 depth.
|
||||
inline rtabmap_msgs::msg::RGBDImage makeRGBDImage(
|
||||
const std::string & frameId, double stamp, int width = 8, int height = 8,
|
||||
const cv::Scalar & rgbColor = cv::Scalar(10, 20, 30), uint16_t depthValue = 1500)
|
||||
{
|
||||
rtabmap_msgs::msg::RGBDImage msg;
|
||||
msg.header.frame_id = frameId;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.rgb = makeImage(frameId, stamp, cv::Mat(height, width, CV_8UC3, rgbColor), "bgr8");
|
||||
msg.depth = makeImage(frameId, stamp,
|
||||
cv::Mat(height, width, CV_16UC1, cv::Scalar(depthValue)), "16UC1");
|
||||
msg.rgb_camera_info = makeCameraInfo(frameId, stamp, width, height);
|
||||
msg.depth_camera_info = makeCameraInfo(frameId, stamp, width, height);
|
||||
return msg;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief An RGB-D message carrying a stereo pair instead of depth.
|
||||
*
|
||||
* The "depth" slot holds the mono8 right image and the second camera info carries the
|
||||
* baseline in P(0,3), which is what makes consumers treat the pair as stereo rather
|
||||
* than as color plus depth.
|
||||
*/
|
||||
inline rtabmap_msgs::msg::RGBDImage makeStereoRGBDImage(
|
||||
const std::string & frameId, double stamp, int width = 8, int height = 8,
|
||||
double baseline = 0.12, double fx = 100.0)
|
||||
{
|
||||
rtabmap_msgs::msg::RGBDImage msg;
|
||||
msg.header.frame_id = frameId;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.rgb = makeImage(frameId, stamp,
|
||||
cv::Mat(height, width, CV_8UC3, cv::Scalar(10, 20, 30)), "bgr8");
|
||||
msg.depth = makeImage(frameId, stamp,
|
||||
cv::Mat(height, width, CV_8UC1, cv::Scalar(60)), "mono8"); // right image
|
||||
msg.rgb_camera_info = makeCameraInfo(frameId, stamp, width, height, 0.0, fx);
|
||||
msg.depth_camera_info =
|
||||
makeCameraInfo(frameId, stamp, width, height, -fx*baseline, fx);
|
||||
return msg;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief A dense unorganized XYZ float cloud.
|
||||
*
|
||||
* @note This writes the points exactly as given: it does not model sensor motion. To
|
||||
* build a cloud that deskewing can actually correct, use makeSkewedWallScan(),
|
||||
* which derives the distortion from the same trajectory the TF describes.
|
||||
*
|
||||
* @param withTimeChannel add a FLOAT32 "t" channel of per-point offsets, as a spinning
|
||||
* lidar publishes, so the cloud can be deskewed.
|
||||
*/
|
||||
inline sensor_msgs::msg::PointCloud2 makeXYZCloud(
|
||||
const std::string & frameId, double stamp,
|
||||
const std::vector<cv::Point3f> & points,
|
||||
bool withTimeChannel = false, double sweepDuration = 0.099)
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2 cloud;
|
||||
cloud.header.frame_id = frameId;
|
||||
cloud.header.stamp = stampOf(stamp);
|
||||
cloud.height = 1;
|
||||
cloud.width = points.size();
|
||||
cloud.is_bigendian = false;
|
||||
cloud.is_dense = true;
|
||||
|
||||
const int fieldCount = withTimeChannel ? 4 : 3;
|
||||
cloud.fields.resize(fieldCount);
|
||||
const char * names[4] = {"x", "y", "z", "t"};
|
||||
for(int i=0; i<fieldCount; ++i)
|
||||
{
|
||||
cloud.fields[i].name = names[i];
|
||||
cloud.fields[i].offset = 4 * i;
|
||||
cloud.fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32;
|
||||
cloud.fields[i].count = 1;
|
||||
}
|
||||
cloud.point_step = 4 * fieldCount;
|
||||
cloud.row_step = cloud.point_step * cloud.width;
|
||||
cloud.data.resize(cloud.row_step * cloud.height);
|
||||
|
||||
for(size_t i=0; i<points.size(); ++i)
|
||||
{
|
||||
float * p = reinterpret_cast<float *>(&cloud.data[i * cloud.point_step]);
|
||||
p[0] = points[i].x;
|
||||
p[1] = points[i].y;
|
||||
p[2] = points[i].z;
|
||||
if(withTimeChannel)
|
||||
{
|
||||
p[3] = points.size() > 1
|
||||
? float(sweepDuration * double(i) / double(points.size() - 1))
|
||||
: 0.0f;
|
||||
}
|
||||
}
|
||||
return cloud;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief The raw scan of a flat wall captured while the sensor moves straight at it.
|
||||
*
|
||||
* Sample @p i is taken at `i * step` seconds into the sweep, by which time the sensor
|
||||
* has closed in by `displacement(elapsed)`. Expressed in the sensor frame at capture
|
||||
* time the wall therefore appears to slide closer: a straight wall is recorded bent.
|
||||
* Deskewing with the same motion must flatten it back to @p wallDistance.
|
||||
*
|
||||
* @param frameId sensor frame
|
||||
* @param stamp stamp of the first sample, which is also the message stamp
|
||||
* @param sampleCount number of samples along the wall
|
||||
* @param sweepDuration seconds from the first sample to the last
|
||||
* @param wallDistance distance to the wall at the first sample, in meters
|
||||
* @param displacement distance travelled as a function of seconds since the first
|
||||
* sample; must match the motion published to TF
|
||||
*/
|
||||
inline sensor_msgs::msg::PointCloud2 makeSkewedWallScan(
|
||||
const std::string & frameId, double stamp,
|
||||
size_t sampleCount, double sweepDuration, float wallDistance,
|
||||
const std::function<double(double)> & displacement)
|
||||
{
|
||||
std::vector<cv::Point3f> points;
|
||||
points.reserve(sampleCount);
|
||||
for(size_t i=0; i<sampleCount; ++i)
|
||||
{
|
||||
const double elapsed =
|
||||
sampleCount > 1 ? sweepDuration * double(i) / double(sampleCount - 1) : 0.0;
|
||||
points.push_back(cv::Point3f(
|
||||
wallDistance - float(displacement(elapsed)), // the skew
|
||||
-1.0f + 2.0f * float(i) / float(sampleCount > 1 ? sampleCount - 1 : 1),
|
||||
0.0f));
|
||||
}
|
||||
return makeXYZCloud(frameId, stamp, points, /*withTimeChannel=*/true, sweepDuration);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Reads the x/y/z of a point from any FLOAT32 xyz cloud.
|
||||
*
|
||||
* Looks the offsets up in the field list rather than assuming they are 0/4/8, so it also
|
||||
* works on clouds produced by laser_geometry, which lay their fields out differently.
|
||||
*/
|
||||
inline cv::Point3f readXYZ(const sensor_msgs::msg::PointCloud2 & cloud, size_t index)
|
||||
{
|
||||
uint32_t xOffset = 0, yOffset = 4, zOffset = 8;
|
||||
for(size_t i=0; i<cloud.fields.size(); ++i)
|
||||
{
|
||||
if(cloud.fields[i].name == "x") { xOffset = cloud.fields[i].offset; }
|
||||
else if(cloud.fields[i].name == "y") { yOffset = cloud.fields[i].offset; }
|
||||
else if(cloud.fields[i].name == "z") { zOffset = cloud.fields[i].offset; }
|
||||
}
|
||||
const unsigned char * base = &cloud.data[index * cloud.point_step];
|
||||
return cv::Point3f(
|
||||
*reinterpret_cast<const float *>(base + xOffset),
|
||||
*reinterpret_cast<const float *>(base + yOffset),
|
||||
*reinterpret_cast<const float *>(base + zOffset));
|
||||
}
|
||||
|
||||
} // namespace rtabmap_util_test
|
||||
|
||||
#endif /* RTABMAP_UTIL_MSG_BUILDERS_HPP_ */
|
||||
@@ -0,0 +1,320 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef RTABMAP_UTIL_NODE_TEST_UTILS_HPP_
|
||||
#define RTABMAP_UTIL_NODE_TEST_UTILS_HPP_
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <geometry_msgs/msg/transform_stamped.hpp>
|
||||
#include <tf2/LinearMath/Quaternion.hpp>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
|
||||
#include <tf2_msgs/msg/tf_message.hpp>
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
namespace rtabmap_util_test {
|
||||
|
||||
/**
|
||||
* @brief Brings rclcpp up once for the whole test binary.
|
||||
*
|
||||
* Registered as a gtest global environment so it runs before the first test and shuts
|
||||
* down after the last one, which keeps gtest_main usable.
|
||||
*/
|
||||
class RclcppEnvironment : public ::testing::Environment
|
||||
{
|
||||
public:
|
||||
void SetUp() override
|
||||
{
|
||||
if(!rclcpp::ok())
|
||||
{
|
||||
rclcpp::init(0, nullptr);
|
||||
}
|
||||
}
|
||||
void TearDown() override
|
||||
{
|
||||
if(rclcpp::ok())
|
||||
{
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
/// Registers RclcppEnvironment. Call once at file scope in each test binary.
|
||||
inline ::testing::Environment * registerRclcppEnvironment()
|
||||
{
|
||||
static ::testing::Environment * const env =
|
||||
::testing::AddGlobalTestEnvironment(new RclcppEnvironment);
|
||||
return env;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Base fixture for driving a node under test over real ROS topics.
|
||||
*
|
||||
* The node under test and a helper node share one single-threaded executor, so
|
||||
* publishing, the node's callback and the assertion all happen on the same thread and
|
||||
* the tests stay deterministic. No launch files and no separate processes are involved:
|
||||
* everything runs in the gtest binary.
|
||||
*/
|
||||
class NodeTest : public ::testing::Test
|
||||
{
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
|
||||
helper_ = std::make_shared<rclcpp::Node>("rtabmap_util_test_helper");
|
||||
executor_->add_node(helper_);
|
||||
}
|
||||
|
||||
void TearDown() override
|
||||
{
|
||||
for(const rclcpp::Node::SharedPtr & node : nodes_)
|
||||
{
|
||||
executor_->remove_node(node);
|
||||
}
|
||||
nodes_.clear();
|
||||
executor_->remove_node(helper_);
|
||||
helper_.reset();
|
||||
executor_.reset();
|
||||
}
|
||||
|
||||
/// Adds a node under test to the shared executor and keeps it alive for the test.
|
||||
template <typename NodeT>
|
||||
std::shared_ptr<NodeT> addNode(const std::shared_ptr<NodeT> & node)
|
||||
{
|
||||
executor_->add_node(node);
|
||||
nodes_.push_back(node);
|
||||
return node;
|
||||
}
|
||||
|
||||
/// The helper node, used to publish inputs and subscribe to outputs.
|
||||
rclcpp::Node::SharedPtr helper() { return helper_; }
|
||||
|
||||
/**
|
||||
* @brief Spins until @p done returns true, or the timeout elapses.
|
||||
* @return true if @p done became true
|
||||
*/
|
||||
bool spinUntil(
|
||||
const std::function<bool()> & done,
|
||||
std::chrono::milliseconds timeout = std::chrono::milliseconds(5000))
|
||||
{
|
||||
const std::chrono::steady_clock::time_point deadline =
|
||||
std::chrono::steady_clock::now() + timeout;
|
||||
while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline)
|
||||
{
|
||||
if(done())
|
||||
{
|
||||
return true;
|
||||
}
|
||||
executor_->spin_once(std::chrono::milliseconds(10));
|
||||
}
|
||||
return done();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Runs every node of the fixture on a multi-threaded executor until @p done.
|
||||
*
|
||||
* A node whose callback waits on another of its own callbacks -- a service call made
|
||||
* from a timer, say -- makes no progress under spinUntil(), because the second
|
||||
* callback cannot run while the first is still on the stack. Such nodes put the two
|
||||
* callbacks in different callback groups precisely so a multi-threaded executor can
|
||||
* overlap them; this hands them the threads to do it, then puts the nodes back on the
|
||||
* usual single-threaded executor.
|
||||
*
|
||||
* @warning Callbacks run on executor threads for the duration, so do not have any
|
||||
* Collector subscribed while this runs: the test thread would read its
|
||||
* messages while another thread appends to them. Use it to get a node
|
||||
* through its start-up handshake, before subscribing to anything.
|
||||
*/
|
||||
bool spinMultiThreadedUntil(
|
||||
const std::function<bool()> & done,
|
||||
std::chrono::milliseconds timeout = std::chrono::milliseconds(15000))
|
||||
{
|
||||
rclcpp::executors::MultiThreadedExecutor booting(rclcpp::ExecutorOptions(), 4);
|
||||
for(const rclcpp::Node::SharedPtr & node : nodes_)
|
||||
{
|
||||
executor_->remove_node(node);
|
||||
booting.add_node(node);
|
||||
}
|
||||
executor_->remove_node(helper_);
|
||||
booting.add_node(helper_);
|
||||
|
||||
std::thread spinner([&booting]() { booting.spin(); });
|
||||
const std::chrono::steady_clock::time_point deadline =
|
||||
std::chrono::steady_clock::now() + timeout;
|
||||
while(rclcpp::ok() && !done() && std::chrono::steady_clock::now() < deadline)
|
||||
{
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
||||
}
|
||||
const bool result = done();
|
||||
booting.cancel();
|
||||
spinner.join();
|
||||
|
||||
booting.remove_node(helper_);
|
||||
executor_->add_node(helper_);
|
||||
for(const rclcpp::Node::SharedPtr & node : nodes_)
|
||||
{
|
||||
booting.remove_node(node);
|
||||
executor_->add_node(node);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/// Spins for a fixed duration, for the "nothing should happen" assertions.
|
||||
void spinFor(std::chrono::milliseconds duration)
|
||||
{
|
||||
const std::chrono::steady_clock::time_point deadline =
|
||||
std::chrono::steady_clock::now() + duration;
|
||||
while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline)
|
||||
{
|
||||
executor_->spin_once(std::chrono::milliseconds(10));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Waits until @p publisher has at least @p count matched subscriptions.
|
||||
*
|
||||
* Publishing before the node under test has discovered the topic silently drops the
|
||||
* message, which is the most common cause of a flaky in-process node test.
|
||||
*/
|
||||
template <typename PublisherT>
|
||||
bool waitForSubscriber(const PublisherT & publisher, size_t count = 1)
|
||||
{
|
||||
return spinUntil([&]() { return publisher->get_subscription_count() >= count; });
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Waits until @p subscription sees at least one publisher.
|
||||
*
|
||||
* Several nodes only publish when they have subscribers, so the test's subscription
|
||||
* has to be discovered before the input is sent.
|
||||
*/
|
||||
template <typename SubscriptionT>
|
||||
bool waitForPublisher(const SubscriptionT & subscription, size_t count = 1)
|
||||
{
|
||||
return spinUntil([&]() { return subscription->get_publisher_count() >= count; });
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Publishes a static transform on /tf_static.
|
||||
*
|
||||
* /tf_static is transient-local, so a listener that subscribes later still receives
|
||||
* it. That makes static frames far less timing-sensitive in tests than /tf.
|
||||
*/
|
||||
void publishStaticTf(
|
||||
const std::string & parent, const std::string & child,
|
||||
double x = 0.0, double y = 0.0, double z = 0.0)
|
||||
{
|
||||
if(!staticTfPublisher_)
|
||||
{
|
||||
staticTfPublisher_ = helper_->create_publisher<tf2_msgs::msg::TFMessage>(
|
||||
"/tf_static", rclcpp::QoS(100).transient_local());
|
||||
}
|
||||
geometry_msgs::msg::TransformStamped t;
|
||||
t.header.stamp = helper_->now();
|
||||
t.header.frame_id = parent;
|
||||
t.child_frame_id = child;
|
||||
t.transform.translation.x = x;
|
||||
t.transform.translation.y = y;
|
||||
t.transform.translation.z = z;
|
||||
t.transform.rotation.w = 1.0;
|
||||
tf2_msgs::msg::TFMessage msg;
|
||||
msg.transforms.push_back(t);
|
||||
staticTfPublisher_->publish(msg);
|
||||
spinFor(std::chrono::milliseconds(100));
|
||||
}
|
||||
|
||||
/// Publishes a static transform with a rotation, given as roll/pitch/yaw.
|
||||
void publishStaticTfRPY(
|
||||
const std::string & parent, const std::string & child,
|
||||
double roll, double pitch, double yaw,
|
||||
double x = 0.0, double y = 0.0, double z = 0.0)
|
||||
{
|
||||
if(!staticTfPublisher_)
|
||||
{
|
||||
staticTfPublisher_ = helper_->create_publisher<tf2_msgs::msg::TFMessage>(
|
||||
"/tf_static", rclcpp::QoS(100).transient_local());
|
||||
}
|
||||
tf2::Quaternion q;
|
||||
q.setRPY(roll, pitch, yaw);
|
||||
geometry_msgs::msg::TransformStamped t;
|
||||
t.header.stamp = helper_->now();
|
||||
t.header.frame_id = parent;
|
||||
t.child_frame_id = child;
|
||||
t.transform.translation.x = x;
|
||||
t.transform.translation.y = y;
|
||||
t.transform.translation.z = z;
|
||||
t.transform.rotation = tf2::toMsg(q);
|
||||
tf2_msgs::msg::TFMessage msg;
|
||||
msg.transforms.push_back(t);
|
||||
staticTfPublisher_->publish(msg);
|
||||
spinFor(std::chrono::milliseconds(100));
|
||||
}
|
||||
|
||||
/// Collects every message received on @p topic, for later assertions.
|
||||
template <typename MsgT>
|
||||
struct Collector
|
||||
{
|
||||
typename rclcpp::Subscription<MsgT>::SharedPtr subscription;
|
||||
std::vector<typename MsgT::ConstSharedPtr> messages;
|
||||
size_t size() const { return messages.size(); }
|
||||
bool empty() const { return messages.empty(); }
|
||||
const MsgT & back() const { return *messages.back(); }
|
||||
const MsgT & front() const { return *messages.front(); }
|
||||
};
|
||||
|
||||
/// Subscribes the helper node to @p topic and records everything it receives.
|
||||
template <typename MsgT>
|
||||
std::shared_ptr<Collector<MsgT>> collect(
|
||||
const std::string & topic, const rclcpp::QoS & qos = rclcpp::QoS(10))
|
||||
{
|
||||
std::shared_ptr<Collector<MsgT>> collector = std::make_shared<Collector<MsgT>>();
|
||||
collector->subscription = helper_->create_subscription<MsgT>(
|
||||
topic, qos,
|
||||
[collector](const typename MsgT::ConstSharedPtr msg) {
|
||||
collector->messages.push_back(msg);
|
||||
});
|
||||
return collector;
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
|
||||
rclcpp::Node::SharedPtr helper_;
|
||||
std::vector<rclcpp::Node::SharedPtr> nodes_;
|
||||
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr staticTfPublisher_;
|
||||
};
|
||||
|
||||
} // namespace rtabmap_util_test
|
||||
|
||||
#endif /* RTABMAP_UTIL_NODE_TEST_UTILS_HPP_ */
|
||||
@@ -0,0 +1,809 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include "node_test_utils.hpp"
|
||||
#include "db_builders.hpp"
|
||||
|
||||
#include <rtabmap_util/db_player.hpp>
|
||||
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
|
||||
#include <rosgraph_msgs/msg/clock.hpp>
|
||||
#include <tf2_msgs/msg/tf_message.hpp>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
using namespace rtabmap_util_test;
|
||||
|
||||
namespace {
|
||||
::testing::Environment * const kEnv = registerRclcppEnvironment();
|
||||
|
||||
/// Replay at 1000x the recorded stamps: db_player sleeps between frames otherwise.
|
||||
constexpr double kReplayRate = 1000.0;
|
||||
|
||||
bool hasParameter(const std::vector<rclcpp::Parameter> & overrides, const std::string & name)
|
||||
{
|
||||
for(size_t i=0; i<overrides.size(); ++i)
|
||||
{
|
||||
if(overrides[i].get_name() == name) { return true; }
|
||||
}
|
||||
return false;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
/**
|
||||
* db_player is driven by its own loop in DbPlayerNode, so the tests call
|
||||
* publishNextFrame() directly instead of waiting on a timer.
|
||||
*
|
||||
* Two things shape every test below. The publishers do not exist until the first frame
|
||||
* has been read -- db_player decides which topics to create from the payloads it finds in
|
||||
* the database -- and everything except /tf is only published when someone is subscribed.
|
||||
* So the sequence is always: replay one frame to create the publishers, subscribe, then
|
||||
* replay again to get the data.
|
||||
*/
|
||||
class DbPlayerTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
void start(const std::string & databasePath, std::vector<rclcpp::Parameter> overrides = {})
|
||||
{
|
||||
if(!hasParameter(overrides, "database"))
|
||||
{
|
||||
overrides.push_back(rclcpp::Parameter("database", databasePath));
|
||||
}
|
||||
if(!hasParameter(overrides, "rate"))
|
||||
{
|
||||
overrides.push_back(rclcpp::Parameter("rate", kReplayRate));
|
||||
}
|
||||
player_ = addNode(std::make_shared<rtabmap_util::DbPlayer>(
|
||||
rclcpp::NodeOptions().parameter_overrides(overrides)));
|
||||
}
|
||||
|
||||
/// Reads one frame, which is what creates the publishers.
|
||||
void primePublishers()
|
||||
{
|
||||
ASSERT_TRUE(player_->publishNextFrame()) << "the database has no readable frame";
|
||||
}
|
||||
|
||||
/// Replays frames until @p done, or the database runs out.
|
||||
bool replayUntil(const std::function<bool()> & done)
|
||||
{
|
||||
for(int i=0; i<kDbFrames && !done(); ++i)
|
||||
{
|
||||
if(!player_->publishNextFrame()) { break; }
|
||||
spinFor(std::chrono::milliseconds(30));
|
||||
}
|
||||
return done();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief The database node a replayed message came from, recovered from its stamp.
|
||||
*
|
||||
* How many frames a test ends up replaying depends on discovery, so the expected
|
||||
* pose is derived from the stamp the message itself carries rather than assumed.
|
||||
* That also checks the stamp really comes from the database.
|
||||
*/
|
||||
static int nodeIdOf(const builtin_interfaces::msg::Time & stamp)
|
||||
{
|
||||
const double seconds = rtabmap_conversions::timestampFromROS(stamp);
|
||||
return int(std::round((seconds - kFirstStamp) / kStampStep)) + 1;
|
||||
}
|
||||
|
||||
/// The most recent transform published for @p parent -> @p child.
|
||||
static bool findTransform(
|
||||
const Collector<tf2_msgs::msg::TFMessage> & tf,
|
||||
const std::string & parent, const std::string & child,
|
||||
geometry_msgs::msg::TransformStamped & out)
|
||||
{
|
||||
bool found = false;
|
||||
for(size_t i=0; i<tf.messages.size(); ++i)
|
||||
{
|
||||
for(size_t j=0; j<tf.messages[i]->transforms.size(); ++j)
|
||||
{
|
||||
const geometry_msgs::msg::TransformStamped & t = tf.messages[i]->transforms[j];
|
||||
if(t.header.frame_id == parent && t.child_frame_id == child)
|
||||
{
|
||||
out = t;
|
||||
found = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
return found;
|
||||
}
|
||||
|
||||
static rtabmap::Transform toRtabmap(const geometry_msgs::msg::TransformStamped & t)
|
||||
{
|
||||
return rtabmap_conversions::transformFromGeometryMsg(t.transform);
|
||||
}
|
||||
|
||||
/// Subscribes to /tf and waits for db_player's broadcaster to be discovered.
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> collectTf()
|
||||
{
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
|
||||
collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
|
||||
EXPECT_TRUE(waitForPublisher(tf->subscription));
|
||||
return tf;
|
||||
}
|
||||
|
||||
std::shared_ptr<rtabmap_util::DbPlayer> player_;
|
||||
};
|
||||
|
||||
//============================================================================
|
||||
// RGB-D
|
||||
//============================================================================
|
||||
|
||||
TEST_F(DbPlayerTest, ReplaysRgbAndDepthImages)
|
||||
{
|
||||
TempDatabase db("rgbd");
|
||||
writeRgbdDatabase(db.path());
|
||||
start(db.path());
|
||||
primePublishers();
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
|
||||
collect<sensor_msgs::msg::Image>("rgb/image");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
|
||||
collect<sensor_msgs::msg::Image>("depth/image");
|
||||
ASSERT_TRUE(waitForPublisher(rgb->subscription));
|
||||
ASSERT_TRUE(waitForPublisher(depth->subscription));
|
||||
|
||||
ASSERT_TRUE(replayUntil([&]() { return !rgb->empty() && !depth->empty(); }))
|
||||
<< "no image replayed";
|
||||
|
||||
EXPECT_EQ(rgb->back().encoding, sensor_msgs::image_encodings::BGR8);
|
||||
EXPECT_EQ(rgb->back().width, uint32_t(kImageWidth));
|
||||
EXPECT_EQ(rgb->back().height, uint32_t(kImageHeight));
|
||||
EXPECT_EQ(rgb->back().header.frame_id, "camera_optical_link");
|
||||
|
||||
EXPECT_EQ(depth->back().encoding, sensor_msgs::image_encodings::TYPE_16UC1);
|
||||
EXPECT_EQ(depth->back().header.frame_id, "camera_optical_link")
|
||||
<< "depth is registered with the color camera, so it shares its frame";
|
||||
EXPECT_EQ(*reinterpret_cast<const uint16_t *>(depth->back().data.data()), kDepthMillimeters);
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, StampsImagesWithTheDatabaseStamps)
|
||||
{
|
||||
TempDatabase db("stamps");
|
||||
writeRgbdDatabase(db.path());
|
||||
start(db.path());
|
||||
primePublishers();
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
|
||||
collect<sensor_msgs::msg::Image>("rgb/image");
|
||||
ASSERT_TRUE(waitForPublisher(rgb->subscription));
|
||||
ASSERT_TRUE(replayUntil([&]() { return !rgb->empty(); }));
|
||||
|
||||
const int id = nodeIdOf(rgb->front().header.stamp);
|
||||
EXPECT_GE(id, 2) << "the first frame only creates the publishers";
|
||||
EXPECT_LE(id, kDbFrames);
|
||||
EXPECT_NEAR(rtabmap_conversions::timestampFromROS(rgb->front().header.stamp),
|
||||
stampOfNode(id), 1e-6);
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, ReplaysCameraCalibration)
|
||||
{
|
||||
TempDatabase db("caminfo");
|
||||
writeRgbdDatabase(db.path());
|
||||
start(db.path());
|
||||
primePublishers();
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
|
||||
collect<sensor_msgs::msg::Image>("rgb/image");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> rgbInfo =
|
||||
collect<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> depthInfo =
|
||||
collect<sensor_msgs::msg::CameraInfo>("depth/camera_info");
|
||||
ASSERT_TRUE(waitForPublisher(rgb->subscription));
|
||||
ASSERT_TRUE(waitForPublisher(rgbInfo->subscription));
|
||||
ASSERT_TRUE(replayUntil([&]() { return !rgbInfo->empty() && !depthInfo->empty(); }));
|
||||
|
||||
EXPECT_EQ(rgbInfo->back().width, uint32_t(kImageWidth));
|
||||
EXPECT_EQ(rgbInfo->back().height, uint32_t(kImageHeight));
|
||||
EXPECT_NEAR(rgbInfo->back().k[0], kFx, 1e-6);
|
||||
EXPECT_NEAR(rgbInfo->back().k[2], kCx, 1e-6);
|
||||
EXPECT_NEAR(rgbInfo->back().k[4], kFy, 1e-6);
|
||||
EXPECT_NEAR(rgbInfo->back().k[5], kCy, 1e-6);
|
||||
EXPECT_EQ(rgbInfo->back().header.frame_id, "camera_optical_link");
|
||||
|
||||
EXPECT_NEAR(depthInfo->back().k[0], kFx, 1e-6)
|
||||
<< "the depth camera info repeats the color calibration";
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, ReplaysImageWithoutCalibrationOnImageTopic)
|
||||
{
|
||||
// A database with no calibration at all is still replayable, on "image".
|
||||
TempDatabase db("imageonly");
|
||||
writeImageOnlyDatabase(db.path());
|
||||
start(db.path());
|
||||
primePublishers();
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> image =
|
||||
collect<sensor_msgs::msg::Image>("image");
|
||||
ASSERT_TRUE(waitForPublisher(image->subscription));
|
||||
ASSERT_TRUE(replayUntil([&]() { return !image->empty(); }));
|
||||
|
||||
EXPECT_EQ(image->back().encoding, sensor_msgs::image_encodings::BGR8);
|
||||
EXPECT_EQ(image->back().width, uint32_t(kImageWidth));
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// Stereo
|
||||
//============================================================================
|
||||
|
||||
TEST_F(DbPlayerTest, ReplaysStereoPairAndCalibration)
|
||||
{
|
||||
TempDatabase db("stereo");
|
||||
writeStereoDatabase(db.path());
|
||||
start(db.path());
|
||||
primePublishers();
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> left =
|
||||
collect<sensor_msgs::msg::Image>("left/image");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> right =
|
||||
collect<sensor_msgs::msg::Image>("right/image");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> leftInfo =
|
||||
collect<sensor_msgs::msg::CameraInfo>("left/camera_info");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> rightInfo =
|
||||
collect<sensor_msgs::msg::CameraInfo>("right/camera_info");
|
||||
ASSERT_TRUE(waitForPublisher(left->subscription));
|
||||
ASSERT_TRUE(waitForPublisher(right->subscription));
|
||||
ASSERT_TRUE(replayUntil([&]() {
|
||||
return !left->empty() && !right->empty() &&
|
||||
!leftInfo->empty() && !rightInfo->empty(); }));
|
||||
|
||||
EXPECT_EQ(left->back().encoding, sensor_msgs::image_encodings::MONO8);
|
||||
EXPECT_EQ(left->back().header.frame_id, "left_camera_optical_link");
|
||||
EXPECT_EQ(right->back().encoding, sensor_msgs::image_encodings::MONO8);
|
||||
EXPECT_EQ(right->back().header.frame_id, "right_camera_optical_link");
|
||||
|
||||
// Both cameras share the intrinsics of a rectified pair and are stamped with the
|
||||
// frame of the image they belong to.
|
||||
EXPECT_EQ(leftInfo->back().width, uint32_t(kImageWidth));
|
||||
EXPECT_EQ(leftInfo->back().height, uint32_t(kImageHeight));
|
||||
EXPECT_NEAR(leftInfo->back().k[0], kFx, 1e-6);
|
||||
EXPECT_NEAR(leftInfo->back().k[2], kCx, 1e-6);
|
||||
EXPECT_NEAR(leftInfo->back().k[4], kFy, 1e-6);
|
||||
EXPECT_NEAR(leftInfo->back().k[5], kCy, 1e-6);
|
||||
EXPECT_EQ(leftInfo->back().header.frame_id, "left_camera_optical_link");
|
||||
EXPECT_NEAR(rightInfo->back().k[0], kFx, 1e-6);
|
||||
EXPECT_EQ(rightInfo->back().header.frame_id, "right_camera_optical_link");
|
||||
|
||||
// Only the right camera carries the baseline: it is P(0,3) = -fx*baseline, and the
|
||||
// left camera of a rectified pair sits at the origin of the stereo frame.
|
||||
EXPECT_NEAR(leftInfo->back().p[3], 0.0, 1e-6);
|
||||
EXPECT_NEAR(rightInfo->back().p[3], -kFx*kBaseline, 1e-6);
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// Laser scans
|
||||
//============================================================================
|
||||
|
||||
TEST_F(DbPlayerTest, Replays2dLaserScan)
|
||||
{
|
||||
TempDatabase db("scan2d");
|
||||
writeScan2dDatabase(db.path());
|
||||
start(db.path());
|
||||
primePublishers();
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::LaserScan>> scan =
|
||||
collect<sensor_msgs::msg::LaserScan>("scan");
|
||||
ASSERT_TRUE(waitForPublisher(scan->subscription));
|
||||
ASSERT_TRUE(replayUntil([&]() { return !scan->empty(); })) << "no scan replayed";
|
||||
|
||||
const sensor_msgs::msg::LaserScan & msg = scan->back();
|
||||
EXPECT_EQ(msg.header.frame_id, "base_laser_link");
|
||||
|
||||
// The scan carries its own angles, so the scan_angle_* parameters are not used.
|
||||
EXPECT_NEAR(msg.angle_min, kScanAngleMin, 1e-6);
|
||||
EXPECT_NEAR(msg.angle_max, kScanAngleMax, 1e-6);
|
||||
EXPECT_NEAR(msg.angle_increment, kScanAngleIncrement, 1e-6);
|
||||
EXPECT_NEAR(msg.range_min, kScanRangeMin, 1e-6);
|
||||
EXPECT_NEAR(msg.range_max, kScanRangeMax, 1e-6);
|
||||
|
||||
// db_player re-bins the cartesian points, so every bin must come back at its range.
|
||||
ASSERT_EQ(msg.ranges.size(), size_t(kScanBins));
|
||||
for(int bin=0; bin<kScanBins; ++bin)
|
||||
{
|
||||
EXPECT_NEAR(msg.ranges[bin], scanRangeOf(bin), 1e-3) << "bin " << bin;
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, A2dScanNeverAdvertisesScanCloud)
|
||||
{
|
||||
// initializePublishers() runs on every frame, so the publisher it creates has to
|
||||
// match the scan being replayed. It used to create whichever one did not exist yet,
|
||||
// which advertised an empty "scan_cloud" from the second frame of a 2D database on.
|
||||
TempDatabase db("scan2donly");
|
||||
writeScan2dDatabase(db.path());
|
||||
start(db.path());
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::LaserScan>> scan =
|
||||
collect<sensor_msgs::msg::LaserScan>("scan");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
|
||||
collect<sensor_msgs::msg::PointCloud2>("scan_cloud");
|
||||
|
||||
while(player_->publishNextFrame()) { spinFor(std::chrono::milliseconds(30)); }
|
||||
|
||||
EXPECT_FALSE(scan->empty()) << "the 2D scan must still be replayed";
|
||||
EXPECT_EQ(cloud->subscription->get_publisher_count(), 0u)
|
||||
<< "a 2D database must not advertise scan_cloud";
|
||||
EXPECT_TRUE(cloud->empty());
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, A3dScanNeverAdvertisesScan)
|
||||
{
|
||||
TempDatabase db("scan3donly");
|
||||
writeScan3dDatabase(db.path());
|
||||
start(db.path());
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::LaserScan>> scan =
|
||||
collect<sensor_msgs::msg::LaserScan>("scan");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
|
||||
collect<sensor_msgs::msg::PointCloud2>("scan_cloud");
|
||||
|
||||
while(player_->publishNextFrame()) { spinFor(std::chrono::milliseconds(30)); }
|
||||
|
||||
EXPECT_FALSE(cloud->empty()) << "the 3D scan must still be replayed";
|
||||
EXPECT_EQ(scan->subscription->get_publisher_count(), 0u)
|
||||
<< "a 3D database must not advertise scan";
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, UsesScanParametersWhenTheScanHasNoAngles)
|
||||
{
|
||||
// A scan saved without angle metadata falls back to the scan_angle_*/scan_range_*
|
||||
// parameters, which is how a database recorded from a 3D lidar can be replayed as 2D.
|
||||
const double angleMin = -0.5;
|
||||
const double angleIncrement = 0.05;
|
||||
const int targetBin = 10;
|
||||
// The center of the target bin: db_player truncates (angle-angle_min)/increment, so a
|
||||
// bearing on a bin boundary would land on either side depending on the rounding.
|
||||
const float bearing = float(angleMin + (double(targetBin) + 0.5) * angleIncrement);
|
||||
const float nearest = 1.0f;
|
||||
|
||||
TempDatabase db("scan2dnoangles");
|
||||
writeDatabase(db.path(), kDbFrames, [bearing, nearest](int id, double stamp) {
|
||||
cv::Mat points(1, kScanBins, CV_32FC2);
|
||||
for(int bin=0; bin<kScanBins; ++bin)
|
||||
{
|
||||
// All at the same bearing, at increasing ranges: db_player keeps the nearest.
|
||||
const float range = nearest + 0.1f * float(bin);
|
||||
points.at<cv::Vec2f>(0, bin) =
|
||||
cv::Vec2f(range * std::cos(bearing), range * std::sin(bearing));
|
||||
}
|
||||
rtabmap::SensorData data;
|
||||
data.setId(id);
|
||||
data.setStamp(stamp);
|
||||
data.setLaserScan(rtabmap::LaserScan(rtabmap::compressData2(points),
|
||||
/*maxPoints=*/0, /*maxRange=*/0.0f, rtabmap::LaserScan::kXY,
|
||||
scanLocalTransform()));
|
||||
return data;
|
||||
});
|
||||
start(db.path(), {rclcpp::Parameter("scan_angle_min", angleMin),
|
||||
rclcpp::Parameter("scan_angle_max", 0.5),
|
||||
rclcpp::Parameter("scan_angle_increment", angleIncrement),
|
||||
rclcpp::Parameter("scan_range_min", 0.2),
|
||||
rclcpp::Parameter("scan_range_max", 20.0)});
|
||||
primePublishers();
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::LaserScan>> scan =
|
||||
collect<sensor_msgs::msg::LaserScan>("scan");
|
||||
ASSERT_TRUE(waitForPublisher(scan->subscription));
|
||||
ASSERT_TRUE(replayUntil([&]() { return !scan->empty(); }));
|
||||
|
||||
const sensor_msgs::msg::LaserScan & msg = scan->back();
|
||||
EXPECT_NEAR(msg.angle_min, angleMin, 1e-6);
|
||||
EXPECT_NEAR(msg.angle_max, 0.5, 1e-6);
|
||||
EXPECT_NEAR(msg.angle_increment, angleIncrement, 1e-6);
|
||||
EXPECT_NEAR(msg.range_min, 0.2, 1e-6);
|
||||
EXPECT_NEAR(msg.range_max, 20.0, 1e-6);
|
||||
ASSERT_EQ(msg.ranges.size(), 20u) << "ceil((0.5 - -0.5)/0.05)";
|
||||
|
||||
EXPECT_NEAR(msg.ranges[targetBin], nearest, 1e-3)
|
||||
<< "every point shares a bearing, so only its bin is filled, at the nearest range";
|
||||
for(size_t bin=0; bin<msg.ranges.size(); ++bin)
|
||||
{
|
||||
if(int(bin) != targetBin)
|
||||
{
|
||||
EXPECT_FLOAT_EQ(msg.ranges[bin], 0.0f) << "bin " << bin << " should be empty";
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, Replays3dScanAsPointCloud)
|
||||
{
|
||||
TempDatabase db("scan3d");
|
||||
writeScan3dDatabase(db.path());
|
||||
start(db.path());
|
||||
primePublishers();
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
|
||||
collect<sensor_msgs::msg::PointCloud2>("scan_cloud");
|
||||
ASSERT_TRUE(waitForPublisher(cloud->subscription));
|
||||
ASSERT_TRUE(replayUntil([&]() { return !cloud->empty(); })) << "no cloud replayed";
|
||||
|
||||
EXPECT_EQ(cloud->back().header.frame_id, "base_laser_link");
|
||||
EXPECT_EQ(cloud->back().width * cloud->back().height, uint32_t(kScanCloudPoints));
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// Odometry
|
||||
//============================================================================
|
||||
|
||||
TEST_F(DbPlayerTest, ReplaysOdometryWithItsCovariance)
|
||||
{
|
||||
TempDatabase db("odom");
|
||||
writeRgbdDatabase(db.path());
|
||||
start(db.path());
|
||||
primePublishers();
|
||||
|
||||
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
|
||||
collect<nav_msgs::msg::Odometry>("odom");
|
||||
ASSERT_TRUE(waitForPublisher(odom->subscription));
|
||||
ASSERT_TRUE(replayUntil([&]() { return !odom->empty(); })) << "no odometry replayed";
|
||||
|
||||
const nav_msgs::msg::Odometry & msg = odom->back();
|
||||
EXPECT_EQ(msg.header.frame_id, "odom");
|
||||
EXPECT_EQ(msg.child_frame_id, "base_link");
|
||||
|
||||
const int id = nodeIdOf(msg.header.stamp);
|
||||
ASSERT_GE(id, 1);
|
||||
ASSERT_LE(id, kDbFrames);
|
||||
EXPECT_NEAR(msg.pose.pose.position.x, poseOf(id).x(), 1e-5)
|
||||
<< "the pose must be the one recorded for node " << id;
|
||||
EXPECT_NEAR(msg.pose.pose.position.y, 0.0, 1e-5);
|
||||
|
||||
// The covariance is the inverse of the neighbor link's information matrix.
|
||||
EXPECT_NEAR(msg.pose.covariance[0], kOdomVariance, 1e-6);
|
||||
EXPECT_NEAR(msg.pose.covariance[35], kOdomVariance, 1e-6);
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, IgnoreOdomDropsTheOdometry)
|
||||
{
|
||||
TempDatabase db("ignoreodom");
|
||||
writeRgbdDatabase(db.path());
|
||||
start(db.path(), {rclcpp::Parameter("ignore_odom", true)});
|
||||
primePublishers();
|
||||
|
||||
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
|
||||
collect<nav_msgs::msg::Odometry>("odom");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
|
||||
collect<sensor_msgs::msg::Image>("rgb/image");
|
||||
ASSERT_TRUE(waitForPublisher(rgb->subscription));
|
||||
ASSERT_TRUE(replayUntil([&]() { return !rgb->empty(); }))
|
||||
<< "the images must still be replayed";
|
||||
|
||||
EXPECT_EQ(odom->subscription->get_publisher_count(), 0u)
|
||||
<< "with no odometry in the stream the topic is never even created";
|
||||
EXPECT_TRUE(odom->empty());
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// Transforms
|
||||
//============================================================================
|
||||
|
||||
TEST_F(DbPlayerTest, BroadcastsOdometryAndCameraTransforms)
|
||||
{
|
||||
TempDatabase db("tf");
|
||||
writeRgbdDatabase(db.path());
|
||||
start(db.path());
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf = collectTf();
|
||||
|
||||
// TF is not gated on subscribers, so the very first frame already broadcasts.
|
||||
ASSERT_TRUE(replayUntil([&]() { return !tf->empty(); })) << "nothing broadcast on /tf";
|
||||
|
||||
geometry_msgs::msg::TransformStamped odomToBase;
|
||||
ASSERT_TRUE(findTransform(*tf, "odom", "base_link", odomToBase));
|
||||
const int id = nodeIdOf(odomToBase.header.stamp);
|
||||
ASSERT_GE(id, 1);
|
||||
ASSERT_LE(id, kDbFrames);
|
||||
EXPECT_NEAR(odomToBase.transform.translation.x, poseOf(id).x(), 1e-5);
|
||||
|
||||
geometry_msgs::msg::TransformStamped baseToCamera;
|
||||
ASSERT_TRUE(findTransform(*tf, "base_link", "camera_optical_link", baseToCamera));
|
||||
EXPECT_LT(toRtabmap(baseToCamera).getDistance(cameraLocalTransform()), 1e-4f)
|
||||
<< "the camera transform is the model's local transform: "
|
||||
<< toRtabmap(baseToCamera).prettyPrint();
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, BroadcastsStereoTransformsShiftedByTheBaseline)
|
||||
{
|
||||
TempDatabase db("stereotf");
|
||||
writeStereoDatabase(db.path());
|
||||
start(db.path());
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf = collectTf();
|
||||
ASSERT_TRUE(replayUntil([&]() { return !tf->empty(); }));
|
||||
|
||||
geometry_msgs::msg::TransformStamped baseToLeft, baseToRight;
|
||||
ASSERT_TRUE(findTransform(*tf, "base_link", "left_camera_optical_link", baseToLeft));
|
||||
ASSERT_TRUE(findTransform(*tf, "base_link", "right_camera_optical_link", baseToRight));
|
||||
|
||||
EXPECT_LT(toRtabmap(baseToLeft).getDistance(cameraLocalTransform()), 1e-4f);
|
||||
|
||||
// The right camera carries the baseline in Tx, which db_player turns back into a
|
||||
// translation along the optical x axis so the frame sits next to the left one.
|
||||
const rtabmap::Transform expectedRight =
|
||||
cameraLocalTransform() * rtabmap::Transform(kBaseline, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
|
||||
EXPECT_LT(toRtabmap(baseToRight).getDistance(expectedRight), 1e-4f)
|
||||
<< toRtabmap(baseToRight).prettyPrint();
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, BroadcastsTheLaserTransform)
|
||||
{
|
||||
TempDatabase db("scantf");
|
||||
writeScan3dDatabase(db.path());
|
||||
start(db.path());
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf = collectTf();
|
||||
ASSERT_TRUE(replayUntil([&]() { return !tf->empty(); }));
|
||||
|
||||
geometry_msgs::msg::TransformStamped baseToLaser;
|
||||
ASSERT_TRUE(findTransform(*tf, "base_link", "base_laser_link", baseToLaser));
|
||||
EXPECT_LT(toRtabmap(baseToLaser).getDistance(scanLocalTransform()), 1e-4f)
|
||||
<< toRtabmap(baseToLaser).prettyPrint();
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, BroadcastsGroundTruthAndImuTransforms)
|
||||
{
|
||||
TempDatabase db("richtf");
|
||||
writeRichDatabase(db.path());
|
||||
start(db.path());
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf = collectTf();
|
||||
ASSERT_TRUE(replayUntil([&]() { return !tf->empty(); }));
|
||||
|
||||
geometry_msgs::msg::TransformStamped worldToGt;
|
||||
ASSERT_TRUE(findTransform(*tf, "world", "base_link_gt", worldToGt));
|
||||
const int id = nodeIdOf(worldToGt.header.stamp);
|
||||
ASSERT_GE(id, 1);
|
||||
ASSERT_LE(id, kDbFrames);
|
||||
EXPECT_LT(toRtabmap(worldToGt).getDistance(groundTruthOf(id)), 1e-4f)
|
||||
<< "the ground truth is published apart from the odometry";
|
||||
|
||||
geometry_msgs::msg::TransformStamped baseToImu;
|
||||
ASSERT_TRUE(findTransform(*tf, "base_link", "imu_link", baseToImu));
|
||||
EXPECT_TRUE(toRtabmap(baseToImu).isIdentity())
|
||||
<< "a gravity link is already expressed in the base frame";
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, RenamesFramesFromParameters)
|
||||
{
|
||||
TempDatabase db("frames");
|
||||
writeRgbdDatabase(db.path());
|
||||
start(db.path(), {rclcpp::Parameter("frame_id", std::string("robot")),
|
||||
rclcpp::Parameter("odom_frame_id", std::string("world_odom")),
|
||||
rclcpp::Parameter("camera_frame_id", std::string("optical"))});
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf = collectTf();
|
||||
ASSERT_TRUE(replayUntil([&]() { return !tf->empty(); }));
|
||||
|
||||
geometry_msgs::msg::TransformStamped t;
|
||||
EXPECT_TRUE(findTransform(*tf, "world_odom", "robot", t));
|
||||
EXPECT_TRUE(findTransform(*tf, "robot", "optical", t));
|
||||
EXPECT_FALSE(findTransform(*tf, "odom", "base_link", t)) << "the defaults must be gone";
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, PublishTfFalseBroadcastsNothing)
|
||||
{
|
||||
TempDatabase db("notf");
|
||||
writeRgbdDatabase(db.path());
|
||||
start(db.path(), {rclcpp::Parameter("publish_tf", false)});
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
|
||||
collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
|
||||
|
||||
ASSERT_TRUE(player_->publishNextFrame());
|
||||
ASSERT_TRUE(player_->publishNextFrame());
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
|
||||
EXPECT_TRUE(tf->empty()) << "publish_tf:=false must not create the broadcaster";
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// The optional channels
|
||||
//============================================================================
|
||||
|
||||
TEST_F(DbPlayerTest, ReplaysGlobalPose)
|
||||
{
|
||||
TempDatabase db("globalpose");
|
||||
writeRichDatabase(db.path());
|
||||
start(db.path());
|
||||
primePublishers();
|
||||
|
||||
std::shared_ptr<Collector<geometry_msgs::msg::PoseWithCovarianceStamped>> pose =
|
||||
collect<geometry_msgs::msg::PoseWithCovarianceStamped>("global_pose");
|
||||
ASSERT_TRUE(waitForPublisher(pose->subscription));
|
||||
ASSERT_TRUE(replayUntil([&]() { return !pose->empty(); })) << "no global pose replayed";
|
||||
|
||||
const int id = nodeIdOf(pose->back().header.stamp);
|
||||
ASSERT_GE(id, 1);
|
||||
ASSERT_LE(id, kDbFrames);
|
||||
EXPECT_EQ(pose->back().header.frame_id, "base_link");
|
||||
EXPECT_NEAR(pose->back().pose.pose.position.y, globalPoseOf(id).y(), 1e-5)
|
||||
<< "the prior pose is offset in y, unlike the odometry";
|
||||
// The prior was saved with an information matrix of 100*I.
|
||||
EXPECT_NEAR(pose->back().pose.covariance[0], 0.01, 1e-6);
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, ReplaysGpsFix)
|
||||
{
|
||||
TempDatabase db("gps");
|
||||
writeRichDatabase(db.path());
|
||||
start(db.path());
|
||||
primePublishers();
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::NavSatFix>> gps =
|
||||
collect<sensor_msgs::msg::NavSatFix>("gps/fix");
|
||||
ASSERT_TRUE(waitForPublisher(gps->subscription));
|
||||
ASSERT_TRUE(replayUntil([&]() { return !gps->empty(); })) << "no GPS replayed";
|
||||
|
||||
const sensor_msgs::msg::NavSatFix & msg = gps->back();
|
||||
EXPECT_NEAR(msg.longitude, kGpsLongitude, 1e-9);
|
||||
EXPECT_NEAR(msg.latitude, kGpsLatitude, 1e-9);
|
||||
EXPECT_NEAR(msg.altitude, kGpsAltitude, 1e-9);
|
||||
EXPECT_EQ(msg.position_covariance_type,
|
||||
uint8_t(sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN));
|
||||
EXPECT_NEAR(msg.position_covariance[0], kGpsError*kGpsError, 1e-9)
|
||||
<< "the reported error is squared into a variance";
|
||||
EXPECT_NEAR(msg.position_covariance[4], kGpsError*kGpsError, 1e-9);
|
||||
EXPECT_NEAR(msg.position_covariance[8], kGpsError*kGpsError, 1e-9);
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, ReplaysImu)
|
||||
{
|
||||
TempDatabase db("imu");
|
||||
writeRichDatabase(db.path());
|
||||
start(db.path());
|
||||
primePublishers();
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Imu>> imu =
|
||||
collect<sensor_msgs::msg::Imu>("imu");
|
||||
ASSERT_TRUE(waitForPublisher(imu->subscription));
|
||||
ASSERT_TRUE(replayUntil([&]() { return !imu->empty(); })) << "no IMU replayed";
|
||||
|
||||
EXPECT_EQ(imu->back().header.frame_id, "imu_link");
|
||||
|
||||
// DBReader rebuilds the IMU from the gravity link, so only the orientation survives.
|
||||
const Eigen::Quaterniond expected = gravityTransform().getQuaterniond();
|
||||
EXPECT_NEAR(std::abs(imu->back().orientation.w), std::abs(expected.w()), 1e-5);
|
||||
EXPECT_NEAR(std::abs(imu->back().orientation.x), std::abs(expected.x()), 1e-5);
|
||||
EXPECT_NEAR(std::abs(imu->back().orientation.y), std::abs(expected.y()), 1e-5);
|
||||
EXPECT_NEAR(std::abs(imu->back().orientation.z), std::abs(expected.z()), 1e-5);
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, ReplaysEnvSensor)
|
||||
{
|
||||
TempDatabase db("envsensor");
|
||||
writeRichDatabase(db.path());
|
||||
start(db.path());
|
||||
primePublishers();
|
||||
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::EnvSensor>> env =
|
||||
collect<rtabmap_msgs::msg::EnvSensor>("env_sensor");
|
||||
ASSERT_TRUE(waitForPublisher(env->subscription));
|
||||
ASSERT_TRUE(replayUntil([&]() { return !env->empty(); })) << "no env sensor replayed";
|
||||
|
||||
EXPECT_EQ(env->back().type, int(rtabmap::EnvSensor::kAmbientTemperature));
|
||||
EXPECT_NEAR(env->back().value, kEnvSensorValue, 1e-9);
|
||||
EXPECT_EQ(env->back().header.frame_id, "base_link");
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, PublishesClockWhenAsked)
|
||||
{
|
||||
TempDatabase db("clock");
|
||||
writeRgbdDatabase(db.path());
|
||||
start(db.path(), {rclcpp::Parameter("publish_clock", true)});
|
||||
std::shared_ptr<Collector<rosgraph_msgs::msg::Clock>> clock =
|
||||
collect<rosgraph_msgs::msg::Clock>("/clock");
|
||||
ASSERT_TRUE(waitForPublisher(clock->subscription));
|
||||
|
||||
// The clock is not gated on subscribers either.
|
||||
ASSERT_TRUE(replayUntil([&]() { return !clock->empty(); })) << "no clock published";
|
||||
|
||||
const int id = nodeIdOf(clock->back().clock);
|
||||
ASSERT_GE(id, 1);
|
||||
ASSERT_LE(id, kDbFrames);
|
||||
EXPECT_NEAR(rtabmap_conversions::timestampFromROS(clock->back().clock),
|
||||
stampOfNode(id), 1e-6) << "the clock follows the database stamps";
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, NoClockByDefault)
|
||||
{
|
||||
TempDatabase db("noclock");
|
||||
writeRgbdDatabase(db.path());
|
||||
start(db.path());
|
||||
std::shared_ptr<Collector<rosgraph_msgs::msg::Clock>> clock =
|
||||
collect<rosgraph_msgs::msg::Clock>("/clock");
|
||||
|
||||
ASSERT_TRUE(player_->publishNextFrame());
|
||||
ASSERT_TRUE(player_->publishNextFrame());
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
|
||||
EXPECT_TRUE(clock->empty());
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// Reading the database
|
||||
//============================================================================
|
||||
|
||||
TEST_F(DbPlayerTest, StopsAtTheEndOfTheDatabase)
|
||||
{
|
||||
TempDatabase db("end");
|
||||
writeRgbdDatabase(db.path(), 4);
|
||||
start(db.path());
|
||||
|
||||
int frames = 0;
|
||||
while(player_->publishNextFrame())
|
||||
{
|
||||
++frames;
|
||||
ASSERT_LE(frames, 10) << "publishNextFrame() never reported the end";
|
||||
}
|
||||
EXPECT_EQ(frames, 4) << "every node must be replayed exactly once";
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, StartIdSkipsTheEarlierNodes)
|
||||
{
|
||||
TempDatabase db("startid");
|
||||
writeRgbdDatabase(db.path(), 4);
|
||||
start(db.path(), {rclcpp::Parameter("start_id", 3)});
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf = collectTf();
|
||||
|
||||
int frames = 0;
|
||||
while(player_->publishNextFrame()) { ++frames; }
|
||||
spinFor(std::chrono::milliseconds(200));
|
||||
EXPECT_EQ(frames, 2) << "nodes 3 and 4 only";
|
||||
|
||||
geometry_msgs::msg::TransformStamped t;
|
||||
ASSERT_TRUE(findTransform(*tf, "odom", "base_link", t));
|
||||
EXPECT_EQ(nodeIdOf(tf->front().transforms[0].header.stamp), 3)
|
||||
<< "the replay must start at node 3";
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// Pause / resume
|
||||
//============================================================================
|
||||
|
||||
TEST_F(DbPlayerTest, StartsRunning)
|
||||
{
|
||||
TempDatabase db("pause");
|
||||
writeRgbdDatabase(db.path());
|
||||
start(db.path());
|
||||
EXPECT_FALSE(player_->isPaused());
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, PauseAndResumeServicesTogglePlayback)
|
||||
{
|
||||
TempDatabase db("pausesrv");
|
||||
writeRgbdDatabase(db.path());
|
||||
start(db.path());
|
||||
|
||||
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr pause =
|
||||
helper()->create_client<std_srvs::srv::Empty>("db_player/pause");
|
||||
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr resume =
|
||||
helper()->create_client<std_srvs::srv::Empty>("db_player/resume");
|
||||
ASSERT_TRUE(spinUntil([&]() { return pause->service_is_ready() && resume->service_is_ready(); }))
|
||||
<< "the pause/resume services were never advertised";
|
||||
|
||||
pause->async_send_request(std::make_shared<std_srvs::srv::Empty::Request>());
|
||||
ASSERT_TRUE(spinUntil([&]() { return player_->isPaused(); })) << "pause had no effect";
|
||||
|
||||
resume->async_send_request(std::make_shared<std_srvs::srv::Empty::Request>());
|
||||
ASSERT_TRUE(spinUntil([&]() { return !player_->isPaused(); })) << "resume had no effect";
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// Opening the database
|
||||
//============================================================================
|
||||
|
||||
TEST_F(DbPlayerTest, ThrowsWithoutADatabaseParameter)
|
||||
{
|
||||
// The node used to exit(-1) here, which took down every other node sharing its
|
||||
// component container. Throwing lets the caller decide.
|
||||
EXPECT_THROW(
|
||||
std::make_shared<rtabmap_util::DbPlayer>(rclcpp::NodeOptions()),
|
||||
std::invalid_argument);
|
||||
}
|
||||
|
||||
TEST_F(DbPlayerTest, ThrowsWhenTheDatabaseCannotBeOpened)
|
||||
{
|
||||
TempDatabase db("missing"); // the path is never written
|
||||
EXPECT_THROW(
|
||||
std::make_shared<rtabmap_util::DbPlayer>(rclcpp::NodeOptions().parameter_overrides(
|
||||
{rclcpp::Parameter("database", db.path())})),
|
||||
std::runtime_error);
|
||||
}
|
||||
@@ -0,0 +1,248 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include "node_test_utils.hpp"
|
||||
|
||||
#include <rtabmap_util/disparity_to_depth.hpp>
|
||||
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <sensor_msgs/image_encodings.hpp>
|
||||
#include <stereo_msgs/msg/disparity_image.hpp>
|
||||
#include <rtabmap/utilite/UException.h>
|
||||
|
||||
using namespace rtabmap_util_test;
|
||||
|
||||
namespace {
|
||||
::testing::Environment * const kEnv = registerRclcppEnvironment();
|
||||
|
||||
constexpr float kBaseline = 0.1f; // t, meters
|
||||
constexpr float kFocal = 500.0f; // f, pixels
|
||||
constexpr int kWidth = 4;
|
||||
constexpr int kHeight = 4;
|
||||
|
||||
/// A 4x4 32FC1 disparity image, every pixel set to @p disparity.
|
||||
stereo_msgs::msg::DisparityImage makeDisparity(
|
||||
float disparity,
|
||||
const std::string & encoding = sensor_msgs::image_encodings::TYPE_32FC1)
|
||||
{
|
||||
stereo_msgs::msg::DisparityImage msg;
|
||||
msg.header.frame_id = "camera_link";
|
||||
msg.header.stamp = rclcpp::Time(1000, 0, RCL_ROS_TIME);
|
||||
msg.t = kBaseline;
|
||||
msg.f = kFocal;
|
||||
msg.min_disparity = 1.0f;
|
||||
msg.max_disparity = 100.0f;
|
||||
|
||||
msg.image.header = msg.header;
|
||||
msg.image.encoding = encoding;
|
||||
msg.image.height = kHeight;
|
||||
msg.image.width = kWidth;
|
||||
msg.image.step = kWidth * sizeof(float);
|
||||
msg.image.data.resize(msg.image.step * kHeight);
|
||||
float * p = reinterpret_cast<float *>(msg.image.data.data());
|
||||
for(int i=0; i<kWidth*kHeight; ++i)
|
||||
{
|
||||
p[i] = disparity;
|
||||
}
|
||||
return msg;
|
||||
}
|
||||
|
||||
float pixel32f(const sensor_msgs::msg::Image & img, int row, int col)
|
||||
{
|
||||
return *reinterpret_cast<const float *>(&img.data[row * img.step + col * sizeof(float)]);
|
||||
}
|
||||
|
||||
uint16_t pixel16u(const sensor_msgs::msg::Image & img, int row, int col)
|
||||
{
|
||||
return *reinterpret_cast<const uint16_t *>(&img.data[row * img.step + col * sizeof(uint16_t)]);
|
||||
}
|
||||
} // namespace
|
||||
|
||||
class DisparityToDepthTest : public NodeTest {};
|
||||
|
||||
TEST_F(DisparityToDepthTest, ConvertsDisparityToMetricDepth)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()));
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
|
||||
collect<sensor_msgs::msg::Image>("depth");
|
||||
rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
|
||||
helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(depth->subscription)) << "the node never advertised depth";
|
||||
|
||||
// depth = baseline * focal / disparity = 0.1 * 500 / 10 = 5 m
|
||||
pub->publish(makeDisparity(10.0f));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); }));
|
||||
|
||||
const sensor_msgs::msg::Image & img = depth->back();
|
||||
EXPECT_EQ(img.encoding, sensor_msgs::image_encodings::TYPE_32FC1);
|
||||
EXPECT_EQ(img.width, uint32_t(kWidth));
|
||||
EXPECT_EQ(img.height, uint32_t(kHeight));
|
||||
EXPECT_EQ(img.header.frame_id, "camera_link") << "the input header must be preserved";
|
||||
EXPECT_NEAR(pixel32f(img, 0, 0), 5.0f, 1e-4);
|
||||
EXPECT_NEAR(pixel32f(img, kHeight-1, kWidth-1), 5.0f, 1e-4);
|
||||
}
|
||||
|
||||
TEST_F(DisparityToDepthTest, PublishesMillimetersOnDepthRaw)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()));
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> raw =
|
||||
collect<sensor_msgs::msg::Image>("depth_raw");
|
||||
rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
|
||||
helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(raw->subscription));
|
||||
|
||||
pub->publish(makeDisparity(10.0f));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !raw->empty(); }));
|
||||
|
||||
const sensor_msgs::msg::Image & img = raw->back();
|
||||
EXPECT_EQ(img.encoding, sensor_msgs::image_encodings::TYPE_16UC1);
|
||||
EXPECT_EQ(pixel16u(img, 0, 0), 5000) << "5 m expressed in millimeters";
|
||||
}
|
||||
|
||||
TEST_F(DisparityToDepthTest, PublishesBothUnitsConsistentlyFromOneInput)
|
||||
{
|
||||
// With both topics subscribed the node fills the 32FC1 and 16UC1 images in the same
|
||||
// pass. The two must describe the same depth, one in meters and one in millimeters.
|
||||
addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()));
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> meters =
|
||||
collect<sensor_msgs::msg::Image>("depth");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> millimeters =
|
||||
collect<sensor_msgs::msg::Image>("depth_raw");
|
||||
rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
|
||||
helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(meters->subscription));
|
||||
ASSERT_TRUE(waitForPublisher(millimeters->subscription));
|
||||
|
||||
// A disparity of 25 gives 0.1 * 500 / 25 = 2 m.
|
||||
pub->publish(makeDisparity(25.0f));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !meters->empty() && !millimeters->empty(); }))
|
||||
<< "both outputs must be produced from a single input";
|
||||
|
||||
EXPECT_EQ(meters->back().encoding, sensor_msgs::image_encodings::TYPE_32FC1);
|
||||
EXPECT_EQ(millimeters->back().encoding, sensor_msgs::image_encodings::TYPE_16UC1);
|
||||
|
||||
for(int row=0; row<kHeight; ++row)
|
||||
{
|
||||
for(int col=0; col<kWidth; ++col)
|
||||
{
|
||||
const float m = pixel32f(meters->back(), row, col);
|
||||
const uint16_t mm = pixel16u(millimeters->back(), row, col);
|
||||
EXPECT_NEAR(m, 2.0f, 1e-4) << "at " << row << "," << col;
|
||||
EXPECT_EQ(mm, 2000) << "at " << row << "," << col;
|
||||
EXPECT_EQ(mm, uint16_t(m * 1000.0f)) << "the two units must agree at " << row << "," << col;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(DisparityToDepthTest, LeavesOutOfRangeDisparityAtZero)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()));
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
|
||||
collect<sensor_msgs::msg::Image>("depth");
|
||||
rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
|
||||
helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(depth->subscription));
|
||||
|
||||
// Above max_disparity (100), so no depth can be computed.
|
||||
pub->publish(makeDisparity(500.0f));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); }));
|
||||
|
||||
EXPECT_FLOAT_EQ(pixel32f(depth->back(), 0, 0), 0.0f);
|
||||
}
|
||||
|
||||
TEST_F(DisparityToDepthTest, RejectsNon32FC1Input)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()));
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
|
||||
collect<sensor_msgs::msg::Image>("depth");
|
||||
rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
|
||||
helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(depth->subscription));
|
||||
|
||||
pub->publish(makeDisparity(10.0f, sensor_msgs::image_encodings::TYPE_16UC1));
|
||||
spinFor(std::chrono::milliseconds(400));
|
||||
|
||||
EXPECT_TRUE(depth->empty()) << "only 32FC1 disparity is supported";
|
||||
}
|
||||
|
||||
TEST_F(DisparityToDepthTest, HonorsTheConfiguredQueueDepths)
|
||||
{
|
||||
// Queue depth is not observable from outside, so this pins down that the parameters
|
||||
// are accepted and the node still converts with them set.
|
||||
addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({rclcpp::Parameter("queue_sub", 20),
|
||||
rclcpp::Parameter("queue_pub", 10)})));
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
|
||||
collect<sensor_msgs::msg::Image>("depth");
|
||||
rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
|
||||
helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(depth->subscription));
|
||||
|
||||
pub->publish(makeDisparity(1.0f));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); }));
|
||||
EXPECT_EQ(depth->back().encoding, sensor_msgs::image_encodings::TYPE_32FC1);
|
||||
}
|
||||
|
||||
TEST_F(DisparityToDepthTest, RejectsAZeroQueueDepth)
|
||||
{
|
||||
EXPECT_THROW(
|
||||
addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({rclcpp::Parameter("queue_pub", 0)}))),
|
||||
UException);
|
||||
}
|
||||
|
||||
TEST_F(DisparityToDepthTest, BridgesABestEffortSourceToAReliableConsumer)
|
||||
{
|
||||
// A reliable subscription refuses to match a best-effort publisher, so setting the
|
||||
// two sides apart is what lets the conversion cross that gap.
|
||||
addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({rclcpp::Parameter("qos_sub", 2),
|
||||
rclcpp::Parameter("qos_pub", 1)})));
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
|
||||
collect<sensor_msgs::msg::Image>("depth", rclcpp::QoS(10).reliable());
|
||||
rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
|
||||
helper()->create_publisher<stereo_msgs::msg::DisparityImage>(
|
||||
"disparity", rclcpp::QoS(10).best_effort());
|
||||
ASSERT_TRUE(waitForSubscriber(pub)) << "a best-effort source must reach the node";
|
||||
ASSERT_TRUE(waitForPublisher(depth->subscription))
|
||||
<< "a reliable consumer must be able to subscribe to the depth output";
|
||||
|
||||
pub->publish(makeDisparity(1.0f));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); }));
|
||||
EXPECT_EQ(depth->back().encoding, sensor_msgs::image_encodings::TYPE_32FC1);
|
||||
}
|
||||
|
||||
TEST_F(DisparityToDepthTest, TheTwoQosSidesFallBackToQos)
|
||||
{
|
||||
// Only qos is given, so both sides must be best effort: a reliable consumer matches
|
||||
// neither the publishers nor, from the other end, the subscription.
|
||||
addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({rclcpp::Parameter("qos", 2)})));
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
|
||||
collect<sensor_msgs::msg::Image>("depth", rclcpp::QoS(10).reliable());
|
||||
rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
|
||||
helper()->create_publisher<stereo_msgs::msg::DisparityImage>(
|
||||
"disparity", rclcpp::QoS(10).best_effort());
|
||||
EXPECT_TRUE(waitForSubscriber(pub)) << "the subscription must have followed qos";
|
||||
|
||||
spinFor(std::chrono::milliseconds(500));
|
||||
EXPECT_EQ(depth->subscription->get_publisher_count(), 0u)
|
||||
<< "the publishers must have followed qos too: best effort, so a reliable "
|
||||
"consumer cannot match them";
|
||||
}
|
||||
@@ -0,0 +1,205 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include "node_test_utils.hpp"
|
||||
|
||||
#include <rtabmap_util/imu_to_tf.hpp>
|
||||
|
||||
#include <sensor_msgs/msg/imu.hpp>
|
||||
#include <tf2_msgs/msg/tf_message.hpp>
|
||||
#include <tf2/LinearMath/Quaternion.hpp>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
|
||||
#include <tf2/utils.hpp>
|
||||
|
||||
using namespace rtabmap_util_test;
|
||||
|
||||
namespace {
|
||||
::testing::Environment * const kEnv = registerRclcppEnvironment();
|
||||
|
||||
/// An Imu message whose orientation is a pure rotation of @p yaw about z.
|
||||
sensor_msgs::msg::Imu makeImu(const std::string & frameId, double stamp, double yaw = 0.0)
|
||||
{
|
||||
tf2::Quaternion q;
|
||||
q.setRPY(0.0, 0.0, yaw);
|
||||
|
||||
sensor_msgs::msg::Imu msg;
|
||||
msg.header.frame_id = frameId;
|
||||
msg.header.stamp = rclcpp::Time(int32_t(stamp), uint32_t((stamp - int32_t(stamp)) * 1e9), RCL_ROS_TIME);
|
||||
msg.orientation = tf2::toMsg(q);
|
||||
return msg;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
class ImuToTFTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr staticTfKeepAlive_;
|
||||
};
|
||||
|
||||
TEST_F(ImuToTFTest, BroadcastsOrientationAsTf)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::ImuToTF>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({rclcpp::Parameter("fixed_frame_id", "odom")})));
|
||||
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
|
||||
collect<tf2_msgs::msg::TFMessage>("/tf");
|
||||
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub)) << "the node never subscribed to imu/data";
|
||||
|
||||
pub->publish(makeImu("imu_link", 1000.0, /*yaw=*/M_PI/2.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); })) << "no transform was broadcast";
|
||||
|
||||
ASSERT_EQ(tf->back().transforms.size(), 1u);
|
||||
const geometry_msgs::msg::TransformStamped & t = tf->back().transforms[0];
|
||||
EXPECT_EQ(t.header.frame_id, "odom");
|
||||
EXPECT_EQ(t.child_frame_id, "imu_link") << "with no base_frame_id the imu frame is used";
|
||||
|
||||
// The broadcast rotation must be the IMU's orientation.
|
||||
tf2::Quaternion q;
|
||||
tf2::fromMsg(t.transform.rotation, q);
|
||||
EXPECT_NEAR(tf2::getYaw(q), M_PI/2.0, 1e-6);
|
||||
|
||||
// It is an orientation only: no translation.
|
||||
EXPECT_NEAR(t.transform.translation.x, 0.0, 1e-9);
|
||||
EXPECT_NEAR(t.transform.translation.y, 0.0, 1e-9);
|
||||
EXPECT_NEAR(t.transform.translation.z, 0.0, 1e-9);
|
||||
}
|
||||
|
||||
TEST_F(ImuToTFTest, UsesTheConfiguredFixedFrame)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::ImuToTF>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({rclcpp::Parameter("fixed_frame_id", "my_odom")})));
|
||||
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
|
||||
collect<tf2_msgs::msg::TFMessage>("/tf");
|
||||
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
|
||||
pub->publish(makeImu("imu_link", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); }));
|
||||
|
||||
EXPECT_EQ(tf->back().transforms[0].header.frame_id, "my_odom");
|
||||
}
|
||||
|
||||
TEST_F(ImuToTFTest, PreservesTheImuStamp)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::ImuToTF>(rclcpp::NodeOptions()));
|
||||
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
|
||||
collect<tf2_msgs::msg::TFMessage>("/tf");
|
||||
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
|
||||
const sensor_msgs::msg::Imu imu = makeImu("imu_link", 1234.5);
|
||||
pub->publish(imu);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); }));
|
||||
|
||||
EXPECT_EQ(tf->back().transforms[0].header.stamp.sec, imu.header.stamp.sec);
|
||||
EXPECT_EQ(tf->back().transforms[0].header.stamp.nanosec, imu.header.stamp.nanosec);
|
||||
}
|
||||
|
||||
TEST_F(ImuToTFTest, ReportsTheOrientationInTheBaseFrame)
|
||||
{
|
||||
// With base_frame_id set and the mounting transform available, the node re-expresses
|
||||
// the IMU orientation in the base frame and broadcasts that frame instead.
|
||||
addNode(std::make_shared<rtabmap_util::ImuToTF>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("fixed_frame_id", "odom"),
|
||||
rclcpp::Parameter("base_frame_id", "base_link"),
|
||||
rclcpp::Parameter("wait_for_transform_duration", 0.5)})));
|
||||
publishStaticTf("base_link", "imu_link", 0.1, 0.0, 0.2); // translation only
|
||||
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
|
||||
collect<tf2_msgs::msg::TFMessage>("/tf");
|
||||
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
|
||||
pub->publish(makeImu("imu_link", 1000.0, /*yaw=*/M_PI/2.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); }))
|
||||
<< "with the mounting transform available a transform must be broadcast";
|
||||
|
||||
const geometry_msgs::msg::TransformStamped & t = tf->back().transforms[0];
|
||||
EXPECT_EQ(t.header.frame_id, "odom");
|
||||
EXPECT_EQ(t.child_frame_id, "base_link")
|
||||
<< "the base frame is broadcast, not the imu frame";
|
||||
|
||||
// The mounting has no rotation, so the orientation is unchanged.
|
||||
tf2::Quaternion q;
|
||||
tf2::fromMsg(t.transform.rotation, q);
|
||||
EXPECT_NEAR(tf2::getYaw(q), M_PI/2.0, 1e-6);
|
||||
}
|
||||
|
||||
TEST_F(ImuToTFTest, IgnoresAYawOnlyMountingOffset)
|
||||
{
|
||||
// The node strips the yaw of the mounting transform (it uses only getYaw to build
|
||||
// the correction), so a purely yaw-rotated mount leaves the reported orientation
|
||||
// alone: the IMU's absolute yaw is what matters, not how it is bolted on.
|
||||
addNode(std::make_shared<rtabmap_util::ImuToTF>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("fixed_frame_id", "odom"),
|
||||
rclcpp::Parameter("base_frame_id", "base_link"),
|
||||
rclcpp::Parameter("wait_for_transform_duration", 0.5)})));
|
||||
|
||||
// base_link -> imu_link rotated 90 degrees about z.
|
||||
{
|
||||
tf2::Quaternion mount;
|
||||
mount.setRPY(0.0, 0.0, M_PI/2.0);
|
||||
geometry_msgs::msg::TransformStamped m;
|
||||
m.header.stamp = helper()->now();
|
||||
m.header.frame_id = "base_link";
|
||||
m.child_frame_id = "imu_link";
|
||||
m.transform.rotation = tf2::toMsg(mount);
|
||||
tf2_msgs::msg::TFMessage msg;
|
||||
msg.transforms.push_back(m);
|
||||
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr staticPub =
|
||||
helper()->create_publisher<tf2_msgs::msg::TFMessage>(
|
||||
"/tf_static", rclcpp::QoS(100).transient_local());
|
||||
staticPub->publish(msg);
|
||||
spinFor(std::chrono::milliseconds(150));
|
||||
staticTfKeepAlive_ = staticPub;
|
||||
}
|
||||
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
|
||||
collect<tf2_msgs::msg::TFMessage>("/tf");
|
||||
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
|
||||
pub->publish(makeImu("imu_link", 1000.0, /*yaw=*/M_PI/4.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); }));
|
||||
|
||||
const geometry_msgs::msg::TransformStamped & t = tf->back().transforms[0];
|
||||
EXPECT_EQ(t.child_frame_id, "base_link");
|
||||
|
||||
tf2::Quaternion q;
|
||||
tf2::fromMsg(t.transform.rotation, q);
|
||||
EXPECT_NEAR(tf2::getYaw(q), M_PI/4.0, 1e-5)
|
||||
<< "the mounting yaw must cancel out, leaving the imu's own yaw";
|
||||
}
|
||||
|
||||
TEST_F(ImuToTFTest, DropsTheMessageWhenTheBaseTransformIsMissing)
|
||||
{
|
||||
// base_frame_id differs from the imu frame, so the node needs imu_link -> base_link
|
||||
// from TF. Nothing publishes it, so nothing may be broadcast.
|
||||
addNode(std::make_shared<rtabmap_util::ImuToTF>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("base_frame_id", "base_link"),
|
||||
rclcpp::Parameter("wait_for_transform_duration", 0.0)})));
|
||||
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
|
||||
collect<tf2_msgs::msg::TFMessage>("/tf");
|
||||
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
|
||||
pub->publish(makeImu("imu_link", 1000.0, M_PI/2.0));
|
||||
spinFor(std::chrono::milliseconds(500));
|
||||
|
||||
EXPECT_TRUE(tf->empty()) << "without the base transform the node must not broadcast";
|
||||
}
|
||||
@@ -0,0 +1,204 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include "node_test_utils.hpp"
|
||||
#include "msg_builders.hpp"
|
||||
|
||||
#include <rtabmap_util/lidar_deskewing.hpp>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <sensor_msgs/msg/laser_scan.hpp>
|
||||
#include <tf2_msgs/msg/tf_message.hpp>
|
||||
|
||||
using namespace rtabmap_util_test;
|
||||
|
||||
namespace {
|
||||
::testing::Environment * const kEnv = registerRclcppEnvironment();
|
||||
}
|
||||
|
||||
class LidarDeskewingTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
static constexpr double kSweep = 0.099; ///< first sample to last, seconds
|
||||
static constexpr double kSpeed = 1.0; ///< m/s, straight at the wall
|
||||
static constexpr float kWall = 5.0f; ///< distance to the wall, meters
|
||||
|
||||
/// Distance travelled since the first sample. Drives both the TF and the skew.
|
||||
static double travelled(double elapsed) { return kSpeed * elapsed; }
|
||||
|
||||
/// Publishes odom -> lidar following exactly that trajectory.
|
||||
void publishOdomMotion(double startStamp)
|
||||
{
|
||||
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub =
|
||||
helper()->create_publisher<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
|
||||
spinFor(std::chrono::milliseconds(100)); // let the node's listener subscribe
|
||||
|
||||
// Covers exactly the sweep, from the first sample to the last. Nothing beyond:
|
||||
// asking for more than laser_geometry needs would be a regression.
|
||||
for(int i=0; i<=2; ++i)
|
||||
{
|
||||
const double elapsed = kSweep * double(i) / 2.0;
|
||||
geometry_msgs::msg::TransformStamped t;
|
||||
t.header.stamp = stampOf(startStamp + elapsed);
|
||||
t.header.frame_id = "odom";
|
||||
t.child_frame_id = "lidar";
|
||||
t.transform.translation.x = travelled(elapsed);
|
||||
t.transform.rotation.w = 1.0;
|
||||
tf2_msgs::msg::TFMessage msg;
|
||||
msg.transforms.push_back(t);
|
||||
tfPub->publish(msg);
|
||||
}
|
||||
spinFor(std::chrono::milliseconds(200)); // let the buffer fill
|
||||
tfPub_ = tfPub; // keep the publisher alive
|
||||
}
|
||||
|
||||
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub_;
|
||||
};
|
||||
|
||||
TEST_F(LidarDeskewingTest, DeskewsACloudUsingTf)
|
||||
{
|
||||
// The wall is recorded bent because the sensor closes in during the sweep, and TF
|
||||
// carries that same motion. A correct deskew must flatten it back to kWall.
|
||||
addNode(std::make_shared<rtabmap_util::LidarDeskewing>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("fixed_frame_id", "odom"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.2)})));
|
||||
|
||||
publishOdomMotion(1000.0);
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
|
||||
collect<sensor_msgs::msg::PointCloud2>("input_cloud/deskewed");
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("input_cloud", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
|
||||
const size_t sampleCount = 20;
|
||||
const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(
|
||||
"lidar", 1000.0, sampleCount, kSweep, kWall, &travelled);
|
||||
|
||||
// The input really is bent: the last sample is a full sweep of travel closer.
|
||||
ASSERT_NEAR(readXYZ(in, 0).x, kWall, 1e-4);
|
||||
ASSERT_NEAR(readXYZ(in, sampleCount-1).x, kWall - float(kSpeed*kSweep), 1e-4);
|
||||
|
||||
pub->publish(in);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })) << "no deskewed cloud published";
|
||||
|
||||
const sensor_msgs::msg::PointCloud2 & cloud = out->back();
|
||||
EXPECT_EQ(cloud.header.frame_id, "lidar") << "output stays in the sensor frame";
|
||||
ASSERT_EQ(cloud.width, sampleCount);
|
||||
|
||||
// Every sample must land back on the wall.
|
||||
for(size_t i=0; i<sampleCount; ++i)
|
||||
{
|
||||
EXPECT_NEAR(readXYZ(cloud, i).x, kWall, 5e-3) << "sample " << i;
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(LidarDeskewingTest, DeskewsAScanUsingTf)
|
||||
{
|
||||
// Same idea as the cloud case, for the 2D path. A ray at angle theta taken once the
|
||||
// sensor has advanced d meters measures (wall - d)/cos(theta), so the raw scan bends.
|
||||
// Deskewing must put every point back on the wall at x = kWall.
|
||||
addNode(std::make_shared<rtabmap_util::LidarDeskewing>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("fixed_frame_id", "odom"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.2)})));
|
||||
|
||||
publishOdomMotion(1000.0);
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
|
||||
collect<sensor_msgs::msg::PointCloud2>("input_scan/deskewed");
|
||||
rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::LaserScan>("input_scan", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
|
||||
sensor_msgs::msg::LaserScan scan;
|
||||
scan.header.frame_id = "lidar";
|
||||
scan.header.stamp = stampOf(1000.0);
|
||||
scan.angle_min = -0.4f;
|
||||
scan.angle_max = 0.4f;
|
||||
scan.angle_increment = 0.05f;
|
||||
scan.range_min = 0.1f;
|
||||
scan.range_max = 30.0f;
|
||||
const size_t rayCount = size_t((scan.angle_max - scan.angle_min) / scan.angle_increment) + 1;
|
||||
scan.time_increment = float(kSweep / double(rayCount - 1));
|
||||
scan.ranges.resize(rayCount);
|
||||
for(size_t i=0; i<rayCount; ++i)
|
||||
{
|
||||
const double elapsed = double(i) * scan.time_increment;
|
||||
const double angle = scan.angle_min + double(i) * scan.angle_increment;
|
||||
// Distance to a wall at x=kWall, from a sensor that has already advanced.
|
||||
scan.ranges[i] = float((kWall - travelled(elapsed)) / std::cos(angle));
|
||||
}
|
||||
|
||||
pub->publish(scan);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })) << "no deskewed scan published";
|
||||
|
||||
const sensor_msgs::msg::PointCloud2 & cloud = out->back();
|
||||
EXPECT_EQ(cloud.header.frame_id, "lidar") << "output stays in the sensor frame";
|
||||
ASSERT_EQ(cloud.width, rayCount);
|
||||
|
||||
// Without deskewing the last ray would sit a full sweep of travel short of the wall.
|
||||
for(size_t i=0; i<rayCount; ++i)
|
||||
{
|
||||
EXPECT_NEAR(readXYZ(cloud, i).x, kWall, 5e-3) << "ray " << i;
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(LidarDeskewingTest, RepublishesTheCloudUnchangedWhenDeskewingFails)
|
||||
{
|
||||
// With no TF the cloud cannot be deskewed, but the node deliberately republishes it
|
||||
// as-is rather than dropping it, so downstream nodes keep receiving data.
|
||||
addNode(std::make_shared<rtabmap_util::LidarDeskewing>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("fixed_frame_id", "odom"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.0)})));
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
|
||||
collect<sensor_msgs::msg::PointCloud2>("input_cloud/deskewed");
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("input_cloud", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
|
||||
const sensor_msgs::msg::PointCloud2 in =
|
||||
makeXYZCloud("lidar", 1000.0, {{5.0f, 0.0f, 0.0f}, {5.0f, 1.0f, 0.0f}}, true);
|
||||
pub->publish(in);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out->empty(); }))
|
||||
<< "the cloud must still be forwarded";
|
||||
|
||||
EXPECT_EQ(out->back().data, in.data) << "and forwarded byte for byte, still skewed";
|
||||
}
|
||||
|
||||
TEST_F(LidarDeskewingTest, DropsAScanWhenTfIsMissing)
|
||||
{
|
||||
// The 2D scan path does the opposite of the cloud path: it returns early and
|
||||
// publishes nothing when the transform is unavailable.
|
||||
addNode(std::make_shared<rtabmap_util::LidarDeskewing>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("fixed_frame_id", "odom"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.0)})));
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
|
||||
collect<sensor_msgs::msg::PointCloud2>("input_scan/deskewed");
|
||||
rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::LaserScan>("input_scan", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
|
||||
sensor_msgs::msg::LaserScan scan;
|
||||
scan.header.frame_id = "lidar";
|
||||
scan.header.stamp = stampOf(1000.0);
|
||||
scan.angle_min = -1.0f;
|
||||
scan.angle_max = 1.0f;
|
||||
scan.angle_increment = 0.1f;
|
||||
scan.time_increment = 0.001f;
|
||||
scan.range_min = 0.1f;
|
||||
scan.range_max = 30.0f;
|
||||
scan.ranges.assign(21, 5.0f);
|
||||
pub->publish(scan);
|
||||
spinFor(std::chrono::milliseconds(400));
|
||||
|
||||
EXPECT_TRUE(out->empty()) << "the scan path drops the message instead of forwarding it";
|
||||
}
|
||||
@@ -0,0 +1,549 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include "node_test_utils.hpp"
|
||||
#include "msg_builders.hpp"
|
||||
|
||||
#include <rtabmap_util/map_assembler.hpp>
|
||||
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap/core/Signature.h>
|
||||
|
||||
#include <nav_msgs/msg/occupancy_grid.hpp>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
#include <std_srvs/srv/empty.hpp>
|
||||
|
||||
#include <thread>
|
||||
|
||||
#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
|
||||
#include <octomap_msgs/srv/get_octomap.hpp>
|
||||
#endif
|
||||
|
||||
using namespace rtabmap_util_test;
|
||||
|
||||
namespace {
|
||||
::testing::Environment * const kEnv = registerRclcppEnvironment();
|
||||
|
||||
constexpr float kCellSize = 0.05f;
|
||||
/// Anything above this is an obstacle once a grid is regenerated from a scan.
|
||||
constexpr float kGroundHeight = 0.1f;
|
||||
constexpr float kObstacleHeight = 0.5f;
|
||||
|
||||
cv::Mat toCellMat(const std::vector<cv::Point3f> & points)
|
||||
{
|
||||
if(points.empty()) { return cv::Mat(); }
|
||||
cv::Mat mat(1, int(points.size()), CV_32FC3);
|
||||
for(size_t i=0; i<points.size(); ++i)
|
||||
{
|
||||
mat.at<cv::Vec3f>(0, int(i)) = cv::Vec3f(points[i].x, points[i].y, points[i].z);
|
||||
}
|
||||
return mat;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief A graph node as it arrives on "mapData".
|
||||
*
|
||||
* map_assembler only caches a node that carries compressed images or a compressed scan,
|
||||
* so the scan is what makes the node acceptable at all. The occupancy grid is what
|
||||
* MapsManager normally uses; the two are deliberately given different geometry so a test
|
||||
* can tell which one ended up in the map.
|
||||
*
|
||||
* @param scan points of the raw scan, in the node's frame
|
||||
* @param ground ground cells of the ready-made grid
|
||||
* @param obstacles obstacle cells of the ready-made grid
|
||||
*/
|
||||
rtabmap::Signature makeNode(
|
||||
int id, const rtabmap::Transform & pose,
|
||||
const std::vector<cv::Point3f> & scan,
|
||||
const std::vector<cv::Point3f> & ground,
|
||||
const std::vector<cv::Point3f> & obstacles)
|
||||
{
|
||||
rtabmap::SensorData data;
|
||||
data.setId(id);
|
||||
data.setStamp(1000.0 + id);
|
||||
data.setLaserScan(rtabmap::LaserScan(rtabmap::compressData2(toCellMat(scan)),
|
||||
/*maxPoints=*/0, /*maxRange=*/0.0f, rtabmap::LaserScan::kXYZ));
|
||||
if(!ground.empty() || !obstacles.empty())
|
||||
{
|
||||
data.setOccupancyGrid(toCellMat(ground), toCellMat(obstacles), cv::Mat(), kCellSize,
|
||||
cv::Point3f(0, 0, 0));
|
||||
}
|
||||
return rtabmap::Signature(id, /*mapId=*/0, /*weight=*/1, data.stamp(), /*label=*/"",
|
||||
pose, rtabmap::Transform(), data);
|
||||
}
|
||||
|
||||
cv::Point3f pointAt(const sensor_msgs::msg::PointCloud2 & cloud, size_t index)
|
||||
{
|
||||
uint32_t xo = 0, yo = 4, zo = 8;
|
||||
for(size_t i=0; i<cloud.fields.size(); ++i)
|
||||
{
|
||||
if(cloud.fields[i].name == "x") { xo = cloud.fields[i].offset; }
|
||||
else if(cloud.fields[i].name == "y") { yo = cloud.fields[i].offset; }
|
||||
else if(cloud.fields[i].name == "z") { zo = cloud.fields[i].offset; }
|
||||
}
|
||||
const unsigned char * base = &cloud.data[index * cloud.point_step];
|
||||
return cv::Point3f(
|
||||
*reinterpret_cast<const float *>(base + xo),
|
||||
*reinterpret_cast<const float *>(base + yo),
|
||||
*reinterpret_cast<const float *>(base + zo));
|
||||
}
|
||||
|
||||
bool containsPoint(const sensor_msgs::msg::PointCloud2 & cloud, const cv::Point3f & expected,
|
||||
float tolerance = 1e-3f)
|
||||
{
|
||||
for(size_t i=0; i<size_t(cloud.width)*cloud.height; ++i)
|
||||
{
|
||||
if(cv::norm(pointAt(cloud, i) - expected) < tolerance) { return true; }
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
size_t pointCount(const sensor_msgs::msg::PointCloud2 & cloud)
|
||||
{
|
||||
return size_t(cloud.width) * cloud.height;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
/**
|
||||
* map_assembler subscribes to "mapData", caches the nodes it carries and hands them to a
|
||||
* MapsManager, which is what actually publishes the maps.
|
||||
*
|
||||
* By default it first asks rtabmap for the map it missed: a one second timer whose
|
||||
* callback calls get_map_data, blocks on the reply, and only then subscribes to
|
||||
* "mapData". That needs two callbacks of the same node to run at once, hence the fake
|
||||
* service and spinMultiThreadedUntil() in startInitializingFromRtabmap(). Most tests
|
||||
* below have nothing to catch up on, so start() sets the timeout to 0 and the node
|
||||
* subscribes immediately.
|
||||
*/
|
||||
class MapAssemblerTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
/// The name the fake get_map_data service is advertised under.
|
||||
static constexpr const char * kRtabmapName = "fake_rtabmap";
|
||||
|
||||
void SetUp() override
|
||||
{
|
||||
NodeTest::SetUp();
|
||||
getMapCalls_ = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Advertises the get_map_data service map_assembler calls on start-up.
|
||||
*
|
||||
* @param initial the map handed back, i.e. what the node starts with in its cache.
|
||||
*/
|
||||
void advertiseGetMapData(const rtabmap_msgs::msg::MapData & initial =
|
||||
rtabmap_msgs::msg::MapData())
|
||||
{
|
||||
initialMap_ = initial;
|
||||
getMapService_ = helper()->create_service<rtabmap_msgs::srv::GetMap>(
|
||||
std::string(kRtabmapName) + "/get_map_data",
|
||||
[this](const std::shared_ptr<rtabmap_msgs::srv::GetMap::Request>,
|
||||
std::shared_ptr<rtabmap_msgs::srv::GetMap::Response> response) {
|
||||
++getMapCalls_;
|
||||
response->data = initialMap_;
|
||||
});
|
||||
}
|
||||
|
||||
/// Creates the node with the start-up call skipped, so it subscribes right away.
|
||||
void start(std::vector<rclcpp::Parameter> overrides = {})
|
||||
{
|
||||
overrides.push_back(rclcpp::Parameter("initialize_from_rtabmap_timeout", 0.0));
|
||||
createNode(overrides);
|
||||
ASSERT_TRUE(waitForSubscriber(mapDataPub_))
|
||||
<< "map_assembler never subscribed to mapData";
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Creates the node with the start-up call enabled, as it is by default.
|
||||
*
|
||||
* It only subscribes to "mapData" once that call has returned, and the call blocks a
|
||||
* callback on another callback of the same node, so it needs a multi-threaded
|
||||
* executor to get through.
|
||||
*/
|
||||
void startInitializingFromRtabmap(double timeout = 5.0,
|
||||
std::vector<rclcpp::Parameter> overrides = {})
|
||||
{
|
||||
overrides.push_back(
|
||||
rclcpp::Parameter("initialize_from_rtabmap_timeout", timeout));
|
||||
createNode(overrides);
|
||||
ASSERT_TRUE(spinMultiThreadedUntil(
|
||||
[&]() { return mapDataPub_->get_subscription_count() > 0; }))
|
||||
<< "map_assembler never subscribed to mapData";
|
||||
}
|
||||
|
||||
void createNode(std::vector<rclcpp::Parameter> overrides)
|
||||
{
|
||||
overrides.push_back(rclcpp::Parameter("rtabmap", std::string(kRtabmapName)));
|
||||
assembler_ = addNode(std::make_shared<rtabmap_util::MapAssembler>(
|
||||
rclcpp::NodeOptions().parameter_overrides(overrides)));
|
||||
mapDataPub_ = helper()->create_publisher<rtabmap_msgs::msg::MapData>("mapData",
|
||||
rclcpp::QoS(1));
|
||||
}
|
||||
|
||||
/// A MapData carrying @p signatures and a graph over their poses.
|
||||
static rtabmap_msgs::msg::MapData makeMapData(
|
||||
const std::vector<rtabmap::Signature> & signatures,
|
||||
const std::vector<int> & graphIds = {})
|
||||
{
|
||||
std::map<int, rtabmap::Transform> poses;
|
||||
rtabmap_msgs::msg::MapData msg;
|
||||
msg.header.frame_id = "map";
|
||||
msg.header.stamp = stampOf(2000.0);
|
||||
|
||||
for(size_t i=0; i<signatures.size(); ++i)
|
||||
{
|
||||
rtabmap_msgs::msg::Node node;
|
||||
rtabmap_conversions::nodeToROS(signatures[i], node);
|
||||
msg.nodes.push_back(node);
|
||||
poses.insert(std::make_pair(signatures[i].id(), signatures[i].getPose()));
|
||||
}
|
||||
// The graph may name nodes whose data is not resent, which is the normal case
|
||||
// once map_assembler has them cached.
|
||||
for(size_t i=0; i<graphIds.size(); ++i)
|
||||
{
|
||||
poses.insert(std::make_pair(graphIds[i], poseOf(graphIds[i])));
|
||||
}
|
||||
rtabmap_conversions::mapGraphToROS(poses, std::multimap<int, rtabmap::Link>(),
|
||||
rtabmap::Transform::getIdentity(), msg.graph);
|
||||
return msg;
|
||||
}
|
||||
|
||||
static rtabmap::Transform poseOf(int id)
|
||||
{
|
||||
return rtabmap::Transform(2.0f * float(id - 1), 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
|
||||
}
|
||||
|
||||
/// Two nodes whose ready-made grids hold one ground and one obstacle cell each.
|
||||
static std::vector<rtabmap::Signature> twoNodes()
|
||||
{
|
||||
return {
|
||||
makeNode(1, poseOf(1),
|
||||
/*scan=*/{cv::Point3f(0.5f, -0.1f, 0.0f),
|
||||
cv::Point3f(0.5f, 0.1f, kObstacleHeight)},
|
||||
/*ground=*/{cv::Point3f(0.5f, -0.1f, 0.0f)},
|
||||
/*obstacles=*/{cv::Point3f(1.0f, 0.1f, 0.0f)}),
|
||||
makeNode(2, poseOf(2),
|
||||
/*scan=*/{cv::Point3f(0.5f, -0.1f, 0.0f),
|
||||
cv::Point3f(0.5f, 0.1f, kObstacleHeight)},
|
||||
/*ground=*/{cv::Point3f(0.5f, -0.1f, 0.0f)},
|
||||
/*obstacles=*/{cv::Point3f(1.0f, 0.1f, 0.0f)})};
|
||||
}
|
||||
|
||||
void publishMapData(const rtabmap_msgs::msg::MapData & msg)
|
||||
{
|
||||
mapDataPub_->publish(msg);
|
||||
}
|
||||
|
||||
std::shared_ptr<rtabmap_util::MapAssembler> assembler_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::MapData>::SharedPtr mapDataPub_;
|
||||
rclcpp::Service<rtabmap_msgs::srv::GetMap>::SharedPtr getMapService_;
|
||||
rtabmap_msgs::msg::MapData initialMap_;
|
||||
std::atomic_int getMapCalls_{0};
|
||||
};
|
||||
|
||||
constexpr const char * MapAssemblerTest::kRtabmapName;
|
||||
|
||||
//============================================================================
|
||||
// Start-up
|
||||
//============================================================================
|
||||
|
||||
TEST_F(MapAssemblerTest, AsksRtabmapForTheMapByDefault)
|
||||
{
|
||||
advertiseGetMapData(makeMapData(twoNodes()));
|
||||
createNode({}); // no overrides at all, so the default timeout applies
|
||||
ASSERT_TRUE(spinMultiThreadedUntil(
|
||||
[&]() { return mapDataPub_->get_subscription_count() > 0; }));
|
||||
|
||||
EXPECT_EQ(getMapCalls_.load(), 1) << "the start-up service call is made exactly once";
|
||||
}
|
||||
|
||||
TEST_F(MapAssemblerTest, SkipsTheStartUpCallWhenTheTimeoutIsZero)
|
||||
{
|
||||
// Nothing to catch up on, so the node should not spend its start-up waiting on a
|
||||
// service: it subscribes immediately instead.
|
||||
advertiseGetMapData(makeMapData(twoNodes()));
|
||||
start();
|
||||
EXPECT_EQ(getMapCalls_.load(), 0) << "rtabmap must not be called with a zero timeout";
|
||||
}
|
||||
|
||||
TEST_F(MapAssemblerTest, SubscribesAnywayWhenRtabmapNeverAnswers)
|
||||
{
|
||||
// Nothing advertises get_map_data, so the start-up call times out. The node must
|
||||
// still come up and subscribe, since rtabmap may be started afterwards.
|
||||
// Short, because unlike every other test here this one waits the timeout out.
|
||||
startInitializingFromRtabmap(/*timeout=*/0.5);
|
||||
EXPECT_EQ(getMapCalls_.load(), 0);
|
||||
}
|
||||
|
||||
TEST_F(MapAssemblerTest, WaitsForRtabmapToShowUpDuringTheTimeout)
|
||||
{
|
||||
// get_map_data is not advertised when the node starts asking for it: it appears part
|
||||
// way through the wait. The call must still go through, which is what makes the
|
||||
// timeout a real wait rather than a check of what happens to be up already.
|
||||
//
|
||||
// The node's timer fires one second after construction and then waits 750 ms, so
|
||||
// advertising at 1250 ms lands inside that window with room on both sides.
|
||||
std::thread rtabmapStartsLate([this]() {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1250));
|
||||
advertiseGetMapData(makeMapData(twoNodes()));
|
||||
});
|
||||
|
||||
startInitializingFromRtabmap(/*timeout=*/0.75);
|
||||
rtabmapStartsLate.join();
|
||||
|
||||
EXPECT_EQ(getMapCalls_.load(), 1) << "rtabmap showed up before the wait expired";
|
||||
}
|
||||
|
||||
TEST_F(MapAssemblerTest, StartsFromTheMapRtabmapHandsBack)
|
||||
{
|
||||
// The nodes come from the start-up call, and the graph that arrives later names them
|
||||
// without resending their data. The map must still be assembled from the cache.
|
||||
advertiseGetMapData(makeMapData(twoNodes()));
|
||||
startInitializingFromRtabmap();
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
|
||||
collect<sensor_msgs::msg::PointCloud2>("cloud_map");
|
||||
ASSERT_TRUE(waitForPublisher(cloud->subscription));
|
||||
|
||||
publishMapData(makeMapData({}, /*graphIds=*/{1, 2}));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !cloud->empty(); })) << "no cloud assembled";
|
||||
|
||||
EXPECT_EQ(pointCount(cloud->back()), 4u) << "one ground and one obstacle cell per node";
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// Assembling
|
||||
//============================================================================
|
||||
|
||||
TEST_F(MapAssemblerTest, AssemblesTheCloudFromMapData)
|
||||
{
|
||||
start();
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
|
||||
collect<sensor_msgs::msg::PointCloud2>("cloud_map");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
|
||||
collect<sensor_msgs::msg::PointCloud2>("cloud_obstacles");
|
||||
ASSERT_TRUE(waitForPublisher(cloud->subscription));
|
||||
|
||||
publishMapData(makeMapData(twoNodes()));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !cloud->empty() && !obstacles->empty(); }))
|
||||
<< "no cloud assembled";
|
||||
|
||||
EXPECT_EQ(cloud->back().header.frame_id, "map") << "the frame comes from the message";
|
||||
EXPECT_EQ(pointCount(cloud->back()), 4u);
|
||||
EXPECT_EQ(pointCount(obstacles->back()), 2u);
|
||||
// Node 2 sits 2 m along x, so its obstacle cell lands at 3 m.
|
||||
EXPECT_TRUE(containsPoint(obstacles->back(), cv::Point3f(1.0f, 0.1f, 0.0f)));
|
||||
EXPECT_TRUE(containsPoint(obstacles->back(), cv::Point3f(3.0f, 0.1f, 0.0f)));
|
||||
}
|
||||
|
||||
TEST_F(MapAssemblerTest, PublishesTheOccupancyGrid)
|
||||
{
|
||||
start();
|
||||
|
||||
std::shared_ptr<Collector<nav_msgs::msg::OccupancyGrid>> grid =
|
||||
collect<nav_msgs::msg::OccupancyGrid>("map");
|
||||
ASSERT_TRUE(waitForPublisher(grid->subscription));
|
||||
|
||||
publishMapData(makeMapData(twoNodes()));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !grid->empty(); })) << "no grid assembled";
|
||||
|
||||
EXPECT_EQ(grid->back().header.frame_id, "map");
|
||||
EXPECT_NEAR(grid->back().info.resolution, kCellSize, 1e-6);
|
||||
EXPECT_GT(grid->back().info.width, 0u);
|
||||
}
|
||||
|
||||
TEST_F(MapAssemblerTest, IgnoresAnEmptyMapData)
|
||||
{
|
||||
start();
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
|
||||
collect<sensor_msgs::msg::PointCloud2>("cloud_map");
|
||||
ASSERT_TRUE(waitForPublisher(cloud->subscription));
|
||||
|
||||
rtabmap_msgs::msg::MapData empty;
|
||||
empty.header.frame_id = "map";
|
||||
empty.header.stamp = stampOf(2000.0);
|
||||
publishMapData(empty);
|
||||
spinFor(std::chrono::milliseconds(400));
|
||||
|
||||
EXPECT_TRUE(cloud->empty()) << "a message with no graph and no nodes is nothing to do";
|
||||
}
|
||||
|
||||
TEST_F(MapAssemblerTest, PublishesAnEmptyMapForAGraphWithNoCachedNodes)
|
||||
{
|
||||
// A graph can name nodes whose data map_assembler has never seen -- it has no cache
|
||||
// at all here. It still publishes, using the poses as they are.
|
||||
start();
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
|
||||
collect<sensor_msgs::msg::PointCloud2>("cloud_map");
|
||||
ASSERT_TRUE(waitForPublisher(cloud->subscription));
|
||||
|
||||
publishMapData(makeMapData({}, /*graphIds=*/{1, 2}));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !cloud->empty(); })) << "nothing published";
|
||||
|
||||
EXPECT_EQ(pointCount(cloud->back()), 0u) << "no data cached, so nothing to assemble";
|
||||
EXPECT_EQ(cloud->back().header.frame_id, "map");
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// regenerate_local_grids
|
||||
//============================================================================
|
||||
|
||||
TEST_F(MapAssemblerTest, UsesTheGridsThatCameWithTheNodes)
|
||||
{
|
||||
// By default the ready-made grid wins: its obstacle is at y=+0.1, the scan's is at
|
||||
// y=-0.1 with the ground point, so the two are told apart by where the cells land.
|
||||
start({rclcpp::Parameter(rtabmap::Parameters::kGridSensor(), std::string("0")),
|
||||
rclcpp::Parameter(rtabmap::Parameters::kGridNormalsSegmentation(),
|
||||
std::string("false")),
|
||||
rclcpp::Parameter(rtabmap::Parameters::kGridMaxGroundHeight(),
|
||||
std::string("0.1"))});
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
|
||||
collect<sensor_msgs::msg::PointCloud2>("cloud_obstacles");
|
||||
ASSERT_TRUE(waitForPublisher(obstacles->subscription));
|
||||
|
||||
publishMapData(makeMapData({twoNodes()[0]}));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !obstacles->empty(); }));
|
||||
|
||||
EXPECT_EQ(pointCount(obstacles->back()), 1u);
|
||||
EXPECT_TRUE(containsPoint(obstacles->back(), cv::Point3f(1.0f, 0.1f, 0.0f)))
|
||||
<< "the obstacle cell of the grid that came with the node";
|
||||
}
|
||||
|
||||
TEST_F(MapAssemblerTest, RegenerateLocalGridsRebuildsThemFromTheScan)
|
||||
{
|
||||
// With regenerate_local_grids the grid that came with the node is thrown away, so
|
||||
// MapsManager segments the scan instead: the raised scan point becomes the obstacle.
|
||||
start({rclcpp::Parameter("regenerate_local_grids", true),
|
||||
rclcpp::Parameter(rtabmap::Parameters::kGridSensor(), std::string("0")),
|
||||
rclcpp::Parameter(rtabmap::Parameters::kGridNormalsSegmentation(),
|
||||
std::string("false")),
|
||||
rclcpp::Parameter(rtabmap::Parameters::kGridMaxGroundHeight(),
|
||||
std::string("0.1"))});
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
|
||||
collect<sensor_msgs::msg::PointCloud2>("cloud_obstacles");
|
||||
ASSERT_TRUE(waitForPublisher(obstacles->subscription));
|
||||
|
||||
publishMapData(makeMapData({twoNodes()[0]}));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !obstacles->empty(); }));
|
||||
|
||||
EXPECT_EQ(pointCount(obstacles->back()), 1u);
|
||||
EXPECT_TRUE(containsPoint(obstacles->back(),
|
||||
cv::Point3f(0.5f, 0.1f, kObstacleHeight), kCellSize))
|
||||
<< "the raised scan point, not the cell the node arrived with";
|
||||
EXPECT_FALSE(containsPoint(obstacles->back(), cv::Point3f(1.0f, 0.1f, 0.0f)))
|
||||
<< "the grid that came with the node must have been discarded";
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// Services
|
||||
//============================================================================
|
||||
|
||||
TEST_F(MapAssemblerTest, ResetEmptiesTheMap)
|
||||
{
|
||||
start();
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
|
||||
collect<sensor_msgs::msg::PointCloud2>("cloud_map");
|
||||
ASSERT_TRUE(waitForPublisher(cloud->subscription));
|
||||
|
||||
publishMapData(makeMapData(twoNodes()));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !cloud->empty(); }));
|
||||
ASSERT_EQ(pointCount(cloud->back()), 4u);
|
||||
|
||||
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr reset =
|
||||
helper()->create_client<std_srvs::srv::Empty>("map_assembler/reset");
|
||||
ASSERT_TRUE(spinUntil([&]() { return reset->service_is_ready(); }))
|
||||
<< "the reset service was never advertised";
|
||||
reset->async_send_request(std::make_shared<std_srvs::srv::Empty::Request>());
|
||||
ASSERT_TRUE(spinUntil([&]() { return cloud->size() >= 1u; }));
|
||||
spinFor(std::chrono::milliseconds(200));
|
||||
|
||||
// The cache is gone, so the same graph now assembles nothing.
|
||||
const size_t before = cloud->size();
|
||||
publishMapData(makeMapData({}, /*graphIds=*/{1, 2}));
|
||||
ASSERT_TRUE(spinUntil([&]() { return cloud->size() > before; }));
|
||||
EXPECT_EQ(pointCount(cloud->back()), 0u) << "reset must drop the cached nodes";
|
||||
}
|
||||
|
||||
#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
|
||||
TEST_F(MapAssemblerTest, ServesTheBinaryOctomap)
|
||||
{
|
||||
start();
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
|
||||
collect<sensor_msgs::msg::PointCloud2>("cloud_map");
|
||||
ASSERT_TRUE(waitForPublisher(cloud->subscription));
|
||||
publishMapData(makeMapData(twoNodes()));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !cloud->empty(); }));
|
||||
|
||||
rclcpp::Client<octomap_msgs::srv::GetOctomap>::SharedPtr client =
|
||||
helper()->create_client<octomap_msgs::srv::GetOctomap>(
|
||||
"map_assembler/octomap_binary");
|
||||
ASSERT_TRUE(spinUntil([&]() { return client->service_is_ready(); }));
|
||||
|
||||
auto future = client->async_send_request(
|
||||
std::make_shared<octomap_msgs::srv::GetOctomap::Request>());
|
||||
ASSERT_TRUE(spinUntil([&]() {
|
||||
return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; }))
|
||||
<< "octomap_binary never answered";
|
||||
|
||||
// Keep the response alive: future.get() hands back a temporary shared_ptr, so binding
|
||||
// a reference into it would dangle.
|
||||
const std::shared_ptr<octomap_msgs::srv::GetOctomap::Response> response = future.get();
|
||||
EXPECT_EQ(response->map.header.frame_id, "map")
|
||||
<< "the frame of the last map data received";
|
||||
EXPECT_TRUE(response->map.binary);
|
||||
EXPECT_FALSE(response->map.data.empty())
|
||||
<< "the octomap is built on demand from the cache";
|
||||
}
|
||||
|
||||
TEST_F(MapAssemblerTest, ServesTheFullOctomap)
|
||||
{
|
||||
start();
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
|
||||
collect<sensor_msgs::msg::PointCloud2>("cloud_map");
|
||||
ASSERT_TRUE(waitForPublisher(cloud->subscription));
|
||||
publishMapData(makeMapData(twoNodes()));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !cloud->empty(); }));
|
||||
|
||||
rclcpp::Client<octomap_msgs::srv::GetOctomap>::SharedPtr client =
|
||||
helper()->create_client<octomap_msgs::srv::GetOctomap>(
|
||||
"map_assembler/octomap_full");
|
||||
ASSERT_TRUE(spinUntil([&]() { return client->service_is_ready(); }));
|
||||
|
||||
auto future = client->async_send_request(
|
||||
std::make_shared<octomap_msgs::srv::GetOctomap::Request>());
|
||||
ASSERT_TRUE(spinUntil([&]() {
|
||||
return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; }));
|
||||
|
||||
EXPECT_FALSE(future.get()->map.binary);
|
||||
}
|
||||
|
||||
TEST_F(MapAssemblerTest, ServesAnEmptyOctomapWithoutData)
|
||||
{
|
||||
start();
|
||||
|
||||
rclcpp::Client<octomap_msgs::srv::GetOctomap>::SharedPtr client =
|
||||
helper()->create_client<octomap_msgs::srv::GetOctomap>(
|
||||
"map_assembler/octomap_binary");
|
||||
ASSERT_TRUE(spinUntil([&]() { return client->service_is_ready(); }));
|
||||
|
||||
auto future = client->async_send_request(
|
||||
std::make_shared<octomap_msgs::srv::GetOctomap::Request>());
|
||||
ASSERT_TRUE(spinUntil([&]() {
|
||||
return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; }));
|
||||
|
||||
EXPECT_TRUE(future.get()->map.data.empty()) << "nothing cached, nothing to serve";
|
||||
}
|
||||
#endif
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,423 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include "node_test_utils.hpp"
|
||||
#include "msg_builders.hpp"
|
||||
|
||||
#include <rtabmap_util/obstacles_detection.hpp>
|
||||
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
|
||||
using namespace rtabmap_util_test;
|
||||
|
||||
namespace {
|
||||
::testing::Environment * const kEnv = registerRclcppEnvironment();
|
||||
|
||||
/**
|
||||
* @brief A dense ground plane at z=0 plus a vertical wall in front of it.
|
||||
*
|
||||
* The 0.05 m spacing matters: the segmentation clusters points with
|
||||
* Grid/ClusterRadius (0.1 m by default) and drops clusters below
|
||||
* Grid/MinClusterSize (10), so a sparser cloud is discarded entirely.
|
||||
*/
|
||||
std::vector<cv::Point3f> groundAndWall()
|
||||
{
|
||||
std::vector<cv::Point3f> points;
|
||||
for(int i=0; i<=20; ++i) // ground: 1 m x 1 m at z=0
|
||||
{
|
||||
for(int j=0; j<=20; ++j)
|
||||
{
|
||||
points.push_back(cv::Point3f(0.3f + 0.05f*i, -0.5f + 0.05f*j, 0.0f));
|
||||
}
|
||||
}
|
||||
for(int j=0; j<=20; ++j) // wall: vertical, 1 m wide, 0.75 m tall
|
||||
{
|
||||
for(int k=1; k<=15; ++k)
|
||||
{
|
||||
points.push_back(cv::Point3f(1.4f, -0.5f + 0.05f*j, 0.05f*k));
|
||||
}
|
||||
}
|
||||
return points;
|
||||
}
|
||||
/**
|
||||
* @brief A plane that is horizontal in the map frame, given a base frame pitched by
|
||||
* @p pitch. In the base frame it therefore rises with x: z = x * tan(pitch).
|
||||
*/
|
||||
std::vector<cv::Point3f> planeLevelInMapFrame(double pitch)
|
||||
{
|
||||
std::vector<cv::Point3f> points;
|
||||
for(int i=0; i<=20; ++i)
|
||||
{
|
||||
const float x = 0.8f + 0.05f*i;
|
||||
for(int j=0; j<=20; ++j)
|
||||
{
|
||||
points.push_back(cv::Point3f(x, -0.5f + 0.05f*j, x * float(std::tan(pitch))));
|
||||
}
|
||||
}
|
||||
return points;
|
||||
}
|
||||
/// Two flat 25-point patches: one about 0.5 m from the sensor, one about 3 m away.
|
||||
std::vector<cv::Point3f> nearAndFarPatches()
|
||||
{
|
||||
std::vector<cv::Point3f> points;
|
||||
for(int i=0; i<5; ++i)
|
||||
{
|
||||
for(int j=0; j<5; ++j)
|
||||
{
|
||||
points.push_back(cv::Point3f(0.4f + 0.05f*i, -0.1f + 0.05f*j, 0.0f));
|
||||
points.push_back(cv::Point3f(2.9f + 0.05f*i, -0.1f + 0.05f*j, 0.0f));
|
||||
}
|
||||
}
|
||||
return points;
|
||||
}
|
||||
|
||||
/// Smallest and largest x in a cloud, to tell the near patch from the far one.
|
||||
std::pair<float, float> xExtent(const sensor_msgs::msg::PointCloud2 & cloud)
|
||||
{
|
||||
float lo = std::numeric_limits<float>::max();
|
||||
float hi = -std::numeric_limits<float>::max();
|
||||
for(size_t i=0; i<cloud.width*cloud.height; ++i)
|
||||
{
|
||||
const float x = readXYZ(cloud, i).x;
|
||||
lo = std::min(lo, x);
|
||||
hi = std::max(hi, x);
|
||||
}
|
||||
return std::make_pair(lo, hi);
|
||||
}
|
||||
} // namespace
|
||||
|
||||
class ObstaclesDetectionTest : public NodeTest {};
|
||||
|
||||
TEST_F(ObstaclesDetectionTest, SeparatesGroundFromObstacles)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("frame_id", "base_link"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.1)})));
|
||||
publishStaticTf("base_link", "lidar");
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
|
||||
collect<sensor_msgs::msg::PointCloud2>("ground");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
|
||||
collect<sensor_msgs::msg::PointCloud2>("obstacles");
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(ground->subscription));
|
||||
|
||||
pub->publish(makeXYZCloud("lidar", 1000.0, groundAndWall()));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }))
|
||||
<< "both ground and obstacles must be published";
|
||||
|
||||
EXPECT_GT(ground->back().width, 0u) << "the flat points must be classified as ground";
|
||||
EXPECT_GT(obstacles->back().width, 0u) << "the wall must be classified as obstacles";
|
||||
// The clouds are transformed back into the frame of the input topic, not frame_id.
|
||||
EXPECT_EQ(ground->back().header.frame_id, "lidar");
|
||||
EXPECT_EQ(obstacles->back().header.frame_id, "lidar");
|
||||
}
|
||||
|
||||
TEST_F(ObstaclesDetectionTest, ProjectsObstaclesOntoTheGroundPlane)
|
||||
{
|
||||
// proj_obstacles is the obstacles cloud flattened to z=0, with flat surfaces removed.
|
||||
// Note it is published in frame_id, unlike ground/obstacles which keep the input frame.
|
||||
addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("frame_id", "base_link"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.1)})));
|
||||
publishStaticTf("base_link", "lidar");
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> proj =
|
||||
collect<sensor_msgs::msg::PointCloud2>("proj_obstacles");
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(proj->subscription));
|
||||
|
||||
pub->publish(makeXYZCloud("lidar", 1000.0, groundAndWall()));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !proj->empty(); }));
|
||||
|
||||
const sensor_msgs::msg::PointCloud2 & cloud = proj->back();
|
||||
ASSERT_GT(cloud.width, 0u) << "the wall must survive as a projected obstacle";
|
||||
EXPECT_EQ(cloud.header.frame_id, "base_link")
|
||||
<< "proj_obstacles uses frame_id, not the input frame";
|
||||
|
||||
for(size_t i=0; i<cloud.width; ++i)
|
||||
{
|
||||
EXPECT_NEAR(readXYZ(cloud, i).z, 0.0f, 1e-6)
|
||||
<< "every projected point is flattened to z=0, point " << i;
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(ObstaclesDetectionTest, HeightSegmentationIsRelativeToTheBaseFrameByDefault)
|
||||
{
|
||||
// With normals segmentation off the split is a plain height threshold. Without a
|
||||
// map frame the heights are those of the cloud in the base frame, so the flat points
|
||||
// at z=0 fall below Grid/MaxGroundHeight and are ground.
|
||||
addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("frame_id", "base_link"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.1),
|
||||
rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")),
|
||||
rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.2"))})));
|
||||
publishStaticTf("base_link", "lidar");
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
|
||||
collect<sensor_msgs::msg::PointCloud2>("ground");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
|
||||
collect<sensor_msgs::msg::PointCloud2>("obstacles");
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(ground->subscription));
|
||||
|
||||
pub->publish(makeXYZCloud("lidar", 1000.0, groundAndWall()));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }));
|
||||
|
||||
EXPECT_GT(ground->back().width, 0u) << "the z=0 plane is below the 0.2 m threshold";
|
||||
EXPECT_GT(obstacles->back().width, 0u) << "the wall rises above it";
|
||||
}
|
||||
|
||||
TEST_F(ObstaclesDetectionTest, MapFrameIdAloneDoesNotMoveTheHeightReference)
|
||||
{
|
||||
// The robot sits 1 m above the map origin, but Grid/MapFrameProjection is false by
|
||||
// default, so pose.z() is ignored and the heights stay relative to the base frame.
|
||||
// Setting map_frame_id on its own therefore changes nothing here.
|
||||
addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("frame_id", "base_link"),
|
||||
rclcpp::Parameter("map_frame_id", "map"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.1),
|
||||
rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")),
|
||||
rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.2"))})));
|
||||
publishStaticTf("base_link", "lidar");
|
||||
publishStaticTf("map", "base_link", 0.0, 0.0, 1.0);
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
|
||||
collect<sensor_msgs::msg::PointCloud2>("ground");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
|
||||
collect<sensor_msgs::msg::PointCloud2>("obstacles");
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(ground->subscription));
|
||||
|
||||
pub->publish(makeXYZCloud("lidar", 1000.0, groundAndWall()));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }));
|
||||
|
||||
EXPECT_GT(ground->back().width, 0u)
|
||||
<< "without Grid/MapFrameProjection the map height is not applied";
|
||||
}
|
||||
|
||||
TEST_F(ObstaclesDetectionTest, MapFrameIdLevelsTheGroundUsingRollAndPitch)
|
||||
{
|
||||
// Only pose.z() is gated by Grid/MapFrameProjection: roll and pitch are always
|
||||
// applied. So map_frame_id on its own still levels the segmentation to the map's
|
||||
// horizontal, which is what matters when the robot is on a slope.
|
||||
const double pitch = 10.0 * M_PI / 180.0;
|
||||
|
||||
// A plane that is level in the map frame, seen from a base frame pitched by 10 deg:
|
||||
// in the base frame it rises to well above the 0.1 m ground threshold.
|
||||
const std::vector<cv::Point3f> plane = planeLevelInMapFrame(pitch);
|
||||
ASSERT_GT(plane.back().z, 0.1f) << "precondition: tilted beyond the threshold";
|
||||
|
||||
// Without a map frame the tilt is taken at face value: not ground.
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("frame_id", "base_link"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.1),
|
||||
rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")),
|
||||
rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.1"))})));
|
||||
publishStaticTf("base_link", "lidar");
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
|
||||
collect<sensor_msgs::msg::PointCloud2>("ground");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
|
||||
collect<sensor_msgs::msg::PointCloud2>("obstacles");
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(ground->subscription));
|
||||
|
||||
pub->publish(makeXYZCloud("lidar", 1000.0, plane));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }));
|
||||
EXPECT_EQ(ground->back().width, 0u)
|
||||
<< "a slope read in the base frame is not ground";
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(ObstaclesDetectionTest, MapFrameIdRecoversTheGroundOnASlope)
|
||||
{
|
||||
// Same tilted plane, but now the node knows the robot is pitched in the map frame,
|
||||
// so it levels the cloud and the slope becomes ground again.
|
||||
const double pitch = 10.0 * M_PI / 180.0;
|
||||
|
||||
addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("frame_id", "base_link"),
|
||||
rclcpp::Parameter("map_frame_id", "map"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.1),
|
||||
rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")),
|
||||
rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.1"))})));
|
||||
publishStaticTf("base_link", "lidar");
|
||||
publishStaticTfRPY("map", "base_link", 0.0, pitch, 0.0);
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
|
||||
collect<sensor_msgs::msg::PointCloud2>("ground");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
|
||||
collect<sensor_msgs::msg::PointCloud2>("obstacles");
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(ground->subscription));
|
||||
|
||||
pub->publish(makeXYZCloud("lidar", 1000.0, planeLevelInMapFrame(pitch)));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }));
|
||||
|
||||
EXPECT_GT(ground->back().width, 0u)
|
||||
<< "leveled by the map pitch, the slope is ground -- roll/pitch apply even "
|
||||
"though Grid/MapFrameProjection is false";
|
||||
}
|
||||
|
||||
TEST_F(ObstaclesDetectionTest, MapFrameProjectionSegmentsRelativeToTheMap)
|
||||
{
|
||||
// Same setup plus Grid/MapFrameProjection=true. Now pose.z() participates, the whole
|
||||
// cloud sits 1 m up in the map frame, and nothing is below the 0.2 m ground
|
||||
// threshold any more.
|
||||
addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("frame_id", "base_link"),
|
||||
rclcpp::Parameter("map_frame_id", "map"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.1),
|
||||
rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")),
|
||||
rclcpp::Parameter("Grid/MapFrameProjection", std::string("true")),
|
||||
rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.2"))})));
|
||||
publishStaticTf("base_link", "lidar");
|
||||
publishStaticTf("map", "base_link", 0.0, 0.0, 1.0);
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
|
||||
collect<sensor_msgs::msg::PointCloud2>("ground");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
|
||||
collect<sensor_msgs::msg::PointCloud2>("obstacles");
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(ground->subscription));
|
||||
|
||||
pub->publish(makeXYZCloud("lidar", 1000.0, groundAndWall()));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }));
|
||||
|
||||
EXPECT_EQ(ground->back().width, 0u)
|
||||
<< "lifted 1 m in the map frame, nothing is below the ground threshold";
|
||||
EXPECT_GT(obstacles->back().width, 0u) << "everything becomes an obstacle instead";
|
||||
}
|
||||
|
||||
/// Runs the node with the given Grid range settings and returns the ground cloud.
|
||||
class ObstaclesDetectionRangeTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
sensor_msgs::msg::PointCloud2 groundWithRange(
|
||||
const std::string & rangeMin, const std::string & rangeMax,
|
||||
const std::vector<cv::Point3f> & points)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("frame_id", "base_link"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.1),
|
||||
rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")),
|
||||
rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.2")),
|
||||
rclcpp::Parameter("Grid/RangeMin", rangeMin),
|
||||
rclcpp::Parameter("Grid/RangeMax", rangeMax)})));
|
||||
publishStaticTf("base_link", "lidar");
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
|
||||
collect<sensor_msgs::msg::PointCloud2>("ground");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
|
||||
collect<sensor_msgs::msg::PointCloud2>("obstacles");
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
|
||||
EXPECT_TRUE(waitForSubscriber(pub));
|
||||
EXPECT_TRUE(waitForPublisher(ground->subscription));
|
||||
|
||||
pub->publish(makeXYZCloud("lidar", 1000.0, points));
|
||||
EXPECT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }));
|
||||
return ground->empty() ? sensor_msgs::msg::PointCloud2() : ground->back();
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(ObstaclesDetectionRangeTest, RangeFilteringDisabledKeepsEverything)
|
||||
{
|
||||
// Grid/RangeMax=0 means no upper limit, so both patches survive.
|
||||
const sensor_msgs::msg::PointCloud2 ground =
|
||||
groundWithRange("0.0", "0.0", nearAndFarPatches());
|
||||
EXPECT_EQ(ground.width, 50u);
|
||||
}
|
||||
|
||||
TEST_F(ObstaclesDetectionRangeTest, GridRangeMaxDropsDistantPoints)
|
||||
{
|
||||
// Only the patch inside 1 m survives.
|
||||
const sensor_msgs::msg::PointCloud2 ground =
|
||||
groundWithRange("0.0", "1.0", nearAndFarPatches());
|
||||
ASSERT_EQ(ground.width, 25u);
|
||||
|
||||
const std::pair<float, float> extent = xExtent(ground);
|
||||
EXPECT_NEAR(extent.first, 0.4f, 1e-3);
|
||||
EXPECT_LT(extent.second, 1.0f) << "nothing beyond the 1 m limit may remain";
|
||||
}
|
||||
|
||||
TEST_F(ObstaclesDetectionRangeTest, GridRangeMinDropsNearbyPoints)
|
||||
{
|
||||
// The mirror image: everything closer than 1 m is discarded instead.
|
||||
const sensor_msgs::msg::PointCloud2 ground =
|
||||
groundWithRange("1.0", "0.0", nearAndFarPatches());
|
||||
ASSERT_EQ(ground.width, 25u);
|
||||
|
||||
const std::pair<float, float> extent = xExtent(ground);
|
||||
EXPECT_GT(extent.first, 1.0f) << "nothing closer than the 1 m limit may remain";
|
||||
EXPECT_NEAR(extent.second, 3.1f, 1e-3);
|
||||
}
|
||||
|
||||
TEST_F(ObstaclesDetectionRangeTest, DefaultRangeMaxIsFiveMeters)
|
||||
{
|
||||
// Grid/RangeMax defaults to 5.0, not infinity: a patch at 6 m is silently dropped
|
||||
// even though no range parameter was set.
|
||||
std::vector<cv::Point3f> points = nearAndFarPatches();
|
||||
for(int i=0; i<5; ++i)
|
||||
{
|
||||
for(int j=0; j<5; ++j)
|
||||
{
|
||||
points.push_back(cv::Point3f(5.9f + 0.05f*i, -0.1f + 0.05f*j, 0.0f));
|
||||
}
|
||||
}
|
||||
|
||||
const sensor_msgs::msg::PointCloud2 ground =
|
||||
groundWithRange("0.0", "5.0", points); // the defaults, stated explicitly
|
||||
EXPECT_EQ(ground.width, 50u) << "the 6 m patch is beyond the default range";
|
||||
EXPECT_LT(xExtent(ground).second, 5.0f);
|
||||
}
|
||||
|
||||
TEST_F(ObstaclesDetectionTest, PublishesEmptyCloudsForAnEmptyInput)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({rclcpp::Parameter("frame_id", "base_link")})));
|
||||
publishStaticTf("base_link", "lidar");
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
|
||||
collect<sensor_msgs::msg::PointCloud2>("ground");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
|
||||
collect<sensor_msgs::msg::PointCloud2>("obstacles");
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(ground->subscription));
|
||||
|
||||
pub->publish(makeXYZCloud("lidar", 1000.0, {}));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }))
|
||||
<< "an empty input must still produce output, not a dropped message";
|
||||
|
||||
EXPECT_EQ(ground->back().width, 0u);
|
||||
EXPECT_EQ(obstacles->back().width, 0u);
|
||||
}
|
||||
@@ -0,0 +1,194 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include "node_test_utils.hpp"
|
||||
#include "msg_builders.hpp"
|
||||
|
||||
#include <rtabmap_util/point_cloud_aggregator.hpp>
|
||||
|
||||
#include <tf2_msgs/msg/tf_message.hpp>
|
||||
|
||||
using namespace rtabmap_util_test;
|
||||
|
||||
namespace {
|
||||
::testing::Environment * const kEnv = registerRclcppEnvironment();
|
||||
}
|
||||
|
||||
class PointCloudAggregatorTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
/// odom -> base_link advancing along x at 1 m/s across the two cloud stamps.
|
||||
void publishOdomMotion(double startStamp, double duration)
|
||||
{
|
||||
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub =
|
||||
helper()->create_publisher<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
|
||||
spinFor(std::chrono::milliseconds(100));
|
||||
for(int i=0; i<=6; ++i)
|
||||
{
|
||||
const double elapsed = duration * double(i) / 6.0;
|
||||
geometry_msgs::msg::TransformStamped t;
|
||||
t.header.stamp = stampOf(startStamp + elapsed);
|
||||
t.header.frame_id = "odom";
|
||||
t.child_frame_id = "base_link";
|
||||
t.transform.translation.x = elapsed; // 1 m/s
|
||||
t.transform.rotation.w = 1.0;
|
||||
tf2_msgs::msg::TFMessage msg;
|
||||
msg.transforms.push_back(t);
|
||||
tfPub->publish(msg);
|
||||
}
|
||||
spinFor(std::chrono::milliseconds(200));
|
||||
tfPub_ = tfPub;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Publishes three pairs of clouds observing one landmark 5 m ahead in odom.
|
||||
*
|
||||
* Pair k is stamped at 1000.0+0.2k and 0.1 s later. The robot drives at 1 m/s, so
|
||||
* each sensor measures the landmark at 5 m minus the distance travelled by then.
|
||||
*/
|
||||
void publishPairs(
|
||||
const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & pub1,
|
||||
const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & pub2)
|
||||
{
|
||||
for(int k=0; k<3; ++k)
|
||||
{
|
||||
const double t1 = 1000.0 + 0.2*double(k);
|
||||
const double t2 = t1 + 0.1;
|
||||
pub1->publish(makeXYZCloud("lidar_a", t1, {{float(5.0-(t1-1000.0)), 0.0f, 0.0f}}));
|
||||
pub2->publish(makeXYZCloud("lidar_b", t2, {{float(5.0-(t2-1000.0)), 0.0f, 0.0f}}));
|
||||
spinFor(std::chrono::milliseconds(50));
|
||||
}
|
||||
}
|
||||
|
||||
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub_;
|
||||
};
|
||||
|
||||
TEST_F(PointCloudAggregatorTest, CombinesTwoSynchronizedClouds)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::PointCloudAggregator>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("count", 2),
|
||||
rclcpp::Parameter("frame_id", "base_link"),
|
||||
rclcpp::Parameter("approx_sync", true),
|
||||
rclcpp::Parameter("wait_for_transform", 0.2)})));
|
||||
publishStaticTf("base_link", "lidar_a", 0.0, 0.2, 0.0);
|
||||
publishStaticTf("base_link", "lidar_b", 0.0, -0.2, 0.0);
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
|
||||
collect<sensor_msgs::msg::PointCloud2>("combined_cloud");
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub1 =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud1", 10);
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub2 =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud2", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub1));
|
||||
ASSERT_TRUE(waitForSubscriber(pub2));
|
||||
|
||||
const std::vector<cv::Point3f> a = {{1.0f, 0.0f, 0.0f}, {2.0f, 0.0f, 0.0f}};
|
||||
const std::vector<cv::Point3f> b = {{3.0f, 0.0f, 0.0f}};
|
||||
pub1->publish(makeXYZCloud("lidar_a", 1000.0, a));
|
||||
pub2->publish(makeXYZCloud("lidar_b", 1000.0, b));
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })) << "no combined cloud published";
|
||||
EXPECT_EQ(out->back().width, a.size() + b.size()) << "every input point must survive";
|
||||
EXPECT_EQ(out->back().header.frame_id, "base_link")
|
||||
<< "the combined cloud is expressed in frame_id";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudAggregatorTest, AlignsCloudsCapturedAtDifferentTimesWhileMoving)
|
||||
{
|
||||
// The two sensors fire 0.1 s apart while the robot drives forward at 1 m/s, so they
|
||||
// see the same world point at different ranges. With fixed_frame_id set, the second
|
||||
// cloud is motion-compensated back to the first one's stamp and the two coincide.
|
||||
addNode(std::make_shared<rtabmap_util::PointCloudAggregator>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("count", 2),
|
||||
rclcpp::Parameter("frame_id", "base_link"),
|
||||
rclcpp::Parameter("fixed_frame_id", "odom"),
|
||||
rclcpp::Parameter("approx_sync", true),
|
||||
rclcpp::Parameter("wait_for_transform", 0.2)})));
|
||||
publishStaticTf("base_link", "lidar_a");
|
||||
publishStaticTf("base_link", "lidar_b");
|
||||
publishOdomMotion(1000.0, 0.6);
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
|
||||
collect<sensor_msgs::msg::PointCloud2>("combined_cloud");
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub1 =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud1", 10);
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub2 =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud2", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub1));
|
||||
ASSERT_TRUE(waitForSubscriber(pub2));
|
||||
|
||||
// A landmark 5 m ahead in odom, the robot driving at 1 m/s. Several pairs are sent
|
||||
// because the ApproximateTime policy needs a following message before it can commit
|
||||
// to a match when the stamps differ; the first emitted pair is the one asserted on.
|
||||
publishPairs(pub1, pub2);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })) << "no combined cloud";
|
||||
const sensor_msgs::msg::PointCloud2 & cloud = out->front();
|
||||
ASSERT_EQ(cloud.width, 2u);
|
||||
|
||||
// Both observations of the same landmark must land on the same point.
|
||||
EXPECT_NEAR(readXYZ(cloud, 0).x, 5.0f, 5e-3);
|
||||
EXPECT_NEAR(readXYZ(cloud, 1).x, 5.0f, 5e-3)
|
||||
<< "the later cloud must be compensated for the 0.1 m of motion";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudAggregatorTest, WithoutAFixedFrameCloudsAreNotMotionCompensated)
|
||||
{
|
||||
// Same inputs, no fixed_frame_id: the second cloud is taken at face value and the
|
||||
// two observations stay 0.1 m apart. This is what fixed_frame_id exists to fix.
|
||||
addNode(std::make_shared<rtabmap_util::PointCloudAggregator>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("count", 2),
|
||||
rclcpp::Parameter("frame_id", "base_link"),
|
||||
rclcpp::Parameter("approx_sync", true),
|
||||
rclcpp::Parameter("wait_for_transform", 0.2)})));
|
||||
publishStaticTf("base_link", "lidar_a");
|
||||
publishStaticTf("base_link", "lidar_b");
|
||||
publishOdomMotion(1000.0, 0.6);
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
|
||||
collect<sensor_msgs::msg::PointCloud2>("combined_cloud");
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub1 =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud1", 10);
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub2 =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud2", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub1));
|
||||
ASSERT_TRUE(waitForSubscriber(pub2));
|
||||
|
||||
publishPairs(pub1, pub2);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out->empty(); }));
|
||||
const sensor_msgs::msg::PointCloud2 & cloud = out->front();
|
||||
ASSERT_EQ(cloud.width, 2u);
|
||||
|
||||
EXPECT_NEAR(readXYZ(cloud, 0).x, 5.0f, 5e-3);
|
||||
EXPECT_NEAR(readXYZ(cloud, 1).x, 4.9f, 5e-3)
|
||||
<< "uncompensated, the second observation stays where it was measured";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudAggregatorTest, WaitsForEveryInput)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::PointCloudAggregator>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("count", 2),
|
||||
rclcpp::Parameter("frame_id", "base_link"),
|
||||
rclcpp::Parameter("approx_sync", true)})));
|
||||
publishStaticTf("base_link", "lidar_a");
|
||||
publishStaticTf("base_link", "lidar_b");
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
|
||||
collect<sensor_msgs::msg::PointCloud2>("combined_cloud");
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub1 =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud1", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub1));
|
||||
|
||||
// Only one of the two inputs arrives: the synchronizer must not fire.
|
||||
pub1->publish(makeXYZCloud("lidar_a", 1000.0, {{1.0f, 0.0f, 0.0f}}));
|
||||
spinFor(std::chrono::milliseconds(500));
|
||||
|
||||
EXPECT_TRUE(out->empty()) << "a single input must not produce a combined cloud";
|
||||
}
|
||||
@@ -0,0 +1,399 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include "node_test_utils.hpp"
|
||||
#include "msg_builders.hpp"
|
||||
|
||||
#include <rtabmap_util/point_cloud_assembler.hpp>
|
||||
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
#include <rtabmap_msgs/msg/odom_info.hpp>
|
||||
|
||||
using namespace rtabmap_util_test;
|
||||
|
||||
namespace {
|
||||
::testing::Environment * const kEnv = registerRclcppEnvironment();
|
||||
}
|
||||
|
||||
class PointCloudAssemblerTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
/// Starts the assembler with @p overrides, plus a static odom -> lidar transform.
|
||||
void start(const std::vector<rclcpp::Parameter> & overrides)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::PointCloudAssembler>(
|
||||
rclcpp::NodeOptions().parameter_overrides(overrides)));
|
||||
publishStaticTf("odom", "lidar");
|
||||
publishStaticTf("lidar", "base_link");
|
||||
out_ = collect<sensor_msgs::msg::PointCloud2>("assembled_cloud");
|
||||
pub_ = helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub_));
|
||||
}
|
||||
|
||||
static bool hasField(const sensor_msgs::msg::PointCloud2 & cloud, const std::string & name)
|
||||
{
|
||||
for(size_t i=0; i<cloud.fields.size(); ++i)
|
||||
{
|
||||
if(cloud.fields[i].name == name) { return true; }
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_;
|
||||
};
|
||||
|
||||
TEST_F(PointCloudAssemblerTest, PublishesAfterMaxCloudsAreAccumulated)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::PointCloudAssembler>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("max_clouds", 3),
|
||||
rclcpp::Parameter("fixed_frame_id", "odom"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.2)})));
|
||||
publishStaticTf("odom", "lidar");
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
|
||||
collect<sensor_msgs::msg::PointCloud2>("assembled_cloud");
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
|
||||
const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}, {2.0f, 0.0f, 0.0f}};
|
||||
|
||||
// The first two clouds are only accumulated.
|
||||
pub->publish(makeXYZCloud("lidar", 1000.0, points));
|
||||
pub->publish(makeXYZCloud("lidar", 1000.1, points));
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
EXPECT_TRUE(out->empty()) << "nothing is published before max_clouds is reached";
|
||||
|
||||
// The third completes the batch.
|
||||
pub->publish(makeXYZCloud("lidar", 1000.2, points));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out->empty(); }))
|
||||
<< "the assembled cloud must be published on the third input";
|
||||
|
||||
EXPECT_EQ(out->back().width, 3 * points.size()) << "all three clouds must be included";
|
||||
EXPECT_EQ(out->back().header.frame_id, "lidar")
|
||||
<< "the assembled cloud comes back in the sensor frame";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudAssemblerTest, AssemblingTimePublishesAfterTheConfiguredSpan)
|
||||
{
|
||||
// An alternative trigger to max_clouds: publish once the newest cloud is at least
|
||||
// assembling_time newer than the oldest one held.
|
||||
start({rclcpp::Parameter("max_clouds", 0),
|
||||
rclcpp::Parameter("assembling_time", 0.25),
|
||||
rclcpp::Parameter("fixed_frame_id", "odom"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.2)});
|
||||
|
||||
const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
|
||||
for(int i=0; i<3; ++i) // 1000.0, 1000.1, 1000.2 -- span 0.2 s, below 0.25
|
||||
{
|
||||
pub_->publish(makeXYZCloud("lidar", 1000.0 + 0.1*i, points));
|
||||
}
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
EXPECT_TRUE(out_->empty()) << "0.2 s of clouds is short of assembling_time";
|
||||
|
||||
pub_->publish(makeXYZCloud("lidar", 1000.3, points)); // span now 0.3 s
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }))
|
||||
<< "crossing assembling_time must publish";
|
||||
EXPECT_EQ(out_->back().width, 4u) << "all four clouds are included";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudAssemblerTest, CircularBufferPublishesOnEveryCloud)
|
||||
{
|
||||
// With a circular buffer the node emits a sliding window instead of filling up,
|
||||
// clearing and starting again: every input produces an output.
|
||||
start({rclcpp::Parameter("max_clouds", 3),
|
||||
rclcpp::Parameter("circular_buffer", true),
|
||||
rclcpp::Parameter("fixed_frame_id", "odom"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.2)});
|
||||
|
||||
const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
|
||||
for(int i=0; i<4; ++i)
|
||||
{
|
||||
pub_->publish(makeXYZCloud("lidar", 1000.0 + 0.1*i, points));
|
||||
ASSERT_TRUE(spinUntil([&]() { return out_->size() >= size_t(i+1); }))
|
||||
<< "cloud " << i << " did not produce an output";
|
||||
}
|
||||
EXPECT_EQ(out_->size(), 4u) << "one output per input, not one per full batch";
|
||||
// The window is capped at max_clouds.
|
||||
EXPECT_LE(out_->back().width, 3u);
|
||||
}
|
||||
|
||||
TEST_F(PointCloudAssemblerTest, RangeMaxDropsDistantPoints)
|
||||
{
|
||||
start({rclcpp::Parameter("max_clouds", 1),
|
||||
rclcpp::Parameter("range_max", 3.0),
|
||||
rclcpp::Parameter("fixed_frame_id", "odom"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.2)});
|
||||
|
||||
// Two points inside 3 m, one well beyond it.
|
||||
pub_->publish(makeXYZCloud("lidar", 1000.0,
|
||||
{{1.0f, 0.0f, 0.0f}, {2.0f, 0.0f, 0.0f}, {9.0f, 0.0f, 0.0f}}));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
EXPECT_EQ(out_->back().width, 2u) << "the 9 m point must be filtered out";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudAssemblerTest, RemoveZDropsTheZField)
|
||||
{
|
||||
start({rclcpp::Parameter("max_clouds", 1),
|
||||
rclcpp::Parameter("remove_z", true),
|
||||
rclcpp::Parameter("fixed_frame_id", "odom"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.2)});
|
||||
|
||||
pub_->publish(makeXYZCloud("lidar", 1000.0, {{1.0f, 0.0f, 0.5f}}));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
// The field is removed entirely, not zeroed: the output is a 2D cloud.
|
||||
EXPECT_TRUE(hasField(out_->back(), "x"));
|
||||
EXPECT_TRUE(hasField(out_->back(), "y"));
|
||||
EXPECT_FALSE(hasField(out_->back(), "z")) << "remove_z drops the field itself";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudAssemblerTest, FrameIdSetsTheOutputFrame)
|
||||
{
|
||||
start({rclcpp::Parameter("max_clouds", 1),
|
||||
rclcpp::Parameter("frame_id", "base_link"),
|
||||
rclcpp::Parameter("fixed_frame_id", "odom"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.2)});
|
||||
|
||||
pub_->publish(makeXYZCloud("lidar", 1000.0, {{1.0f, 0.0f, 0.0f}}));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
EXPECT_EQ(out_->back().header.frame_id, "base_link")
|
||||
<< "the assembled cloud is returned in frame_id when it is set";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudAssemblerTest, SkipCloudsIgnoresIntermediateClouds)
|
||||
{
|
||||
// skip_clouds=1 keeps every other cloud, so reaching max_clouds=2 takes four inputs.
|
||||
start({rclcpp::Parameter("max_clouds", 2),
|
||||
rclcpp::Parameter("skip_clouds", 1),
|
||||
rclcpp::Parameter("fixed_frame_id", "odom"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.2)});
|
||||
|
||||
const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
|
||||
pub_->publish(makeXYZCloud("lidar", 1000.0, points));
|
||||
pub_->publish(makeXYZCloud("lidar", 1000.1, points));
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
EXPECT_TRUE(out_->empty()) << "one of those two was skipped, so the batch is short";
|
||||
|
||||
pub_->publish(makeXYZCloud("lidar", 1000.2, points));
|
||||
pub_->publish(makeXYZCloud("lidar", 1000.3, points));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
EXPECT_EQ(out_->back().width, 2u) << "two kept clouds, two skipped";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudAssemblerTest, LinearUpdateSkipsCloudsWhileStationary)
|
||||
{
|
||||
// With linear_update set, a cloud captured without the robot having moved far enough
|
||||
// is discarded rather than accumulated, so a parked robot never fills a batch.
|
||||
start({rclcpp::Parameter("max_clouds", 3),
|
||||
rclcpp::Parameter("linear_update", 0.5),
|
||||
rclcpp::Parameter("fixed_frame_id", "odom"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.2)});
|
||||
|
||||
const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
|
||||
for(int i=0; i<5; ++i) // the TF is static, so the robot never moves
|
||||
{
|
||||
pub_->publish(makeXYZCloud("lidar", 1000.0 + 0.1*i, points));
|
||||
}
|
||||
spinFor(std::chrono::milliseconds(500));
|
||||
|
||||
EXPECT_TRUE(out_->empty())
|
||||
<< "a stationary robot must not accumulate a batch when linear_update is set";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudAssemblerTest, WithoutLinearUpdateEveryCloudCounts)
|
||||
{
|
||||
// The same stationary robot, with the motion filter disabled: the batch fills.
|
||||
start({rclcpp::Parameter("max_clouds", 3),
|
||||
rclcpp::Parameter("linear_update", 0.0),
|
||||
rclcpp::Parameter("fixed_frame_id", "odom"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.2)});
|
||||
|
||||
const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
|
||||
for(int i=0; i<3; ++i)
|
||||
{
|
||||
pub_->publish(makeXYZCloud("lidar", 1000.0 + 0.1*i, points));
|
||||
}
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
EXPECT_EQ(out_->back().width, 3u);
|
||||
}
|
||||
|
||||
TEST_F(PointCloudAssemblerTest, VoxelSizeDownsamplesTheCloud)
|
||||
{
|
||||
start({rclcpp::Parameter("max_clouds", 1),
|
||||
rclcpp::Parameter("voxel_size", 0.5),
|
||||
rclcpp::Parameter("fixed_frame_id", "odom"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.2)});
|
||||
|
||||
// 100 points packed into a 0.2 m cube: a 0.5 m voxel grid collapses them.
|
||||
std::vector<cv::Point3f> dense;
|
||||
for(int i=0; i<10; ++i)
|
||||
{
|
||||
for(int j=0; j<10; ++j)
|
||||
{
|
||||
dense.push_back(cv::Point3f(1.0f + 0.02f*i, 0.02f*j, 0.0f));
|
||||
}
|
||||
}
|
||||
pub_->publish(makeXYZCloud("lidar", 1000.0, dense));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
EXPECT_LT(out_->back().width, dense.size())
|
||||
<< "voxel_size must reduce the point count";
|
||||
EXPECT_GT(out_->back().width, 0u);
|
||||
}
|
||||
|
||||
/// Fixture for the odometry-synchronized modes, which need fixed_frame_id to be empty.
|
||||
class PointCloudAssemblerOdomTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
void start(std::vector<rclcpp::Parameter> overrides)
|
||||
{
|
||||
// fixed_frame_id defaults to "odom"; it has to be cleared for the node to
|
||||
// subscribe to the odometry topic instead of reading TF directly.
|
||||
overrides.push_back(rclcpp::Parameter("fixed_frame_id", ""));
|
||||
addNode(std::make_shared<rtabmap_util::PointCloudAssembler>(
|
||||
rclcpp::NodeOptions().parameter_overrides(overrides)));
|
||||
publishStaticTf("odom", "lidar");
|
||||
out_ = collect<sensor_msgs::msg::PointCloud2>("assembled_cloud");
|
||||
cloudPub_ = helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
|
||||
odomPub_ = helper()->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
|
||||
odomInfoPub_ = helper()->create_publisher<rtabmap_msgs::msg::OdomInfo>("odom_info", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(cloudPub_));
|
||||
ASSERT_TRUE(waitForSubscriber(odomPub_));
|
||||
}
|
||||
|
||||
/// An odometry message at the origin; a null one has an all-zero orientation.
|
||||
nav_msgs::msg::Odometry makeOdom(double stamp, bool null = false)
|
||||
{
|
||||
nav_msgs::msg::Odometry odom;
|
||||
odom.header.stamp = stampOf(stamp);
|
||||
odom.header.frame_id = "odom";
|
||||
odom.child_frame_id = "lidar";
|
||||
odom.pose.pose.orientation.w = null ? 0.0 : 1.0;
|
||||
return odom;
|
||||
}
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub_;
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odomPub_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::OdomInfo>::SharedPtr odomInfoPub_;
|
||||
};
|
||||
|
||||
TEST_F(PointCloudAssemblerOdomTest, TakesTheFixedFrameFromTheOdometryMessage)
|
||||
{
|
||||
// With fixed_frame_id empty the node syncs cloud with odom and uses the odometry
|
||||
// header's frame as the fixed frame.
|
||||
start({rclcpp::Parameter("max_clouds", 2),
|
||||
rclcpp::Parameter("wait_for_transform", 0.2)});
|
||||
|
||||
const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
|
||||
for(int i=0; i<2; ++i)
|
||||
{
|
||||
const double t = 1000.0 + 0.1*i;
|
||||
cloudPub_->publish(makeXYZCloud("lidar", t, points));
|
||||
odomPub_->publish(makeOdom(t)); // exact sync: identical stamps
|
||||
spinFor(std::chrono::milliseconds(50));
|
||||
}
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }))
|
||||
<< "cloud+odom synchronization must drive the assembly";
|
||||
EXPECT_EQ(out_->back().width, 2u);
|
||||
}
|
||||
|
||||
TEST_F(PointCloudAssemblerOdomTest, NullOdometryResetsTheBuffer)
|
||||
{
|
||||
// A null odometry means tracking was lost, so the accumulated clouds are dropped
|
||||
// rather than being stitched across the discontinuity.
|
||||
start({rclcpp::Parameter("max_clouds", 3),
|
||||
rclcpp::Parameter("wait_for_transform", 0.2)});
|
||||
|
||||
const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
|
||||
|
||||
// Two good clouds, then a lost-tracking frame, then two more.
|
||||
for(int i=0; i<2; ++i)
|
||||
{
|
||||
const double t = 1000.0 + 0.1*i;
|
||||
cloudPub_->publish(makeXYZCloud("lidar", t, points));
|
||||
odomPub_->publish(makeOdom(t));
|
||||
spinFor(std::chrono::milliseconds(50));
|
||||
}
|
||||
cloudPub_->publish(makeXYZCloud("lidar", 1000.2, points));
|
||||
odomPub_->publish(makeOdom(1000.2, /*null=*/true));
|
||||
spinFor(std::chrono::milliseconds(150));
|
||||
EXPECT_TRUE(out_->empty()) << "the null odometry must not complete the batch";
|
||||
|
||||
// After the reset it takes three fresh clouds again, not one.
|
||||
for(int i=0; i<2; ++i)
|
||||
{
|
||||
const double t = 1000.3 + 0.1*i;
|
||||
cloudPub_->publish(makeXYZCloud("lidar", t, points));
|
||||
odomPub_->publish(makeOdom(t));
|
||||
spinFor(std::chrono::milliseconds(50));
|
||||
}
|
||||
EXPECT_TRUE(out_->empty()) << "the buffer restarted, so two clouds are not enough";
|
||||
|
||||
cloudPub_->publish(makeXYZCloud("lidar", 1000.5, points));
|
||||
odomPub_->publish(makeOdom(1000.5));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
EXPECT_EQ(out_->back().width, 3u) << "only the post-reset clouds are assembled";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudAssemblerOdomTest, SubscribeOdomInfoKeepsOnlyKeyFrames)
|
||||
{
|
||||
// With subscribe_odom_info the node also takes OdomInfo and accumulates a cloud only
|
||||
// when that frame became a key frame.
|
||||
start({rclcpp::Parameter("max_clouds", 2),
|
||||
rclcpp::Parameter("subscribe_odom_info", true),
|
||||
rclcpp::Parameter("wait_for_transform", 0.2)});
|
||||
ASSERT_TRUE(waitForSubscriber(odomInfoPub_));
|
||||
|
||||
const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
|
||||
auto publishFrame = [&](double t, bool keyFrame) {
|
||||
rtabmap_msgs::msg::OdomInfo info;
|
||||
info.header.stamp = stampOf(t);
|
||||
info.header.frame_id = "odom";
|
||||
info.key_frame_added = keyFrame;
|
||||
cloudPub_->publish(makeXYZCloud("lidar", t, points));
|
||||
odomPub_->publish(makeOdom(t));
|
||||
odomInfoPub_->publish(info);
|
||||
spinFor(std::chrono::milliseconds(60));
|
||||
};
|
||||
|
||||
publishFrame(1000.0, false);
|
||||
publishFrame(1000.1, false);
|
||||
publishFrame(1000.2, false);
|
||||
EXPECT_TRUE(out_->empty()) << "non key frames must be ignored";
|
||||
|
||||
publishFrame(1000.3, true);
|
||||
publishFrame(1000.4, true);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
EXPECT_EQ(out_->back().width, 2u) << "only the two key frames are assembled";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudAssemblerTest, DropsCloudsWithoutTheFixedFrame)
|
||||
{
|
||||
// No TF at all, so the assembler cannot place the clouds relative to each other.
|
||||
addNode(std::make_shared<rtabmap_util::PointCloudAssembler>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("max_clouds", 2),
|
||||
rclcpp::Parameter("fixed_frame_id", "odom"),
|
||||
rclcpp::Parameter("wait_for_transform", 0.0)})));
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
|
||||
collect<sensor_msgs::msg::PointCloud2>("assembled_cloud");
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
|
||||
pub->publish(makeXYZCloud("lidar", 1000.0, {{1.0f, 0.0f, 0.0f}}));
|
||||
pub->publish(makeXYZCloud("lidar", 1000.1, {{1.0f, 0.0f, 0.0f}}));
|
||||
spinFor(std::chrono::milliseconds(500));
|
||||
|
||||
EXPECT_TRUE(out->empty());
|
||||
}
|
||||
@@ -0,0 +1,344 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include "node_test_utils.hpp"
|
||||
#include "msg_builders.hpp"
|
||||
|
||||
#include <rtabmap_util/point_cloud_xyz.hpp>
|
||||
|
||||
#include <stereo_msgs/msg/disparity_image.hpp>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
using namespace rtabmap_util_test;
|
||||
|
||||
namespace {
|
||||
::testing::Environment * const kEnv = registerRclcppEnvironment();
|
||||
|
||||
constexpr int kWidth = 16;
|
||||
constexpr int kHeight = 16;
|
||||
constexpr double kFx = 100.0;
|
||||
|
||||
/// A depth image where every pixel is at @p meters.
|
||||
sensor_msgs::msg::Image makeDepth(
|
||||
double stamp, float meters,
|
||||
const std::string & encoding = sensor_msgs::image_encodings::TYPE_32FC1)
|
||||
{
|
||||
cv::Mat image;
|
||||
if(encoding == sensor_msgs::image_encodings::TYPE_32FC1)
|
||||
{
|
||||
image = cv::Mat(kHeight, kWidth, CV_32FC1, cv::Scalar(meters));
|
||||
}
|
||||
else
|
||||
{
|
||||
image = cv::Mat(kHeight, kWidth, CV_16UC1, cv::Scalar(uint16_t(meters*1000.0f)));
|
||||
}
|
||||
return makeImage("camera_link", stamp, image, encoding);
|
||||
}
|
||||
|
||||
/// A disparity image where every pixel carries @p disparity, so depth = f*t/disparity.
|
||||
stereo_msgs::msg::DisparityImage makeDisparity(
|
||||
double stamp, float disparity, float focal = float(kFx), float baseline = 0.1f)
|
||||
{
|
||||
stereo_msgs::msg::DisparityImage msg;
|
||||
msg.header.frame_id = "camera_link";
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.f = focal;
|
||||
msg.t = baseline;
|
||||
msg.min_disparity = 1.0f;
|
||||
msg.max_disparity = 100.0f;
|
||||
msg.image = makeImage("camera_link", stamp,
|
||||
cv::Mat(kHeight, kWidth, CV_32FC1, cv::Scalar(disparity)),
|
||||
sensor_msgs::image_encodings::TYPE_32FC1);
|
||||
return msg;
|
||||
}
|
||||
|
||||
/// The same, in the 16SC1 fixed-point form where the stored value is 16*disparity.
|
||||
stereo_msgs::msg::DisparityImage makeDisparity16SC1(double stamp, float disparity)
|
||||
{
|
||||
stereo_msgs::msg::DisparityImage msg = makeDisparity(stamp, disparity);
|
||||
msg.image = makeImage("camera_link", stamp,
|
||||
cv::Mat(kHeight, kWidth, CV_16SC1, cv::Scalar(short(disparity*16.0f))),
|
||||
sensor_msgs::image_encodings::TYPE_16SC1);
|
||||
return msg;
|
||||
}
|
||||
|
||||
bool hasField(const sensor_msgs::msg::PointCloud2 & cloud, const std::string & name)
|
||||
{
|
||||
for(size_t i=0; i<cloud.fields.size(); ++i)
|
||||
{
|
||||
if(cloud.fields[i].name == name) { return true; }
|
||||
}
|
||||
return false;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
class PointCloudXYZTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
void start(const std::vector<rclcpp::Parameter> & overrides = {})
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::PointCloudXYZ>(
|
||||
rclcpp::NodeOptions().parameter_overrides(overrides)));
|
||||
out_ = collect<sensor_msgs::msg::PointCloud2>("cloud");
|
||||
depthPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
|
||||
infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>("depth/camera_info", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(depthPub_));
|
||||
ASSERT_TRUE(waitForSubscriber(infoPub_));
|
||||
ASSERT_TRUE(waitForPublisher(out_->subscription));
|
||||
}
|
||||
|
||||
/// Publishes a synchronized depth + camera_info pair.
|
||||
void publishFrame(double stamp, float meters,
|
||||
const std::string & encoding = sensor_msgs::image_encodings::TYPE_32FC1)
|
||||
{
|
||||
depthPub_->publish(makeDepth(stamp, meters, encoding));
|
||||
infoPub_->publish(makeCameraInfo("camera_link", stamp, kWidth, kHeight, 0.0, kFx));
|
||||
}
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depthPub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub_;
|
||||
};
|
||||
|
||||
TEST_F(PointCloudXYZTest, ProjectsDepthIntoACloud)
|
||||
{
|
||||
start();
|
||||
publishFrame(1000.0, 2.0f);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
|
||||
|
||||
const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
|
||||
EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight))
|
||||
<< "one point per pixel at decimation 1";
|
||||
EXPECT_EQ(cloud.header.frame_id, "camera_link")
|
||||
<< "the cloud takes the depth image's frame";
|
||||
|
||||
// The principal-point pixel projects straight ahead at the measured depth.
|
||||
const size_t center = size_t(kHeight/2) * kWidth + kWidth/2;
|
||||
EXPECT_NEAR(readXYZ(cloud, center).z, 2.0f, 1e-3);
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZTest, Accepts16UC1Millimeters)
|
||||
{
|
||||
start();
|
||||
publishFrame(1000.0, 2.0f, sensor_msgs::image_encodings::TYPE_16UC1);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
const size_t center = size_t(kHeight/2) * kWidth + kWidth/2;
|
||||
EXPECT_NEAR(readXYZ(out_->back(), center).z, 2.0f, 1e-3)
|
||||
<< "millimeter depth must be converted to meters";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZTest, RejectsUnsupportedEncoding)
|
||||
{
|
||||
start();
|
||||
depthPub_->publish(makeImage("camera_link", 1000.0,
|
||||
cv::Mat(kHeight, kWidth, CV_8UC3, cv::Scalar(1,2,3)), "bgr8"));
|
||||
infoPub_->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx));
|
||||
spinFor(std::chrono::milliseconds(400));
|
||||
|
||||
EXPECT_TRUE(out_->empty()) << "only 32FC1, 16UC1 and mono16 depth are supported";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZTest, DecimationReducesThePointCount)
|
||||
{
|
||||
start({rclcpp::Parameter("decimation", 2)});
|
||||
publishFrame(1000.0, 2.0f);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
EXPECT_EQ(out_->back().width * out_->back().height, uint32_t(kWidth*kHeight)/4)
|
||||
<< "decimation 2 keeps one pixel in four";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZTest, MaxDepthMarksFarPointsInvalid)
|
||||
{
|
||||
// cloudFromDepth keeps the cloud organized: points outside the depth range become
|
||||
// NaN rather than disappearing, so the point count is unchanged.
|
||||
start({rclcpp::Parameter("max_depth", 1.0)});
|
||||
publishFrame(1000.0, 5.0f); // beyond the limit
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
|
||||
EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight))
|
||||
<< "the cloud stays organized";
|
||||
EXPECT_TRUE(std::isnan(readXYZ(cloud, 0).z)) << "every point is past max_depth";
|
||||
EXPECT_TRUE(std::isnan(readXYZ(cloud, kWidth*kHeight-1).z));
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZTest, WithinMaxDepthPointsStayValid)
|
||||
{
|
||||
start({rclcpp::Parameter("max_depth", 10.0)});
|
||||
publishFrame(1000.0, 5.0f); // inside the limit
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
const size_t center = size_t(kHeight/2) * kWidth + kWidth/2;
|
||||
EXPECT_FALSE(std::isnan(readXYZ(out_->back(), center).z));
|
||||
EXPECT_NEAR(readXYZ(out_->back(), center).z, 5.0f, 1e-3);
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZTest, MinDepthMarksNearPointsInvalid)
|
||||
{
|
||||
start({rclcpp::Parameter("min_depth", 3.0)});
|
||||
publishFrame(1000.0, 1.0f); // closer than the limit
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
EXPECT_TRUE(std::isnan(readXYZ(out_->back(), 0).z))
|
||||
<< "every point is nearer than min_depth";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZTest, FilterNaNsRemovesInvalidPoints)
|
||||
{
|
||||
// With filter_nans the invalid points are dropped instead, giving an unorganized
|
||||
// cloud that is empty when nothing is in range.
|
||||
start({rclcpp::Parameter("max_depth", 1.0),
|
||||
rclcpp::Parameter("filter_nans", true)});
|
||||
publishFrame(1000.0, 5.0f);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
EXPECT_EQ(out_->back().width * out_->back().height, 0u)
|
||||
<< "filter_nans must remove the out-of-range points";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZTest, NormalKAddsNormalFields)
|
||||
{
|
||||
start({rclcpp::Parameter("normal_k", 10)});
|
||||
publishFrame(1000.0, 2.0f);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
EXPECT_TRUE(hasField(out_->back(), "normal_x"))
|
||||
<< "asking for normals must change the point type";
|
||||
EXPECT_TRUE(hasField(out_->back(), "normal_z"));
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZTest, NoNormalFieldsByDefault)
|
||||
{
|
||||
start();
|
||||
publishFrame(1000.0, 2.0f);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
EXPECT_FALSE(hasField(out_->back(), "normal_x"));
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZTest, StaysSilentWithoutASubscriber)
|
||||
{
|
||||
// The projection is skipped entirely when nobody wants the cloud.
|
||||
addNode(std::make_shared<rtabmap_util::PointCloudXYZ>(rclcpp::NodeOptions()));
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depthPub =
|
||||
helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub =
|
||||
helper()->create_publisher<sensor_msgs::msg::CameraInfo>("depth/camera_info", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(depthPub));
|
||||
|
||||
depthPub->publish(makeDepth(1000.0, 2.0f));
|
||||
infoPub->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx));
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> late =
|
||||
collect<sensor_msgs::msg::PointCloud2>("cloud");
|
||||
spinFor(std::chrono::milliseconds(200));
|
||||
EXPECT_TRUE(late->empty());
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// disparity/image + disparity/camera_info
|
||||
//============================================================================
|
||||
|
||||
class PointCloudXYZDisparityTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
void start(const std::vector<rclcpp::Parameter> & overrides = {})
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::PointCloudXYZ>(
|
||||
rclcpp::NodeOptions().parameter_overrides(overrides)));
|
||||
out_ = collect<sensor_msgs::msg::PointCloud2>("cloud");
|
||||
dispPub_ = helper()->create_publisher<stereo_msgs::msg::DisparityImage>(
|
||||
"disparity/image", 10);
|
||||
infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>(
|
||||
"disparity/camera_info", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(dispPub_));
|
||||
ASSERT_TRUE(waitForSubscriber(infoPub_));
|
||||
ASSERT_TRUE(waitForPublisher(out_->subscription));
|
||||
}
|
||||
|
||||
void publishFrame(double stamp, const stereo_msgs::msg::DisparityImage & disparity)
|
||||
{
|
||||
dispPub_->publish(disparity);
|
||||
infoPub_->publish(makeCameraInfo("camera_link", stamp, kWidth, kHeight, 0.0, kFx));
|
||||
}
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
|
||||
rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr dispPub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub_;
|
||||
};
|
||||
|
||||
TEST_F(PointCloudXYZDisparityTest, ProjectsDisparityIntoACloud)
|
||||
{
|
||||
start();
|
||||
publishFrame(1000.0, makeDisparity(1000.0, 5.0f)); // depth = f*t/d = 100*0.1/5 = 2 m
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
|
||||
|
||||
const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
|
||||
EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight));
|
||||
EXPECT_EQ(cloud.header.frame_id, "camera_link")
|
||||
<< "the cloud takes the disparity image's frame";
|
||||
|
||||
const size_t center = size_t(kHeight/2) * kWidth + kWidth/2;
|
||||
EXPECT_NEAR(readXYZ(cloud, center).z, 2.0f, 1e-3);
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZDisparityTest, Accepts16SC1FixedPointDisparity)
|
||||
{
|
||||
// The 16-bit form stores 16*disparity, so the same 5 px must still give 2 m.
|
||||
start();
|
||||
publishFrame(1000.0, makeDisparity16SC1(1000.0, 5.0f));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
const size_t center = size_t(kHeight/2) * kWidth + kWidth/2;
|
||||
EXPECT_NEAR(readXYZ(out_->back(), center).z, 2.0f, 1e-3);
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZDisparityTest, RejectsUnsupportedDisparityEncoding)
|
||||
{
|
||||
start();
|
||||
stereo_msgs::msg::DisparityImage msg = makeDisparity(1000.0, 5.0f);
|
||||
msg.image = makeImage("camera_link", 1000.0,
|
||||
cv::Mat(kHeight, kWidth, CV_8UC1, cv::Scalar(5)), "mono8");
|
||||
publishFrame(1000.0, msg);
|
||||
spinFor(std::chrono::milliseconds(400));
|
||||
|
||||
EXPECT_TRUE(out_->empty()) << "only 32FC1 and 16SC1 disparity are supported";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZDisparityTest, MaxDepthMarksFarPointsInvalid)
|
||||
{
|
||||
// Like the depth path, cloudFromDisparity keeps the cloud organized and turns the
|
||||
// out-of-range points into NaN instead of removing them.
|
||||
start({rclcpp::Parameter("max_depth", 1.0)});
|
||||
publishFrame(1000.0, makeDisparity(1000.0, 5.0f)); // 2 m, beyond the limit
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
|
||||
EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight));
|
||||
EXPECT_TRUE(std::isnan(readXYZ(cloud, size_t(kHeight/2)*kWidth + kWidth/2).z));
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZDisparityTest, FilterNaNsRemovesInvalidPoints)
|
||||
{
|
||||
start({rclcpp::Parameter("max_depth", 1.0),
|
||||
rclcpp::Parameter("filter_nans", true)});
|
||||
publishFrame(1000.0, makeDisparity(1000.0, 5.0f));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
EXPECT_EQ(out_->back().width * out_->back().height, 0u);
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZDisparityTest, DecimationReducesThePointCount)
|
||||
{
|
||||
start({rclcpp::Parameter("decimation", 2)});
|
||||
publishFrame(1000.0, makeDisparity(1000.0, 5.0f));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
EXPECT_EQ(out_->back().width * out_->back().height, uint32_t(kWidth*kHeight)/4);
|
||||
}
|
||||
@@ -0,0 +1,531 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include "node_test_utils.hpp"
|
||||
#include "msg_builders.hpp"
|
||||
|
||||
#include <rtabmap_util/point_cloud_xyzrgb.hpp>
|
||||
|
||||
#include <stereo_msgs/msg/disparity_image.hpp>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
using namespace rtabmap_util_test;
|
||||
|
||||
namespace {
|
||||
::testing::Environment * const kEnv = registerRclcppEnvironment();
|
||||
|
||||
constexpr int kWidth = 16;
|
||||
constexpr int kHeight = 16;
|
||||
constexpr double kFx = 100.0;
|
||||
constexpr int kCenter = (kHeight/2) * kWidth + kWidth/2;
|
||||
|
||||
/// The color every synthetic RGB image is painted with, in OpenCV's BGR order.
|
||||
const cv::Scalar kColor(10, 20, 30);
|
||||
|
||||
sensor_msgs::msg::Image makeRgb(double stamp, const std::string & encoding = "bgr8")
|
||||
{
|
||||
if(encoding == "mono8")
|
||||
{
|
||||
return makeImage("camera_link", stamp,
|
||||
cv::Mat(kHeight, kWidth, CV_8UC1, cv::Scalar(128)), encoding);
|
||||
}
|
||||
return makeImage("camera_link", stamp,
|
||||
cv::Mat(kHeight, kWidth, CV_8UC3, kColor), encoding);
|
||||
}
|
||||
|
||||
sensor_msgs::msg::Image makeDepth(double stamp, float meters,
|
||||
const std::string & encoding = sensor_msgs::image_encodings::TYPE_32FC1)
|
||||
{
|
||||
cv::Mat image = encoding == sensor_msgs::image_encodings::TYPE_32FC1
|
||||
? cv::Mat(kHeight, kWidth, CV_32FC1, cv::Scalar(meters))
|
||||
: cv::Mat(kHeight, kWidth, CV_16UC1, cv::Scalar(uint16_t(meters*1000.0f)));
|
||||
return makeImage("camera_link", stamp, image, encoding);
|
||||
}
|
||||
|
||||
/// A disparity image where every pixel carries @p disparity, so depth = f*t/disparity.
|
||||
stereo_msgs::msg::DisparityImage makeDisparity(double stamp, float disparity,
|
||||
float focal = float(kFx), float baseline = 0.1f)
|
||||
{
|
||||
stereo_msgs::msg::DisparityImage msg;
|
||||
msg.header.frame_id = "camera_link";
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.f = focal;
|
||||
msg.t = baseline;
|
||||
msg.min_disparity = 1.0f;
|
||||
msg.max_disparity = 100.0f;
|
||||
msg.image = makeImage("camera_link", stamp,
|
||||
cv::Mat(kHeight, kWidth, CV_32FC1, cv::Scalar(disparity)),
|
||||
sensor_msgs::image_encodings::TYPE_32FC1);
|
||||
return msg;
|
||||
}
|
||||
|
||||
bool hasField(const sensor_msgs::msg::PointCloud2 & cloud, const std::string & name)
|
||||
{
|
||||
for(size_t i=0; i<cloud.fields.size(); ++i)
|
||||
{
|
||||
if(cloud.fields[i].name == name) { return true; }
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/// Reads the packed "rgb" float field of a point as (r,g,b).
|
||||
cv::Vec3b readRGB(const sensor_msgs::msg::PointCloud2 & cloud, size_t index)
|
||||
{
|
||||
uint32_t offset = 16;
|
||||
for(size_t i=0; i<cloud.fields.size(); ++i)
|
||||
{
|
||||
if(cloud.fields[i].name == "rgb") { offset = cloud.fields[i].offset; }
|
||||
}
|
||||
uint32_t packed = 0;
|
||||
memcpy(&packed, &cloud.data[index * cloud.point_step + offset], 4);
|
||||
return cv::Vec3b(
|
||||
uint8_t((packed >> 16) & 0xFF),
|
||||
uint8_t((packed >> 8) & 0xFF),
|
||||
uint8_t(packed & 0xFF));
|
||||
}
|
||||
} // namespace
|
||||
|
||||
class PointCloudXYZRGBTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
void start(const std::vector<rclcpp::Parameter> & overrides = {})
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::PointCloudXYZRGB>(
|
||||
rclcpp::NodeOptions().parameter_overrides(overrides)));
|
||||
out_ = collect<sensor_msgs::msg::PointCloud2>("cloud");
|
||||
rgbPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
|
||||
depthPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
|
||||
infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>("rgb/camera_info", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(rgbPub_));
|
||||
ASSERT_TRUE(waitForSubscriber(depthPub_));
|
||||
ASSERT_TRUE(waitForSubscriber(infoPub_));
|
||||
ASSERT_TRUE(waitForPublisher(out_->subscription));
|
||||
}
|
||||
|
||||
/// Publishes a synchronized rgb + depth + camera_info triple.
|
||||
void publishFrame(double stamp, float meters,
|
||||
const std::string & depthEncoding = sensor_msgs::image_encodings::TYPE_32FC1,
|
||||
const std::string & rgbEncoding = "bgr8")
|
||||
{
|
||||
rgbPub_->publish(makeRgb(stamp, rgbEncoding));
|
||||
depthPub_->publish(makeDepth(stamp, meters, depthEncoding));
|
||||
infoPub_->publish(makeCameraInfo("camera_link", stamp, kWidth, kHeight, 0.0, kFx));
|
||||
}
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgbPub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depthPub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub_;
|
||||
};
|
||||
|
||||
//============================================================================
|
||||
// rgb + depth + camera_info
|
||||
//============================================================================
|
||||
|
||||
TEST_F(PointCloudXYZRGBTest, ProjectsRgbAndDepthIntoAColoredCloud)
|
||||
{
|
||||
start();
|
||||
publishFrame(1000.0, 2.0f);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
|
||||
|
||||
const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
|
||||
EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight))
|
||||
<< "one point per pixel at decimation 1";
|
||||
EXPECT_EQ(cloud.header.frame_id, "camera_link")
|
||||
<< "the cloud takes the RGB image's frame";
|
||||
EXPECT_TRUE(hasField(cloud, "rgb")) << "the whole point of this node";
|
||||
EXPECT_NEAR(readXYZ(cloud, kCenter).z, 2.0f, 1e-3);
|
||||
|
||||
// The RGB image is uniform, so every point carries the same color. cv_bridge hands
|
||||
// the node a bgr8 image, which reaches the cloud as r=30, g=20, b=10.
|
||||
const cv::Vec3b rgb = readRGB(cloud, kCenter);
|
||||
EXPECT_EQ(int(rgb[0]), 30);
|
||||
EXPECT_EQ(int(rgb[1]), 20);
|
||||
EXPECT_EQ(int(rgb[2]), 10);
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZRGBTest, Accepts16UC1Millimeters)
|
||||
{
|
||||
start();
|
||||
publishFrame(1000.0, 2.0f, sensor_msgs::image_encodings::TYPE_16UC1);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
EXPECT_NEAR(readXYZ(out_->back(), kCenter).z, 2.0f, 1e-3)
|
||||
<< "millimeter depth must be converted to meters";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZRGBTest, AcceptsMono8Color)
|
||||
{
|
||||
start();
|
||||
publishFrame(1000.0, 2.0f, sensor_msgs::image_encodings::TYPE_32FC1, "mono8");
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
const cv::Vec3b rgb = readRGB(out_->back(), kCenter);
|
||||
EXPECT_EQ(int(rgb[0]), 128) << "a grey image gives grey points";
|
||||
EXPECT_EQ(int(rgb[1]), 128);
|
||||
EXPECT_EQ(int(rgb[2]), 128);
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZRGBTest, RejectsUnsupportedDepthEncoding)
|
||||
{
|
||||
start();
|
||||
rgbPub_->publish(makeRgb(1000.0));
|
||||
depthPub_->publish(makeImage("camera_link", 1000.0,
|
||||
cv::Mat(kHeight, kWidth, CV_8UC3, kColor), "bgr8"));
|
||||
infoPub_->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx));
|
||||
spinFor(std::chrono::milliseconds(400));
|
||||
|
||||
EXPECT_TRUE(out_->empty()) << "only 32FC1, 16UC1 and mono16 depth are supported";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZRGBTest, DecimationReducesThePointCount)
|
||||
{
|
||||
start({rclcpp::Parameter("decimation", 2)});
|
||||
publishFrame(1000.0, 2.0f);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
EXPECT_EQ(out_->back().width * out_->back().height, uint32_t(kWidth*kHeight)/4)
|
||||
<< "decimation 2 keeps one pixel in four";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZRGBTest, RoiRatiosCropTheCloud)
|
||||
{
|
||||
// A quarter off each side of a 16x16 image leaves an 8x8 window.
|
||||
start({rclcpp::Parameter("roi_ratios", std::string("0.25 0.25 0.25 0.25"))});
|
||||
publishFrame(1000.0, 2.0f);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
EXPECT_EQ(out_->back().width * out_->back().height, 64u);
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZRGBTest, MaxDepthMarksFarPointsInvalid)
|
||||
{
|
||||
// cloudFromDepthRGB keeps the cloud organized: out-of-range points become NaN
|
||||
// rather than disappearing, so the point count is unchanged.
|
||||
start({rclcpp::Parameter("max_depth", 1.0)});
|
||||
publishFrame(1000.0, 5.0f);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
EXPECT_EQ(out_->back().width * out_->back().height, uint32_t(kWidth*kHeight));
|
||||
EXPECT_TRUE(std::isnan(readXYZ(out_->back(), kCenter).z));
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZRGBTest, MinDepthMarksNearPointsInvalid)
|
||||
{
|
||||
start({rclcpp::Parameter("min_depth", 3.0)});
|
||||
publishFrame(1000.0, 1.0f);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
EXPECT_TRUE(std::isnan(readXYZ(out_->back(), kCenter).z));
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZRGBTest, FilterNaNsRemovesInvalidPoints)
|
||||
{
|
||||
start({rclcpp::Parameter("max_depth", 1.0),
|
||||
rclcpp::Parameter("filter_nans", true)});
|
||||
publishFrame(1000.0, 5.0f);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
EXPECT_EQ(out_->back().width * out_->back().height, 0u)
|
||||
<< "filter_nans must remove the out-of-range points";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZRGBTest, NormalKAddsNormalFields)
|
||||
{
|
||||
start({rclcpp::Parameter("normal_k", 10)});
|
||||
publishFrame(1000.0, 2.0f);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
EXPECT_TRUE(hasField(out_->back(), "normal_x"))
|
||||
<< "asking for normals must change the point type";
|
||||
EXPECT_TRUE(hasField(out_->back(), "rgb")) << "and must keep the color";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZRGBTest, NoNormalFieldsByDefault)
|
||||
{
|
||||
start();
|
||||
publishFrame(1000.0, 2.0f);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
EXPECT_FALSE(hasField(out_->back(), "normal_x"));
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZRGBTest, VoxelSizeThinsTheCloud)
|
||||
{
|
||||
// A frontal plane at 2 m spans about 0.32 m across a 16-pixel image at fx=100, so a
|
||||
// 0.1 m voxel grid collapses the 256 points into far fewer.
|
||||
start({rclcpp::Parameter("voxel_size", 0.1)});
|
||||
publishFrame(1000.0, 2.0f);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
const uint32_t points = out_->back().width * out_->back().height;
|
||||
EXPECT_GT(points, 0u);
|
||||
EXPECT_LT(points, uint32_t(kWidth*kHeight));
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZRGBTest, StaysSilentWithoutASubscriber)
|
||||
{
|
||||
// The projection is skipped entirely when nobody wants the cloud.
|
||||
addNode(std::make_shared<rtabmap_util::PointCloudXYZRGB>(rclcpp::NodeOptions()));
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgbPub =
|
||||
helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depthPub =
|
||||
helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub =
|
||||
helper()->create_publisher<sensor_msgs::msg::CameraInfo>("rgb/camera_info", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(rgbPub));
|
||||
ASSERT_TRUE(waitForSubscriber(depthPub));
|
||||
|
||||
rgbPub->publish(makeRgb(1000.0));
|
||||
depthPub->publish(makeDepth(1000.0, 2.0f));
|
||||
infoPub->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx));
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> late =
|
||||
collect<sensor_msgs::msg::PointCloud2>("cloud");
|
||||
spinFor(std::chrono::milliseconds(200));
|
||||
EXPECT_TRUE(late->empty());
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// rgbd_image
|
||||
//============================================================================
|
||||
|
||||
class PointCloudXYZRGBRgbdTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
void start(const std::vector<rclcpp::Parameter> & overrides = {})
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::PointCloudXYZRGB>(
|
||||
rclcpp::NodeOptions().parameter_overrides(overrides)));
|
||||
out_ = collect<sensor_msgs::msg::PointCloud2>("cloud");
|
||||
rgbdPub_ = helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(rgbdPub_));
|
||||
ASSERT_TRUE(waitForPublisher(out_->subscription));
|
||||
}
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdPub_;
|
||||
};
|
||||
|
||||
TEST_F(PointCloudXYZRGBRgbdTest, ProjectsAnRgbdImage)
|
||||
{
|
||||
start();
|
||||
rgbdPub_->publish(makeRGBDImage("camera_link", 1000.0, kWidth, kHeight));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
|
||||
|
||||
const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
|
||||
EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight));
|
||||
EXPECT_EQ(cloud.header.frame_id, "camera_link");
|
||||
EXPECT_TRUE(hasField(cloud, "rgb"));
|
||||
EXPECT_NEAR(readXYZ(cloud, kCenter).z, 1.5f, 1e-3)
|
||||
<< "makeRGBDImage() fills the depth image with 1500 mm";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZRGBRgbdTest, IgnoresAnInvalidRgbdImage)
|
||||
{
|
||||
// isValid() is false without any image data, and nothing must be published.
|
||||
start();
|
||||
rtabmap_msgs::msg::RGBDImage msg;
|
||||
msg.header.frame_id = "camera_link";
|
||||
msg.header.stamp = stampOf(1000.0);
|
||||
rgbdPub_->publish(msg);
|
||||
spinFor(std::chrono::milliseconds(400));
|
||||
|
||||
EXPECT_TRUE(out_->empty());
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZRGBRgbdTest, PublishesAnEmptyCloudForAColorOnlyRgbdImage)
|
||||
{
|
||||
// Depth is optional in an RGBDImage, so color alone must not be treated as a broken
|
||||
// message: there is simply nothing to project.
|
||||
start();
|
||||
rtabmap_msgs::msg::RGBDImage msg = makeRGBDImage("camera_link", 1000.0, kWidth, kHeight);
|
||||
msg.depth = sensor_msgs::msg::Image();
|
||||
rgbdPub_->publish(msg);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
|
||||
|
||||
EXPECT_EQ(out_->back().width * out_->back().height, 0u);
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZRGBRgbdTest, ProjectsAStereoRgbdImage)
|
||||
{
|
||||
// A stereo pair in an RGBDImage is dense-matched instead of read as depth.
|
||||
start();
|
||||
rgbdPub_->publish(makeStereoRGBDImage("camera_link", 1000.0, 160, 120));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
|
||||
|
||||
EXPECT_EQ(out_->back().width * out_->back().height, uint32_t(160*120));
|
||||
EXPECT_TRUE(hasField(out_->back(), "rgb"));
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// left/image + disparity + left/camera_info
|
||||
//============================================================================
|
||||
|
||||
class PointCloudXYZRGBDisparityTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
void start(const std::vector<rclcpp::Parameter> & overrides = {})
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::PointCloudXYZRGB>(
|
||||
rclcpp::NodeOptions().parameter_overrides(overrides)));
|
||||
out_ = collect<sensor_msgs::msg::PointCloud2>("cloud");
|
||||
leftPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("left/image", 10);
|
||||
dispPub_ = helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
|
||||
infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>("left/camera_info", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(leftPub_));
|
||||
ASSERT_TRUE(waitForSubscriber(dispPub_));
|
||||
ASSERT_TRUE(waitForSubscriber(infoPub_));
|
||||
ASSERT_TRUE(waitForPublisher(out_->subscription));
|
||||
}
|
||||
|
||||
void publishFrame(double stamp, float disparity)
|
||||
{
|
||||
leftPub_->publish(makeRgb(stamp));
|
||||
dispPub_->publish(makeDisparity(stamp, disparity));
|
||||
infoPub_->publish(makeCameraInfo("camera_link", stamp, kWidth, kHeight, 0.0, kFx));
|
||||
}
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr leftPub_;
|
||||
rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr dispPub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub_;
|
||||
};
|
||||
|
||||
TEST_F(PointCloudXYZRGBDisparityTest, ProjectsDisparityIntoAColoredCloud)
|
||||
{
|
||||
start();
|
||||
publishFrame(1000.0, 5.0f); // depth = f*t/d = 100*0.1/5 = 2 m
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
|
||||
|
||||
const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
|
||||
EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight));
|
||||
EXPECT_EQ(cloud.header.frame_id, "camera_link")
|
||||
<< "the cloud takes the disparity image's frame";
|
||||
EXPECT_TRUE(hasField(cloud, "rgb"));
|
||||
EXPECT_NEAR(readXYZ(cloud, kCenter).z, 2.0f, 1e-3);
|
||||
|
||||
const cv::Vec3b rgb = readRGB(cloud, kCenter);
|
||||
EXPECT_EQ(int(rgb[0]), 30);
|
||||
EXPECT_EQ(int(rgb[2]), 10);
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZRGBDisparityTest, RejectsUnsupportedDisparityEncoding)
|
||||
{
|
||||
start();
|
||||
stereo_msgs::msg::DisparityImage msg = makeDisparity(1000.0, 5.0f);
|
||||
msg.image = makeImage("camera_link", 1000.0,
|
||||
cv::Mat(kHeight, kWidth, CV_8UC1, cv::Scalar(5)), "mono8");
|
||||
leftPub_->publish(makeRgb(1000.0));
|
||||
dispPub_->publish(msg);
|
||||
infoPub_->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx));
|
||||
spinFor(std::chrono::milliseconds(400));
|
||||
|
||||
EXPECT_TRUE(out_->empty()) << "only 32FC1 and 16SC1 disparity are supported";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZRGBDisparityTest, MaxDepthMarksFarPointsInvalid)
|
||||
{
|
||||
start({rclcpp::Parameter("max_depth", 1.0)});
|
||||
publishFrame(1000.0, 5.0f); // 2 m, beyond the limit
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
EXPECT_TRUE(std::isnan(readXYZ(out_->back(), kCenter).z));
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// left/image + right/image + both camera_infos
|
||||
//============================================================================
|
||||
|
||||
class PointCloudXYZRGBStereoTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
static constexpr int kStereoWidth = 160;
|
||||
static constexpr int kStereoHeight = 120;
|
||||
static constexpr float kBaseline = 0.12f;
|
||||
static constexpr int kDisparity = 6; // depth = fx*baseline/d = 100*0.12/6 = 2 m
|
||||
|
||||
void start(const std::vector<rclcpp::Parameter> & overrides = {})
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::PointCloudXYZRGB>(
|
||||
rclcpp::NodeOptions().parameter_overrides(overrides)));
|
||||
out_ = collect<sensor_msgs::msg::PointCloud2>("cloud");
|
||||
leftPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("left/image", 10);
|
||||
rightPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("right/image", 10);
|
||||
leftInfoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>("left/camera_info", 10);
|
||||
rightInfoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>("right/camera_info", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(leftPub_));
|
||||
ASSERT_TRUE(waitForSubscriber(rightPub_));
|
||||
ASSERT_TRUE(waitForSubscriber(leftInfoPub_));
|
||||
ASSERT_TRUE(waitForSubscriber(rightInfoPub_));
|
||||
ASSERT_TRUE(waitForPublisher(out_->subscription));
|
||||
}
|
||||
|
||||
/**
|
||||
* Publishes a textured pair whose true disparity is kDisparity everywhere: the right
|
||||
* image is the left one shifted, which is what a plane at a constant depth looks like.
|
||||
*/
|
||||
void publishFrame(double stamp)
|
||||
{
|
||||
cv::Mat left(kStereoHeight, kStereoWidth + kDisparity, CV_8UC1);
|
||||
cv::RNG rng(42);
|
||||
rng.fill(left, cv::RNG::UNIFORM, 0, 256);
|
||||
|
||||
cv::Mat leftBgr;
|
||||
cv::cvtColor(cv::Mat(left, cv::Rect(0, 0, kStereoWidth, kStereoHeight)),
|
||||
leftBgr, cv::COLOR_GRAY2BGR);
|
||||
cv::Mat right(left, cv::Rect(kDisparity, 0, kStereoWidth, kStereoHeight));
|
||||
|
||||
leftPub_->publish(makeImage("camera_link", stamp, leftBgr, "bgr8"));
|
||||
rightPub_->publish(makeImage("camera_link", stamp, right.clone(), "mono8"));
|
||||
leftInfoPub_->publish(makeCameraInfo(
|
||||
"camera_link", stamp, kStereoWidth, kStereoHeight, 0.0, kFx));
|
||||
rightInfoPub_->publish(makeCameraInfo(
|
||||
"camera_link", stamp, kStereoWidth, kStereoHeight, -kFx*kBaseline, kFx));
|
||||
}
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr leftPub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rightPub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr leftInfoPub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rightInfoPub_;
|
||||
};
|
||||
|
||||
TEST_F(PointCloudXYZRGBStereoTest, MatchesAStereoPairIntoAColoredCloud)
|
||||
{
|
||||
start({rclcpp::Parameter("StereoBM/NumDisparities", std::string("16")),
|
||||
rclcpp::Parameter("StereoBM/BlockSize", std::string("9"))});
|
||||
publishFrame(1000.0);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
|
||||
|
||||
const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
|
||||
EXPECT_EQ(cloud.width * cloud.height, uint32_t(kStereoWidth*kStereoHeight))
|
||||
<< "the cloud stays organized, one point per pixel";
|
||||
EXPECT_EQ(cloud.header.frame_id, "camera_link")
|
||||
<< "the cloud takes the left image's frame";
|
||||
EXPECT_TRUE(hasField(cloud, "rgb"));
|
||||
|
||||
// The pair is a shifted copy of itself, so the whole matched area sits at one depth.
|
||||
const size_t center = size_t(kStereoHeight/2) * kStereoWidth + kStereoWidth/2;
|
||||
EXPECT_NEAR(readXYZ(cloud, center).z, 2.0f, 0.2f);
|
||||
}
|
||||
|
||||
TEST_F(PointCloudXYZRGBStereoTest, RejectsUnsupportedStereoEncoding)
|
||||
{
|
||||
start();
|
||||
leftPub_->publish(makeImage("camera_link", 1000.0,
|
||||
cv::Mat(kStereoHeight, kStereoWidth, CV_32FC1, cv::Scalar(1.0f)), "32FC1"));
|
||||
rightPub_->publish(makeImage("camera_link", 1000.0,
|
||||
cv::Mat(kStereoHeight, kStereoWidth, CV_8UC1, cv::Scalar(0)), "mono8"));
|
||||
leftInfoPub_->publish(makeCameraInfo(
|
||||
"camera_link", 1000.0, kStereoWidth, kStereoHeight, 0.0, kFx));
|
||||
rightInfoPub_->publish(makeCameraInfo(
|
||||
"camera_link", 1000.0, kStereoWidth, kStereoHeight, -kFx*kBaseline, kFx));
|
||||
spinFor(std::chrono::milliseconds(400));
|
||||
|
||||
EXPECT_TRUE(out_->empty()) << "only 8-bit and mono16 stereo images are supported";
|
||||
}
|
||||
@@ -0,0 +1,370 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include "node_test_utils.hpp"
|
||||
#include "msg_builders.hpp"
|
||||
|
||||
#include <rtabmap_util/pointcloud_to_depthimage.hpp>
|
||||
|
||||
#include <tf2_msgs/msg/tf_message.hpp>
|
||||
|
||||
using namespace rtabmap_util_test;
|
||||
|
||||
namespace {
|
||||
::testing::Environment * const kEnv = registerRclcppEnvironment();
|
||||
|
||||
constexpr int kWidth = 16;
|
||||
constexpr int kHeight = 16;
|
||||
constexpr double kFx = 100.0;
|
||||
|
||||
float pixel32f(const sensor_msgs::msg::Image & img, int row, int col)
|
||||
{
|
||||
return *reinterpret_cast<const float *>(&img.data[row * img.step + col * sizeof(float)]);
|
||||
}
|
||||
|
||||
uint16_t pixel16u(const sensor_msgs::msg::Image & img, int row, int col)
|
||||
{
|
||||
return *reinterpret_cast<const uint16_t *>(&img.data[row * img.step + col * sizeof(uint16_t)]);
|
||||
}
|
||||
} // namespace
|
||||
|
||||
class PointCloudToDepthImageTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
void start(const std::vector<rclcpp::Parameter> & overrides = {}, bool withTf = true)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::PointCloudToDepthImage>(
|
||||
rclcpp::NodeOptions().parameter_overrides(overrides)));
|
||||
if(withTf)
|
||||
{
|
||||
publishStaticTf("camera_link", "lidar");
|
||||
}
|
||||
image32_ = collect<sensor_msgs::msg::Image>("image");
|
||||
image16_ = collect<sensor_msgs::msg::Image>("image_raw");
|
||||
cloudPub_ = helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
|
||||
infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>("camera_info", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(cloudPub_));
|
||||
ASSERT_TRUE(waitForSubscriber(infoPub_));
|
||||
ASSERT_TRUE(waitForPublisher(image32_->subscription));
|
||||
}
|
||||
|
||||
/// Publishes a cloud and its camera info with identical stamps.
|
||||
void publishFrame(double stamp, const std::vector<cv::Point3f> & points)
|
||||
{
|
||||
cloudPub_->publish(makeXYZCloud("lidar", stamp, points));
|
||||
infoPub_->publish(makeCameraInfo("camera_link", stamp, kWidth, kHeight, 0.0, kFx));
|
||||
}
|
||||
|
||||
/// A block of points straight ahead of the optical axis at @p depth meters.
|
||||
static std::vector<cv::Point3f> blockAt(float depth)
|
||||
{
|
||||
std::vector<cv::Point3f> points;
|
||||
for(int i=-2; i<=2; ++i)
|
||||
{
|
||||
for(int j=-2; j<=2; ++j)
|
||||
{
|
||||
points.push_back(cv::Point3f(0.01f*i, 0.01f*j, depth));
|
||||
}
|
||||
}
|
||||
return points;
|
||||
}
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> image32_;
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> image16_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub_;
|
||||
};
|
||||
|
||||
TEST_F(PointCloudToDepthImageTest, ProjectsACloudIntoADepthImage)
|
||||
{
|
||||
start();
|
||||
publishFrame(1000.0, blockAt(2.0f));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) << "no depth image";
|
||||
|
||||
const sensor_msgs::msg::Image & img = image32_->back();
|
||||
EXPECT_EQ(img.encoding, sensor_msgs::image_encodings::TYPE_32FC1);
|
||||
EXPECT_EQ(img.width, uint32_t(kWidth));
|
||||
EXPECT_EQ(img.height, uint32_t(kHeight));
|
||||
EXPECT_EQ(img.header.frame_id, "camera_link")
|
||||
<< "the depth image belongs to the camera, not the cloud";
|
||||
|
||||
// The points sit on the optical axis, so they land on the principal point.
|
||||
EXPECT_NEAR(pixel32f(img, kHeight/2, kWidth/2), 2.0f, 1e-3);
|
||||
// A corner sees nothing.
|
||||
EXPECT_FLOAT_EQ(pixel32f(img, 0, 0), 0.0f);
|
||||
}
|
||||
|
||||
TEST_F(PointCloudToDepthImageTest, PublishesMillimetersOnImageRaw)
|
||||
{
|
||||
start();
|
||||
publishFrame(1000.0, blockAt(2.0f));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !image16_->empty(); }));
|
||||
|
||||
const sensor_msgs::msg::Image & img = image16_->back();
|
||||
EXPECT_EQ(img.encoding, sensor_msgs::image_encodings::TYPE_16UC1);
|
||||
EXPECT_EQ(pixel16u(img, kHeight/2, kWidth/2), 2000) << "2 m expressed in millimeters";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudToDepthImageTest, EmptyCloudGivesAnAllZeroImage)
|
||||
{
|
||||
start();
|
||||
publishFrame(1000.0, {});
|
||||
ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); }))
|
||||
<< "an empty cloud must still produce an image, not a dropped frame";
|
||||
|
||||
const sensor_msgs::msg::Image & img = image32_->back();
|
||||
EXPECT_EQ(img.width, uint32_t(kWidth));
|
||||
for(int row=0; row<kHeight; ++row)
|
||||
{
|
||||
for(int col=0; col<kWidth; ++col)
|
||||
{
|
||||
ASSERT_FLOAT_EQ(pixel32f(img, row, col), 0.0f) << "at " << row << "," << col;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(PointCloudToDepthImageTest, DecimationShrinksTheImageAndScalesTheModel)
|
||||
{
|
||||
start({rclcpp::Parameter("decimation", 2)});
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> infoOut =
|
||||
collect<sensor_msgs::msg::CameraInfo>("image/camera_info");
|
||||
ASSERT_TRUE(waitForPublisher(infoOut->subscription));
|
||||
|
||||
publishFrame(1000.0, blockAt(2.0f));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !image32_->empty() && !infoOut->empty(); }));
|
||||
|
||||
EXPECT_EQ(image32_->back().width, uint32_t(kWidth)/2);
|
||||
EXPECT_EQ(image32_->back().height, uint32_t(kHeight)/2);
|
||||
EXPECT_NEAR(infoOut->back().p[0], kFx/2.0, 1e-6)
|
||||
<< "the published camera info must match the decimated image";
|
||||
EXPECT_EQ(infoOut->back().width, uint32_t(kWidth)/2);
|
||||
}
|
||||
|
||||
TEST_F(PointCloudToDepthImageTest, FailsWithoutTheCloudToCameraTransform)
|
||||
{
|
||||
start({rclcpp::Parameter("wait_for_transform", 0.0)}, /*withTf=*/false);
|
||||
publishFrame(1000.0, blockAt(2.0f));
|
||||
spinFor(std::chrono::milliseconds(400));
|
||||
|
||||
EXPECT_TRUE(image32_->empty())
|
||||
<< "without TF the cloud cannot be placed in the camera frame";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudToDepthImageTest, StaysSilentWithoutASubscriber)
|
||||
{
|
||||
// The projection is skipped entirely when neither image topic is subscribed.
|
||||
addNode(std::make_shared<rtabmap_util::PointCloudToDepthImage>(rclcpp::NodeOptions()));
|
||||
publishStaticTf("camera_link", "lidar");
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub =
|
||||
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub =
|
||||
helper()->create_publisher<sensor_msgs::msg::CameraInfo>("camera_info", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(cloudPub));
|
||||
|
||||
cloudPub->publish(makeXYZCloud("lidar", 1000.0, blockAt(2.0f)));
|
||||
infoPub->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx));
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> late =
|
||||
collect<sensor_msgs::msg::Image>("image");
|
||||
spinFor(std::chrono::milliseconds(200));
|
||||
EXPECT_TRUE(late->empty());
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// Motion compensation between the cloud stamp and the camera_info stamp
|
||||
//============================================================================
|
||||
|
||||
/**
|
||||
* With approximate synchronization the cloud and the camera info rarely share a stamp.
|
||||
* When @c fixed_frame_id is set, the node asks TF how the lidar moved over that interval
|
||||
* and folds the displacement into the camera's local transform, so the cloud is projected
|
||||
* from where the camera was at its own stamp instead of where the lidar was.
|
||||
*
|
||||
* The frames follow the usual convention, as in rtabmap's own projectCloudToCamera tests:
|
||||
* the cloud is expressed in a lidar frame with x forward, and the camera is attached to it
|
||||
* through the optical rotation, so a point straight ahead lands on the principal point.
|
||||
*/
|
||||
class PointCloudToDepthImageMotionTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
static constexpr double kSpeed = 1.0; ///< m/s
|
||||
static constexpr double kCloudStamp = 1000.0;
|
||||
static constexpr double kInfoDelay = 0.04; ///< the camera info lags the cloud by this
|
||||
static constexpr float kRange = 2.0f; ///< distance to the point, meters
|
||||
static constexpr int kFrames = 3; ///< see publishFrames()
|
||||
static constexpr double kPeriod = 0.2; ///< seconds between frames
|
||||
|
||||
/// Distance travelled between the two stamps: what the node has to compensate for.
|
||||
static double travelled() { return kSpeed * kInfoDelay; }
|
||||
|
||||
/// The same distance seen sideways by the camera, in pixels.
|
||||
static int shiftInPixels() { return int(kFx * travelled() / double(kRange)); }
|
||||
|
||||
/**
|
||||
* @param axis 'x' to drive straight at the point, 'y' to drive sideways past it,
|
||||
* '0' to stand still
|
||||
*/
|
||||
void start(const std::vector<rclcpp::Parameter> & overrides, char axis = 'x')
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::PointCloudToDepthImage>(
|
||||
rclcpp::NodeOptions().parameter_overrides(overrides)));
|
||||
// The camera is bolted to the lidar, looking the same way: x right, y down,
|
||||
// z forward against the lidar's x forward, y left, z up.
|
||||
publishStaticTfRPY("lidar", "camera_link", -M_PI/2.0, 0.0, -M_PI/2.0);
|
||||
if(axis != '0')
|
||||
{
|
||||
publishOdomMotion(axis);
|
||||
}
|
||||
image32_ = collect<sensor_msgs::msg::Image>("image");
|
||||
cloudPub_ = helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
|
||||
infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>("camera_info", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(cloudPub_));
|
||||
ASSERT_TRUE(waitForSubscriber(infoPub_));
|
||||
ASSERT_TRUE(waitForPublisher(image32_->subscription));
|
||||
}
|
||||
|
||||
/// Publishes odom -> lidar moving at kSpeed, covering every stamp used below.
|
||||
void publishOdomMotion(char axis)
|
||||
{
|
||||
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub =
|
||||
helper()->create_publisher<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
|
||||
spinFor(std::chrono::milliseconds(100)); // let the node's listener subscribe
|
||||
|
||||
// One sample per cloud stamp and per camera info stamp, plus a margin on each side
|
||||
// so that nothing has to be extrapolated.
|
||||
std::vector<double> elapsedSamples;
|
||||
elapsedSamples.push_back(-kPeriod);
|
||||
for(int k=0; k<kFrames; ++k)
|
||||
{
|
||||
elapsedSamples.push_back(kPeriod * double(k));
|
||||
elapsedSamples.push_back(kPeriod * double(k) + kInfoDelay);
|
||||
}
|
||||
elapsedSamples.push_back(kPeriod * double(kFrames));
|
||||
|
||||
for(size_t i=0; i<elapsedSamples.size(); ++i)
|
||||
{
|
||||
const double elapsed = elapsedSamples[i];
|
||||
geometry_msgs::msg::TransformStamped t;
|
||||
t.header.stamp = stampOf(kCloudStamp + elapsed);
|
||||
t.header.frame_id = "odom";
|
||||
t.child_frame_id = "lidar";
|
||||
(axis == 'x' ? t.transform.translation.x : t.transform.translation.y) =
|
||||
kSpeed * elapsed;
|
||||
t.transform.rotation.w = 1.0;
|
||||
tf2_msgs::msg::TFMessage msg;
|
||||
msg.transforms.push_back(t);
|
||||
tfPub->publish(msg);
|
||||
}
|
||||
spinFor(std::chrono::milliseconds(200)); // let the buffer fill
|
||||
tfPub_ = tfPub; // keep the publisher alive
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Publishes kFrames cloud/camera_info pairs, the info @p delay seconds late.
|
||||
*
|
||||
* Each cloud holds a single point straight ahead of the lidar. The speed is constant,
|
||||
* so every pair needs the same correction and the first output is enough to assert on.
|
||||
* A burst is needed because ApproximateTime emits nothing for a lone pair whose stamps
|
||||
* differ: it cannot rule out a better match still to come.
|
||||
*/
|
||||
void publishFrames(double delay)
|
||||
{
|
||||
for(int k=0; k<kFrames; ++k)
|
||||
{
|
||||
const double elapsed = kPeriod * double(k);
|
||||
cloudPub_->publish(makeXYZCloud("lidar", kCloudStamp + elapsed,
|
||||
{cv::Point3f(kRange, 0.0f, 0.0f)}));
|
||||
infoPub_->publish(makeCameraInfo(
|
||||
"camera_link", kCloudStamp + elapsed + delay, kWidth, kHeight, 0.0, kFx));
|
||||
}
|
||||
}
|
||||
|
||||
/// The column the single point landed in on @p row, or -1 if that row is empty.
|
||||
static int hitColumn(const sensor_msgs::msg::Image & img, int row)
|
||||
{
|
||||
for(int col=0; col<int(img.width); ++col)
|
||||
{
|
||||
if(pixel32f(img, row, col) != 0.0f) { return col; }
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> image32_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub_;
|
||||
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub_;
|
||||
};
|
||||
|
||||
TEST_F(PointCloudToDepthImageMotionTest, NoShiftWhenTheStampsMatch)
|
||||
{
|
||||
// The control case: same frames, same scene, nothing to compensate. It also proves the
|
||||
// optical rotation is right, since a wrongly oriented camera sees nothing at all.
|
||||
start({rclcpp::Parameter("fixed_frame_id", std::string("odom"))});
|
||||
publishFrames(0.0);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) << "no depth image";
|
||||
|
||||
const sensor_msgs::msg::Image & img = image32_->front();
|
||||
EXPECT_EQ(hitColumn(img, kHeight/2), kWidth/2)
|
||||
<< "a point straight ahead belongs at the principal point";
|
||||
EXPECT_NEAR(pixel32f(img, kHeight/2, kWidth/2), kRange, 1e-3);
|
||||
}
|
||||
|
||||
TEST_F(PointCloudToDepthImageMotionTest, ClosesTheGapWhenDrivingAtThePoint)
|
||||
{
|
||||
// The camera info is 40 ms younger than the cloud and the robot closes in at 1 m/s, so
|
||||
// by the time of the exposure the point is 4 cm nearer than the lidar measured it.
|
||||
start({rclcpp::Parameter("fixed_frame_id", std::string("odom"))}, 'x');
|
||||
publishFrames(kInfoDelay);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) << "no depth image";
|
||||
|
||||
const sensor_msgs::msg::Image & img = image32_->front();
|
||||
EXPECT_EQ(hitColumn(img, kHeight/2), kWidth/2)
|
||||
<< "driving straight at the point does not move it across the image";
|
||||
EXPECT_NEAR(pixel32f(img, kHeight/2, kWidth/2), kRange - float(travelled()), 1e-3)
|
||||
<< "the depth must be corrected for the distance travelled";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudToDepthImageMotionTest, ShiftsThePointWhenDrivingPastIt)
|
||||
{
|
||||
// Moving sideways instead: the point slides across the image by fx*d/Z pixels, and
|
||||
// stays at the same range.
|
||||
start({rclcpp::Parameter("fixed_frame_id", std::string("odom"))}, 'y');
|
||||
publishFrames(kInfoDelay);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) << "no depth image";
|
||||
|
||||
const sensor_msgs::msg::Image & img = image32_->front();
|
||||
EXPECT_EQ(hitColumn(img, kHeight/2), kWidth/2 + shiftInPixels())
|
||||
<< "the robot moved left, so the point must appear further right";
|
||||
EXPECT_NEAR(pixel32f(img, kHeight/2, kWidth/2 + shiftInPixels()), kRange, 1e-3)
|
||||
<< "only the bearing changed, not the range";
|
||||
EXPECT_FLOAT_EQ(pixel32f(img, kHeight/2, kWidth/2), 0.0f)
|
||||
<< "and it is no longer at the principal point";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudToDepthImageMotionTest, IgnoresTheStampDifferenceWithoutAFixedFrame)
|
||||
{
|
||||
// Without fixed_frame_id there is nothing to measure the motion against, so the cloud
|
||||
// is projected as if both messages were captured at the same instant. That is why the
|
||||
// node logs a fatal error when approximate sync is used without one.
|
||||
start({rclcpp::Parameter("fixed_frame_id", std::string(""))}, 'x');
|
||||
publishFrames(kInfoDelay);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) << "no depth image";
|
||||
|
||||
EXPECT_NEAR(pixel32f(image32_->front(), kHeight/2, kWidth/2), kRange, 1e-3)
|
||||
<< "no fixed frame, no compensation";
|
||||
}
|
||||
|
||||
TEST_F(PointCloudToDepthImageMotionTest, FailsWhenTheFixedFrameIsUnknown)
|
||||
{
|
||||
// fixed_frame_id is set but odom -> lidar was never published: the displacement cannot
|
||||
// be measured, and projecting anyway would silently misplace the points.
|
||||
start({rclcpp::Parameter("fixed_frame_id", std::string("odom")),
|
||||
rclcpp::Parameter("wait_for_transform", 0.0)}, '0');
|
||||
publishFrames(kInfoDelay);
|
||||
spinFor(std::chrono::milliseconds(400));
|
||||
|
||||
EXPECT_TRUE(image32_->empty());
|
||||
}
|
||||
@@ -0,0 +1,350 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include "node_test_utils.hpp"
|
||||
#include "msg_builders.hpp"
|
||||
|
||||
#include <rtabmap_util/rgbd_relay.hpp>
|
||||
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap/utilite/UException.h>
|
||||
|
||||
using namespace rtabmap_util_test;
|
||||
|
||||
namespace {
|
||||
::testing::Environment * const kEnv = registerRclcppEnvironment();
|
||||
}
|
||||
|
||||
class RGBDRelayTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
/// Starts the node, wires up the input publisher and the output collector.
|
||||
void start(bool compress, bool uncompress)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::RGBDRelay>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({
|
||||
rclcpp::Parameter("compress", compress),
|
||||
rclcpp::Parameter("uncompress", uncompress)})));
|
||||
|
||||
out_ = collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay");
|
||||
pub_ = helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub_));
|
||||
ASSERT_TRUE(waitForPublisher(out_->subscription));
|
||||
}
|
||||
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub_;
|
||||
};
|
||||
|
||||
TEST_F(RGBDRelayTest, RepublishesUnchangedByDefault)
|
||||
{
|
||||
start(/*compress=*/false, /*uncompress=*/false);
|
||||
|
||||
const rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
pub_->publish(in);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
const rtabmap_msgs::msg::RGBDImage & got = out_->back();
|
||||
EXPECT_EQ(got.header.frame_id, in.header.frame_id);
|
||||
EXPECT_EQ(got.rgb.data, in.rgb.data) << "the payload must be passed through untouched";
|
||||
EXPECT_EQ(got.depth.data, in.depth.data);
|
||||
EXPECT_TRUE(got.rgb_compressed.data.empty());
|
||||
EXPECT_TRUE(got.depth_compressed.data.empty());
|
||||
EXPECT_NEAR(got.rgb_camera_info.p[0], in.rgb_camera_info.p[0], 1e-9);
|
||||
}
|
||||
|
||||
TEST_F(RGBDRelayTest, CompressesRawImagesWhenAsked)
|
||||
{
|
||||
start(/*compress=*/true, /*uncompress=*/false);
|
||||
|
||||
pub_->publish(makeRGBDImage("camera_link", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
const rtabmap_msgs::msg::RGBDImage & got = out_->back();
|
||||
EXPECT_FALSE(got.rgb_compressed.data.empty()) << "rgb must be compressed";
|
||||
EXPECT_FALSE(got.depth_compressed.data.empty()) << "depth must be compressed";
|
||||
// Depth is lossless png; color is jpg.
|
||||
EXPECT_EQ(got.depth_compressed.format, "png");
|
||||
EXPECT_TRUE(got.rgb.data.empty()) << "the raw image is not carried as well";
|
||||
}
|
||||
|
||||
TEST_F(RGBDRelayTest, CompressesAStereoPairAsJpeg)
|
||||
{
|
||||
// When the camera infos describe a stereo pair, the "depth" slot holds the right
|
||||
// image and is compressed as JPEG, not as a lossless depth PNG.
|
||||
start(/*compress=*/true, /*uncompress=*/false);
|
||||
|
||||
pub_->publish(makeStereoRGBDImage("camera_link", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
const rtabmap_msgs::msg::RGBDImage & got = out_->back();
|
||||
ASSERT_FALSE(got.depth_compressed.data.empty());
|
||||
EXPECT_NE(got.depth_compressed.format, "png")
|
||||
<< "a stereo right image must not take the depth PNG path";
|
||||
EXPECT_NE(got.depth_compressed.format.find("jp"), std::string::npos)
|
||||
<< "expected a jpeg format, got \"" << got.depth_compressed.format << "\"";
|
||||
EXPECT_LT(got.depth_camera_info.p[3], 0.0) << "the baseline must survive the relay";
|
||||
}
|
||||
|
||||
TEST_F(RGBDRelayTest, CompressesDepthAsLosslessPng)
|
||||
{
|
||||
// The same call with no baseline is treated as color + depth instead.
|
||||
start(/*compress=*/true, /*uncompress=*/false);
|
||||
|
||||
pub_->publish(makeRGBDImage("camera_link", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
const rtabmap_msgs::msg::RGBDImage & got = out_->back();
|
||||
ASSERT_FALSE(got.depth_compressed.data.empty());
|
||||
EXPECT_EQ(got.depth_compressed.format, "png") << "depth must stay lossless";
|
||||
EXPECT_DOUBLE_EQ(got.depth_camera_info.p[3], 0.0) << "no baseline: not stereo";
|
||||
}
|
||||
|
||||
TEST_F(RGBDRelayTest, UncompressRestoresAStereoRightImage)
|
||||
{
|
||||
// The uncompress path branches on the format: "jpg" means a stereo right image and
|
||||
// goes through cv_bridge, anything else is a depth image and goes through rtabmap.
|
||||
start(/*compress=*/false, /*uncompress=*/true);
|
||||
|
||||
rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0);
|
||||
const cv::Mat right(8, 8, CV_8UC1, cv::Scalar(60));
|
||||
cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", right)
|
||||
.toCompressedImageMsg(in.depth_compressed, cv_bridge::JPG);
|
||||
in.depth = sensor_msgs::msg::Image();
|
||||
|
||||
pub_->publish(in);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
const rtabmap_msgs::msg::RGBDImage & got = out_->back();
|
||||
ASSERT_FALSE(got.depth.data.empty()) << "the right image must be decompressed";
|
||||
EXPECT_EQ(got.depth.encoding, "mono8") << "restored as an 8-bit image, not depth";
|
||||
EXPECT_EQ(got.depth.width, 8u);
|
||||
EXPECT_EQ(got.depth.height, 8u);
|
||||
}
|
||||
|
||||
TEST_F(RGBDRelayTest, UncompressRestoresRawImages)
|
||||
{
|
||||
start(/*compress=*/false, /*uncompress=*/true);
|
||||
|
||||
// Feed it a message that carries only compressed depth.
|
||||
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
const cv::Mat depth(8, 8, CV_16UC1, cv::Scalar(1500));
|
||||
in.depth = sensor_msgs::msg::Image();
|
||||
in.depth_compressed.header = in.header;
|
||||
in.depth_compressed.format = "png";
|
||||
in.depth_compressed.data = rtabmap::compressImage(depth, ".png");
|
||||
|
||||
pub_->publish(in);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
const rtabmap_msgs::msg::RGBDImage & got = out_->back();
|
||||
ASSERT_FALSE(got.depth.data.empty()) << "depth must be decompressed";
|
||||
EXPECT_EQ(got.depth.encoding, sensor_msgs::image_encodings::TYPE_16UC1);
|
||||
EXPECT_EQ(got.depth.width, 8u);
|
||||
EXPECT_EQ(got.depth.height, 8u);
|
||||
}
|
||||
|
||||
TEST_F(RGBDRelayTest, UncompressPrefersTheRawImageOverTheCompressedOne)
|
||||
{
|
||||
// A message may carry both. The raw image is already usable, so decompressing the
|
||||
// other copy would be wasted work -- and the two paths must agree, as the depth
|
||||
// branch below does.
|
||||
start(/*compress=*/false, /*uncompress=*/true);
|
||||
|
||||
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
// A compressed copy whose content differs, so it is obvious which one was used.
|
||||
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8",
|
||||
cv::Mat(8, 8, CV_8UC3, cv::Scalar(200, 200, 200)))
|
||||
.toCompressedImageMsg(in.rgb_compressed, cv_bridge::PNG);
|
||||
|
||||
pub_->publish(in);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
|
||||
|
||||
ASSERT_FALSE(out_->back().rgb.data.empty());
|
||||
EXPECT_EQ(out_->back().rgb.data, in.rgb.data)
|
||||
<< "the raw image must be forwarded, not the decompressed copy";
|
||||
}
|
||||
|
||||
TEST_F(RGBDRelayTest, StaysSilentWithoutASubscriber)
|
||||
{
|
||||
// No collector, so the relay's output has no subscriber and it must not do the work.
|
||||
addNode(std::make_shared<rtabmap_util::RGBDRelay>(rclcpp::NodeOptions()));
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
|
||||
pub->publish(makeRGBDImage("camera_link", 1000.0));
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
|
||||
// Subscribing only now must not retroactively receive anything.
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> late =
|
||||
collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay");
|
||||
spinFor(std::chrono::milliseconds(200));
|
||||
EXPECT_TRUE(late->empty());
|
||||
}
|
||||
|
||||
/// Feeds a compressed right image in @p format through the uncompress path.
|
||||
class RGBDRelayRightImageTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
rtabmap_msgs::msg::RGBDImage relay(cv_bridge::Format format)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::RGBDRelay>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({rclcpp::Parameter("uncompress", true)})));
|
||||
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out =
|
||||
collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay");
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
EXPECT_TRUE(waitForSubscriber(pub));
|
||||
EXPECT_TRUE(waitForPublisher(out->subscription));
|
||||
|
||||
rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0);
|
||||
cv_bridge::CvImage(std_msgs::msg::Header(), "mono8",
|
||||
cv::Mat(8, 8, CV_8UC1, cv::Scalar(60)))
|
||||
.toCompressedImageMsg(in.depth_compressed, format);
|
||||
in.depth = sensor_msgs::msg::Image();
|
||||
|
||||
pub->publish(in);
|
||||
EXPECT_TRUE(spinUntil([&]() { return !out->empty(); }));
|
||||
return out->empty() ? rtabmap_msgs::msg::RGBDImage() : out->back();
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(RGBDRelayRightImageTest, UncompressesAJpegRightImage)
|
||||
{
|
||||
const rtabmap_msgs::msg::RGBDImage got = relay(cv_bridge::JPG);
|
||||
ASSERT_FALSE(got.depth.data.empty());
|
||||
EXPECT_EQ(got.depth.encoding, sensor_msgs::image_encodings::MONO8);
|
||||
EXPECT_EQ(got.depth.step, 8u);
|
||||
}
|
||||
|
||||
TEST_F(RGBDRelayRightImageTest, UncompressesAPngRightImage)
|
||||
{
|
||||
// A losslessly compressed right image must not be mistaken for depth and abort.
|
||||
const rtabmap_msgs::msg::RGBDImage got = relay(cv_bridge::PNG);
|
||||
ASSERT_FALSE(got.depth.data.empty());
|
||||
EXPECT_EQ(got.depth.encoding, sensor_msgs::image_encodings::MONO8);
|
||||
EXPECT_EQ(got.depth.step, 8u);
|
||||
}
|
||||
|
||||
/// QoS of the two sides, set independently through qos_sub and qos_pub.
|
||||
///
|
||||
/// A reliable subscription refuses to match a best-effort publisher, while a best-effort
|
||||
/// subscription matches either. Every assertion below rests on that asymmetry: whether a
|
||||
/// connection is established at all is what tells us which reliability the node picked.
|
||||
class RGBDRelayQosTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
enum Reliability { kSystemDefault = 0, kReliable = 1, kBestEffort = 2 };
|
||||
|
||||
void startRelay(const std::vector<rclcpp::Parameter> & params)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::RGBDRelay>(
|
||||
rclcpp::NodeOptions().parameter_overrides(params)));
|
||||
}
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr input(Reliability reliability)
|
||||
{
|
||||
rclcpp::QoS qos(10);
|
||||
reliability == kBestEffort ? qos.best_effort() : qos.reliable();
|
||||
return helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", qos);
|
||||
}
|
||||
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> output(Reliability reliability)
|
||||
{
|
||||
rclcpp::QoS qos(10);
|
||||
reliability == kBestEffort ? qos.best_effort() : qos.reliable();
|
||||
return collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay", qos);
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(RGBDRelayQosTest, BridgesABestEffortSourceToAReliableConsumer)
|
||||
{
|
||||
// The point of splitting the parameter: a sensor publishing best effort feeding a
|
||||
// consumer that only accepts reliable. Neither could talk to the other directly.
|
||||
startRelay({rclcpp::Parameter("qos_sub", int(kBestEffort)),
|
||||
rclcpp::Parameter("qos_pub", int(kReliable))});
|
||||
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out = output(kReliable);
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub = input(kBestEffort);
|
||||
ASSERT_TRUE(waitForSubscriber(pub)) << "a best-effort source must reach the relay";
|
||||
ASSERT_TRUE(waitForPublisher(out->subscription))
|
||||
<< "a reliable consumer must be able to subscribe to the relayed topic";
|
||||
|
||||
pub->publish(makeRGBDImage("camera_link", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out->empty(); }));
|
||||
EXPECT_EQ(out->back().header.frame_id, "camera_link");
|
||||
}
|
||||
|
||||
TEST_F(RGBDRelayQosTest, QosSubOverridesQosOnTheInputOnly)
|
||||
{
|
||||
// qos says reliable, which a best-effort source could not match; qos_sub overrides it.
|
||||
startRelay({rclcpp::Parameter("qos", int(kReliable)),
|
||||
rclcpp::Parameter("qos_sub", int(kBestEffort))});
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub = input(kBestEffort);
|
||||
EXPECT_TRUE(waitForSubscriber(pub)) << "qos_sub must win over qos on the subscription";
|
||||
|
||||
// The output side kept qos, so a reliable consumer still matches it.
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out = output(kReliable);
|
||||
EXPECT_TRUE(waitForPublisher(out->subscription))
|
||||
<< "qos_sub must not affect the publisher";
|
||||
}
|
||||
|
||||
TEST_F(RGBDRelayQosTest, QosPubOverridesQosOnTheOutputOnly)
|
||||
{
|
||||
// qos says best effort, which no reliable consumer could match; qos_pub overrides it.
|
||||
startRelay({rclcpp::Parameter("qos", int(kBestEffort)),
|
||||
rclcpp::Parameter("qos_pub", int(kReliable))});
|
||||
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out = output(kReliable);
|
||||
EXPECT_TRUE(waitForPublisher(out->subscription))
|
||||
<< "qos_pub must win over qos on the publisher";
|
||||
|
||||
// The input side kept qos, so it is still best effort and accepts a best-effort source.
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub = input(kBestEffort);
|
||||
EXPECT_TRUE(waitForSubscriber(pub)) << "qos_pub must not affect the subscription";
|
||||
}
|
||||
|
||||
TEST_F(RGBDRelayQosTest, BothSidesFallBackToQos)
|
||||
{
|
||||
// Only qos is given, so both sides must be best effort -- as before the split.
|
||||
startRelay({rclcpp::Parameter("qos", int(kBestEffort))});
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub = input(kBestEffort);
|
||||
EXPECT_TRUE(waitForSubscriber(pub)) << "the subscription must have followed qos";
|
||||
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out = output(kReliable);
|
||||
spinFor(std::chrono::milliseconds(500));
|
||||
EXPECT_EQ(out->subscription->get_publisher_count(), 0u)
|
||||
<< "the publisher must have followed qos too: best effort, so a reliable "
|
||||
"consumer cannot match it";
|
||||
}
|
||||
|
||||
TEST_F(RGBDRelayQosTest, HonorsTheConfiguredQueueDepths)
|
||||
{
|
||||
// Queue depth is not directly observable from outside, so this only pins down that
|
||||
// the parameters are accepted and the relay still works with them set.
|
||||
startRelay({rclcpp::Parameter("queue_sub", 20), rclcpp::Parameter("queue_pub", 10)});
|
||||
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out = output(kReliable);
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub = input(kReliable);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(out->subscription));
|
||||
|
||||
pub->publish(makeRGBDImage("camera_link", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !out->empty(); }));
|
||||
EXPECT_EQ(out->back().header.frame_id, "camera_link");
|
||||
}
|
||||
|
||||
TEST_F(RGBDRelayQosTest, RejectsAZeroQueueDepth)
|
||||
{
|
||||
// rclcpp::QoS(0) is not a meaningful depth, so say so at construction rather than
|
||||
// leaving the relay silently misconfigured.
|
||||
EXPECT_THROW(
|
||||
startRelay({rclcpp::Parameter("queue_sub", 0)}),
|
||||
UException);
|
||||
}
|
||||
@@ -0,0 +1,397 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include "node_test_utils.hpp"
|
||||
#include "msg_builders.hpp"
|
||||
|
||||
#include <rtabmap_util/rgbd_split.hpp>
|
||||
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap/utilite/UException.h>
|
||||
|
||||
using namespace rtabmap_util_test;
|
||||
|
||||
namespace {
|
||||
::testing::Environment * const kEnv = registerRclcppEnvironment();
|
||||
}
|
||||
|
||||
class RGBDSplitTest : public NodeTest {};
|
||||
|
||||
TEST_F(RGBDSplitTest, SplitsIntoImageAndCameraInfoTopics)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()));
|
||||
|
||||
// The node derives its output topics from the input topic name.
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
|
||||
collect<sensor_msgs::msg::Image>("rgbd_image/rgb/image");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
|
||||
collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> rgbInfo =
|
||||
collect<sensor_msgs::msg::CameraInfo>("rgbd_image/rgb/camera_info");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> depthInfo =
|
||||
collect<sensor_msgs::msg::CameraInfo>("rgbd_image/depth/camera_info");
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(rgb->subscription));
|
||||
ASSERT_TRUE(waitForPublisher(depth->subscription));
|
||||
|
||||
const rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
pub->publish(in);
|
||||
ASSERT_TRUE(spinUntil([&]() {
|
||||
return !rgb->empty() && !depth->empty() && !rgbInfo->empty() && !depthInfo->empty();
|
||||
})) << "not all four outputs were published";
|
||||
|
||||
EXPECT_EQ(rgb->back().encoding, "bgr8");
|
||||
EXPECT_EQ(rgb->back().data, in.rgb.data);
|
||||
EXPECT_EQ(depth->back().encoding, sensor_msgs::image_encodings::TYPE_16UC1);
|
||||
EXPECT_EQ(depth->back().data, in.depth.data);
|
||||
|
||||
EXPECT_NEAR(rgbInfo->back().p[0], in.rgb_camera_info.p[0], 1e-9);
|
||||
EXPECT_EQ(rgbInfo->back().width, in.rgb_camera_info.width);
|
||||
EXPECT_NEAR(depthInfo->back().p[0], in.depth_camera_info.p[0], 1e-9);
|
||||
}
|
||||
|
||||
TEST_F(RGBDSplitTest, FallsBackToTheInputHeaderForTheDepthCameraInfo)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()));
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
|
||||
collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> depthInfo =
|
||||
collect<sensor_msgs::msg::CameraInfo>("rgbd_image/depth/camera_info");
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(depth->subscription));
|
||||
|
||||
// Depth camera info with no frame id: the node fills it from the message header.
|
||||
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
in.depth_camera_info.header.frame_id = "";
|
||||
pub->publish(in);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !depthInfo->empty(); }));
|
||||
|
||||
EXPECT_EQ(depthInfo->back().header.frame_id, "camera_link");
|
||||
}
|
||||
|
||||
TEST_F(RGBDSplitTest, PassesAStereoPairThroughUnchanged)
|
||||
{
|
||||
// The node does not distinguish stereo from depth: it forwards whatever is in the
|
||||
// "depth" slot, so a stereo right image is published on .../depth/image along with
|
||||
// the right camera info carrying the baseline.
|
||||
addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()));
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> right =
|
||||
collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> rightInfo =
|
||||
collect<sensor_msgs::msg::CameraInfo>("rgbd_image/depth/camera_info");
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(right->subscription));
|
||||
|
||||
const rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0);
|
||||
pub->publish(in);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !right->empty() && !rightInfo->empty(); }));
|
||||
|
||||
EXPECT_EQ(right->back().encoding, "mono8") << "the right image is forwarded as-is";
|
||||
EXPECT_EQ(right->back().data, in.depth.data);
|
||||
EXPECT_LT(rightInfo->back().p[3], 0.0) << "the baseline must reach the consumer";
|
||||
}
|
||||
|
||||
TEST_F(RGBDSplitTest, DecompressesDepthWithTheCorrectEncoding)
|
||||
{
|
||||
// rtabmap compresses depth as a PNG whose format string cv_bridge cannot interpret.
|
||||
// The node must decode it itself and label it 16UC1, not mono8: the buffer is two
|
||||
// bytes per pixel and a wrong encoding makes every consumer misread it.
|
||||
addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()));
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
|
||||
collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(depth->subscription));
|
||||
|
||||
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
const cv::Mat original(8, 8, CV_16UC1, cv::Scalar(1500));
|
||||
in.depth = sensor_msgs::msg::Image();
|
||||
in.depth_compressed.header = in.header;
|
||||
in.depth_compressed.format = "png";
|
||||
in.depth_compressed.data = rtabmap::compressImage(original, ".png");
|
||||
|
||||
pub->publish(in);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); }));
|
||||
|
||||
const sensor_msgs::msg::Image & got = depth->back();
|
||||
EXPECT_EQ(got.encoding, sensor_msgs::image_encodings::TYPE_16UC1)
|
||||
<< "a 16-bit depth buffer must not be labeled mono8";
|
||||
EXPECT_EQ(got.width, 8u);
|
||||
EXPECT_EQ(got.height, 8u);
|
||||
ASSERT_EQ(got.step, 16u) << "two bytes per pixel";
|
||||
EXPECT_EQ(*reinterpret_cast<const uint16_t *>(&got.data[0]), 1500)
|
||||
<< "and the values must survive the round trip";
|
||||
}
|
||||
|
||||
/// Feeds a compressed right image in @p format and returns what lands on depth/image.
|
||||
class RGBDSplitRightImageTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
sensor_msgs::msg::Image split(cv_bridge::Format format)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()));
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> right =
|
||||
collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
EXPECT_TRUE(waitForSubscriber(pub));
|
||||
EXPECT_TRUE(waitForPublisher(right->subscription));
|
||||
|
||||
rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0);
|
||||
cv_bridge::CvImage(std_msgs::msg::Header(), "mono8",
|
||||
cv::Mat(8, 8, CV_8UC1, cv::Scalar(60)))
|
||||
.toCompressedImageMsg(in.depth_compressed, format);
|
||||
in.depth = sensor_msgs::msg::Image();
|
||||
|
||||
pub->publish(in);
|
||||
EXPECT_TRUE(spinUntil([&]() { return !right->empty(); }))
|
||||
<< "the right image must be decompressed, not rejected";
|
||||
return right->empty() ? sensor_msgs::msg::Image() : right->back();
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(RGBDSplitRightImageTest, DecompressesAJpegRightImage)
|
||||
{
|
||||
// What stereo_sync emits.
|
||||
const sensor_msgs::msg::Image got = split(cv_bridge::JPG);
|
||||
EXPECT_EQ(got.encoding, sensor_msgs::image_encodings::MONO8);
|
||||
EXPECT_EQ(got.step, 8u) << "one byte per pixel, not mistaken for 16-bit depth";
|
||||
}
|
||||
|
||||
TEST_F(RGBDSplitRightImageTest, DecompressesAPngRightImage)
|
||||
{
|
||||
// Nothing forbids a producer from compressing the right image losslessly, and a
|
||||
// stereo pipeline may prefer it since JPEG artifacts hurt matching. Going by the
|
||||
// format string alone would send this down the depth path and abort on the assert.
|
||||
const sensor_msgs::msg::Image got = split(cv_bridge::PNG);
|
||||
EXPECT_EQ(got.encoding, sensor_msgs::image_encodings::MONO8);
|
||||
EXPECT_EQ(got.step, 8u);
|
||||
}
|
||||
|
||||
/// Queue depths and the reach of the qos parameter.
|
||||
class RGBDSplitQosTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
void startSplit(const std::vector<rclcpp::Parameter> & params)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::RGBDSplit>(
|
||||
rclcpp::NodeOptions().parameter_overrides(params)));
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(RGBDSplitQosTest, HonorsTheConfiguredQueueDepths)
|
||||
{
|
||||
// Queue depth is not observable from outside, so this pins down that the parameters
|
||||
// are accepted and the node still splits with them set.
|
||||
startSplit({rclcpp::Parameter("queue_sub", 20), rclcpp::Parameter("queue_pub", 10)});
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
|
||||
collect<sensor_msgs::msg::Image>("rgbd_image/rgb/image");
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(rgb->subscription));
|
||||
|
||||
pub->publish(makeRGBDImage("camera_link", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !rgb->empty(); }));
|
||||
EXPECT_EQ(rgb->back().encoding, "bgr8");
|
||||
}
|
||||
|
||||
TEST_F(RGBDSplitQosTest, RejectsAZeroQueueDepth)
|
||||
{
|
||||
EXPECT_THROW(startSplit({rclcpp::Parameter("queue_pub", 0)}), UException);
|
||||
}
|
||||
|
||||
TEST_F(RGBDSplitQosTest, AppliesQosToTheCameraInfoPublishersToo)
|
||||
{
|
||||
// A best-effort node must be best effort on every output, camera infos included:
|
||||
// a reliable consumer must not match any of them.
|
||||
startSplit({rclcpp::Parameter("qos", 2)});
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> rgbInfo =
|
||||
collect<sensor_msgs::msg::CameraInfo>(
|
||||
"rgbd_image/rgb/camera_info", rclcpp::QoS(10).reliable());
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> depthInfo =
|
||||
collect<sensor_msgs::msg::CameraInfo>(
|
||||
"rgbd_image/depth/camera_info", rclcpp::QoS(10).reliable());
|
||||
spinFor(std::chrono::milliseconds(500));
|
||||
|
||||
EXPECT_EQ(rgbInfo->subscription->get_publisher_count(), 0u)
|
||||
<< "the rgb camera info publisher ignored qos";
|
||||
EXPECT_EQ(depthInfo->subscription->get_publisher_count(), 0u)
|
||||
<< "the depth camera info publisher ignored qos";
|
||||
}
|
||||
|
||||
/// Output topic naming, controlled by the stereo parameter.
|
||||
class RGBDSplitStereoNamingTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
void startSplit(bool stereo)
|
||||
{
|
||||
addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()
|
||||
.parameter_overrides({rclcpp::Parameter("stereo", stereo)})));
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(RGBDSplitStereoNamingTest, PublishesOnLeftAndRightWhenStereoIsSet)
|
||||
{
|
||||
startSplit(/*stereo=*/true);
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> left =
|
||||
collect<sensor_msgs::msg::Image>("rgbd_image/left/image");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> right =
|
||||
collect<sensor_msgs::msg::Image>("rgbd_image/right/image");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> leftInfo =
|
||||
collect<sensor_msgs::msg::CameraInfo>("rgbd_image/left/camera_info");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> rightInfo =
|
||||
collect<sensor_msgs::msg::CameraInfo>("rgbd_image/right/camera_info");
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(left->subscription));
|
||||
ASSERT_TRUE(waitForPublisher(right->subscription));
|
||||
|
||||
const rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0);
|
||||
pub->publish(in);
|
||||
ASSERT_TRUE(spinUntil([&]() {
|
||||
return !left->empty() && !right->empty() && !leftInfo->empty() && !rightInfo->empty();
|
||||
})) << "not all four outputs were published";
|
||||
|
||||
EXPECT_EQ(left->back().data, in.rgb.data) << "the rgb slot feeds the left topic";
|
||||
EXPECT_EQ(right->back().data, in.depth.data) << "the depth slot feeds the right topic";
|
||||
EXPECT_LT(rightInfo->back().p[3], 0.0) << "the baseline must reach the right camera info";
|
||||
}
|
||||
|
||||
TEST_F(RGBDSplitStereoNamingTest, DoesNotPublishOnRgbAndDepthWhenStereoIsSet)
|
||||
{
|
||||
// The two namings are exclusive: nothing must be left publishing the old names.
|
||||
startSplit(/*stereo=*/true);
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
|
||||
collect<sensor_msgs::msg::Image>("rgbd_image/rgb/image");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
|
||||
collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
|
||||
spinFor(std::chrono::milliseconds(500));
|
||||
|
||||
EXPECT_EQ(rgb->subscription->get_publisher_count(), 0u);
|
||||
EXPECT_EQ(depth->subscription->get_publisher_count(), 0u);
|
||||
}
|
||||
|
||||
TEST_F(RGBDSplitStereoNamingTest, KeepsRgbAndDepthByDefault)
|
||||
{
|
||||
startSplit(/*stereo=*/false);
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
|
||||
collect<sensor_msgs::msg::Image>("rgbd_image/rgb/image");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> left =
|
||||
collect<sensor_msgs::msg::Image>("rgbd_image/left/image");
|
||||
ASSERT_TRUE(waitForPublisher(rgb->subscription));
|
||||
EXPECT_EQ(left->subscription->get_publisher_count(), 0u)
|
||||
<< "left/right naming must be opt-in";
|
||||
}
|
||||
|
||||
TEST_F(RGBDSplitStereoNamingTest, StillPublishesADepthImageOnRightWithStereoSet)
|
||||
{
|
||||
// A depth image with stereo set is a misconfiguration: the node warns (once) but
|
||||
// keeps forwarding, so an existing pipeline is never silently broken.
|
||||
startSplit(/*stereo=*/true);
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> right =
|
||||
collect<sensor_msgs::msg::Image>("rgbd_image/right/image");
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(right->subscription));
|
||||
|
||||
// makeRGBDImage carries 16UC1 depth, not a right image.
|
||||
pub->publish(makeRGBDImage("camera_link", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !right->empty(); }))
|
||||
<< "the image must still be forwarded, warning or not";
|
||||
EXPECT_EQ(right->back().encoding, sensor_msgs::image_encodings::TYPE_16UC1);
|
||||
}
|
||||
|
||||
TEST_F(RGBDSplitStereoNamingTest, StillPublishesARightImageOnDepthWithStereoUnset)
|
||||
{
|
||||
// The inverse misconfiguration, and the one this node has always allowed: a stereo
|
||||
// pair with stereo left false. It warns, but the right image must still come out.
|
||||
startSplit(/*stereo=*/false);
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
|
||||
collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(depth->subscription));
|
||||
|
||||
const rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0);
|
||||
pub->publish(in);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); }))
|
||||
<< "the right image must still be forwarded, warning or not";
|
||||
EXPECT_EQ(depth->back().encoding, "mono8");
|
||||
EXPECT_EQ(depth->back().data, in.depth.data);
|
||||
}
|
||||
|
||||
TEST_F(RGBDSplitStereoNamingTest, DoesNotWarnOnAnEmptySecondHalf)
|
||||
{
|
||||
// A color-only RGBDImage leaves the depth slot empty, whose encoding is "". That
|
||||
// must not be mistaken for a right image: nothing is published, nothing to warn about.
|
||||
startSplit(/*stereo=*/false);
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
|
||||
collect<sensor_msgs::msg::Image>("rgbd_image/rgb/image");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
|
||||
collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(rgb->subscription));
|
||||
|
||||
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
|
||||
in.depth = sensor_msgs::msg::Image();
|
||||
pub->publish(in);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !rgb->empty(); }));
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
|
||||
EXPECT_EQ(rgb->back().encoding, "bgr8") << "the color half is unaffected";
|
||||
if(!depth->empty())
|
||||
{
|
||||
EXPECT_TRUE(depth->back().data.empty())
|
||||
<< "an absent depth image must not turn into a non-empty one";
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(RGBDSplitQosTest, QosSubAndQosPubOverrideQosPerSide)
|
||||
{
|
||||
// qos says reliable, which a best-effort source could not match; qos_sub overrides
|
||||
// it, while qos_pub keeps the outputs reliable for a strict consumer.
|
||||
startSplit({rclcpp::Parameter("qos", 1),
|
||||
rclcpp::Parameter("qos_sub", 2),
|
||||
rclcpp::Parameter("qos_pub", 1)});
|
||||
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
|
||||
collect<sensor_msgs::msg::Image>("rgbd_image/rgb/image", rclcpp::QoS(10).reliable());
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>(
|
||||
"rgbd_image", rclcpp::QoS(10).best_effort());
|
||||
ASSERT_TRUE(waitForSubscriber(pub)) << "qos_sub must win over qos on the subscription";
|
||||
ASSERT_TRUE(waitForPublisher(rgb->subscription)) << "qos_pub must keep the output reliable";
|
||||
|
||||
pub->publish(makeRGBDImage("camera_link", 1000.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !rgb->empty(); }));
|
||||
EXPECT_EQ(rgb->back().encoding, "bgr8");
|
||||
}
|
||||
Reference in New Issue
Block a user