mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 08:47:45 +08:00
rtabmap_slam tests and doc (#1460)
* rtabmap_slam tests and doc * another round of review of the doc * Disable by default use_intra_process_comms on latched/transient publishers * Added test to catch not unlocked mutex from early error exit * updated coverage settings * Using UScopeMutex on all tryLock()
This commit is contained in:
matlabbe
@@ -240,4 +240,43 @@ install(DIRECTORY include/
|
||||
FILES_MATCHING PATTERN "*.h"
|
||||
)
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
find_package(rtabmap_conversions REQUIRED)
|
||||
|
||||
# Each test binary drives its own rtabmap node: a crash or a stuck executor in one
|
||||
# cannot take the others down, and each starts from a clean DDS graph.
|
||||
#
|
||||
# Each binary also gets its own DDS domain. colcon tests packages in parallel and ctest
|
||||
# can run these binaries in parallel, while these suites share topic names -- odom,
|
||||
# scan, info -- with the other packages' suites. On a shared domain they discover each
|
||||
# other's publishers and assertions then see traffic the test never sent. rtabmap_util
|
||||
# numbers from 30, rtabmap_sync from 50 and rtabmap_odom from 70; keep the ranges apart.
|
||||
set(rtabmap_slam_test_domain_id 90)
|
||||
macro(rtabmap_slam_add_node_test test_name)
|
||||
ament_add_gtest(${test_name} test/${test_name}.cpp
|
||||
ENV ROS_DOMAIN_ID=${rtabmap_slam_test_domain_id}
|
||||
TIMEOUT 300)
|
||||
math(EXPR rtabmap_slam_test_domain_id "${rtabmap_slam_test_domain_id} + 1")
|
||||
if(TARGET ${test_name})
|
||||
target_include_directories(${test_name} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test)
|
||||
target_link_libraries(${test_name} rtabmap_slam_plugins)
|
||||
if("$ENV{ROS_DISTRO}" STRLESS "lyrical")
|
||||
ament_target_dependencies(${test_name} ${AmentLibraries} rtabmap_conversions)
|
||||
else()
|
||||
target_link_libraries(${test_name} ${Libraries} rtabmap_conversions::rtabmap_conversions)
|
||||
endif()
|
||||
endif()
|
||||
endmacro()
|
||||
|
||||
rtabmap_slam_add_node_test(test_core_wrapper_parameters)
|
||||
rtabmap_slam_add_node_test(test_core_wrapper_mapping)
|
||||
rtabmap_slam_add_node_test(test_core_wrapper_services)
|
||||
rtabmap_slam_add_node_test(test_core_wrapper_planning)
|
||||
rtabmap_slam_add_node_test(test_core_wrapper_inputs)
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
|
||||
@@ -0,0 +1,111 @@
|
||||
# rtabmap_slam
|
||||
|
||||
The SLAM node of [RTAB-Map](https://github.com/introlab/rtabmap): it takes a pose from odometry and data from the sensors, builds a graph of where the robot has been, and corrects that graph whenever it recognizes a place it has seen before — in the current run or in a previous one, so a map can be extended over [several sessions](#the-database).
|
||||
|
||||
## Contents
|
||||
|
||||
- [Nodes](#nodes)
|
||||
- [Conventions](#conventions)
|
||||
- [Frames and TF](#frames-and-tf)
|
||||
- [The database](#the-database)
|
||||
- [Update rate and dropped updates](#update-rate-and-dropped-updates)
|
||||
- [Odometry, covariance and new maps](#odometry-covariance-and-new-maps)
|
||||
- [Mapping and localization](#mapping-and-localization)
|
||||
- [License](#license)
|
||||
|
||||
## Nodes
|
||||
|
||||
| Node | Description |
|
||||
|---|---|
|
||||
| [rtabmap](doc/rtabmap.md) | Graph SLAM with appearance- and proximity-based loop closure detection, memory management, map assembly and planning on the graph. |
|
||||
|
||||
```mermaid
|
||||
flowchart LR
|
||||
SYNC["<div style='text-align:left'><b>synchronized</b><br>RGB-D camera(s)<br>Stereo camera(s)<br>2D LiDAR<br>3D LiDAR<br>Odometry</div>"]
|
||||
ASYNC["<div style='text-align:left'><b>asynchronous</b><br>IMU<br>GPS<br>Landmarks (markers, tags, fiducials)</div>"]
|
||||
RTAB(["<b>rtabmap</b>"])
|
||||
GRAPH["Graph"]
|
||||
INFO["Info"]
|
||||
MAPS["<div style='text-align:left'><b>maps</b><br>2D occupancy grid<br>OctoMap<br>Elevation map<br>3D point cloud</div>"]
|
||||
TF["TF map → odom"]
|
||||
SYNC --> RTAB
|
||||
ASYNC --> RTAB
|
||||
RTAB --> GRAPH
|
||||
RTAB --> INFO
|
||||
RTAB --> MAPS
|
||||
RTAB --> TF
|
||||
```
|
||||
|
||||
## Conventions
|
||||
|
||||
### Frames and TF
|
||||
|
||||
The node publishes `map` → `odom`, the correction from the optimized graph; odometry publishes `odom` → `base_link`, and the sensors are attached to `base_link`. See [Frames and TF](doc/rtabmap.md#frames-and-tf) for the parameters.
|
||||
|
||||
```mermaid
|
||||
flowchart TB
|
||||
MAP(["map<br><i>map_frame_id</i>"])
|
||||
ODOM(["odom<br><i>odometry frame</i>"])
|
||||
BASE(["base_link<br><i>frame_id</i>"])
|
||||
SENSOR(["camera, lidar, imu..."])
|
||||
MAP -->|this node| ODOM
|
||||
ODOM -->|odometry| BASE
|
||||
BASE -->|static, URDF| SENSOR
|
||||
```
|
||||
|
||||
### The database
|
||||
|
||||
**The map is stored in `database_path`**, `~/.ros/rtabmap.db` by default (under `$ROS_HOME` if that is set). Set `delete_db_on_start`, or pass `-d` as an argument, to start from an empty one. Otherwise restarting on an existing database continues it: the map is reloaded, the next update starts a **new session**, and a loop closure between the new session and an old one merges the two. That is how a map is extended over several runs.
|
||||
|
||||
**The database is saved on shutdown**. A node that is killed rather than shut down loses whatever had not been written yet.
|
||||
|
||||
**The database also remembers the parameters it was built with**, and reopening it without setting them again brings them back. For example, a map made with ICP registration keeps using ICP, which is what makes the new session compatible with the old ones. Anything set explicitly still wins, and `delete_db_on_start` forgets them along with the map.
|
||||
|
||||
### Update rate and dropped updates
|
||||
|
||||
`Rtabmap/DetectionRate` is how many updates per second are processed. It is 1 Hz by default because SLAM does not need more: odometry carries the pose between nodes, and each node costs memory, loop closure detection and optimization time for as long as the map exists.
|
||||
|
||||
> **Warning: `Rtabmap/DetectionRate` at `0` with sensor updates faster than about 2 Hz makes loop closure detection, graph optimization and map generation intractable fast.** Every update then becomes a node, and each node is compared against all the nodes in working memory, adds a pose to optimize and data to assemble into the maps, so the map, and the time each update takes, grow at the sensors' rate until the node cannot keep up. Keep a detection rate of 1 to 2 Hz, or bound working memory with `Rtabmap/TimeThr` or `Rtabmap/MemoryThr`.
|
||||
|
||||
**A robot standing still does not grow the map.** An update that moved less than both `RGBD/LinearUpdate` and `RGBD/AngularUpdate` since the last node is still used to detect loop closures, and then dropped. Set both to `0` to add a node every time.
|
||||
|
||||
**An update arriving while the previous one is still being processed is dropped**, not queued. SLAM time grows with the map, so a queue would only fall further behind; dropping keeps the node on the newest data. `info` shows how long each update took (`RtabmapROS/TimeTotal/ms`), and `/diagnostics` how many arrived versus how many were processed.
|
||||
|
||||
### Odometry, covariance and new maps
|
||||
|
||||
The link between two consecutive nodes is the odometry between them, weighted by its covariance — its inverse becomes the link's information matrix, so the optimizer knows how far to trust each one.
|
||||
|
||||
Which covariance is used:
|
||||
|
||||
- **The twist covariance, if it is set.** It is the uncertainty of the motion since the previous message, which is what a link between two nodes is.
|
||||
- **Otherwise half the pose covariance**, for odometry sources that only fill that one. This assumes it is the error of the motion since the previous message, as visual odometry often publishes it, not the unbounded uncertainty of the pose estimated by a filtered odometry.
|
||||
- **Otherwise `odom_tf_linear_variance` and `odom_tf_angular_variance`** (`0.001` by default), for a covariance that is zero, not finite, or exactly `1` — which is what several drivers publish to mean "not set". Many do publish zeros, and taking those at face value would make each link infinitely confident.
|
||||
|
||||
Between two nodes, the largest covariance seen is kept, so updates dropped by the rate do not make the link look more certain than any of the motions that made it up.
|
||||
|
||||
**An odometry reset starts a new map**, in the same database, rather than deforming the graph across a jump the robot never made:
|
||||
|
||||
```
|
||||
Odometry is reset (identity pose or high variance detected). Increment map id!
|
||||
```
|
||||
|
||||
A reset is an identity pose after a non-identity one, or `9999` on both the pose and the twist covariance diagonals — which is what the [odometry nodes publish](../rtabmap_odom/README.md#lost-frames-resets-and-new-maps) when they lose track or restart. Odometry read from TF has no covariance, so only the identity pose counts there. The new map is merged back into the old one on the first loop closure between them.
|
||||
|
||||
A consequence worth knowing: an odometry that returns to *exactly* the identity is taken for a reset. Real odometry never does, but a simulator or a test that drives back to the origin will.
|
||||
|
||||
**`staleness_factor`** treats a long silence the same way. With `Rtabmap/DetectionRate` at 1 Hz and a factor of 2, an update more than 2 seconds after the previous one starts a new map. It is for odometry sources that go quiet instead of reporting a reset — a gap that long means the motion across it is not known, even if the next pose looks plausible.
|
||||
|
||||
### Mapping and localization
|
||||
|
||||
`Mem/IncrementalMemory` chooses between the two:
|
||||
|
||||
- **`true`, mapping (SLAM)**, the default. Updates become nodes and the map grows. This is the mode to create a map of the environment.
|
||||
- **`false`, localization.** The map is loaded and not extended: each update is compared against it, localizes the robot if it matches, and is not added to the database. This is the mode to localize in a map already recorded, without increasing CPU and RAM usage, since the map is kept fixed.
|
||||
|
||||
`set_mode_localization` and `set_mode_mapping` switch at runtime. Going back to mapping starts a new session, since nothing links where the robot is now to where it left the map — until a loop closure does.
|
||||
|
||||
See [Localization](doc/rtabmap.md#localization) for where the robot starts on the map in localization mode.
|
||||
|
||||
## License
|
||||
|
||||
BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license).
|
||||
@@ -0,0 +1,623 @@
|
||||
# rtabmap
|
||||
|
||||
Graph SLAM: each update that moved far enough becomes a node, linked to the previous one by odometry and to earlier ones by the loop closures found, and the graph is optimized every time a loop closure is added.
|
||||
|
||||
Loop closures are found two ways:
|
||||
|
||||
- **Appearance-based**: the node's visual words are compared against every node in working memory with an incremental bag-of-words (BoW) approach, which is independent of the odometry pose, and so of its drift.
|
||||
- **Proximity-based**: the node is registered against the nodes the graph says are nearby, based on the previous localization and the current odometry pose. This is what a lidar-only setup relies on.
|
||||
|
||||
**Memory management**: working memory can be bounded, by update time (`Rtabmap/TimeThr`, in ms) or by node count (`Rtabmap/MemoryThr`): older nodes are then moved to the database and brought back when the robot returns near them, so the update time stays flat on large maps. Both are `0` by default, which leaves working memory unbounded: every node stays in it, and the update time grows with the map. Before enabling memory management, we strongly recommend reading [Long-Term Online Multi-Session Graph-Based SPLAM with Memory Management](https://arxiv.org/abs/2301.00050), which explains how it works and what it implies for mapping, localization and planning.
|
||||
|
||||
## Contents
|
||||
|
||||
- [Usage](#usage)
|
||||
- [Choosing the inputs](#choosing-the-inputs)
|
||||
- [RGB-D camera (RGB-D visual SLAM)](#rgb-d-camera-rgb-d-visual-slam)
|
||||
- [Stereo camera (stereo visual SLAM)](#stereo-camera-stereo-visual-slam)
|
||||
- [RGB-D or stereo camera and lidar](#rgb-d-or-stereo-camera-and-lidar)
|
||||
- [Several RGB-D or stereo cameras](#several-rgb-d-or-stereo-cameras)
|
||||
- [Several RGB-D or stereo cameras and lidar](#several-rgb-d-or-stereo-cameras-and-lidar)
|
||||
- [Lidar alone](#lidar-alone)
|
||||
- [RGB camera with odometry](#rgb-camera-with-odometry)
|
||||
- [RGB camera alone (appearance-based loop closure detection)](#rgb-camera-alone-appearance-based-loop-closure-detection)
|
||||
- [Odometry from TF](#odometry-from-tf)
|
||||
- [Automatic adjustments](#automatic-adjustments)
|
||||
- [Sensors not stamped together](#sensors-not-stamped-together)
|
||||
- [Subscribed Topics](#subscribed-topics)
|
||||
- [Published Topics](#published-topics)
|
||||
- [Services](#services)
|
||||
- [Parameters](#parameters)
|
||||
- [RTAB-Map's own parameters](#rtab-maps-own-parameters)
|
||||
- [Frames and TF](#frames-and-tf)
|
||||
- [Asynchronous inputs](#asynchronous-inputs)
|
||||
- [Landmarks](#landmarks)
|
||||
- [GPS and global pose](#gps-and-global-pose)
|
||||
- [IMU](#imu)
|
||||
- [User data and environment sensors](#user-data-and-environment-sensors)
|
||||
- [Intermediate odometry](#intermediate-odometry)
|
||||
- [Deriving missing data](#deriving-missing-data)
|
||||
- [Localization](#localization)
|
||||
- [Planning](#planning)
|
||||
- [Diagnostics](#diagnostics)
|
||||
|
||||
## Usage
|
||||
|
||||
RGB-D camera, with odometry from [rgbd_odometry](../../rtabmap_odom/doc/rgbd_odometry.md) or any other source on `odom`, and the camera synchronized by [rgbd_sync](../../rtabmap_sync/doc/rgbd_sync.md):
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_slam rtabmap --ros-args \
|
||||
-p subscribe_depth:=false -p subscribe_rgb:=false -p subscribe_rgbd:=true \
|
||||
-p frame_id:=base_link \
|
||||
-r rgbd_image:=/camera/rgbd_image \
|
||||
-r odom:=/odom
|
||||
```
|
||||
|
||||
2D lidar, with odometry from TF:
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_slam rtabmap --ros-args \
|
||||
-p subscribe_depth:=false -p subscribe_rgb:=false -p subscribe_scan:=true \
|
||||
-p frame_id:=base_link \
|
||||
-p odom_frame_id:=odom \
|
||||
-p "Reg/Force3DoF:='true'" \
|
||||
-r scan:=/scan
|
||||
```
|
||||
|
||||
```python
|
||||
ComposableNode(
|
||||
package='rtabmap_slam',
|
||||
plugin='rtabmap_slam::CoreWrapper',
|
||||
name='rtabmap',
|
||||
parameters=[{'frame_id': 'base_link',
|
||||
'subscribe_depth': False,
|
||||
'subscribe_rgb': False,
|
||||
'subscribe_rgbd': True,
|
||||
'subscribe_scan': True,
|
||||
'approx_sync': True,
|
||||
'RGBD/LinearUpdate': '0.1',
|
||||
'Reg/Force3DoF': 'true'}],
|
||||
remappings=[('rgbd_image', '/camera/rgbd_image'),
|
||||
('scan', '/scan'),
|
||||
('odom', '/odom')])
|
||||
```
|
||||
|
||||
The executable runs the node on a **multi-threaded** executor, and the node relies on it. SLAM runs in its own callback group, so the synchronized inputs keep arriving while an update is being processed; the asynchronous inputs (GPS, IMU, landmarks, user data...) have groups of their own, so they are buffered rather than blocked. Loaded into a single-threaded component container, it still works, but everything is serialized behind the SLAM update.
|
||||
|
||||
In a component container with intra-process communication enabled (`use_intra_process_comms`), the latched publishers (`mapGraph` and the [`MapsManager`](../../rtabmap_util/README.md#mapsmanager) maps, with `latch` on, the default) automatically opt out of it, since intra-process communication does not support transient local durability. The other publishers keep the container's setting, and with `latch` off, all of them do.
|
||||
|
||||
[`rtabmap_launch`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_launch) wraps all of this, together with odometry and `rtabmap_viz`, and is where most setups should start.
|
||||
|
||||
## Choosing the inputs
|
||||
|
||||
The common setups are below. How the input topics are synchronized — `approx_sync`, `topic_queue_size`, `sync_queue_size` and the `qos*` parameters — is documented in [rtabmap_sync](../../rtabmap_sync/README.md#conventions).
|
||||
|
||||
### RGB-D camera (RGB-D visual SLAM)
|
||||
|
||||
```yaml
|
||||
subscribe_depth: true # default
|
||||
subscribe_rgb: true # default
|
||||
```
|
||||
|
||||
This is the legacy default. The recommended way is instead to synchronize the camera topics together with [rgbd_sync](../../rtabmap_sync/doc/rgbd_sync.md), and subscribe to its `rgbd_image`, as in [RGB-D or stereo camera and lidar](#rgb-d-or-stereo-camera-and-lidar) without the lidar.
|
||||
|
||||
```mermaid
|
||||
%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
|
||||
flowchart LR
|
||||
CAM(["rgb/image<br>depth/image<br>rgb/camera_info"])
|
||||
ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
|
||||
R["<b>rtabmap</b>"]
|
||||
CAM --> R
|
||||
ODOM --> R
|
||||
```
|
||||
|
||||
### Stereo camera (stereo visual SLAM)
|
||||
|
||||
```yaml
|
||||
subscribe_stereo: true
|
||||
subscribe_depth: false
|
||||
subscribe_rgb: false
|
||||
```
|
||||
|
||||
`approx_sync` defaults to `false` here: the left and right images, and the odometry, are expected with exactly the same stamp, as when the odometry comes from [stereo_odometry](../../rtabmap_odom/doc/stereo_odometry.md) on the same camera. The images are assumed to be already rectified; if they are not, set `Rtabmap/ImagesAlreadyRectified` to `false` to rectify them here, at rtabmap's update rate.
|
||||
|
||||
```mermaid
|
||||
%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
|
||||
flowchart LR
|
||||
CAM(["left/image_rect<br>left/camera_info<br>right/image_rect<br>right/camera_info"])
|
||||
ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
|
||||
R["<b>rtabmap</b>"]
|
||||
CAM --> R
|
||||
ODOM --> R
|
||||
```
|
||||
|
||||
### RGB-D or stereo camera and lidar
|
||||
|
||||
```yaml
|
||||
subscribe_rgbd: true
|
||||
subscribe_scan: true # for a 2D lidar
|
||||
#subscribe_scan_cloud: true # for a 3D lidar
|
||||
subscribe_depth: false
|
||||
subscribe_rgb: false
|
||||
```
|
||||
|
||||
The camera comes as one `rgbd_image`, from [rgbd_sync](../../rtabmap_sync/doc/rgbd_sync.md) for an RGB-D camera or [stereo_sync](../../rtabmap_sync/doc/stereo_sync.md) for a stereo camera. Loop closures are still detected visually; with `Reg/Strategy` set to `1`, they are then refined with the lidar (ICP). `RGBD/NeighborLinkRefining` set to `true` also refines, with that registration, the link between each new node and the previous one, which corrects the odometry.
|
||||
|
||||
```mermaid
|
||||
%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
|
||||
flowchart LR
|
||||
SYNC["rgbd_sync<br>or stereo_sync"]
|
||||
SCAN(["scan or scan_cloud"])
|
||||
ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
|
||||
R["<b>rtabmap</b>"]
|
||||
SYNC -->|rgbd_image| R
|
||||
SCAN --> R
|
||||
ODOM --> R
|
||||
```
|
||||
|
||||
### Several RGB-D or stereo cameras
|
||||
|
||||
```yaml
|
||||
subscribe_rgbd: true
|
||||
rgbd_cameras: 4
|
||||
subscribe_depth: false
|
||||
subscribe_rgb: false
|
||||
```
|
||||
|
||||
Each camera is synchronized by its own [rgbd_sync](../../rtabmap_sync/doc/rgbd_sync.md) or [stereo_sync](../../rtabmap_sync/doc/stereo_sync.md), and rtabmap subscribes to their `rgbd_image0`, `rgbd_image1`... directly. This requires `rtabmap_sync` built with [`RTABMAP_SYNC_MULTI_RGBD`](../../rtabmap_sync/README.md#build-options), which is off by default; otherwise, see [Several RGB-D or stereo cameras and lidar](#several-rgb-d-or-stereo-cameras-and-lidar).
|
||||
|
||||
```mermaid
|
||||
%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
|
||||
flowchart LR
|
||||
S0["rgbd_sync<br>or stereo_sync"]
|
||||
S1["rgbd_sync<br>or stereo_sync"]
|
||||
S2["rgbd_sync<br>or stereo_sync"]
|
||||
S3["rgbd_sync<br>or stereo_sync"]
|
||||
ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
|
||||
R["<b>rtabmap</b>"]
|
||||
S0 -->|rgbd_image0| R
|
||||
S1 -->|rgbd_image1| R
|
||||
S2 -->|rgbd_image2| R
|
||||
S3 -->|rgbd_image3| R
|
||||
ODOM --> R
|
||||
```
|
||||
|
||||
### Several RGB-D or stereo cameras and lidar
|
||||
|
||||
```yaml
|
||||
subscribe_rgbd: true
|
||||
rgbd_cameras: 0
|
||||
subscribe_scan: true # optional, for a 2D lidar
|
||||
#subscribe_scan_cloud: true # optional, for a 3D lidar
|
||||
subscribe_depth: false
|
||||
subscribe_rgb: false
|
||||
```
|
||||
|
||||
[rgbdx_sync](../../rtabmap_sync/doc/rgbdx_sync.md) combines the cameras' `rgbd_image` into one `rgbd_images`. This works without any build option.
|
||||
|
||||
```mermaid
|
||||
%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
|
||||
flowchart LR
|
||||
S0["rgbd_sync<br>or stereo_sync"]
|
||||
S1["rgbd_sync<br>or stereo_sync"]
|
||||
S2["rgbd_sync<br>or stereo_sync"]
|
||||
S3["rgbd_sync<br>or stereo_sync"]
|
||||
X["rgbdx_sync"]
|
||||
SCAN(["scan or scan_cloud"])
|
||||
ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
|
||||
R["<b>rtabmap</b>"]
|
||||
S0 -->|rgbd_image0| X
|
||||
S1 -->|rgbd_image1| X
|
||||
S2 -->|rgbd_image2| X
|
||||
S3 -->|rgbd_image3| X
|
||||
X -->|rgbd_images| R
|
||||
SCAN --> R
|
||||
ODOM --> R
|
||||
```
|
||||
|
||||
### Lidar alone
|
||||
|
||||
```yaml
|
||||
subscribe_scan: true # for a 2D lidar
|
||||
#subscribe_scan_cloud: true # for a 3D lidar
|
||||
subscribe_depth: false
|
||||
subscribe_rgb: false
|
||||
```
|
||||
|
||||
There are no images: bag-of-words is disabled, and loop closures are found by proximity alone.
|
||||
|
||||
```mermaid
|
||||
%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
|
||||
flowchart LR
|
||||
SCAN(["scan or scan_cloud"])
|
||||
ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
|
||||
R["<b>rtabmap</b>"]
|
||||
SCAN --> R
|
||||
ODOM --> R
|
||||
```
|
||||
|
||||
### RGB camera with odometry
|
||||
|
||||
```yaml
|
||||
subscribe_depth: false
|
||||
subscribe_rgb: true # default
|
||||
```
|
||||
|
||||
Without depth, the images cannot build a metric map, so this is mainly useful in localization mode, to localize a single camera on a map built with a depth camera. [rgb_sync](../../rtabmap_sync/doc/rgb_sync.md) can also be used to synchronize the image with its camera_info, and rtabmap then subscribes to its `rgbd_image` with `subscribe_rgbd:=true` and `subscribe_rgb:=false`.
|
||||
|
||||
```mermaid
|
||||
%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
|
||||
flowchart LR
|
||||
CAM(["rgb/image<br>rgb/camera_info"])
|
||||
ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
|
||||
R["<b>rtabmap</b>"]
|
||||
CAM --> R
|
||||
ODOM --> R
|
||||
```
|
||||
|
||||
### RGB camera alone (appearance-based loop closure detection)
|
||||
|
||||
```yaml
|
||||
subscribe_depth: false
|
||||
subscribe_rgb: false
|
||||
subscribe_odom: false
|
||||
RGBD/Enabled: "false"
|
||||
```
|
||||
|
||||
The node then subscribes to `image` and only detects loop closures between images: no odometry, no graph optimization, no metric map.
|
||||
|
||||
```mermaid
|
||||
%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
|
||||
flowchart LR
|
||||
CAM(["image"])
|
||||
R["<b>rtabmap</b>"]
|
||||
CAM --> R
|
||||
```
|
||||
|
||||
## Odometry from TF
|
||||
|
||||
Setting `odom_frame_id` reads odometry from TF instead: `odom_frame_id` → `frame_id` is looked up at the stamp of each sensor message, and `subscribe_odom` is turned off. This is the natural arrangement when the odometry source publishes only TF, and it saves synchronizing one more topic.
|
||||
|
||||
The trade-off is covariance: TF has none, so every link gets `odom_tf_linear_variance` and `odom_tf_angular_variance`, and an odometry reset can only be recognized by an identity pose — not by the `9999` covariance the [odometry nodes](../../rtabmap_odom/README.md#lost-frames-resets-and-new-maps) publish when they lose track. Prefer the topic when the source provides a meaningful covariance.
|
||||
|
||||
A sensor message whose stamp cannot be found in TF within `wait_for_transform` is dropped. The same goes for a sensor frame that is not connected to `frame_id`.
|
||||
|
||||
## Automatic adjustments
|
||||
|
||||
Some RTAB-Map defaults only make sense for a camera. With a lidar, or without a camera, the node adjusts them — for example, the occupancy grid built from the scan, and loop closures registered with ICP — unless they were set explicitly, and the log says what it changed.
|
||||
|
||||
## Sensors not stamped together
|
||||
|
||||
On a real robot the sensors are rarely stamped together: odometry, a lidar and cameras run at their own rates and are triggered independently. The synchronizer (`approx_sync`) groups the closest messages into one update, and the node then makes them consistent in time:
|
||||
|
||||
- **The node's stamp is the lidar's**, when there is one, or else the first camera's.
|
||||
- **The odometry is taken at that stamp**, interpolated in TF between odometry samples. Without odometry in TF, the synchronized odometry message is used as it is, pose and stamp: the node then takes the odometry's stamp rather than the lidar's.
|
||||
- **The link's covariance is the synchronized odometry message's**, not the last one received: the largest among the updates merged into the node.
|
||||
|
||||
`odom_sensor_sync`, on by default, uses the odometry in TF to correct each sensor for the robot's motion:
|
||||
|
||||
- **Each camera** is moved by the motion between its own stamp and the node's: an image taken 15 ms after the lidar is placed where the robot was 15 ms later. With several cameras triggered one after the other -- in one `rgbd_images` message or on separate topics -- each keeps its own stamp and is placed separately.
|
||||
- **A 3D cloud** (`scan_cloud`) is assumed already deskewed, and is moved as a whole, the same way. Deskew it upstream, with [`lidar_deskewing`](../../rtabmap_util/doc/lidar_deskewing.md) or [`icp_odometry`'s deskewing](../../rtabmap_odom/doc/icp_odometry.md#deskewing).
|
||||
- **A 2D scan** (`scan`) is deskewed ray by ray, when its `time_increment` is set: each ray is placed where the robot was when it was measured. That needs the odometry in TF across the whole sweep, within `wait_for_transform`.
|
||||
|
||||
**Without odometry in TF** -- odometry published as a topic only -- none of this is possible, and the sensors are used as they are: cameras at their mount with a warning, 2D scans without deskewing with a warning shown once. Nothing is dropped.
|
||||
|
||||
With `odom_sensor_sync` off, every sensor is placed at its mount, as if it had been stamped with the lidar, and 2D scans are not deskewed. On a moving robot, that costs centimeters: a camera triggered 15 ms late on a robot turning at 0.5 rad/s misplaces what it sees 3 m away by 2 cm, and a 0.1 s lidar sweep at 1 m/s bends the scan by 10 cm.
|
||||
|
||||
## Subscribed Topics
|
||||
|
||||
**Synchronized** — see [Choosing the inputs](#choosing-the-inputs).
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `rgb/image`, `depth/image`, `rgb/camera_info` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html), [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | RGB-D camera, depth registered to color. |
|
||||
| `left/image_rect`, `right/image_rect`, `left/camera_info`, `right/camera_info` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html), [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Stereo camera. |
|
||||
| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | One camera, from [rgbd_sync](../../rtabmap_sync/doc/rgbd_sync.md), [stereo_sync](../../rtabmap_sync/doc/stereo_sync.md) or [rgb_sync](../../rtabmap_sync/doc/rgb_sync.md). |
|
||||
| `rgbd_images` | [`rtabmap_msgs/msg/RGBDImages`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImages.html) | Several cameras, from [rgbdx_sync](../../rtabmap_sync/doc/rgbdx_sync.md). |
|
||||
| `scan` | [`sensor_msgs/msg/LaserScan`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/LaserScan.html) | 2D lidar. |
|
||||
| `scan_cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | 3D lidar, or [`icp_odometry`](../../rtabmap_odom/doc/icp_odometry.md)'s [filtered scan](../../rtabmap_odom/doc/icp_odometry.md#reusing-the-filtered-scan-downstream). |
|
||||
| `scan_descriptor` | [`rtabmap_msgs/msg/ScanDescriptor`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/ScanDescriptor.html) | A scan with a global descriptor for loop closure detection. |
|
||||
| `sensor_data` | [`rtabmap_msgs/msg/SensorData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/SensorData.html) | Everything a node holds in one message, as the odometry nodes republish it on `odom_sensor_data/*`. |
|
||||
| `odom` | [`nav_msgs/msg/Odometry`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Odometry.html) | Odometry. Mainly used for its covariance, which weights the link between consecutive nodes in the graph (see [Odometry, covariance and new maps](../README.md#odometry-covariance-and-new-maps)). The pose is taken from TF at the sensors' stamp when available; the message's pose is used otherwise. |
|
||||
| `odom_info` | [`rtabmap_msgs/msg/OdomInfo`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/OdomInfo.html) | From an `rtabmap_odom` node: its statistics are stored with the node, and its measured motion gives the velocity. |
|
||||
| `user_data` | [`rtabmap_msgs/msg/UserData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/UserData.html) | Arbitrary data stored with the node. |
|
||||
|
||||
**Asynchronous** — buffered, and attached to the next node. See [Asynchronous inputs](#asynchronous-inputs).
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `user_data_async` | [`rtabmap_msgs/msg/UserData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/UserData.html) | Arbitrary data for the next node. See [User data and environment sensors](#user-data-and-environment-sensors). |
|
||||
| `gps/fix` | [`sensor_msgs/msg/NavSatFix`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/NavSatFix.html) | GPS. See [GPS and global pose](#gps-and-global-pose). |
|
||||
| `global_pose` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseWithCovarianceStamped.html) | An absolute pose from outside, added as a prior. See [GPS and global pose](#gps-and-global-pose). |
|
||||
| `imu` | [`sensor_msgs/msg/Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Only the orientation is used, for gravity constraints: it must already be estimated, by [`imu_filter_madgwick` or `imu_complementary_filter`](https://github.com/CCNYRoboticsLab/imu_tools) for example. See [IMU](#imu). |
|
||||
| `landmark_detection`, `landmark_detections` | [`rtabmap_msgs/msg/LandmarkDetection`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/LandmarkDetection.html), [`rtabmap_msgs/msg/LandmarkDetections`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/LandmarkDetections.html) | Fiducials or any other identified landmark. See [Landmarks](#landmarks). |
|
||||
| `apriltag/detections` | [`apriltag_msgs/msg/AprilTagDetectionArray`](https://github.com/christianrauch/apriltag_msgs/blob/master/msg/AprilTagDetectionArray.msg) | Landmarks straight from [apriltag_ros](https://github.com/christianrauch/apriltag_ros). `tag_detections` is its deprecated name. |
|
||||
| `aruco/detections` | [`aruco_msgs/msg/MarkerArray`](https://docs.ros.org/en/jazzy/p/aruco_msgs/msg/MarkerArray.html) | Landmarks straight from [aruco_ros](https://github.com/pal-robotics/aruco_ros). |
|
||||
| `aruco_opencv/detections` | [`aruco_opencv_msgs/msg/ArucoDetection`](https://docs.ros.org/en/jazzy/p/aruco_opencv_msgs/msg/ArucoDetection.html) | Landmarks straight from [ros_aruco_opencv](https://github.com/fictionlab/ros_aruco_opencv). |
|
||||
| `aruco_markers/detections` | [`aruco_markers_msgs/msg/MarkerArray`](https://docs.ros.org/en/jazzy/p/aruco_markers_msgs/msg/MarkerArray.html) | Landmarks straight from [aruco_markers](https://github.com/namo-robotics/aruco_markers). |
|
||||
| `aruco_interfaces/detections` | [`ros2_aruco_interfaces/msg/ArucoMarkers`](https://github.com/JMU-ROBOTICS-VIVA/ros2_aruco/blob/main/ros2_aruco_interfaces/msg/ArucoMarkers.msg) | Landmarks straight from [ros2_aruco](https://github.com/JMU-ROBOTICS-VIVA/ros2_aruco). |
|
||||
| `env_sensor` | [`rtabmap_msgs/msg/EnvSensor`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/EnvSensor.html) | A scalar reading stored with the next node: WiFi signal strength, or one of the [environment sensors Android devices have](https://developer.android.com/develop/sensors-and-location/sensors/sensors_environment). See [User data and environment sensors](#user-data-and-environment-sensors). |
|
||||
| `inter_odom` | [`nav_msgs/msg/Odometry`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Odometry.html) | A faster odometry, to fill the gaps between nodes with intermediate nodes. See [Intermediate odometry](#intermediate-odometry). |
|
||||
| `inter_odom_info` | [`rtabmap_msgs/msg/OdomInfo`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/OdomInfo.html) | Its statistics, with `subscribe_inter_odom_info`. See [Intermediate odometry](#intermediate-odometry). |
|
||||
|
||||
Each detector topic (`apriltag/detections` to `aruco_interfaces/detections`) exists only when this package was built with that detector's messages package. They all feed the same landmarks as `landmark_detection`; see [Landmarks](#landmarks).
|
||||
|
||||
**Commands**
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `initialpose` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseWithCovarianceStamped.html) | Where the robot is, in localization mode. |
|
||||
| `goal` | [`geometry_msgs/msg/PoseStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseStamped.html) | A goal pose to plan to. |
|
||||
| `goal_node` | [`rtabmap_msgs/msg/Goal`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/Goal.html) | A goal node, by id or label. |
|
||||
| `~/republish_node_data` | [`std_msgs/msg/Int32MultiArray`](https://docs.ros.org/en/jazzy/p/std_msgs/msg/Int32MultiArray.html) | Node ids whose data to include in the next `mapData`, for a visualizer catching up on a map it joined late. |
|
||||
|
||||
## Published Topics
|
||||
|
||||
**Every topic is published only when something is subscribed** — the work of building each message is skipped otherwise. The TF broadcast is not gated this way.
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `info` | [`rtabmap_msgs/msg/Info`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/Info.html) | Everything about the last update: the node id, loop closure and proximity detection results, and all of RTAB-Map's statistics with their timings. One per processed update — intermediate nodes excepted. The first thing to look at when the map misbehaves. |
|
||||
| `mapData` | [`rtabmap_msgs/msg/MapData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/MapData.html) | The optimized graph, plus the data of the node just added. What `rtabmap_viz` and [map_assembler](../../rtabmap_util/doc/map_assembler.md) consume. |
|
||||
| `mapGraph` | [`rtabmap_msgs/msg/MapGraph`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/MapGraph.html) | The optimized graph alone: poses, links and the map → odom correction. Latched when `latch` is on. |
|
||||
| `mapPath` | [`nav_msgs/msg/Path`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Path.html) | The optimized trajectory, for display. |
|
||||
| `mapOdomCache` | [`rtabmap_msgs/msg/MapGraph`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/MapGraph.html) | In localization mode, the recent odometry poses kept to localize against, with their links to the map. |
|
||||
| `localization_pose` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseWithCovarianceStamped.html) | The robot in the map frame after each update, with RTAB-Map's covariance — in mapping mode, the odometry's accumulated along the graph. See [Localization](#localization). |
|
||||
| `landmarks` | [`geometry_msgs/msg/PoseArray`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseArray.html) | The optimized landmark poses. |
|
||||
| `labels` | [`visualization_msgs/msg/MarkerArray`](https://docs.ros.org/en/jazzy/p/visualization_msgs/msg/MarkerArray.html) | Node ids, labels and landmark ids as text, for RViz. |
|
||||
| `local_grid_obstacle`, `local_grid_empty`, `local_grid_ground` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The local occupancy grid of the node just added, in `frame_id`. |
|
||||
| `map`, `grid_prob_map`, `cloud_map`, `cloud_obstacles`, `cloud_ground`, `octomap_*`, `elevation_map` | [`nav_msgs/msg/OccupancyGrid`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/OccupancyGrid.html), [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html), [`octomap_msgs/msg/Octomap`](https://docs.ros.org/en/jazzy/p/octomap_msgs/msg/Octomap.html), [`grid_map_msgs/msg/GridMap`](https://github.com/ANYbotics/grid_map/blob/master/grid_map_msgs/msg/GridMap.msg) | The assembled maps, from [`MapsManager`](../../rtabmap_util/README.md#mapsmanager), which lists them. |
|
||||
| `goal_out`, `goal_reached`, `global_path`, `local_path`, `global_path_nodes`, `local_path_nodes` | [`geometry_msgs/msg/PoseStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseStamped.html), [`std_msgs/msg/Bool`](https://docs.ros.org/en/jazzy/p/std_msgs/msg/Bool.html), [`nav_msgs/msg/Path`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Path.html), [`rtabmap_msgs/msg/Path`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/Path.html) | Planning. See [Planning](#planning). |
|
||||
|
||||
## Services
|
||||
|
||||
All under the node's name: `/rtabmap/reset`, not `/reset`. They run in the same callback group as SLAM, so a call waits for the current update to finish, and no update runs while a service does.
|
||||
|
||||
| Service | Type | Description |
|
||||
|---|---|---|
|
||||
| `reset` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | **Erase the map**, in memory and in the database. Node ids start over from 1. |
|
||||
| `trigger_new_map` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Start a new session in the same database; the old one is kept. |
|
||||
| `pause`, `resume` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Stop taking input, and start again. Input received while paused is dropped, not queued. Mirrored in the `is_rtabmap_paused` parameter, which can also start the node paused. |
|
||||
| `set_mode_localization`, `set_mode_mapping` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | See [Mapping and localization](../README.md#mapping-and-localization). |
|
||||
| `backup` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Save the database now, copy it to `<database_path>.back`, and carry on in a new session. |
|
||||
| `load_database` | [`rtabmap_msgs/srv/LoadDatabase`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/LoadDatabase.html) | Save the current map and switch to another database; `clear` empties the target first. The current parameters are kept — a warning lists those the target database was built with differently. |
|
||||
| `update_parameters` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Re-read every RTAB-Map ROS parameter and apply it. |
|
||||
| `get_map_data` | [`rtabmap_msgs/srv/GetMap`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/GetMap.html) | The graph and its nodes. `graph_only` leaves out their images, scans and user data, which are most of the size; `global_map` includes the nodes not in working memory; `optimized` returns optimized poses rather than odometry ones. |
|
||||
| `get_map_data2` | [`rtabmap_msgs/srv/GetMap2`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/GetMap2.html) | The same, choosing each kind of node data separately. |
|
||||
| `get_node_data` | [`rtabmap_msgs/srv/GetNodeData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/GetNodeData.html) | Given nodes, with the data asked for. No id means the latest node. |
|
||||
| `get_map`, `get_prob_map` | [`nav_msgs/srv/GetMap`](https://docs.ros.org/en/jazzy/p/nav_msgs/srv/GetMap.html) | The occupancy grid, as trinary or as probabilities. Empty if the map has no grid. |
|
||||
| `publish_map` | [`rtabmap_msgs/srv/PublishMap`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/PublishMap.html) | Republish the map on the topics that have subscribers, with the same `global_map`, `optimized` and `graph_only` options. |
|
||||
| `get_nodes_in_radius` | [`rtabmap_msgs/srv/GetNodesInRadius`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/GetNodesInRadius.html) | Nodes within a radius of a node (not counting it) or of a position, which is used when `node_id` is 0 and it is not the origin. |
|
||||
| `set_label`, `list_labels`, `remove_label` | [`rtabmap_msgs/srv/SetLabel`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/SetLabel.html), [`rtabmap_msgs/srv/ListLabels`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/ListLabels.html), [`rtabmap_msgs/srv/RemoveLabel`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/RemoveLabel.html) | Name nodes, so a goal can be `"kitchen"` rather than an id. Node 0 means the latest node. A label is unique in the map. |
|
||||
| `add_link` | [`rtabmap_msgs/srv/AddLink`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/AddLink.html) | Add a constraint found outside the node — a loop closure from another process, for instance. |
|
||||
| `detect_more_loop_closures` | [`rtabmap_msgs/srv/DetectMoreLoopClosures`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/DetectMoreLoopClosures.html) | Post-processing: look for loop closures between nodes close to each other in the optimized graph. |
|
||||
| `global_bundle_adjustment` | [`rtabmap_msgs/srv/GlobalBundleAdjustment`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/GlobalBundleAdjustment.html) | Post-processing: refine the graph with bundle adjustment on the visual features. |
|
||||
| `cleanup_local_grids` | [`rtabmap_msgs/srv/CleanupLocalGrids`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/CleanupLocalGrids.html) | Post-processing: remove from each node's local grid the obstacles the global map says are free — people who walked through, for instance. |
|
||||
| `set_goal`, `cancel_goal`, `get_plan`, `get_plan_nodes` | [`rtabmap_msgs/srv/SetGoal`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/SetGoal.html), [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html), [`nav_msgs/srv/GetPlan`](https://docs.ros.org/en/jazzy/p/nav_msgs/srv/GetPlan.html), [`rtabmap_msgs/srv/GetPlan`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/GetPlan.html) | Planning. See [Planning](#planning). |
|
||||
| `octomap_binary`, `octomap_full` | [`octomap_msgs/srv/GetOctomap`](https://docs.ros.org/en/jazzy/p/octomap_msgs/srv/GetOctomap.html) | The octomap. Only with RTAB-Map built with OctoMap and this package built with `octomap_msgs`. |
|
||||
| `log_debug`, `log_info`, `log_warning`, `log_error` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Set RTAB-Map's own log level, independently from ROS's. |
|
||||
|
||||
## Parameters
|
||||
|
||||
The node's own ROS parameters, with their real types. The ones about frames are in [Frames and TF](#frames-and-tf); the input ones in [Choosing the inputs](#choosing-the-inputs); map assembly (`map_*`, `cloud_*`, `octomap_*`, `latch`) with [`MapsManager`](../../rtabmap_util/README.md#mapsmanager).
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `database_path` | `string` | `"~/.ros/rtabmap.db"` | The map. `~` is expanded, and a relative path is taken from the working directory of the process. Under `$ROS_HOME` if that is set. |
|
||||
| `delete_db_on_start` | `bool` | `false` | Start from an empty map. `-d` or `--delete_db_on_start` as an argument does the same. |
|
||||
| `use_saved_map` | `bool` | `true` | Load the occupancy grid saved in the database at startup, instead of reassembling it from the nodes. |
|
||||
| `config_path` | `string` | `""` | INI file of RTAB-Map parameters, read at startup and written on shutdown. |
|
||||
| `is_rtabmap_paused` | `bool` | `false` | Start paused, waiting for the `resume` service. |
|
||||
| `initial_pose` | `string` | `""` | `"x y z roll pitch yaw"` to start from in localization mode. See [Localization](#localization). |
|
||||
| `pub_loc_pose_only_when_localizing` | `bool` | `false` | Publish `localization_pose` only on updates that found a loop closure, a proximity detection or a landmark. |
|
||||
| `loc_thr` | `double` | `0.0` | Localization error, in meters, above which diagnostics report an error. Localization mode only; `0` disables. |
|
||||
| `odom_tf_linear_variance` | `double` | `0.001` | Translational variance used when the odometry carries no usable covariance. See [Odometry, covariance and new maps](../README.md#odometry-covariance-and-new-maps). |
|
||||
| `odom_tf_angular_variance` | `double` | `0.001` | Rotational variance used when the odometry carries no usable covariance. |
|
||||
| `staleness_factor` | `double` | `0.0` | Start a new map after a gap longer than this many detection periods. `0` disables; values under `1` are refused and disable it too. See [Odometry, covariance and new maps](../README.md#odometry-covariance-and-new-maps). |
|
||||
| `landmark_linear_variance` | `double` | `0.001` | Translational variance of a landmark detection that carries no covariance. |
|
||||
| `landmark_angular_variance` | `double` | `0.001` | Rotational variance of a landmark detection that carries no covariance. |
|
||||
| `use_action_for_goal` | `bool` | `false` | Send goals to nav2's `navigate_to_pose` action instead of publishing them on `goal_out`. Requires the package built with `nav2_msgs`. |
|
||||
| `gen_scan` | `bool` | `false` | Derive a 2D scan from the depth image(s) when no scan is subscribed. See [Deriving missing data](#deriving-missing-data). |
|
||||
| `gen_scan_max_depth` | `double` | `4.0` | Farthest depth used for it, in meters. |
|
||||
| `gen_scan_min_depth` | `double` | `0.0` | Nearest. |
|
||||
| `gen_depth` | `bool` | `false` | Project `scan_cloud` into the camera to make a depth image, for an RGB camera with a lidar. |
|
||||
| `gen_depth_decimation` | `int` | `1` | Resolution divider for it; must divide the image size. |
|
||||
| `gen_depth_fill_holes_size` | `int` | `0` | Fill holes up to this many pixels. `0` disables. |
|
||||
| `gen_depth_fill_iterations` | `int` | `1` | Hole-filling passes. |
|
||||
| `gen_depth_fill_holes_error` | `double` | `0.1` | Maximum depth difference, in meters, across a hole for it to be filled. |
|
||||
| `stereo_to_depth` | `bool` | `false` | Compute a depth image from the stereo pair (with the `StereoBM/*` parameters) and map it as RGB-D. |
|
||||
| `scan_cloud_max_points` | `int` | `0` | Points in a full `scan_cloud` sweep, for an organized or fixed-size cloud; used by ICP as the reference for its correspondence ratio. `0` takes each cloud's own size. |
|
||||
| `scan_cloud_is_2d` | `bool` | `false` | `scan_cloud` is a 2D lidar published as a cloud. |
|
||||
| `odom_sensor_sync` | `bool` | `true` | Place each sensor where the robot was at that sensor's stamp, and deskew 2D scans ray by ray, using the odometry in TF. See [Sensors not stamped together](#sensors-not-stamped-together). |
|
||||
| `subscribe_inter_odom_info` | `bool` | `false` | Synchronize `inter_odom` with `inter_odom_info`. See [Intermediate odometry](#intermediate-odometry). |
|
||||
| `log_to_rosout_level` | `int` | `4` | RTAB-Map's own log messages at or above this level (`0` debug to `4` fatal) are forwarded to `/rosout`. |
|
||||
| `qos_gps`, `qos_imu`, `qos_env_sensor` | `int` | `0` | Reliability of those subscriptions: `0` system default, `1` reliable, `2` best effort. |
|
||||
|
||||
And every RTAB-Map parameter, as strings, as described below.
|
||||
|
||||
### RTAB-Map's own parameters
|
||||
|
||||
Everything in RTAB-Map's parameter set is exposed as a ROS parameter **under its RTAB-Map name**, except the odometry ones (`Odom/*`, `OdomF2M/*`...), which belong to the [odometry nodes](../../rtabmap_odom/README.md):
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_slam rtabmap --ros-args \
|
||||
-p "Rtabmap/DetectionRate:='2'" \
|
||||
-p "RGBD/LinearUpdate:='0.2'" \
|
||||
-p "Mem/IncrementalMemory:='false'"
|
||||
```
|
||||
|
||||
**Every RTAB-Map parameter is declared as a string**, whatever it looks like, because that is how RTAB-Map's own parameter map stores them. `-p RGBD/LinearUpdate:=0.2` makes ROS infer a double, and the node throws on startup. The inner quotes are what keep it a string; in a launch file, `{'RGBD/LinearUpdate': '0.2'}`. The node's own ROS parameters — `frame_id`, `publish_tf`, `subscribe_scan` — have their real types and take plain values.
|
||||
|
||||
`rtabmap --params` prints them all with their defaults and descriptions, and so does [RTAB-Map's parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html). Two defaults differ from RTAB-Map's own: **`RGBD/CreateOccupancyGrid` is `true`**, since a robot's map is usually meant for navigation, and **`Rtabmap/WorkingDirectory` is `$ROS_HOME`**, or `~/.ros`.
|
||||
|
||||
A value can come from several places. From the highest priority to the lowest:
|
||||
|
||||
1. **Arguments**, `--Param/Name value` after the executable name, or in a launch file's `arguments=[...]`.
|
||||
2. **ROS parameters.**
|
||||
3. **`config_path`**, an INI file of RTAB-Map parameters. The node writes its parameters back to it on shutdown.
|
||||
4. **The node's own adjustments to its inputs** — ICP registration for a lidar with no camera, for example. See [Automatic adjustments](#automatic-adjustments).
|
||||
5. **The database**, which remembers the parameters it was built with. See [The database](../README.md#the-database).
|
||||
6. The defaults.
|
||||
|
||||
A parameter changed while the node runs, with `ros2 param set`, is applied straight away. `update_parameters` re-reads them all, for a change the node might have missed.
|
||||
|
||||
Parameters RTAB-Map has renamed are still accepted under their old name, with a warning naming the new one — worth heeding, since the old names are not declared and so do not show up in `ros2 param list`.
|
||||
|
||||
The ones that set how often a node is added, explained in [Update rate and dropped updates](../README.md#update-rate-and-dropped-updates):
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `Rtabmap/DetectionRate` | `string` | `"1"` | Updates per second, in Hz. `0` processes every one; see the [warning](../README.md#update-rate-and-dropped-updates). |
|
||||
| `Rtabmap/CreateIntermediateNodes` | `string` | `"false"` | Keep the updates that `Rtabmap/DetectionRate` would skip, as intermediate nodes instead. |
|
||||
| `RGBD/LinearUpdate` | `string` | `"0.1"` | Minimum distance, in meters, the robot must have moved for an update to add a node. |
|
||||
| `RGBD/AngularUpdate` | `string` | `"0.1"` | Minimum rotation, in radians, the robot must have made for an update to add a node. |
|
||||
|
||||
## Frames and TF
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `frame_id` | `string` | `"base_link"` | The robot frame. Every sensor is placed relative to it through TF. |
|
||||
| `odom_frame_id` | `string` | `""` | Read odometry from TF, as `odom_frame_id` → `frame_id` at each sensor stamp, instead of from the `odom` topic. Setting it forces `subscribe_odom` off. See [Odometry from TF](#odometry-from-tf). |
|
||||
| `odom_frame_id_init` | `string` | `""` | The odometry frame to publish `map` → it from the start, before any odometry has been received. Ignored when `odom_frame_id` is set. |
|
||||
| `map_frame_id` | `string` | `"map"` | The map frame, on TF and in the header of everything published in it. |
|
||||
| `publish_tf` | `bool` | `true` | Publish `map_frame_id` → odometry frame. |
|
||||
| `tf_delay` | `double` | `0.05` | Period of that publication, in seconds (20 Hz). `0` disables it. |
|
||||
| `tf_tolerance` | `double` | `0.1` | How far in the future the transform is stamped, in seconds, so that lookups at the latest sensor stamp do not have to wait for it. |
|
||||
| `wait_for_transform` | `double` | `0.2` | Seconds to wait for a TF lookup before giving up on it. |
|
||||
| `ground_truth_frame_id` | `string` | `""` | The fixed frame of a ground truth system, for example `world` published by an external localization system like Vicon or OptiTrack. `ground_truth_frame_id` → `ground_truth_base_frame_id` is looked up and stored with each node, for evaluating a trajectory afterwards. |
|
||||
| `ground_truth_base_frame_id` | `string` | value of `frame_id` | The robot frame in the ground truth tree, for example `base_link_gt`. To avoid breaking the TF tree, it represents the same frame as `frame_id`, but in a parallel TF tree, so that the robot frame does not get two parents. |
|
||||
|
||||
**This node publishes exactly one transform: `map` → `odom`.** It is the correction that puts the odometry frame where the optimized graph says it belongs — the identity until a loop closure moves it. Odometry keeps publishing `odom` → `base_link`, and the sensors must be attached to `base_link` in TF, as in the [TF tree](../README.md#frames-and-tf).
|
||||
|
||||
The odometry frame is taken from the odometry messages themselves, so the transform only starts once the first update has been processed — unless `odom_frame_id` or `odom_frame_id_init` says what it will be. It is published from a thread of its own at a fixed rate, independently from how fast SLAM runs.
|
||||
|
||||
**With `Optimizer/Iterations` set to `0`, the `map` → `odom` transform is not published at all**, even with `publish_tf` on: with graph optimization disabled there is no correction to publish. That is the arrangement where another node optimizes the graph and publishes the transform instead.
|
||||
|
||||
## Asynchronous inputs
|
||||
|
||||
These are not synchronized with the sensors. Each is buffered as it arrives and attached to the next update, then cleared, so each value is stored with one node only. They are received on callback groups of their own, and keep being buffered while an update is processed.
|
||||
|
||||
### Landmarks
|
||||
|
||||
A landmark is anything recognized with an identity and a pose relative to the robot — typically a fiducial marker. It becomes a node of the graph under the **negative** of its id, linked to each node that saw it, so seeing the same marker again is a loop closure however far the odometry has drifted.
|
||||
|
||||
- **Ids must be positive.** A detection with id 0 or less is refused.
|
||||
- The detection's frame must be in TF, connected to `frame_id`. Its pose is also corrected for the motion between its stamp and the node's, with the odometry in TF.
|
||||
- Without a covariance in the message, `landmark_linear_variance` and `landmark_angular_variance` are used. Their default of `0.001` is a standard deviation of about 3 cm, fitting a marker seen close; raise them for markers seen far away.
|
||||
- Between two updates, only the latest detection of each id is kept.
|
||||
|
||||
`apriltag/detections` expects the apriltag_ros convention, where each detection is also published on TF as `family:id` from the camera frame; the pose is taken from there.
|
||||
|
||||
The optimized landmarks are published on `landmarks`, and their ids on `labels`.
|
||||
|
||||
**Landmarks can place the map in the world.** `Marker/Priors` gives some of them known world poses, `"id x y z roll pitch yaw"` with angles in radians, several separated by `|`: `"1 0 0 1 0 0 0|2 1 0 1 0 0 1.57"` puts marker 2 one meter in front of marker 1, turned 90 degrees. As soon as one of them is seen, the map is transformed into that world frame: the robot's poses, and `map`, are then world coordinates. The priors are weighted by `Marker/PriorsVarianceLinear` and `Marker/PriorsVarianceAngular` (`0.001` by default). **They only apply with `Optimizer/PriorsIgnored` set to `false`**; at its default, `true`, they are ignored without a warning, like the GPS and global pose priors.
|
||||
|
||||
### GPS and global pose
|
||||
|
||||
**`gps/fix`** stores a GPS fix with the node closest in time to it, provided it is within one detection period of it (any, with `Rtabmap/DetectionRate` at `0`). Its error is the square root of the largest position variance, or 10 m when the covariance type is unknown. The fixes are stored for export and for georeferencing the map; `Rtabmap/LoopGPS` also uses them to discard loop closure candidates that are too far apart.
|
||||
|
||||
**`global_pose`** is an absolute pose from outside — a motion capture system, a localization against another map. It is added to the node as a **pose prior**: a link from the node to itself, weighted by the message's covariance. The same time window applies. The message's frame is taken as the sensor frame, and it is transformed to `frame_id` with TF.
|
||||
|
||||
**Priors are stored, but ignored by the optimizer by default.** GPS and global poses only pull the graph once `Optimizer/PriorsIgnored` is `false`, with an optimizer that supports them (g2o, GTSAM).
|
||||
|
||||
### IMU
|
||||
|
||||
The orientation from `imu`, interpolated at the node's stamp, is transformed to `frame_id` and turned into a **gravity constraint**: a link from the node to itself that holds its roll and pitch, used by the optimizer when `Optimizer/GravitySigma` is above `0` and the optimizer supports it (g2o, GTSAM). It keeps a long 3D map level where odometry alone would let it bend.
|
||||
|
||||
- **Only the orientation is used**; the angular velocity and linear acceleration are ignored. Most IMU drivers publish raw rates and accelerations only: estimate the orientation first with a filter such as [`imu_filter_madgwick` or `imu_complementary_filter`](https://github.com/CCNYRoboticsLab/imu_tools), and feed its output here.
|
||||
- An IMU message with no orientation (all zeros) is ignored.
|
||||
- The node's stamp must match an IMU message or lie between two, or the IMU is not used for that node.
|
||||
- The IMU frame must not change: a message from another frame clears the buffer, since it means two sources are publishing on the same topic.
|
||||
|
||||
### User data and environment sensors
|
||||
|
||||
**`user_data_async`** is arbitrary data — a matrix, or bytes — stored with the next node only. It cannot be combined with the synchronized `user_data`: when both are present, the asynchronous one is dropped with a warning. The same goes for `sensor_data`, whose message has a user data field of its own: the async user data is attached when that field is empty, and dropped with a warning when it is set, never carried over to a later node.
|
||||
|
||||
**`env_sensor`** readings are stored with the next node, the latest value of each type. The types mirror the [environment sensors Android devices have](https://developer.android.com/develop/sensors-and-location/sensors/sensors_environment), plus WiFi and custom values:
|
||||
|
||||
| `type` | Reading | Unit |
|
||||
|---|---|---|
|
||||
| `TYPE_WIFI_SIGNAL_STRENGTH` | WiFi signal strength | dBm |
|
||||
| `TYPE_AMBIENT_TEMPERATURE` | Ambient temperature | °C |
|
||||
| `TYPE_AMBIENT_AIR_PRESSURE` | Air pressure | hPa |
|
||||
| `TYPE_AMBIENT_LIGHT` | Illuminance | lx |
|
||||
| `TYPE_AMBIENT_RELATIVE_HUMIDITY` | Relative humidity | % |
|
||||
| `TYPE_CUSTOM1` to `TYPE_CUSTOM9` | Anything else | yours |
|
||||
|
||||
### Intermediate odometry
|
||||
|
||||
**Intermediate nodes** record the trajectory between two nodes, with no loop closure detection on them. `Rtabmap/CreateIntermediateNodes` makes them two ways:
|
||||
|
||||
- **With `Rtabmap/DetectionRate` above `0`**, the updates that arrive too soon after the last processed one, and would be skipped, become intermediate nodes instead (see [Update rate and dropped updates](../README.md#update-rate-and-dropped-updates)). They can only come as fast as the synchronized sensor updates, since they are those updates, and keep their sensor data only with `Mem/IntermediateNodeDataKept`, which helps for building a map from every scan, at the price of a larger database.
|
||||
- **With `Rtabmap/DetectionRate` at `0`**, every update is already a full node -- which is only tractable with slow sensor updates, see the [warning](../README.md#update-rate-and-dropped-updates) -- and `inter_odom` adds poses between them: a faster odometry, whose messages between two updates become intermediate nodes without sensor data. That is for when the sensor updates are slow (2 Hz or less) while the odometry is fast (10 Hz or more): the trajectory is then as dense as the odometry rather than as the sensors.
|
||||
|
||||
`inter_odom` is only subscribed when the node starts with `Rtabmap/CreateIntermediateNodes` on and `Rtabmap/DetectionRate` at `0` (every update processed); changing either later has no effect on it. Intermediate poses are only added once the map has a node.
|
||||
|
||||
With `subscribe_inter_odom_info`, `inter_odom` is synchronized by exact stamp with `inter_odom_info`, the `OdomInfo` of an `rtabmap_odom` node: each intermediate node then also stores that odometry's statistics, and its velocity is taken from the measured motion. A message on one topic without its match on the other is not used.
|
||||
|
||||
## Deriving missing data
|
||||
|
||||
**`gen_scan`** makes a 2D scan out of the depth image: its middle row, between `gen_scan_min_depth` and `gen_scan_max_depth`, as a lidar at the camera's height would see it. With a depth camera and no lidar, this lets the occupancy grid be built the way it would be from a lidar — it also triggers the scan [adjustments](#automatic-adjustments) — and lets proximity detection register scans. **The cameras must be level**, looking parallel to the ground, as for [depthimage_to_laserscan](https://github.com/ros-perception/depthimage_to_laserscan): the middle row of a tilted camera sees the floor or the ceiling, not the walls around the robot.
|
||||
|
||||
**`gen_depth`** goes the other way: with an RGB camera (`subscribe_rgb`) and a lidar (`subscribe_scan_cloud`), the cloud is projected into the camera to give a sparse depth image, filled by `gen_depth_fill_*`, so visual loop closures get 3D features.
|
||||
|
||||
**`stereo_to_depth`** computes a dense depth image from a stereo pair, so a stereo camera is mapped like an RGB-D one — denser grids and clouds, at the cost of the disparity computation.
|
||||
|
||||
## Localization
|
||||
|
||||
`localization_pose` is the robot's pose in the map frame — `map` → `odom` composed with the odometry — after each update, with RTAB-Map's covariance. While mapping, that is the odometry covariance accumulated along the graph, growing with distance until a loop closure brings it down. In localization mode before the first loop closure, it is `9999`: the robot is not localized yet.
|
||||
|
||||
In localization mode (`Mem/IncrementalMemory` at `false`, see the [README](../README.md#mapping-and-localization)), the robot is placed on the map by the first loop closure. Until then:
|
||||
|
||||
- **The `initial_pose` parameter**, `"x y z roll pitch yaw"`, read at startup, says where the robot starts, and the odometry is added to it.
|
||||
- **The `initialpose` topic** ([`geometry_msgs/msg/PoseWithCovarianceStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseWithCovarianceStamped.html)), as RViz's *2D Pose Estimate* publishes it, does the same at any time. A pose in another frame is transformed to the map frame with TF; one without a frame is taken as being in the map frame.
|
||||
- **Without either**, the robot is assumed to restart at the last localization pose saved in the database, where it was when the node last shut down. With **`RGBD/StartAtOrigin`** set to `true`, it is assumed to start at the map's origin instead.
|
||||
|
||||
All three are ignored in mapping mode, `initial_pose` and `initialpose` with a warning.
|
||||
|
||||
`pub_loc_pose_only_when_localizing` restricts `localization_pose` to the updates that actually localized — found a loop closure, a proximity detection or a landmark — for a consumer that should only hear about corrections.
|
||||
|
||||
## Planning
|
||||
|
||||
The node plans on its own graph: a goal is a node, the plan is the chain of nodes leading to it, and a local planner is handed the next one to reach. That gives global planning across a map the local planner cannot see all of — through areas the robot has mapped, and only those.
|
||||
|
||||
**It does not replace nav2's planner: it is a layer over it, there for memory management.** With working memory bounded (`Rtabmap/TimeThr` or `Rtabmap/MemoryThr`), the nodes moved to long-term memory stop contributing to the occupancy grid, so parts of the map published on `map` disappear over time, and nav2 alone cannot plan to them. RTAB-Map's graph still holds them: it can plan to a node in long-term memory, and as the robot moves toward it, it brings back the areas ahead of the robot, so the robot stays localized and the map around it is there for nav2 again. The plan and the retrieval are described in [Long-Term Online Multi-Session Graph-Based SPLAM with Memory Management](https://arxiv.org/abs/2301.00050) (Labbé and Michaud, *Autonomous Robots*).
|
||||
|
||||
```mermaid
|
||||
flowchart LR
|
||||
GOAL(["goal, goal_node<br>or set_goal"])
|
||||
RTAB["rtabmap<br><i>global plan on the graph</i>"]
|
||||
NAV2["nav2<br><i>planner and controller</i>"]
|
||||
GOAL --> RTAB
|
||||
RTAB -->|"map (occupancy grid)"| NAV2
|
||||
RTAB -->|"next node: navigate_to_pose action<br>or goal_out topic"| NAV2
|
||||
NAV2 -->|action result| RTAB
|
||||
```
|
||||
|
||||
**Setting a goal:**
|
||||
|
||||
| How | Goal |
|
||||
|---|---|
|
||||
| `set_goal` service | A node id, or a label. Returns the planned path and the planning time. |
|
||||
| `goal_node` topic | A node id, or a label. A message with neither is refused. |
|
||||
| `goal` topic | A pose, in the map frame or any frame TF can transform to it. A pose in a frame it cannot is refused. |
|
||||
|
||||
**A pose goal within `RGBD/LocalRadius` (10 m by default) of the robot is not planned through the graph**: the plan is the node the robot is at, followed by the pose itself, and it is up to the local planner to get there. Further away, the plan goes through the graph to the node nearest the pose, and the pose is appended after it.
|
||||
|
||||
**Following it:**
|
||||
|
||||
- `goal_out` is the next node to reach, as a pose in the map frame, sent again whenever it changes. Point a local planner at it — or set `use_action_for_goal` to send it to nav2's `navigate_to_pose` action instead. nav2 listens for goals on `goal_pose`, so to use the topic with nav2, remap `goal_out` to `goal_pose`.
|
||||
- `global_path` and `global_path_nodes` are the whole plan, as poses and as node ids; a pose goal appears at the end with node id `0`. `local_path` and `local_path_nodes` are the part of it within the local radius.
|
||||
- `goal_reached` says `true` once the robot is within `RGBD/GoalReachedRadius` (0.5 m by default) of the goal — straight away if it already is — and `false` when planning fails, the goal cannot be found or transformed, the plan is cancelled, or the robot strays too far from the path.
|
||||
|
||||
`cancel_goal` abandons the plan (and cancels the nav2 goal, if any).
|
||||
|
||||
**`get_plan`** (`nav_msgs/srv/GetPlan`) and **`get_plan_nodes`** compute a plan and return it, without following it or publishing anything. `get_plan` answers in the goal's frame; `get_plan_nodes` also takes a node id and returns the node ids along the plan.
|
||||
|
||||
Labels, set with `set_label`, are what make goals readable: `set_goal` with `node_label: "kitchen"` rather than an id that changes from one map to the next.
|
||||
|
||||
## Diagnostics
|
||||
|
||||
`/diagnostics` carries the input and output rates of the synchronizer, as for every [`rtabmap_sync`](../../rtabmap_sync/README.md#library) consumer: a healthy input rate with a low output rate means updates arrive but are dropped — by the rate, or because SLAM takes longer than the period.
|
||||
|
||||
In localization mode with `loc_thr` set, a *Localization status* entry says whether the robot is localized: an error with `Not localized!` until a loop closure has placed it, an error with `Localization error is high!` while the localization error — the square root of the largest translational variance — is over `loc_thr` meters, and OK under it. It is only set up when the node **starts** in localization mode.
|
||||
@@ -121,9 +121,35 @@ class StereoDense;
|
||||
|
||||
namespace rtabmap_slam {
|
||||
|
||||
/**
|
||||
* @brief The `rtabmap` node: graph SLAM around an rtabmap::Rtabmap instance.
|
||||
*
|
||||
* Registered as the `rtabmap_slam::CoreWrapper` component, and run by the `rtabmap`
|
||||
* executable. The node is always named `rtabmap` unless remapped, and advertises its
|
||||
* services under that name (`/rtabmap/reset`...).
|
||||
*
|
||||
* The input topics come from rtabmap_sync::CommonDataSubscriber, chosen by the
|
||||
* `subscribe_*` parameters; each synchronized update is converted to an
|
||||
* rtabmap::SensorData and processed on a callback group of its own, while asynchronous
|
||||
* inputs (GPS, IMU, landmarks, user data...) are buffered on theirs and attached to the
|
||||
* next update. An update arriving while the previous one is still processed is dropped.
|
||||
*
|
||||
* The graph is published on `mapGraph`, `mapData` and `mapPath`, the assembled maps
|
||||
* through an rtabmap_util::MapsManager, and the correction `map` -> odometry frame on TF.
|
||||
* The database is saved when the node is destroyed.
|
||||
*
|
||||
* See the package README and doc/rtabmap.md for the topics, parameters and services.
|
||||
*/
|
||||
class CoreWrapper : public rclcpp::Node, public rtabmap_sync::CommonDataSubscriber
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Declares the parameters, opens the database and sets up every topic and
|
||||
* service.
|
||||
*
|
||||
* RTAB-Map parameters are declared as strings under their RTAB-Map names, except the
|
||||
* odometry ones. Throws if one is given with another type.
|
||||
*/
|
||||
RTABMAP_SLAM_PUBLIC
|
||||
explicit CoreWrapper(const rclcpp::NodeOptions & options);
|
||||
virtual ~CoreWrapper();
|
||||
|
||||
@@ -34,9 +34,13 @@
|
||||
<depend>rtabmap_msgs</depend>
|
||||
<depend>rtabmap_util</depend>
|
||||
<depend>rtabmap_sync</depend>
|
||||
|
||||
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
<test_depend>rtabmap_conversions</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_slam
|
||||
## Build the docs locally with:
|
||||
## rosdoc2 build --package-path rtabmap_slam --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_slam 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_slam',
|
||||
doxygen_xml_directory: 'generated/doxygen/xml',
|
||||
output_dir: ''
|
||||
}
|
||||
@@ -139,7 +139,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
|
||||
tfThreadRunning_(false),
|
||||
interOdomSync_(0),
|
||||
stereoToDepth_(false),
|
||||
odomSensorSync_(false),
|
||||
odomSensorSync_(true),
|
||||
rate_(Parameters::defaultRtabmapDetectionRate()),
|
||||
createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()),
|
||||
mappingMaxNodes_(Parameters::defaultGridGlobalMaxNodes()),
|
||||
@@ -232,6 +232,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
|
||||
|
||||
stereoToDepth_ = this->declare_parameter("stereo_to_depth", stereoToDepth_);
|
||||
odomSensorSync_ = this->declare_parameter("odom_sensor_sync", odomSensorSync_);
|
||||
bool interOdomInfo = false;
|
||||
interOdomInfo = this->declare_parameter("subscribe_inter_odom_info", interOdomInfo);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "rtabmap: frame_id = \"%s\"", frameId_.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), "rtabmap: odom_frame_id = \"%s\"", odomFrameId_.c_str());
|
||||
@@ -289,7 +291,14 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
|
||||
|
||||
infoPub_ = this->create_publisher<rtabmap_msgs::msg::Info>("info", 1);
|
||||
mapDataPub_ = this->create_publisher<rtabmap_msgs::msg::MapData>("mapData", 1);
|
||||
mapGraphPub_ = this->create_publisher<rtabmap_msgs::msg::MapGraph>("mapGraph", rclcpp::QoS(1).reliable().durability(mapsManager_.isLatching()?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
|
||||
// Intra-process communication doesn't support transient local durability: when latching,
|
||||
// disable it on this publisher, otherwise keep the node's setting.
|
||||
rclcpp::PublisherOptions latchedPubOptions;
|
||||
if(mapsManager_.isLatching())
|
||||
{
|
||||
latchedPubOptions.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
|
||||
}
|
||||
mapGraphPub_ = this->create_publisher<rtabmap_msgs::msg::MapGraph>("mapGraph", rclcpp::QoS(1).reliable().durability(mapsManager_.isLatching()?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), latchedPubOptions);
|
||||
odomCachePub_ = this->create_publisher<rtabmap_msgs::msg::MapGraph>("mapOdomCache", 1);
|
||||
landmarksPub_ = this->create_publisher<geometry_msgs::msg::PoseArray>("landmarks", 1);
|
||||
labelsPub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("labels", 1);
|
||||
@@ -418,11 +427,12 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
|
||||
iter!=Parameters::getRemovedParameters().end();
|
||||
++iter)
|
||||
{
|
||||
// Old names are never declared, so they can only be found among the overrides.
|
||||
std::string paramValue;
|
||||
rclcpp::Parameter parameter;
|
||||
if(get_parameter(iter->first, parameter))
|
||||
std::map<std::string, rclcpp::ParameterValue>::const_iterator oter = overrides.find(iter->first);
|
||||
if(oter != overrides.end() && oter->second.get_type() == rclcpp::ParameterType::PARAMETER_STRING)
|
||||
{
|
||||
paramValue = parameter.as_string();
|
||||
paramValue = oter->second.get<std::string>();
|
||||
}
|
||||
if(!paramValue.empty())
|
||||
{
|
||||
@@ -563,8 +573,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
|
||||
RCLCPP_INFO(this->get_logger(), "Create intermediate nodes");
|
||||
if(rate_ == 0.0f)
|
||||
{
|
||||
bool interOdomInfo = false;
|
||||
if(get_parameter("subscribe_inter_odom_info", interOdomInfo))
|
||||
if(interOdomInfo)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Subscribe to inter odom + info messages");
|
||||
interOdomSync_ = new message_filters::Synchronizer<MyExactInterOdomSyncPolicy>(MyExactInterOdomSyncPolicy(100), interOdomSyncSub_, interOdomInfoSyncSub_);
|
||||
@@ -808,13 +817,14 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
|
||||
|
||||
if(modifiedParameters.find(Parameters::kRGBDProximityPathMaxNeighbors()) == modifiedParameters.end())
|
||||
{
|
||||
if(this->isSubscribedToScan2d())
|
||||
if(this->isSubscribedToScan2d() || (this->isSubscribedToScan3d() && scanCloudIs2d_))
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Setting \"%s\" parameter to 10 (default 0) as \"subscribe_scan\" is "
|
||||
RCLCPP_WARN(this->get_logger(), "Setting \"%s\" parameter to 10 (default 0) as \"%s\" is "
|
||||
"true and \"%s\" uses ICP. Proximity detection by space will be also done by merging close "
|
||||
"scans. To disable, set \"%s\" to 0. To suppress this warning, "
|
||||
"add <param name=\"%s\" type=\"string\" value=\"10\"/>",
|
||||
Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
|
||||
this->isSubscribedToScan2d()?"subscribe_scan":"scan_cloud_is_2d",
|
||||
Parameters::kRegStrategy().c_str(),
|
||||
Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
|
||||
Parameters::kRGBDProximityPathMaxNeighbors().c_str());
|
||||
@@ -1392,7 +1402,8 @@ void CoreWrapper::commonMultiCameraCallback(
|
||||
}
|
||||
}
|
||||
|
||||
if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0)
|
||||
UScopeMutex syncDataLock(syncDataMutex_, false);
|
||||
if(syncTimer_->is_canceled() && syncDataLock.lockTry() == 0)
|
||||
{
|
||||
UScopeMutex lock(lastPoseMutex_);
|
||||
commonMultiCameraCallbackImpl(odomFrameId,
|
||||
@@ -1412,7 +1423,6 @@ void CoreWrapper::commonMultiCameraCallback(
|
||||
if(syncData_.valid) {
|
||||
syncTimer_->reset();
|
||||
}
|
||||
syncDataMutex_.unlock();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1783,7 +1793,8 @@ void CoreWrapper::commonLaserScanCallback(
|
||||
}
|
||||
}
|
||||
|
||||
if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0)
|
||||
UScopeMutex syncDataLock(syncDataMutex_, false);
|
||||
if(syncTimer_->is_canceled() && syncDataLock.lockTry() == 0)
|
||||
{
|
||||
UScopeMutex lock(lastPoseMutex_);
|
||||
LaserScan scan;
|
||||
@@ -1801,7 +1812,6 @@ void CoreWrapper::commonLaserScanCallback(
|
||||
rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmap update...");
|
||||
syncDataMutex_.unlock();
|
||||
return;
|
||||
}
|
||||
}
|
||||
@@ -1820,7 +1830,6 @@ void CoreWrapper::commonLaserScanCallback(
|
||||
scanCloudIs2d_))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmap update...");
|
||||
syncDataMutex_.unlock();
|
||||
return;
|
||||
}
|
||||
}
|
||||
@@ -1880,7 +1889,6 @@ void CoreWrapper::commonLaserScanCallback(
|
||||
lastPoseCovariance_ = cv::Mat();
|
||||
|
||||
syncTimer_->reset();
|
||||
syncDataMutex_.unlock();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1897,7 +1905,8 @@ void CoreWrapper::commonOdomCallback(
|
||||
return;
|
||||
}
|
||||
|
||||
if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0)
|
||||
UScopeMutex syncDataLock(syncDataMutex_, false);
|
||||
if(syncTimer_->is_canceled() && syncDataLock.lockTry() == 0)
|
||||
{
|
||||
UScopeMutex lock(lastPoseMutex_);
|
||||
cv::Mat userData;
|
||||
@@ -1949,7 +1958,6 @@ void CoreWrapper::commonOdomCallback(
|
||||
lastPoseCovariance_ = cv::Mat();
|
||||
|
||||
syncTimer_->reset();
|
||||
syncDataMutex_.unlock();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1985,12 +1993,29 @@ void CoreWrapper::commonSensorDataCallback(
|
||||
}
|
||||
}
|
||||
|
||||
if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0)
|
||||
UScopeMutex syncDataLock(syncDataMutex_, false);
|
||||
if(syncTimer_->is_canceled() && syncDataLock.lockTry() == 0)
|
||||
{
|
||||
UScopeMutex lock(lastPoseMutex_);
|
||||
syncData_.data = rtabmap_conversions::sensorDataFromROS(*sensorDataMsg);
|
||||
syncData_.data.setId(lastPoseIntermediate_?-1:0);
|
||||
|
||||
{
|
||||
UScopeMutex lock(userDataMutex_);
|
||||
if(!userData_.empty())
|
||||
{
|
||||
if(!syncData_.data.userDataRaw().empty() || !syncData_.data.userDataCompressed().empty())
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Sensor data received already contains user data. Async user data dropped!");
|
||||
}
|
||||
else
|
||||
{
|
||||
syncData_.data.setUserData(userData_);
|
||||
}
|
||||
userData_ = cv::Mat();
|
||||
}
|
||||
}
|
||||
|
||||
OdometryInfo odomInfo;
|
||||
if(odomInfoMsg.get())
|
||||
{
|
||||
@@ -2014,7 +2039,6 @@ void CoreWrapper::commonSensorDataCallback(
|
||||
lastPoseCovariance_ = cv::Mat();
|
||||
|
||||
syncTimer_->reset();
|
||||
syncDataMutex_.unlock();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -2059,7 +2083,7 @@ void CoreWrapper::process(
|
||||
// Add intermediate nodes?
|
||||
for(std::list<std::pair<nav_msgs::msg::Odometry, rtabmap_msgs::msg::OdomInfo> >::iterator iter=interOdoms_.begin(); iter!=interOdoms_.end();)
|
||||
{
|
||||
if(rclcpp::Time(iter->first.header.stamp.sec, iter->first.header.stamp.nanosec) < stamp)
|
||||
if(rclcpp::Time(iter->first.header.stamp) < stamp)
|
||||
{
|
||||
Transform interOdom;
|
||||
if(!rtabmap_.getLocalOptimizedPoses().empty())
|
||||
@@ -2208,8 +2232,8 @@ void CoreWrapper::process(
|
||||
Transform correction = rtabmap_conversions::getMovingTransform(
|
||||
frameId_,
|
||||
odomFrameId,
|
||||
stamp,
|
||||
rclcpp::Time(globalPoseMsg.header.stamp.sec, globalPoseMsg.header.stamp.nanosec),
|
||||
stamp,
|
||||
*tfBuffer_,
|
||||
waitForTransform_);
|
||||
if(!correction.isNull())
|
||||
|
||||
@@ -0,0 +1,431 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#ifndef RTABMAP_SLAM_CORE_WRAPPER_FIXTURE_HPP_
|
||||
#define RTABMAP_SLAM_CORE_WRAPPER_FIXTURE_HPP_
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <tf2_msgs/msg/tf_message.hpp>
|
||||
#include <tf2_ros/static_transform_broadcaster.hpp>
|
||||
#include <std_srvs/srv/empty.hpp>
|
||||
|
||||
#include <rtabmap_msgs/msg/info.hpp>
|
||||
#include <rtabmap_msgs/msg/map_graph.hpp>
|
||||
#include <rtabmap_msgs/srv/get_node_data.hpp>
|
||||
#include <rtabmap_msgs/srv/get_map.hpp>
|
||||
|
||||
#include <rtabmap/utilite/UDirectory.h>
|
||||
#include <rtabmap/utilite/UFile.h>
|
||||
|
||||
#include <rtabmap_slam/CoreWrapper.h>
|
||||
|
||||
#include "msg_builders.hpp"
|
||||
#include "node_test_utils.hpp"
|
||||
|
||||
#include <unistd.h>
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <cstdlib>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
namespace rtabmap_slam_test {
|
||||
|
||||
/**
|
||||
* @brief Drives one `rtabmap` node over real ROS topics, against its own database.
|
||||
*
|
||||
* Every test gets a fresh directory for the database and the working directory, so no
|
||||
* test reads another's map and nothing lands in ~/.ros. The node is destroyed before the
|
||||
* directory is removed: its destructor is what saves the database, and a test that wants
|
||||
* to reopen a map does it by destroying the node itself and building another one.
|
||||
*
|
||||
* By default the node subscribes to odometry only (`subscribe_depth` and `subscribe_rgb`
|
||||
* off), the cheapest input that still builds a graph: each odometry message is a node.
|
||||
* `Rtabmap/DetectionRate` is 0 so every message is processed rather than one per second.
|
||||
*/
|
||||
class CoreWrapperTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
NodeTest::SetUp();
|
||||
static std::atomic<int> counter(0);
|
||||
const char * tmp = std::getenv("TMPDIR");
|
||||
dir_ = std::string(tmp && *tmp ? tmp : "/tmp") + "/rtabmap_slam_test_" +
|
||||
std::to_string(::getpid()) + "_" + std::to_string(counter++);
|
||||
UDirectory::makeDir(dir_);
|
||||
}
|
||||
|
||||
void TearDown() override
|
||||
{
|
||||
// Removes the node from the executor first: the destructor then runs with nothing
|
||||
// left to call back into it.
|
||||
stopNodeThreads();
|
||||
NodeTest::TearDown();
|
||||
node_.reset();
|
||||
staticTf_.reset();
|
||||
tfPub_.reset();
|
||||
removeDir(dir_);
|
||||
}
|
||||
|
||||
/// Where this test's database lives.
|
||||
std::string databasePath() const { return dir_ + "/rtabmap.db"; }
|
||||
const std::string & dir() const { return dir_; }
|
||||
|
||||
/// The parameters every test starts from; @p params are applied on top.
|
||||
std::vector<rclcpp::Parameter> defaultParameters(
|
||||
const std::vector<rclcpp::Parameter> & params = {}) const
|
||||
{
|
||||
std::vector<rclcpp::Parameter> all = {
|
||||
rclcpp::Parameter("database_path", databasePath()),
|
||||
rclcpp::Parameter("Rtabmap/WorkingDirectory", dir_),
|
||||
rclcpp::Parameter("subscribe_depth", false),
|
||||
rclcpp::Parameter("subscribe_rgb", false),
|
||||
rclcpp::Parameter("Rtabmap/DetectionRate", "0"),
|
||||
};
|
||||
for(const rclcpp::Parameter & p : params)
|
||||
{
|
||||
bool replaced = false;
|
||||
for(rclcpp::Parameter & q : all)
|
||||
{
|
||||
if(q.get_name() == p.get_name())
|
||||
{
|
||||
q = p;
|
||||
replaced = true;
|
||||
}
|
||||
}
|
||||
if(!replaced)
|
||||
{
|
||||
all.push_back(p);
|
||||
}
|
||||
}
|
||||
return all;
|
||||
}
|
||||
|
||||
/// Builds the node under test with defaultParameters() plus @p params.
|
||||
std::shared_ptr<rtabmap_slam::CoreWrapper> makeNode(
|
||||
const std::vector<rclcpp::Parameter> & params = {},
|
||||
const std::vector<std::string> & arguments = {})
|
||||
{
|
||||
rclcpp::NodeOptions options;
|
||||
options.parameter_overrides(defaultParameters(params));
|
||||
if(!arguments.empty())
|
||||
{
|
||||
options.arguments(arguments);
|
||||
}
|
||||
node_ = addNode(std::make_shared<rtabmap_slam::CoreWrapper>(options));
|
||||
return node_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Builds the node under test like makeNode(), but spins it on a multi-threaded
|
||||
* executor of its own, in the background, as the `rtabmap` executable does.
|
||||
*
|
||||
* The node's mutexes are recursive: on the shared single-threaded executor, a mutex a
|
||||
* callback leaves locked is simply taken again by the next callback, on the same thread,
|
||||
* and nothing shows. With callbacks on several threads, the next one is blocked or skips
|
||||
* its update, as in the real node. The helper node keeps spinning on the shared executor.
|
||||
*/
|
||||
std::shared_ptr<rtabmap_slam::CoreWrapper> makeMultiThreadedNode(
|
||||
const std::vector<rclcpp::Parameter> & params = {})
|
||||
{
|
||||
rclcpp::NodeOptions options;
|
||||
options.parameter_overrides(defaultParameters(params));
|
||||
node_ = std::make_shared<rtabmap_slam::CoreWrapper>(options);
|
||||
nodeExecutor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>(
|
||||
rclcpp::ExecutorOptions(), 4);
|
||||
nodeExecutor_->add_node(node_);
|
||||
nodeThread_ = std::thread([this]() { nodeExecutor_->spin(); });
|
||||
spinFor(std::chrono::milliseconds(50)); // see NodeTest::addNode()
|
||||
return node_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Destroys the node under test, which is what saves its database.
|
||||
*
|
||||
* The executor and the helper node are rebuilt along with it, so publishers and
|
||||
* collectors made before this call are dead afterwards. Reusing the executor is not
|
||||
* an option: on Humble, one that had a node removed from it still holds that node's
|
||||
* guard condition and dereferences it on the next spin.
|
||||
*/
|
||||
void destroyNode()
|
||||
{
|
||||
stopNodeThreads();
|
||||
staticTf_.reset();
|
||||
tfPub_.reset();
|
||||
NodeTest::TearDown();
|
||||
node_.reset();
|
||||
NodeTest::SetUp();
|
||||
}
|
||||
|
||||
/// A parameter of the node under test, as the string RTAB-Map stores it as.
|
||||
std::string param(const std::string & name) const
|
||||
{
|
||||
return node_->get_parameter(name).as_string();
|
||||
}
|
||||
|
||||
/// Latches base_link -> @p child as a static transform, as a URDF would.
|
||||
void publishStaticTf(const std::string & child, double x = 0.0, double y = 0.0,
|
||||
double z = 0.0, const std::string & parent = "base_link")
|
||||
{
|
||||
geometry_msgs::msg::TransformStamped tf = makeTransform(parent, child, 0.0, x, y);
|
||||
tf.header.stamp = helper()->now();
|
||||
tf.transform.translation.z = z;
|
||||
publishStaticTf(tf);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief base_link -> @p child as a camera optical frame, @p z meters up.
|
||||
*
|
||||
* An optical frame looks along its own +z, with +x to the right of the image: rotated
|
||||
* here so the camera looks along the robot's +x, as mounted on the front of a robot.
|
||||
*/
|
||||
static geometry_msgs::msg::TransformStamped opticalTransform(
|
||||
const std::string & child, double z = 0.0)
|
||||
{
|
||||
geometry_msgs::msg::TransformStamped tf = makeTransform("base_link", child, 0.0);
|
||||
tf.transform.translation.z = z;
|
||||
tf.transform.rotation.x = -0.5;
|
||||
tf.transform.rotation.y = 0.5;
|
||||
tf.transform.rotation.z = -0.5;
|
||||
tf.transform.rotation.w = 0.5;
|
||||
return tf;
|
||||
}
|
||||
|
||||
/// Latches opticalTransform() as a static transform.
|
||||
void publishOpticalTf(const std::string & child, double z = 0.0)
|
||||
{
|
||||
geometry_msgs::msg::TransformStamped tf = opticalTransform(child, z);
|
||||
tf.header.stamp = helper()->now();
|
||||
publishStaticTf(tf);
|
||||
}
|
||||
|
||||
/// Latches @p tf as a static transform.
|
||||
void publishStaticTf(const geometry_msgs::msg::TransformStamped & tf)
|
||||
{
|
||||
if(!staticTf_)
|
||||
{
|
||||
staticTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
|
||||
}
|
||||
staticTf_->sendTransform(tf);
|
||||
spinFor(std::chrono::milliseconds(100));
|
||||
}
|
||||
|
||||
/// Publishes one transform on /tf, as a moving odometry source does.
|
||||
void publishTf(const geometry_msgs::msg::TransformStamped & tf)
|
||||
{
|
||||
if(!tfPub_)
|
||||
{
|
||||
tfPub_ = helper()->create_publisher<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
|
||||
// The node's listener and nothing else: publishing before it is matched loses
|
||||
// the transform.
|
||||
waitForSubscriber(tfPub_);
|
||||
}
|
||||
tf2_msgs::msg::TFMessage msg;
|
||||
msg.transforms.push_back(tf);
|
||||
tfPub_->publish(msg);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Publishes one odometry update the way an odometry node does: TF, then topic.
|
||||
*
|
||||
* The node looks odom -> base_link up in TF at the message's stamp and prefers it to
|
||||
* the pose in the message, so the two are published together and agree.
|
||||
*/
|
||||
void sendOdom(
|
||||
const rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr & pub,
|
||||
double stamp, double x, double y = 0.0, double yaw = 0.0,
|
||||
double variance = 0.001)
|
||||
{
|
||||
publishTf(makeTransform("odom", "base_link", stamp, x, y, yaw));
|
||||
pub->publish(makeOdometry(stamp, x, y, yaw, variance));
|
||||
}
|
||||
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odomPublisher()
|
||||
{
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub =
|
||||
helper()->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
|
||||
EXPECT_TRUE(waitForSubscriber(pub));
|
||||
return pub;
|
||||
}
|
||||
|
||||
/// Subscribes to `info`, which the node publishes once per processed update.
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> collectInfo()
|
||||
{
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info =
|
||||
collect<rtabmap_msgs::msg::Info>("info");
|
||||
EXPECT_TRUE(waitForPublisher(info->subscription));
|
||||
return info;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Sends @p count odometry updates @p step meters apart along x, one second apart.
|
||||
*
|
||||
* Waits for each to come out on @p info before sending the next, so the node never has
|
||||
* one queued while it is still processing the previous: the processing timer only
|
||||
* takes a new update once the last one is done, and drops what arrives in between.
|
||||
*/
|
||||
void driveStraight(
|
||||
const rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr & pub,
|
||||
const std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> & info,
|
||||
int count, double step = 0.5, double firstStamp = 1.0, double firstX = 0.0)
|
||||
{
|
||||
for(int i=0; i<count; ++i)
|
||||
{
|
||||
const size_t before = info->size();
|
||||
sendOdom(pub, firstStamp + double(i), firstX + step*double(i));
|
||||
ASSERT_TRUE(spinUntil([&]() { return info->size() > before; }))
|
||||
<< "update " << i << " was not processed";
|
||||
}
|
||||
}
|
||||
|
||||
/// Calls @p service on the node and returns its response, or null if it never came.
|
||||
template <typename SrvT>
|
||||
typename SrvT::Response::SharedPtr call(
|
||||
const std::string & service,
|
||||
typename SrvT::Request::SharedPtr request = std::make_shared<typename SrvT::Request>(),
|
||||
std::chrono::milliseconds timeout = std::chrono::milliseconds(10000))
|
||||
{
|
||||
// The node advertises its services under its own name: /rtabmap/reset, not /reset.
|
||||
typename rclcpp::Client<SrvT>::SharedPtr client =
|
||||
helper()->create_client<SrvT>("/rtabmap/" + service);
|
||||
if(!spinUntil([&]() { return client->service_is_ready(); }))
|
||||
{
|
||||
return typename SrvT::Response::SharedPtr();
|
||||
}
|
||||
auto future = client->async_send_request(request).future.share();
|
||||
if(!spinUntil([&]() {
|
||||
return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; },
|
||||
timeout))
|
||||
{
|
||||
return typename SrvT::Response::SharedPtr();
|
||||
}
|
||||
return future.get();
|
||||
}
|
||||
|
||||
bool callEmpty(const std::string & service)
|
||||
{
|
||||
return call<std_srvs::srv::Empty>(service).get() != nullptr;
|
||||
}
|
||||
|
||||
/// The whole graph, as `get_map_data` returns it, graph only.
|
||||
rtabmap_msgs::msg::MapData getGraph(bool global = true, bool optimized = true)
|
||||
{
|
||||
rtabmap_msgs::srv::GetMap::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
|
||||
req->global_map = global;
|
||||
req->optimized = optimized;
|
||||
req->graph_only = true;
|
||||
rtabmap_msgs::srv::GetMap::Response::SharedPtr res =
|
||||
call<rtabmap_msgs::srv::GetMap>("get_map_data", req);
|
||||
EXPECT_TRUE(res.get() != nullptr);
|
||||
return res ? res->data : rtabmap_msgs::msg::MapData();
|
||||
}
|
||||
|
||||
/// Node @p id with everything it stores; its `id` is 0 if the node does not exist.
|
||||
rtabmap_msgs::msg::Node getNode(int id)
|
||||
{
|
||||
rtabmap_msgs::srv::GetNodeData::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::GetNodeData::Request>();
|
||||
req->ids.push_back(id);
|
||||
req->images = true;
|
||||
req->scan = true;
|
||||
req->grid = true;
|
||||
req->user_data = true;
|
||||
rtabmap_msgs::srv::GetNodeData::Response::SharedPtr res =
|
||||
call<rtabmap_msgs::srv::GetNodeData>("get_node_data", req);
|
||||
EXPECT_TRUE(res.get() != nullptr);
|
||||
return res && !res->data.empty() ? res->data.front() : rtabmap_msgs::msg::Node();
|
||||
}
|
||||
|
||||
/// The map ids of every node in the graph, in node id order.
|
||||
std::vector<int> mapIds()
|
||||
{
|
||||
std::vector<int> ids;
|
||||
rtabmap_msgs::srv::GetMap::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
|
||||
req->global_map = true;
|
||||
req->optimized = false;
|
||||
req->graph_only = false;
|
||||
rtabmap_msgs::srv::GetMap::Response::SharedPtr res =
|
||||
call<rtabmap_msgs::srv::GetMap>("get_map_data", req);
|
||||
if(res)
|
||||
{
|
||||
for(const rtabmap_msgs::msg::Node & n : res->data.nodes)
|
||||
{
|
||||
ids.push_back(n.map_id);
|
||||
}
|
||||
}
|
||||
return ids;
|
||||
}
|
||||
|
||||
/// The value of RTAB-Map statistic @p key in @p info, or @p fallback if absent.
|
||||
static float stat(const rtabmap_msgs::msg::Info & info, const std::string & key,
|
||||
float fallback = -1.0f)
|
||||
{
|
||||
for(size_t i=0; i<info.stats_keys.size(); ++i)
|
||||
{
|
||||
if(info.stats_keys[i] == key)
|
||||
{
|
||||
return info.stats_values[i];
|
||||
}
|
||||
}
|
||||
return fallback;
|
||||
}
|
||||
|
||||
std::shared_ptr<rtabmap_slam::CoreWrapper> node_;
|
||||
|
||||
private:
|
||||
/**
|
||||
* Stops the executor of makeMultiThreadedNode(), if any. After a failure, a callback may
|
||||
* be blocked for good on a leaked lock, and joining would hang the binary instead of
|
||||
* reporting it: the thread and the node are then abandoned, to die with the process.
|
||||
*/
|
||||
void stopNodeThreads()
|
||||
{
|
||||
if(!nodeExecutor_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
nodeExecutor_->cancel();
|
||||
if(HasFailure())
|
||||
{
|
||||
nodeThread_.detach();
|
||||
new std::shared_ptr<rtabmap_slam::CoreWrapper>(node_); // never destroyed
|
||||
new std::shared_ptr<rclcpp::executors::MultiThreadedExecutor>(nodeExecutor_);
|
||||
}
|
||||
else
|
||||
{
|
||||
nodeThread_.join();
|
||||
nodeExecutor_->remove_node(node_);
|
||||
}
|
||||
nodeExecutor_.reset();
|
||||
}
|
||||
|
||||
rclcpp::executors::MultiThreadedExecutor::SharedPtr nodeExecutor_;
|
||||
std::thread nodeThread_;
|
||||
|
||||
static void removeDir(const std::string & dir)
|
||||
{
|
||||
UDirectory d(dir);
|
||||
for(std::string f = d.getNextFilePath(); !f.empty(); f = d.getNextFilePath())
|
||||
{
|
||||
UFile::erase(f);
|
||||
}
|
||||
UDirectory::removeDir(dir);
|
||||
}
|
||||
|
||||
std::string dir_;
|
||||
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> staticTf_;
|
||||
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub_;
|
||||
};
|
||||
|
||||
} // namespace rtabmap_slam_test
|
||||
|
||||
#endif /* RTABMAP_SLAM_CORE_WRAPPER_FIXTURE_HPP_ */
|
||||
@@ -0,0 +1,348 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#ifndef RTABMAP_SLAM_MSG_BUILDERS_HPP_
|
||||
#define RTABMAP_SLAM_MSG_BUILDERS_HPP_
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
|
||||
#include <geometry_msgs/msg/transform_stamped.hpp>
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
#include <sensor_msgs/msg/camera_info.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <sensor_msgs/msg/imu.hpp>
|
||||
#include <sensor_msgs/msg/laser_scan.hpp>
|
||||
#include <sensor_msgs/msg/nav_sat_fix.hpp>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
#include <rtabmap_msgs/msg/landmark_detection.hpp>
|
||||
#include <rtabmap_msgs/msg/user_data.hpp>
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <rtabmap/core/LaserScan.h>
|
||||
#include <rtabmap/core/Transform.h>
|
||||
#include <rtabmap/core/util3d.h>
|
||||
#include <rtabmap/core/util3d_transforms.h>
|
||||
#ifdef PRE_ROS_IRON
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#else
|
||||
#include <cv_bridge/cv_bridge.hpp>
|
||||
#endif
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace rtabmap_slam_test {
|
||||
|
||||
/// A ROS time from a double, the way sensor stamps are written throughout these tests.
|
||||
inline rclcpp::Time stampOf(double seconds)
|
||||
{
|
||||
return rclcpp::Time(
|
||||
int32_t(seconds), uint32_t((seconds - int32_t(seconds)) * 1e9), RCL_ROS_TIME);
|
||||
}
|
||||
|
||||
/// A planar pose as a TF: @p x, @p y in meters and @p yaw in radians.
|
||||
inline geometry_msgs::msg::TransformStamped makeTransform(
|
||||
const std::string & parent, const std::string & child, double stamp,
|
||||
double x = 0.0, double y = 0.0, double yaw = 0.0)
|
||||
{
|
||||
geometry_msgs::msg::TransformStamped tf;
|
||||
tf.header.frame_id = parent;
|
||||
tf.header.stamp = stampOf(stamp);
|
||||
tf.child_frame_id = child;
|
||||
tf.transform.translation.x = x;
|
||||
tf.transform.translation.y = y;
|
||||
tf.transform.rotation.z = std::sin(yaw/2.0);
|
||||
tf.transform.rotation.w = std::cos(yaw/2.0);
|
||||
return tf;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief An odometry message at (@p x, @p y, @p yaw), with a small valid covariance.
|
||||
*
|
||||
* The covariance matters to rtabmap: 9999 on both diagonals, or an identity pose after a
|
||||
* non-identity one, is read as an odometry reset and starts a new map.
|
||||
*/
|
||||
inline nav_msgs::msg::Odometry makeOdometry(
|
||||
double stamp, double x = 0.0, double y = 0.0, double yaw = 0.0,
|
||||
double variance = 0.001,
|
||||
const std::string & frameId = "odom", const std::string & childFrameId = "base_link")
|
||||
{
|
||||
nav_msgs::msg::Odometry msg;
|
||||
msg.header.frame_id = frameId;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.child_frame_id = childFrameId;
|
||||
msg.pose.pose.position.x = x;
|
||||
msg.pose.pose.position.y = y;
|
||||
msg.pose.pose.orientation.z = std::sin(yaw/2.0);
|
||||
msg.pose.pose.orientation.w = std::cos(yaw/2.0);
|
||||
for(int i=0; i<6; ++i)
|
||||
{
|
||||
msg.pose.covariance[i*7] = variance;
|
||||
msg.twist.covariance[i*7] = variance;
|
||||
}
|
||||
return msg;
|
||||
}
|
||||
|
||||
/// What an odometry node publishes when it is lost or has just been reset.
|
||||
inline nav_msgs::msg::Odometry makeResetOdometry(double stamp)
|
||||
{
|
||||
nav_msgs::msg::Odometry msg = makeOdometry(stamp, 0.0, 0.0, 0.0, 9999.0);
|
||||
return msg;
|
||||
}
|
||||
|
||||
/**
|
||||
* @name The room the tests' robot drives in
|
||||
*
|
||||
* A rectangle fixed in the world (the odom and map frames coincide in these tests), with
|
||||
* walls 1 m high. Scans and clouds are generated from the sensor's actual pose in it, so
|
||||
* every node sees the same walls wherever the robot is -- which is what makes the
|
||||
* assembled map, and the occupancy grid in particular, comparable to the room.
|
||||
* @{
|
||||
*/
|
||||
constexpr double kRoomXMin = -1.5;
|
||||
constexpr double kRoomXMax = 3.5;
|
||||
constexpr double kRoomYMin = -2.0;
|
||||
constexpr double kRoomYMax = 2.0;
|
||||
constexpr double kRoomHeight = 1.0;
|
||||
|
||||
/// Distance from (@p x, @p y), inside the room, to its walls along direction @p theta.
|
||||
inline double rayToRoom(double x, double y, double theta)
|
||||
{
|
||||
const double dx = std::cos(theta);
|
||||
const double dy = std::sin(theta);
|
||||
double t = std::numeric_limits<double>::infinity();
|
||||
if(dx > 1e-9) { t = std::min(t, (kRoomXMax - x) / dx); }
|
||||
else if(dx < -1e-9) { t = std::min(t, (kRoomXMin - x) / dx); }
|
||||
if(dy > 1e-9) { t = std::min(t, (kRoomYMax - y) / dy); }
|
||||
else if(dy < -1e-9) { t = std::min(t, (kRoomYMin - y) / dy); }
|
||||
return t;
|
||||
}
|
||||
|
||||
/// A 360 degree LaserScan of the room from a laser at (@p x, @p y, @p yaw) in the world.
|
||||
inline sensor_msgs::msg::LaserScan makeRoomScan(
|
||||
const std::string & frameId, double stamp,
|
||||
double x, double y = 0.0, double yaw = 0.0, size_t count = 720)
|
||||
{
|
||||
sensor_msgs::msg::LaserScan scan;
|
||||
scan.header.frame_id = frameId;
|
||||
scan.header.stamp = stampOf(stamp);
|
||||
scan.angle_increment = float(2.0 * M_PI / double(count));
|
||||
scan.angle_min = float(-M_PI);
|
||||
scan.angle_max = scan.angle_min + scan.angle_increment * float(count - 1);
|
||||
scan.time_increment = 0.0f;
|
||||
scan.scan_time = 0.1f;
|
||||
scan.range_min = 0.1f;
|
||||
scan.range_max = 10.0f;
|
||||
scan.ranges.resize(count);
|
||||
for(size_t i=0; i<count; ++i)
|
||||
{
|
||||
scan.ranges[i] = float(rayToRoom(x, y, yaw + scan.angle_min + scan.angle_increment * double(i)));
|
||||
}
|
||||
return scan;
|
||||
}
|
||||
|
||||
/**
|
||||
* The room as a 3D scan in the world frame: its walls, floor to top, every 0.05 m along
|
||||
* them and every 0.1 m up, dense enough for the grid's obstacle clustering
|
||||
* (Grid/ClusterRadius); and its floor every 0.025 m, so that each 0.05 m grid cell gets
|
||||
* ground points. Built once: only its pose relative to the sensor changes.
|
||||
*/
|
||||
inline const rtabmap::LaserScan & roomPoints()
|
||||
{
|
||||
static const rtabmap::LaserScan room = []() {
|
||||
const double step = 0.05;
|
||||
std::vector<cv::Vec3f> points;
|
||||
for(double fx=kRoomXMin + 0.025; fx<kRoomXMax - 1e-6; fx+=0.025)
|
||||
{
|
||||
for(double fy=kRoomYMin + 0.025; fy<kRoomYMax - 1e-6; fy+=0.025)
|
||||
{
|
||||
points.push_back(cv::Vec3f(float(fx), float(fy), 0.0f));
|
||||
}
|
||||
}
|
||||
for(double h=0.0; h<=kRoomHeight + 1e-6; h+=0.1)
|
||||
{
|
||||
for(double t=kRoomXMin; t<=kRoomXMax + 1e-6; t+=step)
|
||||
{
|
||||
points.push_back(cv::Vec3f(float(t), float(kRoomYMin), float(h)));
|
||||
points.push_back(cv::Vec3f(float(t), float(kRoomYMax), float(h)));
|
||||
}
|
||||
for(double t=kRoomYMin + step; t<kRoomYMax - 1e-6; t+=step)
|
||||
{
|
||||
points.push_back(cv::Vec3f(float(kRoomXMin), float(t), float(h)));
|
||||
points.push_back(cv::Vec3f(float(kRoomXMax), float(t), float(h)));
|
||||
}
|
||||
}
|
||||
return rtabmap::LaserScan(cv::Mat(points, true).reshape(3, 1), int(points.size()),
|
||||
0.0f, rtabmap::LaserScan::kXYZ);
|
||||
}();
|
||||
return room;
|
||||
}
|
||||
|
||||
/**
|
||||
* The room as seen by a 3D lidar at (@p x, @p y, @p z) in the world, facing @p yaw, in the
|
||||
* lidar's frame. Every point of the room is visible from anywhere inside it, so moving the
|
||||
* room is all it takes -- unlike a 2D LaserScan message, whose ranges are per angle from
|
||||
* the sensor and are ray-cast again from each pose by makeRoomScan().
|
||||
*/
|
||||
inline rtabmap::LaserScan roomScan3d(double x, double y = 0.0, double z = 0.0, double yaw = 0.0)
|
||||
{
|
||||
return rtabmap::util3d::transformLaserScan(
|
||||
roomPoints(), rtabmap::Transform(x, y, z, 0, 0, yaw).inverse());
|
||||
}
|
||||
|
||||
/// @p scan as the PointCloud2 a 3D lidar driver would publish.
|
||||
inline sensor_msgs::msg::PointCloud2 makeCloud(
|
||||
const std::string & frameId, double stamp, const rtabmap::LaserScan & scan)
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2 msg;
|
||||
pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(scan), msg);
|
||||
msg.header.frame_id = frameId;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
return msg;
|
||||
}
|
||||
/** @} */
|
||||
|
||||
/// A rectified pinhole CameraInfo.
|
||||
inline sensor_msgs::msg::CameraInfo makeCameraInfo(
|
||||
const std::string & frameId, double stamp, int width = 64, int height = 48,
|
||||
double fx = 50.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, 0.0, 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;
|
||||
}
|
||||
|
||||
/// A random bgr8 texture: something a feature detector finds corners in.
|
||||
inline cv::Mat texturedImage(int width = 64, int height = 48, uint64_t seed = 42)
|
||||
{
|
||||
cv::Mat image(height, width, CV_8UC3);
|
||||
cv::RNG rng(seed);
|
||||
rng.fill(image, cv::RNG::UNIFORM, 0, 255);
|
||||
return image;
|
||||
}
|
||||
|
||||
inline sensor_msgs::msg::Image makeTexturedImage(
|
||||
const std::string & frameId, double stamp, int width = 64, int height = 48,
|
||||
uint64_t seed = 42)
|
||||
{
|
||||
return makeImage(frameId, stamp, texturedImage(width, height, seed), "bgr8");
|
||||
}
|
||||
|
||||
/// A 16UC1 depth image of a flat wall @p millimeters away.
|
||||
inline cv::Mat depthImage(int width = 64, int height = 48, uint16_t millimeters = 1500)
|
||||
{
|
||||
return cv::Mat(height, width, CV_16UC1, cv::Scalar(millimeters));
|
||||
}
|
||||
|
||||
inline sensor_msgs::msg::Image makeDepthImage(
|
||||
const std::string & frameId, double stamp, int width = 64, int height = 48,
|
||||
uint16_t millimeters = 1500)
|
||||
{
|
||||
return makeImage(frameId, stamp, depthImage(width, height, millimeters), "16UC1");
|
||||
}
|
||||
|
||||
/// An uncompressed 2x2 user data matrix.
|
||||
inline rtabmap_msgs::msg::UserData makeUserData(double stamp, uint8_t first = 1)
|
||||
{
|
||||
rtabmap_msgs::msg::UserData msg;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.rows = 2;
|
||||
msg.cols = 2;
|
||||
msg.type = CV_8UC1;
|
||||
msg.data = {first, 2, 3, 4};
|
||||
return msg;
|
||||
}
|
||||
|
||||
inline sensor_msgs::msg::NavSatFix makeGpsFix(
|
||||
double stamp, double latitude, double longitude, double altitude = 100.0,
|
||||
double variance = 4.0)
|
||||
{
|
||||
sensor_msgs::msg::NavSatFix msg;
|
||||
msg.header.frame_id = "gps";
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.latitude = latitude;
|
||||
msg.longitude = longitude;
|
||||
msg.altitude = altitude;
|
||||
msg.position_covariance = {variance, 0, 0, 0, variance, 0, 0, 0, variance};
|
||||
msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
|
||||
return msg;
|
||||
}
|
||||
|
||||
/// An IMU carrying only an orientation, rolled by @p roll radians.
|
||||
inline sensor_msgs::msg::Imu makeImu(
|
||||
const std::string & frameId, double stamp, double roll = 0.0)
|
||||
{
|
||||
sensor_msgs::msg::Imu msg;
|
||||
msg.header.frame_id = frameId;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.orientation.x = std::sin(roll/2.0);
|
||||
msg.orientation.w = std::cos(roll/2.0);
|
||||
return msg;
|
||||
}
|
||||
|
||||
/// A landmark (fiducial) detected @p x meters in front of @p frameId.
|
||||
inline rtabmap_msgs::msg::LandmarkDetection makeLandmark(
|
||||
const std::string & frameId, double stamp, int id, double x = 1.0)
|
||||
{
|
||||
rtabmap_msgs::msg::LandmarkDetection msg;
|
||||
msg.header.frame_id = frameId;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.landmark_frame_id = "tag_" + std::to_string(id);
|
||||
msg.id = id;
|
||||
msg.size = 0.1f;
|
||||
msg.pose.pose.position.x = x;
|
||||
msg.pose.pose.orientation.w = 1.0;
|
||||
for(int i=0; i<6; ++i)
|
||||
{
|
||||
msg.pose.covariance[i*7] = 0.01;
|
||||
}
|
||||
return msg;
|
||||
}
|
||||
|
||||
inline geometry_msgs::msg::PoseWithCovarianceStamped makePoseWithCovariance(
|
||||
const std::string & frameId, double stamp, double x, double y = 0.0,
|
||||
double yaw = 0.0, double variance = 0.01)
|
||||
{
|
||||
geometry_msgs::msg::PoseWithCovarianceStamped msg;
|
||||
msg.header.frame_id = frameId;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.pose.pose.position.x = x;
|
||||
msg.pose.pose.position.y = y;
|
||||
msg.pose.pose.orientation.z = std::sin(yaw/2.0);
|
||||
msg.pose.pose.orientation.w = std::cos(yaw/2.0);
|
||||
for(int i=0; i<6; ++i)
|
||||
{
|
||||
msg.pose.covariance[i*7] = variance;
|
||||
}
|
||||
return msg;
|
||||
}
|
||||
|
||||
} // namespace rtabmap_slam_test
|
||||
|
||||
#endif /* RTABMAP_SLAM_MSG_BUILDERS_HPP_ */
|
||||
@@ -0,0 +1,256 @@
|
||||
/*
|
||||
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_SLAM_NODE_TEST_UTILS_HPP_
|
||||
#define RTABMAP_SLAM_NODE_TEST_UTILS_HPP_
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
#include <chrono>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace rtabmap_slam_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_slam_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();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Adds a node under test to the shared executor and keeps it alive for the test.
|
||||
*
|
||||
* The wait is for tf2_ros, not for anything the test does with the node.
|
||||
* ~TransformListener cancels its worker's executor and joins it without ordering the
|
||||
* cancel after the worker reached spin(), so a node dropped microseconds after it was
|
||||
* built -- which a test that only reads a parameter back does -- hangs the binary for
|
||||
* good (ros2/geometry2#517). The window is a few instructions wide and nothing here
|
||||
* can observe that thread, so this buys time instead. Drop it once #752 lands.
|
||||
*/
|
||||
template <typename NodeT>
|
||||
std::shared_ptr<NodeT> addNode(const std::shared_ptr<NodeT> & node)
|
||||
{
|
||||
executor_->add_node(node);
|
||||
nodes_.push_back(node);
|
||||
spinFor(std::chrono::milliseconds(50));
|
||||
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();
|
||||
}
|
||||
|
||||
/// 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.
|
||||
*
|
||||
* Every node here publishes only when it has 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; });
|
||||
}
|
||||
|
||||
/// 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(); }
|
||||
|
||||
/**
|
||||
* @brief The last (first) message received.
|
||||
*
|
||||
* A test that reads these without having waited for the topic it is reading --
|
||||
* having waited for a different one, say -- gets a legible failure rather than a
|
||||
* segmentation fault: std::vector::back() on an empty vector dereferences
|
||||
* nullptr-1, which crashes the whole binary and takes the rest of its tests with
|
||||
* it. gtest turns the exception into a failure of the test that threw it.
|
||||
*/
|
||||
const MsgT & back() const { return *checked(messages.empty()?0:&messages.back()); }
|
||||
const MsgT & front() const { return *checked(messages.empty()?0:&messages.front()); }
|
||||
|
||||
private:
|
||||
const typename MsgT::ConstSharedPtr & checked(
|
||||
const typename MsgT::ConstSharedPtr * msg) const
|
||||
{
|
||||
if(msg == 0)
|
||||
{
|
||||
throw std::out_of_range(
|
||||
std::string("nothing was received on \"") +
|
||||
(subscription?subscription->get_topic_name():"?") +
|
||||
"\", so there is no message to read: wait for it to arrive first");
|
||||
}
|
||||
return *msg;
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Subscribes the helper node to @p topic and records everything it receives.
|
||||
*
|
||||
* The callback holds the collector weakly. Capturing it by shared_ptr would close a
|
||||
* cycle -- collector owns the subscription, the subscription owns the callback, the
|
||||
* callback owns the collector -- and neither would ever be freed. A subscription that
|
||||
* outlives its test keeps the helper node's rcl handle alive with it, which leaves the
|
||||
* node's rosout publisher registered and greets the next test with "Publisher already
|
||||
* registered for node name: 'rtabmap_slam_test_helper'".
|
||||
*/
|
||||
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>>();
|
||||
std::weak_ptr<Collector<MsgT>> weak = collector;
|
||||
collector->subscription = helper_->create_subscription<MsgT>(
|
||||
topic, qos,
|
||||
[weak](const typename MsgT::ConstSharedPtr msg) {
|
||||
if(std::shared_ptr<Collector<MsgT>> collector = weak.lock())
|
||||
{
|
||||
collector->messages.push_back(msg);
|
||||
}
|
||||
});
|
||||
return collector;
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
|
||||
rclcpp::Node::SharedPtr helper_;
|
||||
std::vector<rclcpp::Node::SharedPtr> nodes_;
|
||||
};
|
||||
|
||||
} // namespace rtabmap_slam_test
|
||||
|
||||
#endif /* RTABMAP_SLAM_NODE_TEST_UTILS_HPP_ */
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,428 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
|
||||
#include <nav_msgs/msg/path.hpp>
|
||||
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
|
||||
#include "core_wrapper_fixture.hpp"
|
||||
|
||||
namespace rtabmap_slam_test {
|
||||
|
||||
namespace {
|
||||
|
||||
::testing::Environment * const kRclcppEnv = registerRclcppEnvironment();
|
||||
|
||||
using rtabmap::Parameters;
|
||||
|
||||
class CoreWrapperMappingTest : public CoreWrapperTest
|
||||
{
|
||||
protected:
|
||||
/// The x of every pose in @p graph, in node id order.
|
||||
static std::vector<double> xs(const rtabmap_msgs::msg::MapGraph & graph)
|
||||
{
|
||||
std::vector<double> out;
|
||||
for(const geometry_msgs::msg::Pose & p : graph.poses)
|
||||
{
|
||||
out.push_back(p.position.x);
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
/// The neighbor link between @p from and @p to, or one with from_id 0 if none.
|
||||
static rtabmap_msgs::msg::Link neighborLink(
|
||||
const rtabmap_msgs::msg::MapGraph & graph, int from, int to)
|
||||
{
|
||||
for(const rtabmap_msgs::msg::Link & l : graph.links)
|
||||
{
|
||||
if(l.type == 0 && ((l.from_id == from && l.to_id == to) ||
|
||||
(l.from_id == to && l.to_id == from)))
|
||||
{
|
||||
return l;
|
||||
}
|
||||
}
|
||||
return rtabmap_msgs::msg::Link();
|
||||
}
|
||||
|
||||
/// Counts the map -> odom transforms seen on /tf.
|
||||
static size_t countTransforms(
|
||||
const std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> & tf,
|
||||
const std::string & parent, const std::string & child)
|
||||
{
|
||||
size_t found = 0;
|
||||
for(const tf2_msgs::msg::TFMessage::ConstSharedPtr & msg : tf->messages)
|
||||
{
|
||||
for(const geometry_msgs::msg::TransformStamped & t : msg->transforms)
|
||||
{
|
||||
found += (t.header.frame_id == parent && t.child_frame_id == child) ? 1 : 0;
|
||||
}
|
||||
}
|
||||
return found;
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* The simplest input rtabmap accepts is odometry alone, and every update that moved far
|
||||
* enough becomes a node, linked to the previous one by the odometry between them.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, adds_a_node_per_odometry_update)
|
||||
{
|
||||
makeNode();
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
driveStraight(odom, info, 3);
|
||||
|
||||
ASSERT_EQ(3u, info->size());
|
||||
EXPECT_EQ(1, info->messages[0]->ref_id);
|
||||
EXPECT_EQ(2, info->messages[1]->ref_id);
|
||||
EXPECT_EQ(3, info->messages[2]->ref_id);
|
||||
|
||||
rtabmap_msgs::msg::MapData map = getGraph();
|
||||
ASSERT_EQ(3u, map.graph.poses_id.size());
|
||||
std::vector<double> x = xs(map.graph);
|
||||
EXPECT_NEAR(0.0, x[0], 1e-4);
|
||||
EXPECT_NEAR(0.5, x[1], 1e-4);
|
||||
EXPECT_NEAR(1.0, x[2], 1e-4);
|
||||
EXPECT_NE(0, neighborLink(map.graph, 1, 2).from_id);
|
||||
EXPECT_NE(0, neighborLink(map.graph, 2, 3).from_id);
|
||||
}
|
||||
|
||||
/**
|
||||
* An update that did not move at least RGBD/LinearUpdate (or turn RGBD/AngularUpdate)
|
||||
* since the last node is not added: a robot standing still does not grow the map.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, does_not_add_nodes_while_standing_still)
|
||||
{
|
||||
makeNode({rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), "0.1")});
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
driveStraight(odom, info, 4, 0.02);
|
||||
|
||||
EXPECT_EQ(1u, getGraph().graph.poses_id.size());
|
||||
}
|
||||
|
||||
/**
|
||||
* Rtabmap/DetectionRate throttles the updates by their stamps, not by when they arrive:
|
||||
* one closer than 1/rate to the last one processed is dropped.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, throttles_updates_to_the_detection_rate)
|
||||
{
|
||||
makeNode({rclcpp::Parameter(Parameters::kRtabmapDetectionRate(), "1")});
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
for(int i=0; i<6; ++i)
|
||||
{
|
||||
sendOdom(odom, 1.0 + 0.25*i, 0.5*i);
|
||||
spinFor(std::chrono::milliseconds(150));
|
||||
}
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
|
||||
// 1.0 and 2.0 are a full period apart; 1.25, 1.5, 1.75 and 2.25 are not.
|
||||
EXPECT_EQ(2u, info->size());
|
||||
EXPECT_EQ(2u, getGraph().graph.poses_id.size());
|
||||
}
|
||||
|
||||
/**
|
||||
* With Rtabmap/CreateIntermediateNodes, the updates the detection rate would have dropped
|
||||
* are kept as intermediate nodes instead: poses in the graph, without the sensor data or
|
||||
* the loop closure detection. They do not publish `info`.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, keeps_throttled_updates_as_intermediate_nodes)
|
||||
{
|
||||
makeNode({rclcpp::Parameter(Parameters::kRtabmapDetectionRate(), "1"),
|
||||
rclcpp::Parameter(Parameters::kRtabmapCreateIntermediateNodes(), "true")});
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
for(int i=0; i<5; ++i)
|
||||
{
|
||||
sendOdom(odom, 1.0 + 0.25*i, 0.5*i);
|
||||
spinFor(std::chrono::milliseconds(150));
|
||||
}
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
|
||||
EXPECT_EQ(2u, info->size()) << "only the updates at 1.0 and 2.0 are full nodes";
|
||||
rtabmap_msgs::msg::MapData map = getGraph();
|
||||
EXPECT_EQ(5u, map.graph.poses_id.size());
|
||||
}
|
||||
|
||||
/**
|
||||
* An odometry that resets -- an identity pose after a non-identity one, or 9999 on both
|
||||
* covariance diagonals -- starts a new map in the same database, rather than tearing the
|
||||
* graph across a jump the robot never made.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, starts_a_new_map_when_odometry_resets)
|
||||
{
|
||||
makeNode();
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
driveStraight(odom, info, 2);
|
||||
{
|
||||
const size_t before = info->size();
|
||||
publishTf(makeTransform("odom", "base_link", 3.0));
|
||||
odom->publish(makeResetOdometry(3.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return info->size() > before; }));
|
||||
}
|
||||
driveStraight(odom, info, 2, 0.5, 4.0, 0.5);
|
||||
|
||||
EXPECT_EQ(std::vector<int>({0, 0, 1, 1, 1}), mapIds());
|
||||
}
|
||||
|
||||
/**
|
||||
* staleness_factor: when the gap between two updates exceeds that many detection
|
||||
* periods, the odometry is not trusted across it and a new map is started, as if it had
|
||||
* reset.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, starts_a_new_map_after_a_stale_gap)
|
||||
{
|
||||
makeNode({rclcpp::Parameter(Parameters::kRtabmapDetectionRate(), "1"),
|
||||
rclcpp::Parameter("staleness_factor", 2.0)});
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
driveStraight(odom, info, 2); // stamps 1 and 2
|
||||
driveStraight(odom, info, 2, 0.5, 6.0, 1.0); // 4 s later: more than 2 periods
|
||||
|
||||
EXPECT_EQ(std::vector<int>({0, 0, 1, 1}), mapIds());
|
||||
}
|
||||
|
||||
/// Values of staleness_factor between 0 and 1 make no sense and disable it.
|
||||
TEST_F(CoreWrapperMappingTest, ignores_a_staleness_factor_below_one)
|
||||
{
|
||||
makeNode({rclcpp::Parameter(Parameters::kRtabmapDetectionRate(), "1"),
|
||||
rclcpp::Parameter("staleness_factor", 0.5)});
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
driveStraight(odom, info, 2);
|
||||
driveStraight(odom, info, 2, 0.5, 6.0, 1.0);
|
||||
|
||||
EXPECT_EQ(std::vector<int>({0, 0, 0, 0}), mapIds());
|
||||
}
|
||||
|
||||
/// A message with a zero stamp cannot be placed in time and is dropped.
|
||||
TEST_F(CoreWrapperMappingTest, drops_updates_with_a_null_stamp)
|
||||
{
|
||||
makeNode();
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
odom->publish(makeOdometry(0.0, 1.0));
|
||||
spinFor(std::chrono::milliseconds(500));
|
||||
|
||||
EXPECT_TRUE(info->empty());
|
||||
}
|
||||
|
||||
/**
|
||||
* The odometry's covariance becomes the information matrix of the link between two nodes
|
||||
* (its inverse). The twist covariance is preferred, since it is the uncertainty of the
|
||||
* motion between the two rather than accumulated since the start.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, weights_links_with_the_odometry_covariance)
|
||||
{
|
||||
makeNode();
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
sendOdom(odom, 1.0, 0.0, 0.0, 0.0, 0.01);
|
||||
ASSERT_TRUE(spinUntil([&]() { return info->size() == 1; }));
|
||||
sendOdom(odom, 2.0, 0.5, 0.0, 0.0, 0.01);
|
||||
ASSERT_TRUE(spinUntil([&]() { return info->size() == 2; }));
|
||||
|
||||
rtabmap_msgs::msg::Link link = neighborLink(getGraph().graph, 1, 2);
|
||||
ASSERT_NE(0, link.from_id);
|
||||
EXPECT_NEAR(100.0, link.information[0], 1e-3);
|
||||
EXPECT_NEAR(100.0, link.information[35], 1e-3);
|
||||
}
|
||||
|
||||
/**
|
||||
* An odometry with no covariance -- all zeros, as many drivers publish -- gets
|
||||
* odom_tf_linear_variance and odom_tf_angular_variance instead of an infinitely
|
||||
* confident link.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, falls_back_to_default_variances_without_covariance)
|
||||
{
|
||||
makeNode({rclcpp::Parameter("odom_tf_linear_variance", 0.04),
|
||||
rclcpp::Parameter("odom_tf_angular_variance", 0.25)});
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
sendOdom(odom, 1.0, 0.0, 0.0, 0.0, 0.0);
|
||||
ASSERT_TRUE(spinUntil([&]() { return info->size() == 1; }));
|
||||
sendOdom(odom, 2.0, 0.5, 0.0, 0.0, 0.0);
|
||||
ASSERT_TRUE(spinUntil([&]() { return info->size() == 2; }));
|
||||
|
||||
rtabmap_msgs::msg::Link link = neighborLink(getGraph().graph, 1, 2);
|
||||
ASSERT_NE(0, link.from_id);
|
||||
EXPECT_NEAR(25.0, link.information[0], 1e-3);
|
||||
EXPECT_NEAR(4.0, link.information[35], 1e-3);
|
||||
}
|
||||
|
||||
/**
|
||||
* The node's job on TF is map -> odom: the correction that puts the odometry frame where
|
||||
* the optimized graph says it is. Identity until a loop closure moves it. It is published
|
||||
* from its own thread once the odometry frame is known, at a rate of 1/tf_delay.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, publishes_map_to_odom_on_tf)
|
||||
{
|
||||
makeNode();
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
|
||||
collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
EXPECT_EQ(0u, countTransforms(tf, "map", "odom"))
|
||||
<< "the odometry frame is not known before the first update";
|
||||
|
||||
driveStraight(odom, info, 1);
|
||||
ASSERT_TRUE(spinUntil([&]() { return countTransforms(tf, "map", "odom") >= 3; }));
|
||||
}
|
||||
|
||||
/// odom_frame_id_init publishes map -> odom from the start, before any odometry arrives.
|
||||
TEST_F(CoreWrapperMappingTest, odom_frame_id_init_publishes_tf_before_the_first_update)
|
||||
{
|
||||
makeNode({rclcpp::Parameter("odom_frame_id_init", "odom")});
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
|
||||
collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
|
||||
|
||||
EXPECT_TRUE(spinUntil([&]() { return countTransforms(tf, "map", "odom") >= 3; }));
|
||||
}
|
||||
|
||||
TEST_F(CoreWrapperMappingTest, publish_tf_false_publishes_no_tf)
|
||||
{
|
||||
makeNode({rclcpp::Parameter("publish_tf", false)});
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
|
||||
collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
|
||||
driveStraight(odom, info, 2);
|
||||
spinFor(std::chrono::milliseconds(300));
|
||||
|
||||
EXPECT_EQ(0u, countTransforms(tf, "map", "odom"));
|
||||
}
|
||||
|
||||
/// map_frame_id renames the map frame everywhere: TF and every map-frame topic.
|
||||
TEST_F(CoreWrapperMappingTest, map_frame_id_renames_the_map_frame)
|
||||
{
|
||||
makeNode({rclcpp::Parameter("map_frame_id", "world")});
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
|
||||
collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
|
||||
std::shared_ptr<Collector<nav_msgs::msg::Path>> path =
|
||||
collect<nav_msgs::msg::Path>("mapPath");
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
ASSERT_TRUE(waitForPublisher(path->subscription));
|
||||
|
||||
driveStraight(odom, info, 2);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return countTransforms(tf, "world", "odom") > 0; }));
|
||||
EXPECT_EQ(0u, countTransforms(tf, "map", "odom"));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !path->empty(); }));
|
||||
EXPECT_EQ("world", path->back().header.frame_id);
|
||||
EXPECT_EQ("world", info->back().header.frame_id);
|
||||
}
|
||||
|
||||
/**
|
||||
* mapPath and mapGraph carry the optimized graph after every update: the trajectory for
|
||||
* display, and the graph with its links for the nodes that assemble maps from it.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, publishes_the_graph_after_every_update)
|
||||
{
|
||||
makeNode();
|
||||
std::shared_ptr<Collector<nav_msgs::msg::Path>> path =
|
||||
collect<nav_msgs::msg::Path>("mapPath");
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::MapGraph>> graph =
|
||||
collect<rtabmap_msgs::msg::MapGraph>("mapGraph",
|
||||
rclcpp::QoS(1).reliable().transient_local());
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
ASSERT_TRUE(waitForPublisher(path->subscription));
|
||||
ASSERT_TRUE(waitForPublisher(graph->subscription));
|
||||
|
||||
driveStraight(odom, info, 3);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() {
|
||||
return !path->empty() && path->back().poses.size() == 3 &&
|
||||
!graph->empty() && graph->back().poses_id.size() == 3; }));
|
||||
EXPECT_EQ("map", path->back().header.frame_id);
|
||||
EXPECT_NEAR(1.0, path->back().poses[2].pose.position.x, 1e-4);
|
||||
EXPECT_EQ(2u, graph->back().links.size());
|
||||
}
|
||||
|
||||
/**
|
||||
* localization_pose is the robot's pose in the map frame -- map -> odom composed with
|
||||
* the odometry -- published on every update. While mapping, its covariance is the
|
||||
* odometry's accumulated along the graph, so it grows with distance until a loop closure
|
||||
* brings it back down.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, publishes_the_pose_in_the_map_frame)
|
||||
{
|
||||
makeNode();
|
||||
std::shared_ptr<Collector<geometry_msgs::msg::PoseWithCovarianceStamped>> pose =
|
||||
collect<geometry_msgs::msg::PoseWithCovarianceStamped>("localization_pose");
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
ASSERT_TRUE(waitForPublisher(pose->subscription));
|
||||
|
||||
driveStraight(odom, info, 2);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return pose->size() == 2; }));
|
||||
EXPECT_EQ("map", pose->back().header.frame_id);
|
||||
EXPECT_NEAR(0.5, pose->back().pose.pose.position.x, 1e-4);
|
||||
EXPECT_GT(pose->back().pose.covariance[0], 0.0);
|
||||
EXPECT_LT(pose->back().pose.covariance[0], 1.0) << "not the 9999 of an unknown pose";
|
||||
}
|
||||
|
||||
/// pub_loc_pose_only_when_localizing holds it back until a loop closure has localized.
|
||||
TEST_F(CoreWrapperMappingTest, pub_loc_pose_only_when_localizing_holds_back_the_pose)
|
||||
{
|
||||
makeNode({rclcpp::Parameter("pub_loc_pose_only_when_localizing", true)});
|
||||
std::shared_ptr<Collector<geometry_msgs::msg::PoseWithCovarianceStamped>> pose =
|
||||
collect<geometry_msgs::msg::PoseWithCovarianceStamped>("localization_pose");
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
ASSERT_TRUE(waitForPublisher(pose->subscription));
|
||||
|
||||
driveStraight(odom, info, 2);
|
||||
spinFor(std::chrono::milliseconds(200));
|
||||
|
||||
EXPECT_TRUE(pose->empty());
|
||||
}
|
||||
|
||||
/**
|
||||
* The map survives a restart: the database saved on shutdown is reopened, the next update
|
||||
* starts a new session in it, and the new nodes carry on numbering after the old ones.
|
||||
*/
|
||||
TEST_F(CoreWrapperMappingTest, continues_the_saved_map_after_a_restart)
|
||||
{
|
||||
{
|
||||
makeNode();
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
driveStraight(odom, info, 2);
|
||||
}
|
||||
destroyNode();
|
||||
|
||||
makeNode();
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
|
||||
driveStraight(odom, info, 2, 0.5, 10.0);
|
||||
|
||||
EXPECT_EQ(3, info->front().ref_id);
|
||||
EXPECT_EQ(std::vector<int>({0, 0, 1, 1}), mapIds());
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
} // namespace rtabmap_slam_test
|
||||
@@ -0,0 +1,308 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <fstream>
|
||||
#include <sstream>
|
||||
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
|
||||
#include "core_wrapper_fixture.hpp"
|
||||
|
||||
namespace rtabmap_slam_test {
|
||||
|
||||
namespace {
|
||||
|
||||
::testing::Environment * const kRclcppEnv = registerRclcppEnvironment();
|
||||
|
||||
using rtabmap::Parameters;
|
||||
|
||||
class CoreWrapperParametersTest : public CoreWrapperTest
|
||||
{
|
||||
protected:
|
||||
/// RTAB-Map's own default for @p key, spelled the way the node stores it.
|
||||
static std::string rtabmapDefault(const std::string & key)
|
||||
{
|
||||
return Parameters::getDefaultParameters().at(key);
|
||||
}
|
||||
|
||||
static std::string readFile(const std::string & path)
|
||||
{
|
||||
std::ifstream in(path);
|
||||
std::stringstream s;
|
||||
s << in.rdbuf();
|
||||
return s.str();
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* Every RTAB-Map parameter is a ROS parameter under its own name, declared as a string --
|
||||
* that is how RTAB-Map's own parameter map stores them, whatever the value looks like.
|
||||
* The odometry ones are left out: they belong to the odometry nodes, and declaring them
|
||||
* here would suggest that setting them on rtabmap does something.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, declares_rtabmap_parameters_as_strings_except_odometry)
|
||||
{
|
||||
makeNode();
|
||||
|
||||
ASSERT_TRUE(node_->has_parameter(Parameters::kRtabmapDetectionRate()));
|
||||
EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING,
|
||||
node_->get_parameter(Parameters::kRtabmapDetectionRate()).get_type());
|
||||
EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING,
|
||||
node_->get_parameter(Parameters::kMemIncrementalMemory()).get_type());
|
||||
|
||||
EXPECT_FALSE(node_->has_parameter(Parameters::kOdomStrategy()));
|
||||
EXPECT_FALSE(node_->has_parameter(Parameters::kOdomResetCountdown()));
|
||||
EXPECT_FALSE(node_->has_parameter(Parameters::kOdomF2MMaxSize()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Two defaults differ from RTAB-Map's own: the occupancy grid is built by default, since
|
||||
* on a robot that is what the map is for, and the working directory is ~/.ros, or
|
||||
* $ROS_HOME, instead of RTAB-Map's own.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, builds_the_occupancy_grid_by_default)
|
||||
{
|
||||
ASSERT_FALSE(Parameters::defaultRGBDCreateOccupancyGrid())
|
||||
<< "RTAB-Map's own default changed: this test no longer shows a difference";
|
||||
|
||||
makeNode();
|
||||
|
||||
EXPECT_EQ("true", param(Parameters::kRGBDCreateOccupancyGrid()));
|
||||
EXPECT_EQ(dir(), param(Parameters::kRtabmapWorkingDirectory()));
|
||||
}
|
||||
|
||||
TEST_F(CoreWrapperParametersTest, applies_rtabmap_parameters_set_as_ros_parameters)
|
||||
{
|
||||
makeNode({rclcpp::Parameter(Parameters::kMemRehearsalSimilarity(), "0.45"),
|
||||
rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), "0.3")});
|
||||
|
||||
EXPECT_EQ("0.45", param(Parameters::kMemRehearsalSimilarity()));
|
||||
EXPECT_EQ("0.3", param(Parameters::kRGBDLinearUpdate()));
|
||||
}
|
||||
|
||||
/**
|
||||
* The declared type is string, so a value given with its natural type is refused at
|
||||
* construction rather than silently converted: the quoting in `-p "Rtabmap/DetectionRate:='2'"`
|
||||
* is not optional.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, refuses_a_rtabmap_parameter_given_as_a_number)
|
||||
{
|
||||
rclcpp::NodeOptions options;
|
||||
options.parameter_overrides(defaultParameters(
|
||||
{rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), 0.3)}));
|
||||
EXPECT_ANY_THROW(std::make_shared<rtabmap_slam::CoreWrapper>(options));
|
||||
}
|
||||
|
||||
TEST_F(CoreWrapperParametersTest, applies_rtabmap_parameters_passed_as_arguments)
|
||||
{
|
||||
makeNode({}, {"--Mem/RehearsalSimilarity", "0.21"});
|
||||
|
||||
EXPECT_EQ("0.21", param(Parameters::kMemRehearsalSimilarity()));
|
||||
}
|
||||
|
||||
/**
|
||||
* config_path is an INI file of RTAB-Map parameters, read at startup. The odometry
|
||||
* parameters in it are ignored, like everywhere else on this node, and ROS parameters
|
||||
* set explicitly win over the file.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, loads_parameters_from_config_path)
|
||||
{
|
||||
const std::string ini = dir() + "/config.ini";
|
||||
{
|
||||
std::ofstream out(ini);
|
||||
out << "[Core]\n"
|
||||
<< "Mem/RehearsalSimilarity = 0.44\n"
|
||||
<< "RGBD/LinearUpdate = 0.7\n"
|
||||
<< "Odom/Strategy = 1\n";
|
||||
}
|
||||
|
||||
makeNode({rclcpp::Parameter("config_path", ini),
|
||||
rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), "0.2")});
|
||||
|
||||
EXPECT_EQ("0.44", param(Parameters::kMemRehearsalSimilarity()));
|
||||
EXPECT_EQ("0.2", param(Parameters::kRGBDLinearUpdate()));
|
||||
EXPECT_FALSE(node_->has_parameter(Parameters::kOdomStrategy()));
|
||||
}
|
||||
|
||||
/// The node writes its parameters back to config_path when it shuts down.
|
||||
TEST_F(CoreWrapperParametersTest, saves_parameters_to_config_path_on_shutdown)
|
||||
{
|
||||
const std::string ini = dir() + "/generated.ini";
|
||||
ASSERT_FALSE(UFile::exists(ini));
|
||||
|
||||
makeNode({rclcpp::Parameter("config_path", ini),
|
||||
rclcpp::Parameter(Parameters::kMemRehearsalSimilarity(), "0.37")});
|
||||
destroyNode();
|
||||
|
||||
ASSERT_TRUE(UFile::exists(ini));
|
||||
rtabmap::ParametersMap saved;
|
||||
Parameters::readINI(ini, saved);
|
||||
ASSERT_TRUE(saved.count(Parameters::kMemRehearsalSimilarity()));
|
||||
EXPECT_EQ("0.37", saved.at(Parameters::kMemRehearsalSimilarity()));
|
||||
}
|
||||
|
||||
/**
|
||||
* A database remembers the parameters it was built with, and reopening it without
|
||||
* setting them again brings them back: a map made with a given configuration is reopened
|
||||
* with that configuration. What is set explicitly still wins.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, reuses_the_parameters_stored_in_the_database)
|
||||
{
|
||||
makeNode({rclcpp::Parameter(Parameters::kMemRehearsalSimilarity(), "0.33"),
|
||||
rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), "0.25")});
|
||||
destroyNode();
|
||||
ASSERT_TRUE(UFile::exists(databasePath()));
|
||||
|
||||
makeNode({rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), "0.15")});
|
||||
|
||||
EXPECT_EQ("0.33", param(Parameters::kMemRehearsalSimilarity()));
|
||||
EXPECT_EQ("0.15", param(Parameters::kRGBDLinearUpdate()));
|
||||
}
|
||||
|
||||
/// delete_db_on_start starts over: a new, empty database, with none of the old parameters.
|
||||
TEST_F(CoreWrapperParametersTest, delete_db_on_start_forgets_the_stored_parameters)
|
||||
{
|
||||
makeNode({rclcpp::Parameter(Parameters::kMemRehearsalSimilarity(), "0.33")});
|
||||
destroyNode();
|
||||
|
||||
makeNode({rclcpp::Parameter("delete_db_on_start", true)});
|
||||
|
||||
EXPECT_EQ(rtabmapDefault(Parameters::kMemRehearsalSimilarity()),
|
||||
param(Parameters::kMemRehearsalSimilarity()));
|
||||
}
|
||||
|
||||
/// `-d` and `--delete_db_on_start` as arguments do the same, the form launch files used.
|
||||
TEST_F(CoreWrapperParametersTest, delete_db_on_start_can_be_passed_as_an_argument)
|
||||
{
|
||||
makeNode({rclcpp::Parameter(Parameters::kMemRehearsalSimilarity(), "0.33")});
|
||||
destroyNode();
|
||||
|
||||
makeNode({}, {"-d"});
|
||||
|
||||
EXPECT_EQ(rtabmapDefault(Parameters::kMemRehearsalSimilarity()),
|
||||
param(Parameters::kMemRehearsalSimilarity()));
|
||||
}
|
||||
|
||||
/**
|
||||
* With no camera subscribed, there is nothing to extract visual words from: bag-of-words
|
||||
* loop closure detection is switched off rather than left to fail on every frame.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, odometry_only_input_disables_bag_of_words)
|
||||
{
|
||||
makeNode();
|
||||
|
||||
EXPECT_EQ("-1", param(Parameters::kKpMaxFeatures()));
|
||||
EXPECT_EQ(rtabmapDefault(Parameters::kRegStrategy()), param(Parameters::kRegStrategy()));
|
||||
}
|
||||
|
||||
/**
|
||||
* With a 2D lidar and no camera, the node reconfigures itself for it: the grid is built
|
||||
* from the scan without a range limit, loop closures are registered with ICP, proximity
|
||||
* detection merges the last 10 scans, and bag-of-words is off.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, laser_scan_input_switches_to_icp_and_scan_grid)
|
||||
{
|
||||
makeNode({rclcpp::Parameter("subscribe_scan", true)});
|
||||
|
||||
EXPECT_EQ("0", param(Parameters::kGridSensor()));
|
||||
EXPECT_EQ("0", param(Parameters::kGridRangeMax()));
|
||||
EXPECT_EQ("1", param(Parameters::kRegStrategy()));
|
||||
EXPECT_EQ("10", param(Parameters::kRGBDProximityPathMaxNeighbors()));
|
||||
EXPECT_EQ("-1", param(Parameters::kKpMaxFeatures()));
|
||||
}
|
||||
|
||||
/// None of those adjustments overrides a value set explicitly.
|
||||
TEST_F(CoreWrapperParametersTest, laser_scan_adjustments_keep_explicit_values)
|
||||
{
|
||||
makeNode({rclcpp::Parameter("subscribe_scan", true),
|
||||
rclcpp::Parameter(Parameters::kGridSensor(), "1"),
|
||||
rclcpp::Parameter(Parameters::kRGBDProximityPathMaxNeighbors(), "3")});
|
||||
|
||||
EXPECT_EQ("1", param(Parameters::kGridSensor()));
|
||||
EXPECT_EQ(rtabmapDefault(Parameters::kGridRangeMax()), param(Parameters::kGridRangeMax()))
|
||||
<< "the range limit is only lifted for a grid built from the scan";
|
||||
EXPECT_EQ("3", param(Parameters::kRGBDProximityPathMaxNeighbors()));
|
||||
}
|
||||
|
||||
/**
|
||||
* A 3D lidar gets the same treatment, with one difference: proximity detection registers
|
||||
* against the single nearest scan rather than merging ten.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, scan_cloud_input_switches_to_icp)
|
||||
{
|
||||
makeNode({rclcpp::Parameter("subscribe_scan_cloud", true)});
|
||||
|
||||
EXPECT_EQ("0", param(Parameters::kGridSensor()));
|
||||
EXPECT_EQ("1", param(Parameters::kRegStrategy()));
|
||||
EXPECT_EQ("1", param(Parameters::kRGBDProximityPathMaxNeighbors()));
|
||||
EXPECT_EQ("-1", param(Parameters::kKpMaxFeatures()));
|
||||
}
|
||||
|
||||
/**
|
||||
* A cloud flagged as 2D -- a 2D lidar published as a cloud -- is treated like a laser
|
||||
* scan, merging ten, whether ICP was selected explicitly or by the node's own switch to it
|
||||
* for lack of a camera.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, scan_cloud_flagged_2d_merges_scans_like_a_laser_scan)
|
||||
{
|
||||
makeNode({rclcpp::Parameter("subscribe_scan_cloud", true),
|
||||
rclcpp::Parameter("scan_cloud_is_2d", true),
|
||||
rclcpp::Parameter(Parameters::kRegStrategy(), "1")});
|
||||
EXPECT_EQ("10", param(Parameters::kRGBDProximityPathMaxNeighbors())) << "ICP selected explicitly";
|
||||
destroyNode();
|
||||
|
||||
makeNode({rclcpp::Parameter("subscribe_scan_cloud", true),
|
||||
rclcpp::Parameter("scan_cloud_is_2d", true),
|
||||
rclcpp::Parameter("delete_db_on_start", true)});
|
||||
EXPECT_EQ("1", param(Parameters::kRegStrategy())) << "switched to ICP by the node";
|
||||
EXPECT_EQ("10", param(Parameters::kRGBDProximityPathMaxNeighbors()));
|
||||
}
|
||||
|
||||
/**
|
||||
* A parameter RTAB-Map has renamed is still honoured under its old name, with a warning,
|
||||
* so that an old launch file keeps working. Old names are never declared, so this is only
|
||||
* possible by reading them from the overrides.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, migrates_a_renamed_parameter)
|
||||
{
|
||||
// g2o/PixelVariance became Optimizer/PixelVariance.
|
||||
ASSERT_TRUE(Parameters::getRemovedParameters().count("g2o/PixelVariance"));
|
||||
makeNode({rclcpp::Parameter("g2o/PixelVariance", "2.5")});
|
||||
|
||||
EXPECT_EQ("2.5", param(Parameters::kOptimizerPixelVariance()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Loaded in a component container with intra-process communication on, the node must
|
||||
* still start. Intra-process communication doesn't support transient local durability,
|
||||
* so the latched publishers (`latch`, on by default) opt out of it, while the others keep
|
||||
* the container's setting.
|
||||
*/
|
||||
TEST_F(CoreWrapperParametersTest, starts_with_intra_process_comms_whether_latching_or_not)
|
||||
{
|
||||
for(bool latch : {true, false})
|
||||
{
|
||||
SCOPED_TRACE(latch ? "latch" : "no latch");
|
||||
rclcpp::NodeOptions options;
|
||||
options.use_intra_process_comms(true);
|
||||
options.parameter_overrides(defaultParameters({rclcpp::Parameter("latch", latch)}));
|
||||
ASSERT_NO_THROW(node_ = addNode(std::make_shared<rtabmap_slam::CoreWrapper>(options)));
|
||||
|
||||
for(const std::string & topic : {std::string("mapGraph"), std::string("map")})
|
||||
{
|
||||
auto infos = node_->get_publishers_info_by_topic(node_->get_node_topics_interface()->resolve_topic_name(topic));
|
||||
ASSERT_EQ(1u, infos.size()) << topic;
|
||||
EXPECT_EQ(latch ? rclcpp::DurabilityPolicy::TransientLocal : rclcpp::DurabilityPolicy::Volatile,
|
||||
infos[0].qos_profile().durability()) << topic;
|
||||
}
|
||||
destroyNode();
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
} // namespace rtabmap_slam_test
|
||||
@@ -0,0 +1,400 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <nav_msgs/msg/path.hpp>
|
||||
#include <nav_msgs/srv/get_plan.hpp>
|
||||
#include <rosgraph_msgs/msg/clock.hpp>
|
||||
#include <std_msgs/msg/bool.hpp>
|
||||
|
||||
#include <rtabmap_msgs/msg/goal.hpp>
|
||||
#include <rtabmap_msgs/msg/path.hpp>
|
||||
#include <rtabmap_msgs/srv/get_plan.hpp>
|
||||
#include <rtabmap_msgs/srv/set_goal.hpp>
|
||||
#include <rtabmap_msgs/srv/set_label.hpp>
|
||||
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
|
||||
#include "core_wrapper_fixture.hpp"
|
||||
|
||||
namespace rtabmap_slam_test {
|
||||
|
||||
namespace {
|
||||
|
||||
::testing::Environment * const kRclcppEnv = registerRclcppEnvironment();
|
||||
|
||||
/**
|
||||
* Planning happens on the graph: a goal is a node (or a pose near one), the plan is the
|
||||
* chain of nodes leading to it, and the node hands the next one to reach to a local
|
||||
* planner on goal_out. These tests drive a straight corridor, x = 0 to 2 m in 0.5 m steps,
|
||||
* and plan back along it.
|
||||
*/
|
||||
class CoreWrapperPlanningTest : public CoreWrapperTest
|
||||
{
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
CoreWrapperTest::SetUp();
|
||||
makeNode(nodeParameters());
|
||||
goalOut_ = collect<geometry_msgs::msg::PoseStamped>("goal_out");
|
||||
goalReached_ = collect<std_msgs::msg::Bool>("goal_reached");
|
||||
globalPath_ = collect<nav_msgs::msg::Path>("global_path");
|
||||
globalPathNodes_ = collect<rtabmap_msgs::msg::Path>("global_path_nodes");
|
||||
info_ = collectInfo();
|
||||
odom_ = odomPublisher();
|
||||
ASSERT_TRUE(waitForPublisher(goalOut_->subscription));
|
||||
ASSERT_TRUE(waitForPublisher(goalReached_->subscription));
|
||||
ASSERT_TRUE(waitForPublisher(globalPath_->subscription));
|
||||
ASSERT_TRUE(waitForPublisher(globalPathNodes_->subscription));
|
||||
driveStraight(odom_, info_, 5); // nodes 1..5 at x = 0, 0.5, 1.0, 1.5, 2.0
|
||||
nextStamp_ = 6.0;
|
||||
}
|
||||
|
||||
rtabmap_msgs::srv::SetGoal::Response::SharedPtr setGoal(int id, const std::string & label = "")
|
||||
{
|
||||
rtabmap_msgs::srv::SetGoal::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::SetGoal::Request>();
|
||||
req->node_id = id;
|
||||
req->node_label = label;
|
||||
return call<rtabmap_msgs::srv::SetGoal>("set_goal", req);
|
||||
}
|
||||
|
||||
virtual std::vector<rclcpp::Parameter> nodeParameters() { return {}; }
|
||||
|
||||
/// Moves the robot to @p x and waits for the update to be processed.
|
||||
bool moveTo(double x)
|
||||
{
|
||||
const size_t before = info_->size();
|
||||
sendOdom(odom_, nextStamp_, x);
|
||||
nextStamp_ += 1.0;
|
||||
return spinUntil([&]() { return info_->size() > before; });
|
||||
}
|
||||
|
||||
std::shared_ptr<Collector<geometry_msgs::msg::PoseStamped>> goalOut_;
|
||||
std::shared_ptr<Collector<std_msgs::msg::Bool>> goalReached_;
|
||||
std::shared_ptr<Collector<nav_msgs::msg::Path>> globalPath_;
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Path>> globalPathNodes_;
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info_;
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_;
|
||||
double nextStamp_ = 0.0;
|
||||
};
|
||||
|
||||
/**
|
||||
* set_goal plans to a node and returns the path; the next node to reach goes out on
|
||||
* goal_out, and the whole plan on global_path and global_path_nodes.
|
||||
*/
|
||||
TEST_F(CoreWrapperPlanningTest, set_goal_plans_to_a_node)
|
||||
{
|
||||
rtabmap_msgs::srv::SetGoal::Response::SharedPtr res = setGoal(1);
|
||||
|
||||
ASSERT_TRUE(res.get() != nullptr);
|
||||
ASSERT_FALSE(res->path_ids.empty());
|
||||
EXPECT_EQ(1, res->path_ids.back());
|
||||
EXPECT_EQ(res->path_ids.size(), res->path_poses.size());
|
||||
EXPECT_NEAR(0.0, res->path_poses.back().position.x, 1e-4);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalOut_->empty(); }));
|
||||
EXPECT_EQ("map", goalOut_->back().header.frame_id);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !globalPath_->empty() && !globalPathNodes_->empty(); }));
|
||||
EXPECT_EQ(res->path_ids.size(), globalPath_->back().poses.size());
|
||||
EXPECT_EQ(res->path_ids, globalPathNodes_->back().node_ids);
|
||||
}
|
||||
|
||||
/// The goal can be named by its label instead of its id.
|
||||
TEST_F(CoreWrapperPlanningTest, set_goal_plans_to_a_label)
|
||||
{
|
||||
rtabmap_msgs::srv::SetLabel::Request::SharedPtr label =
|
||||
std::make_shared<rtabmap_msgs::srv::SetLabel::Request>();
|
||||
label->node_id = 2;
|
||||
label->node_label = "kitchen";
|
||||
ASSERT_TRUE(call<rtabmap_msgs::srv::SetLabel>("set_label", label).get() != nullptr);
|
||||
|
||||
rtabmap_msgs::srv::SetGoal::Response::SharedPtr res = setGoal(0, "kitchen");
|
||||
|
||||
ASSERT_TRUE(res.get() != nullptr);
|
||||
ASSERT_FALSE(res->path_ids.empty());
|
||||
EXPECT_EQ(2, res->path_ids.back());
|
||||
}
|
||||
|
||||
/// A goal on a node that does not exist fails, and says so on goal_reached.
|
||||
TEST_F(CoreWrapperPlanningTest, reports_failure_for_an_unknown_node)
|
||||
{
|
||||
rtabmap_msgs::srv::SetGoal::Response::SharedPtr res = setGoal(42);
|
||||
|
||||
ASSERT_TRUE(res.get() != nullptr);
|
||||
EXPECT_TRUE(res->path_ids.empty());
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
|
||||
EXPECT_FALSE(goalReached_->back().data);
|
||||
}
|
||||
|
||||
TEST_F(CoreWrapperPlanningTest, reports_failure_for_an_unknown_label)
|
||||
{
|
||||
rtabmap_msgs::srv::SetGoal::Response::SharedPtr res = setGoal(0, "nowhere");
|
||||
|
||||
ASSERT_TRUE(res.get() != nullptr);
|
||||
EXPECT_TRUE(res->path_ids.empty());
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
|
||||
EXPECT_FALSE(goalReached_->back().data);
|
||||
}
|
||||
|
||||
/// A goal on the node the robot is already at is reached straight away.
|
||||
TEST_F(CoreWrapperPlanningTest, reports_a_goal_already_reached)
|
||||
{
|
||||
rtabmap_msgs::srv::SetGoal::Response::SharedPtr res = setGoal(5);
|
||||
|
||||
ASSERT_TRUE(res.get() != nullptr);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
|
||||
EXPECT_TRUE(goalReached_->back().data);
|
||||
}
|
||||
|
||||
/**
|
||||
* The plan is followed as the robot moves: once it is back at the goal node,
|
||||
* goal_reached says so and the goal is cleared.
|
||||
*
|
||||
* The last step stops 5 cm short of the origin on purpose: an odometry pose of exactly
|
||||
* identity after a non-identity one is how an odometry reset looks, and would start a new
|
||||
* map instead.
|
||||
*/
|
||||
TEST_F(CoreWrapperPlanningTest, reports_the_goal_reached_when_the_robot_gets_there)
|
||||
{
|
||||
ASSERT_TRUE(setGoal(1).get() != nullptr);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalOut_->empty(); }));
|
||||
ASSERT_TRUE(goalReached_->empty());
|
||||
|
||||
for(double x : {1.5, 1.0, 0.5, 0.05})
|
||||
{
|
||||
ASSERT_TRUE(moveTo(x));
|
||||
}
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
|
||||
EXPECT_TRUE(goalReached_->back().data);
|
||||
}
|
||||
|
||||
/// cancel_goal abandons the plan, which counts as not reaching it.
|
||||
TEST_F(CoreWrapperPlanningTest, cancel_goal_abandons_the_plan)
|
||||
{
|
||||
ASSERT_TRUE(setGoal(1).get() != nullptr);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalOut_->empty(); }));
|
||||
|
||||
ASSERT_TRUE(callEmpty("cancel_goal"));
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
|
||||
EXPECT_FALSE(goalReached_->back().data);
|
||||
const size_t sent = goalOut_->size();
|
||||
ASSERT_TRUE(moveTo(1.5));
|
||||
spinFor(std::chrono::milliseconds(200));
|
||||
EXPECT_EQ(sent, goalOut_->size()) << "no new goal after cancelling";
|
||||
}
|
||||
|
||||
/**
|
||||
* A pose on the goal topic within RGBD/LocalRadius of the robot is not planned through
|
||||
* the graph at all: the plan is the node the robot is at, followed by the pose itself as
|
||||
* a last waypoint with node id 0, and it is left to the local planner to get there.
|
||||
*/
|
||||
TEST_F(CoreWrapperPlanningTest, plans_to_a_pose_on_the_goal_topic)
|
||||
{
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal =
|
||||
helper()->create_publisher<geometry_msgs::msg::PoseStamped>("goal", 1);
|
||||
ASSERT_TRUE(waitForSubscriber(goal));
|
||||
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.header.frame_id = "map";
|
||||
pose.pose.position.x = 0.1;
|
||||
pose.pose.orientation.w = 1.0;
|
||||
goal->publish(pose);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return !globalPathNodes_->empty(); }));
|
||||
EXPECT_EQ(std::vector<int>({5, 0}), globalPathNodes_->back().node_ids);
|
||||
EXPECT_NEAR(0.1, globalPathNodes_->back().poses.back().position.x, 1e-4);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalOut_->empty(); }));
|
||||
}
|
||||
|
||||
/**
|
||||
* Beyond RGBD/LocalRadius, a pose goal is planned through the graph to the node nearest
|
||||
* to it, and the pose is appended after that node.
|
||||
*/
|
||||
TEST_F(CoreWrapperPlanningTest, plans_through_the_graph_beyond_the_local_radius)
|
||||
{
|
||||
ASSERT_TRUE(node_->set_parameter(
|
||||
rclcpp::Parameter(rtabmap::Parameters::kRGBDLocalRadius(), "1.0")).successful);
|
||||
spinFor(std::chrono::milliseconds(300)); // applied on the parameter event
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal =
|
||||
helper()->create_publisher<geometry_msgs::msg::PoseStamped>("goal", 1);
|
||||
ASSERT_TRUE(waitForSubscriber(goal));
|
||||
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.header.frame_id = "map";
|
||||
pose.pose.position.x = 0.1;
|
||||
pose.pose.orientation.w = 1.0;
|
||||
goal->publish(pose);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return !globalPathNodes_->empty(); }));
|
||||
EXPECT_EQ(std::vector<int>({5, 4, 3, 2, 1, 0}), globalPathNodes_->back().node_ids);
|
||||
EXPECT_NEAR(0.1, globalPathNodes_->back().poses.back().position.x, 1e-4);
|
||||
}
|
||||
|
||||
/**
|
||||
* A goal in a frame the node cannot transform to the map frame is refused rather than
|
||||
* taken as a map-frame pose.
|
||||
*/
|
||||
/**
|
||||
* The same corridor, with the node on the tests' own clock (use_sim_time): map -> odom is
|
||||
* then stamped in the odometry's time base, and with tf_tolerance at 0, exactly at the
|
||||
* clock's time. Only for tests whose TF lookups never have to wait: with a clock that only
|
||||
* moves when told to, a lookup waiting for a transform that is not there -- a goal in an
|
||||
* unknown frame, say -- would wait forever.
|
||||
*/
|
||||
class CoreWrapperPlanningSimTimeTest : public CoreWrapperPlanningTest
|
||||
{
|
||||
protected:
|
||||
std::vector<rclcpp::Parameter> nodeParameters() override
|
||||
{
|
||||
return {rclcpp::Parameter("use_sim_time", true),
|
||||
rclcpp::Parameter("tf_tolerance", 0.0)};
|
||||
}
|
||||
|
||||
/// Sets the node's clock to @p seconds.
|
||||
void setClock(double seconds)
|
||||
{
|
||||
if(!clock_)
|
||||
{
|
||||
clock_ = helper()->create_publisher<rosgraph_msgs::msg::Clock>("/clock", rclcpp::ClockQoS());
|
||||
ASSERT_TRUE(waitForSubscriber(clock_));
|
||||
}
|
||||
rosgraph_msgs::msg::Clock msg;
|
||||
msg.clock = stampOf(seconds);
|
||||
clock_->publish(msg);
|
||||
spinFor(std::chrono::milliseconds(50));
|
||||
}
|
||||
|
||||
rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_;
|
||||
};
|
||||
|
||||
TEST_F(CoreWrapperPlanningSimTimeTest, transforms_a_goal_in_the_robot_frame_to_the_map_frame)
|
||||
{
|
||||
// Turn the robot to face +y where it stands, at x = 2.
|
||||
const size_t before = info_->size();
|
||||
sendOdom(odom_, nextStamp_, 2.0, 0.0, M_PI/2.0);
|
||||
ASSERT_TRUE(spinUntil([&]() { return info_->size() > before; }));
|
||||
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal =
|
||||
helper()->create_publisher<geometry_msgs::msg::PoseStamped>("goal", 1);
|
||||
ASSERT_TRUE(waitForSubscriber(goal));
|
||||
|
||||
// The goal is looked up through map -> odom -> base_link at its stamp: bring the clock
|
||||
// to the turn's stamp, and wait for map -> odom to be published at it.
|
||||
const double stamp = nextStamp_;
|
||||
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
|
||||
collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
|
||||
setClock(stamp);
|
||||
ASSERT_TRUE(spinUntil([&]() {
|
||||
for(const tf2_msgs::msg::TFMessage::ConstSharedPtr & msg : tf->messages)
|
||||
{
|
||||
for(const geometry_msgs::msg::TransformStamped & t : msg->transforms)
|
||||
{
|
||||
if(t.child_frame_id == "odom" && rclcpp::Time(t.header.stamp) == stampOf(stamp))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
return false; }));
|
||||
|
||||
// 1 m straight ahead of the robot, facing where it faces.
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.header.frame_id = "base_link";
|
||||
pose.header.stamp = stampOf(stamp);
|
||||
pose.pose.position.x = 1.0;
|
||||
pose.pose.orientation.w = 1.0;
|
||||
goal->publish(pose);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return !globalPathNodes_->empty(); }));
|
||||
const geometry_msgs::msg::Pose & target = globalPathNodes_->back().poses.back();
|
||||
EXPECT_EQ(0, globalPathNodes_->back().node_ids.back()) << "the pose itself, last";
|
||||
EXPECT_NEAR(2.0, target.position.x, 1e-3);
|
||||
EXPECT_NEAR(1.0, target.position.y, 1e-3);
|
||||
EXPECT_NEAR(std::sin(M_PI/4.0), target.orientation.z, 1e-3) << "facing +y";
|
||||
EXPECT_NEAR(std::cos(M_PI/4.0), target.orientation.w, 1e-3);
|
||||
EXPECT_TRUE(goalReached_->empty());
|
||||
}
|
||||
|
||||
TEST_F(CoreWrapperPlanningTest, refuses_a_goal_in_an_unknown_frame)
|
||||
{
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal =
|
||||
helper()->create_publisher<geometry_msgs::msg::PoseStamped>("goal", 1);
|
||||
ASSERT_TRUE(waitForSubscriber(goal));
|
||||
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.header.frame_id = "nowhere";
|
||||
pose.header.stamp = stampOf(5.0);
|
||||
pose.pose.orientation.w = 1.0;
|
||||
goal->publish(pose);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
|
||||
EXPECT_FALSE(goalReached_->back().data);
|
||||
EXPECT_TRUE(goalOut_->empty());
|
||||
}
|
||||
|
||||
/// goal_node takes a node id or a label, and refuses a message with neither.
|
||||
TEST_F(CoreWrapperPlanningTest, goal_node_topic_plans_to_a_node)
|
||||
{
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::Goal>::SharedPtr goal =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::Goal>("goal_node", 1);
|
||||
ASSERT_TRUE(waitForSubscriber(goal));
|
||||
|
||||
rtabmap_msgs::msg::Goal msg;
|
||||
goal->publish(msg);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
|
||||
EXPECT_FALSE(goalReached_->back().data);
|
||||
|
||||
msg.node_id = 2;
|
||||
goal->publish(msg);
|
||||
ASSERT_TRUE(spinUntil([&]() { return !globalPathNodes_->empty(); }));
|
||||
EXPECT_EQ(2, globalPathNodes_->back().node_ids.back());
|
||||
}
|
||||
|
||||
/**
|
||||
* get_plan only computes a plan -- nothing is followed and nothing is published -- and
|
||||
* returns it in the goal's frame.
|
||||
*/
|
||||
TEST_F(CoreWrapperPlanningTest, get_plan_computes_without_following)
|
||||
{
|
||||
nav_msgs::srv::GetPlan::Request::SharedPtr req =
|
||||
std::make_shared<nav_msgs::srv::GetPlan::Request>();
|
||||
req->goal.header.frame_id = "map";
|
||||
req->goal.pose.position.x = 0.0;
|
||||
req->goal.pose.orientation.w = 1.0;
|
||||
nav_msgs::srv::GetPlan::Response::SharedPtr res =
|
||||
call<nav_msgs::srv::GetPlan>("get_plan", req);
|
||||
|
||||
ASSERT_TRUE(res.get() != nullptr);
|
||||
ASSERT_FALSE(res->plan.poses.empty());
|
||||
EXPECT_EQ("map", res->plan.header.frame_id);
|
||||
EXPECT_NEAR(0.0, res->plan.poses.back().pose.position.x, 1e-4);
|
||||
spinFor(std::chrono::milliseconds(200));
|
||||
EXPECT_TRUE(goalOut_->empty());
|
||||
}
|
||||
|
||||
/// get_plan_nodes is the same, with the node ids along the plan, to a node or a pose.
|
||||
TEST_F(CoreWrapperPlanningTest, get_plan_nodes_returns_the_node_ids)
|
||||
{
|
||||
rtabmap_msgs::srv::GetPlan::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::GetPlan::Request>();
|
||||
req->goal_node = 2;
|
||||
rtabmap_msgs::srv::GetPlan::Response::SharedPtr res =
|
||||
call<rtabmap_msgs::srv::GetPlan>("get_plan_nodes", req);
|
||||
|
||||
ASSERT_TRUE(res.get() != nullptr);
|
||||
ASSERT_FALSE(res->plan.node_ids.empty());
|
||||
EXPECT_EQ(2, res->plan.node_ids.back());
|
||||
EXPECT_EQ(res->plan.node_ids.size(), res->plan.poses.size());
|
||||
EXPECT_TRUE(goalOut_->empty());
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
} // namespace rtabmap_slam_test
|
||||
@@ -0,0 +1,892 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <tuple>
|
||||
#include <utility>
|
||||
|
||||
#include <rtabmap_msgs/msg/rgbd_image.hpp>
|
||||
#include <rtabmap_msgs/msg/sensor_data.hpp>
|
||||
#include <rtabmap_msgs/srv/add_link.hpp>
|
||||
#include <rtabmap_msgs/srv/get_map2.hpp>
|
||||
#include <rtabmap_msgs/srv/get_nodes_in_radius.hpp>
|
||||
#include <rtabmap_msgs/srv/list_labels.hpp>
|
||||
#include <rtabmap_msgs/srv/load_database.hpp>
|
||||
#include <rtabmap_msgs/srv/publish_map.hpp>
|
||||
#include <rtabmap_msgs/srv/remove_label.hpp>
|
||||
#include <rtabmap_msgs/srv/set_label.hpp>
|
||||
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap/core/GlobalDescriptor.h>
|
||||
#include <rtabmap/core/SensorData.h>
|
||||
#include <rtabmap/core/Link.h>
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
#include <tf2_ros/buffer.hpp>
|
||||
|
||||
#include "core_wrapper_fixture.hpp"
|
||||
|
||||
namespace rtabmap_slam_test {
|
||||
|
||||
namespace {
|
||||
|
||||
::testing::Environment * const kRclcppEnv = registerRclcppEnvironment();
|
||||
|
||||
using rtabmap::Parameters;
|
||||
|
||||
class CoreWrapperServicesTest : public CoreWrapperTest
|
||||
{
|
||||
protected:
|
||||
/// A node with @p count nodes already in its map, 0.5 m apart along x.
|
||||
void makeMap(int count = 3, const std::vector<rclcpp::Parameter> & params = {})
|
||||
{
|
||||
makeNode(params);
|
||||
info_ = collectInfo();
|
||||
odom_ = odomPublisher();
|
||||
driveStraight(odom_, info_, count);
|
||||
}
|
||||
|
||||
/// Sends one more update, @p x meters along, and waits for it to be processed.
|
||||
bool updateAt(double stamp, double x)
|
||||
{
|
||||
const size_t before = info_->size();
|
||||
sendOdom(odom_, stamp, x);
|
||||
return spinUntil([&]() { return info_->size() > before; });
|
||||
}
|
||||
|
||||
rtabmap_msgs::srv::ListLabels::Response::SharedPtr listLabels()
|
||||
{
|
||||
return call<rtabmap_msgs::srv::ListLabels>("list_labels");
|
||||
}
|
||||
|
||||
bool setLabel(int id, const std::string & label)
|
||||
{
|
||||
rtabmap_msgs::srv::SetLabel::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::SetLabel::Request>();
|
||||
req->node_id = id;
|
||||
req->node_label = label;
|
||||
return call<rtabmap_msgs::srv::SetLabel>("set_label", req).get() != nullptr;
|
||||
}
|
||||
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info_;
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_;
|
||||
};
|
||||
|
||||
/// Every service the node offers is advertised under its own name, /rtabmap/<service>.
|
||||
TEST_F(CoreWrapperServicesTest, advertises_its_services_under_its_name)
|
||||
{
|
||||
makeNode();
|
||||
const std::vector<std::string> expected = {
|
||||
"update_parameters", "reset", "pause", "resume", "load_database",
|
||||
"trigger_new_map", "backup", "detect_more_loop_closures", "global_bundle_adjustment",
|
||||
"cleanup_local_grids", "set_mode_localization", "set_mode_mapping", "get_node_data",
|
||||
"get_map_data", "get_map_data2", "get_map", "get_prob_map", "publish_map",
|
||||
"get_plan", "get_plan_nodes", "set_goal", "cancel_goal", "set_label", "list_labels",
|
||||
"remove_label", "add_link", "get_nodes_in_radius",
|
||||
"log_debug", "log_info", "log_warning", "log_error"};
|
||||
|
||||
std::map<std::string, std::vector<std::string>> advertised;
|
||||
ASSERT_TRUE(spinUntil([&]() {
|
||||
advertised = helper()->get_service_names_and_types_by_node("rtabmap", "/");
|
||||
return advertised.size() >= expected.size(); }));
|
||||
for(const std::string & name : expected)
|
||||
{
|
||||
EXPECT_TRUE(advertised.count("/rtabmap/" + name)) << "/rtabmap/" << name;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* pause stops the node from taking any input at all -- the odometry is dropped, not
|
||||
* queued -- and resume picks up from the next message. The state is mirrored in the
|
||||
* is_rtabmap_paused parameter.
|
||||
*/
|
||||
TEST_F(CoreWrapperServicesTest, pause_drops_input_until_resume)
|
||||
{
|
||||
makeMap(1);
|
||||
|
||||
ASSERT_TRUE(callEmpty("pause"));
|
||||
EXPECT_TRUE(node_->get_parameter("is_rtabmap_paused").as_bool());
|
||||
sendOdom(odom_, 2.0, 0.5);
|
||||
spinFor(std::chrono::milliseconds(500));
|
||||
EXPECT_EQ(1u, info_->size());
|
||||
|
||||
ASSERT_TRUE(callEmpty("resume"));
|
||||
EXPECT_FALSE(node_->get_parameter("is_rtabmap_paused").as_bool());
|
||||
EXPECT_TRUE(updateAt(3.0, 1.0));
|
||||
EXPECT_EQ(2u, getGraph().graph.poses_id.size());
|
||||
}
|
||||
|
||||
/// is_rtabmap_paused starts the node paused, waiting for a resume.
|
||||
TEST_F(CoreWrapperServicesTest, is_rtabmap_paused_starts_paused)
|
||||
{
|
||||
makeNode({rclcpp::Parameter("is_rtabmap_paused", true)});
|
||||
info_ = collectInfo();
|
||||
odom_ = odomPublisher();
|
||||
|
||||
sendOdom(odom_, 1.0, 0.0);
|
||||
spinFor(std::chrono::milliseconds(500));
|
||||
EXPECT_TRUE(info_->empty());
|
||||
|
||||
ASSERT_TRUE(callEmpty("resume"));
|
||||
EXPECT_TRUE(updateAt(2.0, 0.5));
|
||||
}
|
||||
|
||||
/// reset erases the map, in memory and in the database, and numbering starts over.
|
||||
TEST_F(CoreWrapperServicesTest, reset_erases_the_map)
|
||||
{
|
||||
makeMap(3);
|
||||
|
||||
ASSERT_TRUE(callEmpty("reset"));
|
||||
EXPECT_TRUE(getGraph().graph.poses_id.empty());
|
||||
|
||||
ASSERT_TRUE(updateAt(10.0, 5.0));
|
||||
EXPECT_EQ(1, info_->back().ref_id);
|
||||
}
|
||||
|
||||
/// trigger_new_map starts a new session in the same database; the old one is kept.
|
||||
TEST_F(CoreWrapperServicesTest, trigger_new_map_starts_a_new_session)
|
||||
{
|
||||
makeMap(2);
|
||||
|
||||
ASSERT_TRUE(callEmpty("trigger_new_map"));
|
||||
ASSERT_TRUE(updateAt(10.0, 1.0));
|
||||
|
||||
EXPECT_EQ(std::vector<int>({0, 0, 1}), mapIds());
|
||||
}
|
||||
|
||||
/**
|
||||
* Labels name nodes, so a goal can be given as "kitchen" rather than as an id. Node 0
|
||||
* means the latest node.
|
||||
*/
|
||||
TEST_F(CoreWrapperServicesTest, labels_nodes)
|
||||
{
|
||||
makeMap(3);
|
||||
|
||||
ASSERT_TRUE(setLabel(1, "kitchen"));
|
||||
ASSERT_TRUE(setLabel(0, "door"));
|
||||
|
||||
rtabmap_msgs::srv::ListLabels::Response::SharedPtr labels = listLabels();
|
||||
ASSERT_TRUE(labels.get() != nullptr);
|
||||
ASSERT_EQ(2u, labels->ids.size());
|
||||
EXPECT_EQ(1, labels->ids[0]);
|
||||
EXPECT_EQ("kitchen", labels->labels[0]);
|
||||
EXPECT_EQ(3, labels->ids[1]);
|
||||
EXPECT_EQ("door", labels->labels[1]);
|
||||
EXPECT_EQ("kitchen", getNode(1).label);
|
||||
}
|
||||
|
||||
TEST_F(CoreWrapperServicesTest, removes_a_label)
|
||||
{
|
||||
makeMap(2);
|
||||
ASSERT_TRUE(setLabel(1, "kitchen"));
|
||||
ASSERT_TRUE(setLabel(2, "door"));
|
||||
|
||||
rtabmap_msgs::srv::RemoveLabel::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::RemoveLabel::Request>();
|
||||
req->label = "kitchen";
|
||||
ASSERT_TRUE(call<rtabmap_msgs::srv::RemoveLabel>("remove_label", req).get() != nullptr);
|
||||
|
||||
rtabmap_msgs::srv::ListLabels::Response::SharedPtr labels = listLabels();
|
||||
ASSERT_TRUE(labels.get() != nullptr);
|
||||
EXPECT_EQ(std::vector<std::string>({"door"}), labels->labels);
|
||||
}
|
||||
|
||||
/// A label is unique in the map: setting it on another node is refused.
|
||||
TEST_F(CoreWrapperServicesTest, refuses_a_duplicate_label)
|
||||
{
|
||||
makeMap(2);
|
||||
ASSERT_TRUE(setLabel(1, "kitchen"));
|
||||
ASSERT_TRUE(setLabel(2, "kitchen"));
|
||||
|
||||
rtabmap_msgs::srv::ListLabels::Response::SharedPtr labels = listLabels();
|
||||
ASSERT_TRUE(labels.get() != nullptr);
|
||||
EXPECT_EQ(std::vector<int>({1}), labels->ids);
|
||||
}
|
||||
|
||||
/// get_node_data with no id returns the latest node.
|
||||
TEST_F(CoreWrapperServicesTest, get_node_data_defaults_to_the_latest_node)
|
||||
{
|
||||
makeMap(3);
|
||||
|
||||
rtabmap_msgs::srv::GetNodeData::Response::SharedPtr res =
|
||||
call<rtabmap_msgs::srv::GetNodeData>("get_node_data");
|
||||
ASSERT_TRUE(res.get() != nullptr);
|
||||
ASSERT_EQ(1u, res->data.size());
|
||||
EXPECT_EQ(3, res->data[0].id);
|
||||
EXPECT_NEAR(1.0, res->data[0].pose.position.x, 1e-4);
|
||||
}
|
||||
|
||||
//==========================================================================================
|
||||
// What each map service returns, payload by payload
|
||||
//==========================================================================================
|
||||
|
||||
/**
|
||||
* Every kind of data a node can hold, as one of the map services returned it for node 1.
|
||||
* The graph itself (poses, links) is returned whatever is asked for.
|
||||
*/
|
||||
struct Payloads
|
||||
{
|
||||
bool images = false;
|
||||
bool scans = false;
|
||||
bool userData = false;
|
||||
bool grids = false;
|
||||
bool words = false;
|
||||
bool globalDescriptors = false;
|
||||
|
||||
static Payloads of(const rtabmap_msgs::msg::Node & node)
|
||||
{
|
||||
Payloads p;
|
||||
p.images = !node.data.left_compressed.empty() && !node.data.right_compressed.empty();
|
||||
p.scans = !node.data.laser_scan_compressed.empty();
|
||||
p.userData = !node.data.user_data.empty();
|
||||
p.grids = !node.data.grid_obstacles.empty() || !node.data.grid_empty_cells.empty();
|
||||
p.words = !node.word_id_keys.empty();
|
||||
p.globalDescriptors = !node.data.global_descriptors.empty();
|
||||
return p;
|
||||
}
|
||||
|
||||
bool operator==(const Payloads & o) const
|
||||
{
|
||||
return images == o.images && scans == o.scans && userData == o.userData &&
|
||||
grids == o.grids && words == o.words && globalDescriptors == o.globalDescriptors;
|
||||
}
|
||||
};
|
||||
|
||||
std::ostream & operator<<(std::ostream & os, const Payloads & p)
|
||||
{
|
||||
return os << "{images=" << p.images << " scans=" << p.scans << " user_data=" << p.userData
|
||||
<< " grids=" << p.grids << " words=" << p.words
|
||||
<< " global_descriptors=" << p.globalDescriptors << "}";
|
||||
}
|
||||
|
||||
/// How the sensor data reaches the node.
|
||||
enum class MapInput
|
||||
{
|
||||
RgbdAndScan, ///< rgbd_image and scan, synchronized with odom; user data on user_data_async
|
||||
SensorData ///< the same data packed in one rtabmap_msgs/SensorData, user data included
|
||||
};
|
||||
|
||||
std::string toString(MapInput input)
|
||||
{
|
||||
return input == MapInput::RgbdAndScan ? "rgbd_and_scan" : "sensor_data";
|
||||
}
|
||||
|
||||
/**
|
||||
* A map whose nodes carry everything at once: an RGB-D camera, from which visual words
|
||||
* are extracted, with a global descriptor, a 2D lidar, from which the local occupancy
|
||||
* grid is built, and user data. Built from either input, with the same data.
|
||||
*/
|
||||
class CoreWrapperMapPayloadsBase : public CoreWrapperServicesTest
|
||||
{
|
||||
protected:
|
||||
static constexpr int kWidth = 320;
|
||||
static constexpr int kHeight = 240;
|
||||
static constexpr double kCameraHeight = 0.3;
|
||||
|
||||
void buildMap(MapInput input, const std::vector<rclcpp::Parameter> & extra = {})
|
||||
{
|
||||
publishStaticTf("laser", 0.1);
|
||||
publishOpticalTf("camera", kCameraHeight);
|
||||
|
||||
std::vector<rclcpp::Parameter> params = {
|
||||
rclcpp::Parameter("subscribe_depth", false),
|
||||
rclcpp::Parameter("subscribe_rgb", false),
|
||||
// Set explicitly: the node switches the grid to the scan by itself only when a
|
||||
// scan topic is subscribed, not for a scan inside sensor_data.
|
||||
rclcpp::Parameter(Parameters::kGridSensor(), "0"),
|
||||
rclcpp::Parameter(Parameters::kGridRangeMax(), "0")};
|
||||
if(input == MapInput::RgbdAndScan)
|
||||
{
|
||||
params.push_back(rclcpp::Parameter("subscribe_rgbd", true));
|
||||
params.push_back(rclcpp::Parameter("subscribe_scan", true));
|
||||
}
|
||||
else
|
||||
{
|
||||
params.push_back(rclcpp::Parameter("subscribe_sensor_data", true));
|
||||
}
|
||||
params.insert(params.end(), extra.begin(), extra.end());
|
||||
makeNode(params);
|
||||
info_ = collectInfo();
|
||||
odom_ = odomPublisher();
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbd;
|
||||
rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::UserData>::SharedPtr userData;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::SensorData>::SharedPtr sensorData;
|
||||
if(input == MapInput::RgbdAndScan)
|
||||
{
|
||||
rgbd = helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
scan = helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
|
||||
userData = helper()->create_publisher<rtabmap_msgs::msg::UserData>("user_data_async", 1);
|
||||
ASSERT_TRUE(waitForSubscriber(rgbd));
|
||||
ASSERT_TRUE(waitForSubscriber(scan));
|
||||
ASSERT_TRUE(waitForSubscriber(userData));
|
||||
}
|
||||
else
|
||||
{
|
||||
sensorData = helper()->create_publisher<rtabmap_msgs::msg::SensorData>("sensor_data", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(sensorData));
|
||||
}
|
||||
|
||||
// For packing the scan the way the node converts it: in base_link, from the laser.
|
||||
tf2_ros::Buffer tfBuffer(helper()->get_clock());
|
||||
tfBuffer.setUsingDedicatedThread(true); // static transform set below, nothing to wait for
|
||||
geometry_msgs::msg::TransformStamped laserTf = makeTransform("base_link", "laser", 0.0, 0.1);
|
||||
tfBuffer.setTransform(laserTf, "test", true);
|
||||
|
||||
for(int i=0; i<2; ++i)
|
||||
{
|
||||
const double stamp = 1.0 + i;
|
||||
const cv::Mat rgb = texturedImage(kWidth, kHeight, 7 + i);
|
||||
const cv::Mat depth = depthImage(kWidth, kHeight);
|
||||
const sensor_msgs::msg::CameraInfo cameraInfo =
|
||||
makeCameraInfo("camera", stamp, kWidth, kHeight, 250.0);
|
||||
const sensor_msgs::msg::LaserScan scanMsg = makeRoomScan("laser", stamp, 0.5*i + 0.1);
|
||||
const rtabmap_msgs::msg::UserData userDataMsg = makeUserData(stamp);
|
||||
const cv::Mat descriptor = cv::Mat::ones(1, 8, CV_32FC1);
|
||||
|
||||
const size_t before = info_->size();
|
||||
if(input == MapInput::RgbdAndScan)
|
||||
{
|
||||
userData->publish(userDataMsg);
|
||||
spinFor(std::chrono::milliseconds(50));
|
||||
|
||||
rtabmap_msgs::msg::RGBDImage msg;
|
||||
msg.header.frame_id = "camera";
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.rgb = makeImage("camera", stamp, rgb, "bgr8");
|
||||
msg.depth = makeImage("camera", stamp, depth, "16UC1");
|
||||
msg.rgb_camera_info = cameraInfo;
|
||||
msg.depth_camera_info = cameraInfo;
|
||||
msg.global_descriptor.header = msg.header;
|
||||
msg.global_descriptor.data = rtabmap::compressData(descriptor);
|
||||
|
||||
sendOdom(odom_, stamp, 0.5*i);
|
||||
rgbd->publish(msg);
|
||||
scan->publish(scanMsg);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Packed with the node's own conversions, as the odometry nodes republish
|
||||
// what they processed on odom_sensor_data/raw.
|
||||
rtabmap::LaserScan laserScan;
|
||||
ASSERT_TRUE(rtabmap_conversions::convertScanMsg(
|
||||
scanMsg, "base_link", "", stampOf(stamp), laserScan, tfBuffer, 0.0));
|
||||
rtabmap::SensorData data(
|
||||
laserScan, rgb, depth,
|
||||
rtabmap_conversions::cameraModelFromROS(cameraInfo,
|
||||
rtabmap_conversions::transformFromGeometryMsg(
|
||||
opticalTransform("camera", kCameraHeight).transform)),
|
||||
0, stamp,
|
||||
rtabmap_conversions::userDataFromROS(userDataMsg));
|
||||
data.setGlobalDescriptors(std::vector<rtabmap::GlobalDescriptor>(
|
||||
1, rtabmap::GlobalDescriptor(0, descriptor)));
|
||||
rtabmap_msgs::msg::SensorData msg;
|
||||
rtabmap_conversions::sensorDataToROS(data, msg, "base_link", true);
|
||||
|
||||
sendOdom(odom_, stamp, 0.5*i);
|
||||
sensorData->publish(msg);
|
||||
}
|
||||
ASSERT_TRUE(spinUntil([&]() { return info_->size() > before; }))
|
||||
<< "update " << i << " was not processed";
|
||||
}
|
||||
}
|
||||
|
||||
rtabmap_msgs::msg::MapData getMapData2(const Payloads & asked)
|
||||
{
|
||||
rtabmap_msgs::srv::GetMap2::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::GetMap2::Request>();
|
||||
req->global_map = true;
|
||||
req->optimized = true;
|
||||
req->with_images = asked.images;
|
||||
req->with_scans = asked.scans;
|
||||
req->with_user_data = asked.userData;
|
||||
req->with_grids = asked.grids;
|
||||
req->with_words = asked.words;
|
||||
req->with_global_descriptors = asked.globalDescriptors;
|
||||
rtabmap_msgs::srv::GetMap2::Response::SharedPtr res =
|
||||
call<rtabmap_msgs::srv::GetMap2>("get_map_data2", req);
|
||||
EXPECT_TRUE(res.get() != nullptr);
|
||||
return res ? res->data : rtabmap_msgs::msg::MapData();
|
||||
}
|
||||
|
||||
rtabmap_msgs::msg::MapData getMapData(bool graphOnly)
|
||||
{
|
||||
rtabmap_msgs::srv::GetMap::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
|
||||
req->global_map = true;
|
||||
req->optimized = true;
|
||||
req->graph_only = graphOnly;
|
||||
rtabmap_msgs::srv::GetMap::Response::SharedPtr res =
|
||||
call<rtabmap_msgs::srv::GetMap>("get_map_data", req);
|
||||
EXPECT_TRUE(res.get() != nullptr);
|
||||
return res ? res->data : rtabmap_msgs::msg::MapData();
|
||||
}
|
||||
|
||||
/// Node 1 of @p map, with the graph checked to be complete whatever was asked for.
|
||||
static rtabmap_msgs::msg::Node node1(const rtabmap_msgs::msg::MapData & map)
|
||||
{
|
||||
EXPECT_EQ(2u, map.graph.poses_id.size());
|
||||
EXPECT_EQ("map", map.header.frame_id);
|
||||
for(const rtabmap_msgs::msg::Node & n : map.nodes)
|
||||
{
|
||||
if(n.id == 1)
|
||||
{
|
||||
return n;
|
||||
}
|
||||
}
|
||||
ADD_FAILURE() << "node 1 is missing";
|
||||
return rtabmap_msgs::msg::Node();
|
||||
}
|
||||
|
||||
/// All six kinds of payload.
|
||||
static Payloads all()
|
||||
{
|
||||
Payloads p;
|
||||
p.images = p.scans = p.userData = p.grids = p.words = p.globalDescriptors = true;
|
||||
return p;
|
||||
}
|
||||
};
|
||||
|
||||
/// The payload tests below, run once per input.
|
||||
class CoreWrapperMapPayloadsTest :
|
||||
public CoreWrapperMapPayloadsBase,
|
||||
public ::testing::WithParamInterface<MapInput>
|
||||
{
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
CoreWrapperMapPayloadsBase::SetUp();
|
||||
buildMap(GetParam());
|
||||
}
|
||||
};
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(Inputs, CoreWrapperMapPayloadsTest,
|
||||
::testing::Values(MapInput::RgbdAndScan, MapInput::SensorData),
|
||||
[](const ::testing::TestParamInfo<MapInput> & info) { return toString(info.param); });
|
||||
|
||||
/// The map these tests build does hold every kind of payload, or the tests below prove nothing.
|
||||
TEST_P(CoreWrapperMapPayloadsTest, the_map_holds_every_payload)
|
||||
{
|
||||
EXPECT_EQ(all(), Payloads::of(node1(getMapData2(all()))));
|
||||
}
|
||||
|
||||
/**
|
||||
* get_map_data2 returns each kind of payload only when asked for it, so a client that
|
||||
* only needs, say, the scans does not download the images too.
|
||||
*/
|
||||
TEST_P(CoreWrapperMapPayloadsTest, get_map_data2_returns_only_the_payloads_asked_for)
|
||||
{
|
||||
EXPECT_EQ(Payloads(), Payloads::of(node1(getMapData2(Payloads()))));
|
||||
|
||||
const std::vector<std::pair<std::string, bool Payloads::*>> flags = {
|
||||
{"with_images", &Payloads::images},
|
||||
{"with_scans", &Payloads::scans},
|
||||
{"with_user_data", &Payloads::userData},
|
||||
{"with_grids", &Payloads::grids},
|
||||
{"with_words", &Payloads::words},
|
||||
{"with_global_descriptors", &Payloads::globalDescriptors}};
|
||||
for(const auto & flag : flags)
|
||||
{
|
||||
SCOPED_TRACE(flag.first);
|
||||
Payloads asked;
|
||||
asked.*(flag.second) = true;
|
||||
EXPECT_EQ(asked, Payloads::of(node1(getMapData2(asked))));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* get_map_data is get_map_data2 with a single switch: everything, or with graph_only,
|
||||
* nothing but the graph and the nodes' metadata.
|
||||
*/
|
||||
TEST_P(CoreWrapperMapPayloadsTest, get_map_data_returns_everything_unless_graph_only)
|
||||
{
|
||||
EXPECT_EQ(all(), Payloads::of(node1(getMapData(false))));
|
||||
|
||||
rtabmap_msgs::msg::MapData graphOnly = getMapData(true);
|
||||
rtabmap_msgs::msg::Node node = node1(graphOnly);
|
||||
EXPECT_EQ(Payloads(), Payloads::of(node));
|
||||
EXPECT_NEAR(0.0, node.pose.position.x, 1e-4) << "the nodes are still there, without data";
|
||||
}
|
||||
|
||||
/**
|
||||
* get_node_data selects the images, the scan, the grid and the user data separately. The
|
||||
* visual words and the global descriptors have no switch: they always come along.
|
||||
*/
|
||||
TEST_P(CoreWrapperMapPayloadsTest, get_node_data_returns_only_the_payloads_asked_for)
|
||||
{
|
||||
const auto getNodeData = [&](bool images, bool scan, bool grid, bool userData) {
|
||||
rtabmap_msgs::srv::GetNodeData::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::GetNodeData::Request>();
|
||||
req->ids = {1};
|
||||
req->images = images;
|
||||
req->scan = scan;
|
||||
req->grid = grid;
|
||||
req->user_data = userData;
|
||||
rtabmap_msgs::srv::GetNodeData::Response::SharedPtr res =
|
||||
call<rtabmap_msgs::srv::GetNodeData>("get_node_data", req);
|
||||
EXPECT_TRUE(res.get() != nullptr);
|
||||
EXPECT_TRUE(res && res->data.size() == 1u);
|
||||
return res && !res->data.empty() ? Payloads::of(res->data[0]) : Payloads();
|
||||
};
|
||||
|
||||
Payloads alwaysThere;
|
||||
alwaysThere.words = true;
|
||||
alwaysThere.globalDescriptors = true;
|
||||
|
||||
EXPECT_EQ(alwaysThere, getNodeData(false, false, false, false));
|
||||
{
|
||||
SCOPED_TRACE("images");
|
||||
Payloads expected = alwaysThere;
|
||||
expected.images = true;
|
||||
EXPECT_EQ(expected, getNodeData(true, false, false, false));
|
||||
}
|
||||
{
|
||||
SCOPED_TRACE("scan");
|
||||
Payloads expected = alwaysThere;
|
||||
expected.scans = true;
|
||||
EXPECT_EQ(expected, getNodeData(false, true, false, false));
|
||||
}
|
||||
{
|
||||
SCOPED_TRACE("grid");
|
||||
Payloads expected = alwaysThere;
|
||||
expected.grids = true;
|
||||
EXPECT_EQ(expected, getNodeData(false, false, true, false));
|
||||
}
|
||||
{
|
||||
SCOPED_TRACE("user_data");
|
||||
Payloads expected = alwaysThere;
|
||||
expected.userData = true;
|
||||
EXPECT_EQ(expected, getNodeData(false, false, false, true));
|
||||
}
|
||||
EXPECT_EQ(all(), getNodeData(true, true, true, true));
|
||||
}
|
||||
|
||||
class CoreWrapperMapInputsTest : public CoreWrapperMapPayloadsBase
|
||||
{
|
||||
protected:
|
||||
static void expectSameMat(const cv::Mat & a, const cv::Mat & b, const std::string & what,
|
||||
bool mayBeEmpty = false, double tolerance = 0.0)
|
||||
{
|
||||
if(!mayBeEmpty)
|
||||
{
|
||||
EXPECT_FALSE(a.empty()) << what << " is empty, so comparing it proves nothing";
|
||||
}
|
||||
ASSERT_EQ(a.empty(), b.empty()) << what;
|
||||
if(a.empty())
|
||||
{
|
||||
return;
|
||||
}
|
||||
ASSERT_EQ(a.size(), b.size()) << what;
|
||||
ASSERT_EQ(a.type(), b.type()) << what;
|
||||
EXPECT_LE(cv::norm(a, b, cv::NORM_INF), tolerance) << what;
|
||||
}
|
||||
|
||||
/// The words' keypoints of @p node, in pixels, sorted.
|
||||
static std::vector<std::pair<float, float>> keypoints(const rtabmap_msgs::msg::Node & node)
|
||||
{
|
||||
std::vector<std::pair<float, float>> out;
|
||||
for(const rtabmap_msgs::msg::KeyPoint & k : node.word_kpts)
|
||||
{
|
||||
out.push_back(std::make_pair(k.pt.x, k.pt.y));
|
||||
}
|
||||
std::sort(out.begin(), out.end());
|
||||
return out;
|
||||
}
|
||||
|
||||
/// The words' 3D points of @p node, sorted.
|
||||
static std::vector<std::tuple<float, float, float>> points(const rtabmap_msgs::msg::Node & node)
|
||||
{
|
||||
std::vector<std::tuple<float, float, float>> out;
|
||||
for(const rtabmap_msgs::msg::Point3f & p : node.word_pts)
|
||||
{
|
||||
out.push_back(std::make_tuple(p.x, p.y, p.z));
|
||||
}
|
||||
std::sort(out.begin(), out.end());
|
||||
return out;
|
||||
}
|
||||
|
||||
/// Node @p a and node @p b hold the same data, down to the pixel and the point.
|
||||
static void expectSameNode(const rtabmap_msgs::msg::Node & a, const rtabmap_msgs::msg::Node & b)
|
||||
{
|
||||
SCOPED_TRACE("node " + std::to_string(a.id));
|
||||
EXPECT_EQ(a.map_id, b.map_id);
|
||||
EXPECT_DOUBLE_EQ(a.stamp, b.stamp);
|
||||
EXPECT_NEAR(a.pose.position.x, b.pose.position.x, 1e-6);
|
||||
|
||||
rtabmap::SensorData da = rtabmap_conversions::sensorDataFromROS(a.data);
|
||||
rtabmap::SensorData db = rtabmap_conversions::sensorDataFromROS(b.data);
|
||||
cv::Mat rgbA, depthA, userA, groundA, obstaclesA, emptyA;
|
||||
cv::Mat rgbB, depthB, userB, groundB, obstaclesB, emptyB;
|
||||
rtabmap::LaserScan scanA, scanB;
|
||||
da.uncompressData(&rgbA, &depthA, &scanA, &userA, &groundA, &obstaclesA, &emptyA);
|
||||
db.uncompressData(&rgbB, &depthB, &scanB, &userB, &groundB, &obstaclesB, &emptyB);
|
||||
|
||||
expectSameMat(rgbA, rgbB, "rgb");
|
||||
expectSameMat(depthA, depthB, "depth");
|
||||
ASSERT_EQ(1u, da.cameraModels().size());
|
||||
ASSERT_EQ(1u, db.cameraModels().size());
|
||||
EXPECT_DOUBLE_EQ(da.cameraModels()[0].fx(), db.cameraModels()[0].fx());
|
||||
EXPECT_DOUBLE_EQ(da.cameraModels()[0].cx(), db.cameraModels()[0].cx());
|
||||
EXPECT_EQ(da.cameraModels()[0].imageSize(), db.cameraModels()[0].imageSize());
|
||||
EXPECT_EQ(da.cameraModels()[0].localTransform().prettyPrint(),
|
||||
db.cameraModels()[0].localTransform().prettyPrint());
|
||||
|
||||
// Converted through the odometry frame on one side (odom_sensor_sync) and straight
|
||||
// into base_link on the other: the same points, to float rounding.
|
||||
expectSameMat(scanA.data(), scanB.data(), "scan", false, 1e-5);
|
||||
EXPECT_EQ(scanA.format(), scanB.format());
|
||||
EXPECT_EQ(scanA.maxPoints(), scanB.maxPoints());
|
||||
EXPECT_FLOAT_EQ(scanA.rangeMax(), scanB.rangeMax());
|
||||
EXPECT_EQ(scanA.localTransform().prettyPrint(), scanB.localTransform().prettyPrint());
|
||||
|
||||
expectSameMat(userA, userB, "user data");
|
||||
|
||||
EXPECT_FLOAT_EQ(da.gridCellSize(), db.gridCellSize());
|
||||
expectSameMat(obstaclesA, obstaclesB, "grid obstacles", false, 1e-5);
|
||||
expectSameMat(emptyA, emptyB, "grid empty cells", false, 1e-5);
|
||||
expectSameMat(groundA, groundB, "grid ground", true, 1e-5);
|
||||
|
||||
// The same features are extracted, but not necessarily given the same word ids:
|
||||
// matching them against the dictionary is approximate, and the latest node's ids
|
||||
// differ from one run to the next even with the same input. So the keypoints and
|
||||
// their 3D points are compared, as sets.
|
||||
EXPECT_FALSE(a.word_kpts.empty());
|
||||
EXPECT_EQ(a.word_id_keys.size(), b.word_id_keys.size());
|
||||
EXPECT_EQ(keypoints(a), keypoints(b));
|
||||
EXPECT_EQ(points(a), points(b));
|
||||
|
||||
ASSERT_EQ(1u, a.data.global_descriptors.size());
|
||||
ASSERT_EQ(1u, b.data.global_descriptors.size());
|
||||
EXPECT_EQ(a.data.global_descriptors[0].type, b.data.global_descriptors[0].type);
|
||||
EXPECT_EQ(a.data.global_descriptors[0].data, b.data.global_descriptors[0].data);
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* sensor_data is the same map as rgbd_image and scan, given the same data: every node
|
||||
* stores the same images, calibration, scan, user data, grid, visual words and global
|
||||
* descriptor, whichever way it arrived.
|
||||
*/
|
||||
TEST_F(CoreWrapperMapInputsTest, sensor_data_maps_like_rgbd_and_scan)
|
||||
{
|
||||
buildMap(MapInput::RgbdAndScan);
|
||||
const rtabmap_msgs::msg::MapData viaTopics = getMapData2(all());
|
||||
destroyNode();
|
||||
|
||||
buildMap(MapInput::SensorData, {rclcpp::Parameter("delete_db_on_start", true)});
|
||||
const rtabmap_msgs::msg::MapData viaSensorData = getMapData2(all());
|
||||
|
||||
ASSERT_EQ(2u, viaTopics.nodes.size());
|
||||
ASSERT_EQ(viaTopics.nodes.size(), viaSensorData.nodes.size());
|
||||
for(size_t i=0; i<viaTopics.nodes.size(); ++i)
|
||||
{
|
||||
ASSERT_EQ(viaTopics.nodes[i].id, viaSensorData.nodes[i].id);
|
||||
expectSameNode(viaTopics.nodes[i], viaSensorData.nodes[i]);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* get_nodes_in_radius finds the nodes within a radius of either a node -- not counting
|
||||
* that node -- or a position, which is used when node_id is 0 and it is not the origin.
|
||||
*/
|
||||
TEST_F(CoreWrapperServicesTest, finds_nodes_in_a_radius)
|
||||
{
|
||||
makeMap(4); // x = 0, 0.5, 1.0, 1.5
|
||||
|
||||
rtabmap_msgs::srv::GetNodesInRadius::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::GetNodesInRadius::Request>();
|
||||
req->node_id = 1;
|
||||
req->radius = 0.6f;
|
||||
rtabmap_msgs::srv::GetNodesInRadius::Response::SharedPtr res =
|
||||
call<rtabmap_msgs::srv::GetNodesInRadius>("get_nodes_in_radius", req);
|
||||
ASSERT_TRUE(res.get() != nullptr);
|
||||
std::vector<int> ids = res->ids;
|
||||
EXPECT_EQ(std::vector<int>({2}), ids);
|
||||
ASSERT_EQ(1u, res->dists_sqr.size());
|
||||
EXPECT_NEAR(0.25, res->dists_sqr[0], 1e-4);
|
||||
|
||||
req->node_id = 0;
|
||||
req->x = 1.4f;
|
||||
res = call<rtabmap_msgs::srv::GetNodesInRadius>("get_nodes_in_radius", req);
|
||||
ASSERT_TRUE(res.get() != nullptr);
|
||||
ids = res->ids;
|
||||
std::sort(ids.begin(), ids.end());
|
||||
EXPECT_EQ(std::vector<int>({3, 4}), ids);
|
||||
}
|
||||
|
||||
/**
|
||||
* In localization mode the map is not extended: updates are localized against it and
|
||||
* then forgotten. set_mode_mapping goes back to extending it, in a new session, since
|
||||
* nothing links where the robot is now to the map it left. Both are mirrored in the
|
||||
* Mem/IncrementalMemory parameter.
|
||||
*/
|
||||
TEST_F(CoreWrapperServicesTest, localization_mode_stops_extending_the_map)
|
||||
{
|
||||
makeMap(2);
|
||||
|
||||
ASSERT_TRUE(callEmpty("set_mode_localization"));
|
||||
EXPECT_EQ("false", param(Parameters::kMemIncrementalMemory()));
|
||||
ASSERT_TRUE(updateAt(10.0, 3.0));
|
||||
ASSERT_TRUE(updateAt(11.0, 3.5));
|
||||
EXPECT_EQ(2u, getGraph().graph.poses_id.size());
|
||||
|
||||
ASSERT_TRUE(callEmpty("set_mode_mapping"));
|
||||
EXPECT_EQ("true", param(Parameters::kMemIncrementalMemory()));
|
||||
ASSERT_TRUE(updateAt(12.0, 4.0));
|
||||
EXPECT_EQ(std::vector<int>({0, 0, 1}), mapIds());
|
||||
}
|
||||
|
||||
/**
|
||||
* RTAB-Map parameters can be changed while the node runs, with `ros2 param set`: the
|
||||
* node applies them as soon as they change.
|
||||
*/
|
||||
TEST_F(CoreWrapperServicesTest, applies_parameters_changed_at_runtime)
|
||||
{
|
||||
makeMap(1);
|
||||
|
||||
ASSERT_TRUE(node_->set_parameter(
|
||||
rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), "2.0")).successful);
|
||||
spinFor(std::chrono::milliseconds(300)); // the change arrives as a parameter event
|
||||
ASSERT_TRUE(updateAt(2.0, 0.5));
|
||||
ASSERT_TRUE(updateAt(3.0, 1.0));
|
||||
|
||||
EXPECT_EQ(1u, getGraph().graph.poses_id.size())
|
||||
<< "0.5 m steps are now below the 2 m linear update";
|
||||
}
|
||||
|
||||
/**
|
||||
* backup saves the database as it is now to <database_path>.back, reloads it, and carries
|
||||
* on in a new session, as after a restart.
|
||||
*/
|
||||
TEST_F(CoreWrapperServicesTest, backup_copies_the_database)
|
||||
{
|
||||
makeMap(2);
|
||||
|
||||
ASSERT_TRUE(callEmpty("backup"));
|
||||
|
||||
EXPECT_TRUE(UFile::exists(databasePath() + ".back"));
|
||||
EXPECT_EQ(2u, getGraph().graph.poses_id.size());
|
||||
ASSERT_TRUE(updateAt(10.0, 2.0));
|
||||
EXPECT_EQ(std::vector<int>({0, 0, 1}), mapIds());
|
||||
}
|
||||
|
||||
/**
|
||||
* load_database saves the current map and switches to another database -- a new one, or
|
||||
* one whose map is reloaded. clear starts the target over.
|
||||
*/
|
||||
TEST_F(CoreWrapperServicesTest, load_database_switches_maps)
|
||||
{
|
||||
makeMap(2);
|
||||
|
||||
rtabmap_msgs::srv::LoadDatabase::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::LoadDatabase::Request>();
|
||||
req->database_path = dir() + "/other.db";
|
||||
req->clear = true;
|
||||
ASSERT_TRUE(call<rtabmap_msgs::srv::LoadDatabase>("load_database", req).get() != nullptr);
|
||||
EXPECT_TRUE(getGraph().graph.poses_id.empty());
|
||||
ASSERT_TRUE(updateAt(10.0, 0.0));
|
||||
EXPECT_EQ(1, info_->back().ref_id);
|
||||
|
||||
req->database_path = databasePath();
|
||||
req->clear = false;
|
||||
ASSERT_TRUE(call<rtabmap_msgs::srv::LoadDatabase>("load_database", req).get() != nullptr);
|
||||
EXPECT_EQ(2u, getGraph().graph.poses_id.size());
|
||||
EXPECT_TRUE(UFile::exists(dir() + "/other.db"));
|
||||
}
|
||||
|
||||
/// A database path in a directory that does not exist is refused, and the map is kept.
|
||||
TEST_F(CoreWrapperServicesTest, load_database_refuses_a_missing_directory)
|
||||
{
|
||||
makeMap(2);
|
||||
|
||||
rtabmap_msgs::srv::LoadDatabase::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::LoadDatabase::Request>();
|
||||
req->database_path = dir() + "/no/such/dir/other.db";
|
||||
ASSERT_TRUE(call<rtabmap_msgs::srv::LoadDatabase>("load_database", req).get() != nullptr);
|
||||
|
||||
EXPECT_EQ(2u, getGraph().graph.poses_id.size());
|
||||
}
|
||||
|
||||
/**
|
||||
* publish_map republishes the map on demand to whatever is subscribed -- the whole
|
||||
* database's with global_map, and just the graph with graph_only.
|
||||
*/
|
||||
TEST_F(CoreWrapperServicesTest, publish_map_republishes_on_demand)
|
||||
{
|
||||
makeMap(3);
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::MapGraph>> graph =
|
||||
collect<rtabmap_msgs::msg::MapGraph>("mapGraph",
|
||||
rclcpp::QoS(1).reliable().transient_local());
|
||||
ASSERT_TRUE(waitForPublisher(graph->subscription));
|
||||
spinFor(std::chrono::milliseconds(200));
|
||||
const size_t before = graph->size();
|
||||
|
||||
rtabmap_msgs::srv::PublishMap::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::PublishMap::Request>();
|
||||
req->global_map = true;
|
||||
req->optimized = true;
|
||||
req->graph_only = true;
|
||||
ASSERT_TRUE(call<rtabmap_msgs::srv::PublishMap>("publish_map", req).get() != nullptr);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return graph->size() > before; }));
|
||||
EXPECT_EQ(3u, graph->back().poses_id.size());
|
||||
}
|
||||
|
||||
/**
|
||||
* add_link adds a constraint from outside -- a loop closure found by another process,
|
||||
* say -- to the graph, which is then optimized with it.
|
||||
*/
|
||||
TEST_F(CoreWrapperServicesTest, add_link_adds_a_constraint)
|
||||
{
|
||||
makeMap(3);
|
||||
|
||||
rtabmap_msgs::srv::AddLink::Request::SharedPtr req =
|
||||
std::make_shared<rtabmap_msgs::srv::AddLink::Request>();
|
||||
req->link.from_id = 3;
|
||||
req->link.to_id = 1;
|
||||
req->link.type = rtabmap::Link::kUserClosure;
|
||||
req->link.transform.translation.x = -1.0;
|
||||
req->link.transform.rotation.w = 1.0;
|
||||
for(int i=0; i<6; ++i)
|
||||
{
|
||||
req->link.information[i*7] = 100.0;
|
||||
}
|
||||
ASSERT_TRUE(call<rtabmap_msgs::srv::AddLink>("add_link", req).get() != nullptr);
|
||||
|
||||
bool found = false;
|
||||
for(const rtabmap_msgs::msg::Link & l : getGraph().graph.links)
|
||||
{
|
||||
found = found || (l.type == rtabmap::Link::kUserClosure &&
|
||||
((l.from_id == 3 && l.to_id == 1) || (l.from_id == 1 && l.to_id == 3)));
|
||||
}
|
||||
EXPECT_TRUE(found);
|
||||
}
|
||||
|
||||
/// The log_* services set RTAB-Map's own log level, independently from ROS's.
|
||||
TEST_F(CoreWrapperServicesTest, log_services_set_rtabmap_log_level)
|
||||
{
|
||||
makeNode();
|
||||
const ULogger::Level initial = ULogger::level();
|
||||
|
||||
ASSERT_TRUE(callEmpty("log_debug"));
|
||||
EXPECT_EQ(ULogger::kDebug, ULogger::level());
|
||||
ASSERT_TRUE(callEmpty("log_info"));
|
||||
EXPECT_EQ(ULogger::kInfo, ULogger::level());
|
||||
ASSERT_TRUE(callEmpty("log_error"));
|
||||
EXPECT_EQ(ULogger::kError, ULogger::level());
|
||||
ASSERT_TRUE(callEmpty("log_warning"));
|
||||
EXPECT_EQ(ULogger::kWarning, ULogger::level());
|
||||
|
||||
ULogger::setLevel(initial);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
} // namespace rtabmap_slam_test
|
||||
Reference in New Issue
Block a user