From 5207dab7c22cd593433248953e881302bf11643e Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 28 Sep 2026 13:09:51 -0700 Subject: [PATCH] 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() --- .github/lcovrc | 45 + .github/workflows/coverage.yml | 10 +- .github/workflows/ros2.yml | 8 +- README.md | 2 +- codecov.yml | 9 +- rtabmap_conversions/CMakeLists.txt | 2 +- .../rtabmap_conversions/MsgConversion.h | 10 +- rtabmap_conversions/src/MsgConversion.cpp | 51 +- .../test/test_msg_conversion.cpp | 139 ++ rtabmap_launch/launch/rtabmap.launch.py | 2 +- rtabmap_odom/src/OdometryROS.cpp | 9 +- rtabmap_slam/CMakeLists.txt | 39 + rtabmap_slam/README.md | 111 ++ rtabmap_slam/doc/rtabmap.md | 623 +++++++ .../include/rtabmap_slam/CoreWrapper.h | 26 + rtabmap_slam/package.xml | 6 +- rtabmap_slam/rosdoc2.yaml | 35 + rtabmap_slam/src/CoreWrapper.cpp | 66 +- rtabmap_slam/test/core_wrapper_fixture.hpp | 431 +++++ rtabmap_slam/test/msg_builders.hpp | 348 ++++ rtabmap_slam/test/node_test_utils.hpp | 256 +++ .../test/test_core_wrapper_inputs.cpp | 1551 +++++++++++++++++ .../test/test_core_wrapper_mapping.cpp | 428 +++++ .../test/test_core_wrapper_parameters.cpp | 308 ++++ .../test/test_core_wrapper_planning.cpp | 400 +++++ .../test/test_core_wrapper_services.cpp | 892 ++++++++++ rtabmap_util/README.md | 54 +- rtabmap_util/doc/map_assembler.md | 52 +- rtabmap_util/src/MapsManager.cpp | 35 +- rtabmap_viz/src/GuiWrapper.cpp | 2 +- 30 files changed, 5832 insertions(+), 118 deletions(-) create mode 100644 .github/lcovrc create mode 100644 rtabmap_slam/README.md create mode 100644 rtabmap_slam/doc/rtabmap.md create mode 100644 rtabmap_slam/rosdoc2.yaml create mode 100644 rtabmap_slam/test/core_wrapper_fixture.hpp create mode 100644 rtabmap_slam/test/msg_builders.hpp create mode 100644 rtabmap_slam/test/node_test_utils.hpp create mode 100644 rtabmap_slam/test/test_core_wrapper_inputs.cpp create mode 100644 rtabmap_slam/test/test_core_wrapper_mapping.cpp create mode 100644 rtabmap_slam/test/test_core_wrapper_parameters.cpp create mode 100644 rtabmap_slam/test/test_core_wrapper_planning.cpp create mode 100644 rtabmap_slam/test/test_core_wrapper_services.cpp diff --git a/.github/lcovrc b/.github/lcovrc new file mode 100644 index 00000000..2eb80cc7 --- /dev/null +++ b/.github/lcovrc @@ -0,0 +1,45 @@ +# lcov configuration for .github/workflows/coverage.yml. +# +# A copy of colcon-lcov-result 0.5.0's default (colcon_lcov_result/verb/configuration/lcovrc), +# with branch coverage turned off below. The whole file is needed: --lcov-config-file +# replaces the default rather than adding to it. +# +# Why no branch coverage: gcc emits a hidden "it threw" branch on nearly every line that +# calls a function, and no test takes it. Each such line -- a declaration like +# 'rtabmap::Transform t;' included -- then counts as a partial, and Codecov reports a +# partial line as not covered. lcov's geninfo_no_exception_branch would drop those +# branches, but not with the lcov 1.15 of Ubuntu 22.04 and gcc 11: its JSON reader +# ignores the flag, and its text reader cannot read gcc 11's .gcno files. So only line +# coverage is reported: a line is covered when a test ran it. + +geninfo_auto_base=1 + +# Specify size of tabs +genhtml_num_spaces = 2 + +# Include color legend in HTML output if non-zero +genhtml_legend = 1 + +# Include function coverage data display +genhtml_function_coverage = 1 + +# Include branch coverage data display +genhtml_branch_coverage = 0 + +# Specify whether to capture coverage data for external source +# files +geninfo_external = 0 + +# Less verbose output +lcov_quiet = 1 + +# Specify if function coverage data should be collected and +# processed. +lcov_function_coverage = 1 + +# Specify if branch coverage data should be collected and +# processed. +lcov_branch_coverage = 0 + +## Follow symlinks +lcov_follow = 1 diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml index 03d5ee80..71ba59d3 100644 --- a/.github/workflows/coverage.yml +++ b/.github/workflows/coverage.yml @@ -60,7 +60,7 @@ jobs: # tested package depends on them (--packages-up-to), just not measured. - uses: ros-tooling/action-ros-ci@v0.4 with: - package-name: rtabmap_conversions rtabmap_util rtabmap_sync rtabmap_odom rtabmap_python + package-name: rtabmap_conversions rtabmap_util rtabmap_sync rtabmap_odom rtabmap_slam rtabmap_python target-ros2-distro: humble # RTAB-Map is installed in the image, not as an apt package, so rosdep # cannot resolve the key and must not try. @@ -120,16 +120,18 @@ jobs: . /opt/ros/humble/setup.sh # C++ packages only -- rtabmap_python emits no .gcno for lcov to read, # and is measured by the coveragepy step below instead. - PKGS="rtabmap_conversions rtabmap_util rtabmap_sync rtabmap_odom" + PKGS="rtabmap_conversions rtabmap_util rtabmap_sync rtabmap_odom rtabmap_slam" # Baseline from the .gcno files. Without it a source file that no test # ever loaded is missing from the report altogether rather than # counted as 0%, which quietly inflates the result. - colcon lcov-result --initial --packages-select $PKGS + # Line coverage only, no branch coverage: see the comment in .github/lcovrc. + LCOVRC=src/rtabmap_ros/.github/lcovrc + colcon lcov-result --initial --packages-select $PKGS --lcov-config-file $LCOVRC # The verb ends by running genhtml and returns *its* exit code, so a # cosmetic HTML hiccup fails the whole job even though the report was # written. The deliverable is total_coverage.info; the HTML is a # convenience. Tolerate the former, then gate on the latter. - colcon lcov-result --packages-select $PKGS --verbose \ + colcon lcov-result --packages-select $PKGS --lcov-config-file $LCOVRC --verbose \ --filter '*/test/*' '/usr/*' '/opt/*' \ '*CompilerId*' '*/CMakeFiles/*' || true test -s lcov/total_coverage.info diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 6e0cf1f5..ab5c6105 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -25,16 +25,16 @@ jobs: include: - ros_distro: humble skip_keys: '' - packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python' + packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_slam rtabmap_sync rtabmap_util rtabmap_python' - ros_distro: jazzy skip_keys: '' - packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python' + packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_slam rtabmap_sync rtabmap_util rtabmap_python' - ros_distro: kilted skip_keys: 'grid_map_ros' - packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python' + packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_slam rtabmap_sync rtabmap_util rtabmap_python' - ros_distro: lyrical skip_keys: 'velodyne grid_map_ros libpointmatcher' - packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python' + packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_slam rtabmap_sync rtabmap_util rtabmap_python' - ros_distro: rolling # Rolling has moved to Ubuntu resolute and much of it has not been rebuilt for # that yet, so rosdep finds nothing to install for these. They are all optional diff --git a/README.md b/README.md index 0ede8901..c0812e0a 100644 --- a/README.md +++ b/README.md @@ -35,7 +35,7 @@ The stack is split into small packages so a pipeline only pulls in what it uses. | Package | Description | |---|---| -| `rtabmap_slam` | The `rtabmap` node itself: appearance-based loop closure detection, graph optimization, memory management and map assembly. | +| [`rtabmap_slam`](rtabmap_slam/README.md) | The `rtabmap` node itself: appearance-based loop closure detection, graph optimization, memory management and map assembly. | | [`rtabmap_odom`](rtabmap_odom/README.md) | Odometry nodes — `rgbd_odometry`, `stereo_odometry` and `icp_odometry`. Any external odometry can be used instead. | | [`rtabmap_sync`](rtabmap_sync/README.md) | Synchronizes camera and lidar topics into a single message so they reach the SLAM node together — `rgbd_sync`, `stereo_sync`, `rgbdx_sync`. | diff --git a/codecov.yml b/codecov.yml index fe2e77e4..3ffcb55f 100644 --- a/codecov.yml +++ b/codecov.yml @@ -60,6 +60,10 @@ component_management: name: rtabmap_odom paths: - rtabmap_odom/** + - component_id: rtabmap_slam + name: rtabmap_slam + paths: + - rtabmap_slam/** - component_id: rtabmap_python name: rtabmap_python paths: @@ -70,8 +74,8 @@ comment: behavior: default require_changes: true # stay quiet when coverage doesn't move -# Only rtabmap_conversions, rtabmap_util, rtabmap_sync, rtabmap_odom and rtabmap_python -# have tests today, so +# Only rtabmap_conversions, rtabmap_util, rtabmap_sync, rtabmap_odom, rtabmap_slam and +# rtabmap_python have tests today, so # everything else would report as 0% and drag the total down to a number that # says nothing. As a package gains tests, drop its line here and add it to # individual_components above. @@ -82,7 +86,6 @@ ignore: - "rtabmap_launch/**" - "rtabmap_msgs/**" - "rtabmap_rviz_plugins/**" - - "rtabmap_slam/**" - "rtabmap_viz/**" - "**/test/**" - "**/setup.py" # packaging scaffolding, not code under test diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index f0db191f..af0a93d4 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/rtabmap_conversions/CMakeLists.txt @@ -30,7 +30,7 @@ find_package(tf2_eigen REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) -find_package(RTABMap 0.23.12 REQUIRED) +find_package(RTABMap 0.23.13 REQUIRED) # libraries SET(Libraries diff --git a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h index dbaf94df..666cdb38 100644 --- a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h +++ b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h @@ -832,9 +832,9 @@ bool convertStereoMsg( * @param odomStamp stamp the scan is synchronized to * @param[out] scan the converted scan * @param tfBuffer must contain @p frameId -> the laser frame at the scan stamp, - * and the laser frame relative to @p odomFrameId (or @p frameId - * when that is empty) across the whole sweep, since the points - * are projected through it + * and, to deskew, the laser frame relative to @p odomFrameId + * across the whole sweep, since the points are projected through + * it * @param waitForTransform seconds to wait for TF, 0 to not wait * @param outputInFrameId express the points in @p frameId rather than the laser frame * @return false if the scan is malformed (zero angle increment, inverted range or angle @@ -846,7 +846,9 @@ bool convertStereoMsg( * target is a fixed frame, i.e. if @p odomFrameId is set — with it empty the * target is @p frameId, which does not move relative to itself. This is also why * the laser frame must be known across the whole sweep, which the function checks - * up front. + * up front. When it is not known relative to @p odomFrameId -- odometry not + * published on TF -- the scan is converted as with @p odomFrameId empty, neither + * deskewed nor synchronized, with a warning shown once, rather than refused. * @note The odometry correction is applied only when the scan stamp differs from * @p odomStamp; a failed correction lookup warns and leaves the pose uncorrected. */ diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 9e00d0a1..6da75420 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -1305,7 +1305,8 @@ rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::msg::SensorData & msg) pcl::PCLPointCloud2 cloud; pcl_conversions::toPCL(msg.laser_scan, cloud); s.setLaserScan(rtabmap::LaserScan( - rtabmap::util3d::laserScanFromPointCloud(cloud), + rtabmap::util3d::laserScanFromPointCloud(cloud, true, + rtabmap::LaserScan::isScan2d((rtabmap::LaserScan::Format)msg.laser_scan_format)), msg.laser_scan_max_pts, msg.laser_scan_max_range, transformFromGeometryMsg(msg.laser_scan_local_transform)), @@ -2714,13 +2715,41 @@ bool convertScanMsg( } // make sure the frame of the laser is updated during the whole scan time - rtabmap::Transform tmpT = getMovingTransform( - scan2dMsg.header.frame_id, - odomFrameId.empty()?frameId:odomFrameId, - rclcpp::Time(scan2dMsg.header.stamp.sec, scan2dMsg.header.stamp.nanosec), - rclcpp::Time(scan2dMsg.header.stamp.sec, scan2dMsg.header.stamp.nanosec) + rclcpp::Duration::from_seconds((scan2dMsg.ranges.empty()?0:scan2dMsg.ranges.size()-1)*scan2dMsg.time_increment), - tfBuffer, - waitForTransform); + const rclcpp::Time scanStart(scan2dMsg.header.stamp); + const rclcpp::Time scanEnd = scanStart + rclcpp::Duration::from_seconds((scan2dMsg.ranges.empty()?0:scan2dMsg.ranges.size()-1)*scan2dMsg.time_increment); + std::string fixedFrameId = odomFrameId.empty()?frameId:odomFrameId; + rtabmap::Transform tmpT; + if(fixedFrameId == frameId || tfBuffer._frameExists(fixedFrameId)) // don't wait for a frame never published + { + tmpT = getMovingTransform( + scan2dMsg.header.frame_id, + fixedFrameId, + scanStart, + scanEnd, + tfBuffer, + waitForTransform); + } + if(tmpT.isNull() && fixedFrameId != frameId) + { + // Odometry not in TF: use the scan as it is rather than dropping it. + static bool warned = false; + if(!warned) + { + UWARN("Could not get laser frame \"%s\" relative to odometry frame \"%s\" over the scan " + "(%fs to %fs). Laser scans are used without deskewing nor synchronization with " + "odometry. Publish odometry on TF to have them deskewed. This message is only shown once.", + scan2dMsg.header.frame_id.c_str(), odomFrameId.c_str(), scanStart.seconds(), scanEnd.seconds()); + warned = true; + } + fixedFrameId = frameId; + tmpT = getMovingTransform( + scan2dMsg.header.frame_id, + fixedFrameId, + scanStart, + scanEnd, + tfBuffer, + waitForTransform); + } if(tmpT.isNull()) { return false; @@ -2740,12 +2769,12 @@ bool convertScanMsg( //transform in frameId_ frame sensor_msgs::msg::PointCloud2 scanOut; laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(odomFrameId.empty()?frameId:odomFrameId, scan2dMsg, scanOut, tfBuffer); + projection.transformLaserScanToPointCloud(fixedFrameId, scan2dMsg, scanOut, tfBuffer); //transform back in laser frame rtabmap::Transform laserToOdom = getTransform( scan2dMsg.header.frame_id, - odomFrameId.empty()?frameId:odomFrameId, + fixedFrameId, scan2dMsg.header.stamp, tfBuffer, waitForTransform); @@ -2755,7 +2784,7 @@ bool convertScanMsg( } // sync with odometry stamp - if(!odomFrameId.empty() && odomStamp != scan2dMsg.header.stamp) + if(fixedFrameId != frameId && odomStamp != scan2dMsg.header.stamp) { rtabmap::Transform sensorT = getMovingTransform( frameId, diff --git a/rtabmap_conversions/test/test_msg_conversion.cpp b/rtabmap_conversions/test/test_msg_conversion.cpp index e9033389..8cc12760 100644 --- a/rtabmap_conversions/test/test_msg_conversion.cpp +++ b/rtabmap_conversions/test/test_msg_conversion.cpp @@ -2360,6 +2360,76 @@ TEST(MsgConversion, sensorDataLaserScanRoundTrip) expectTransformNear(outScan.localTransform(), localTransform, 1e-4f); } +/** + * A raw scan survives the round trip through a SensorData message in every format: it + * goes out as a cloud with one field per channel, and comes back in laser_scan_format + * with the same values. A 2D scan in particular goes out as an x/y/z cloud with z at 0, + * which alone cannot tell it was 2D -- the odometry nodes' odom_sensor_data/raw does this + * for a 2D lidar -- and must come back 2D rather than as a 3D scan that fails the format + * check. + */ +TEST(MsgConversion, sensorDataLaserScanRoundTripEveryFormat) +{ + for(int f = rtabmap::LaserScan::kXY; f <= rtabmap::LaserScan::kXYZIRT; ++f) + { + const rtabmap::LaserScan::Format format = (rtabmap::LaserScan::Format)f; + SCOPED_TRACE(rtabmap::LaserScan::formatName(format)); + const int channels = rtabmap::LaserScan::channels(format); + ASSERT_GT(channels, 0); + + // Small integers in every channel: exact through float and through the integer + // fields some formats use, like the ring. + cv::Mat points(1, 3, CV_32FC(channels)); + for(int i = 0; i < points.cols; ++i) + { + float * p = points.ptr(0, i); + for(int c = 0; c < channels; ++c) + { + p[c] = float(1 + i + c); + } + } + const rtabmap::Transform localTransform(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f); + const rtabmap::LaserScan scan(points, /*maxPoints=*/360, /*maxRange=*/10.0f, format, localTransform); + if(scan.hasRGB()) + { + // A packed 0x00RRGGBB color, as PCL stores it. + for(int i = 0; i < points.cols; ++i) + { + const uint32_t rgb = 0x00102030u + i; + memcpy(points.ptr(0, i) + scan.getRGBOffset(), &rgb, sizeof(float)); + } + } + + rtabmap::SensorData in; + in.setStamp(1000.0); + in.setLaserScan(scan); + + rtabmap_msgs::msg::SensorData msg; + sensorDataToROS(in, msg, "base_link", /*copyRawData=*/true); + ASSERT_FALSE(msg.laser_scan.data.empty()); + EXPECT_EQ(msg.laser_scan_format, (int)format); + + rtabmap::SensorData out; + bool converted = false; + EXPECT_NO_THROW({ out = sensorDataFromROS(msg); converted = true; }); + if(!converted) + { + continue; // reported above; carry on so every failing format is listed + } + const rtabmap::LaserScan & outScan = out.laserScanRaw(); + ASSERT_FALSE(outScan.isEmpty()); + EXPECT_EQ(outScan.format(), format); + EXPECT_EQ(outScan.is2d(), scan.is2d()); + EXPECT_EQ(outScan.maxPoints(), scan.maxPoints()); + EXPECT_FLOAT_EQ(outScan.rangeMax(), scan.rangeMax()); + expectTransformNear(outScan.localTransform(), localTransform, 1e-4f); + ASSERT_EQ(outScan.data().size(), scan.data().size()); + ASSERT_EQ(outScan.data().type(), scan.data().type()); + EXPECT_EQ(0, memcmp(outScan.data().data, scan.data().data, + scan.data().total() * scan.data().elemSize())) << "the values changed"; + } +} + TEST(MsgConversion, sensorDataStereoModelRoundTrip) { const double fx = 525.0; @@ -3290,6 +3360,75 @@ TEST(MsgConversion, convertScanMsgSyncsToOdomStamp) EXPECT_NEAR(scan.localTransform().x(), 1.2, 1e-3) << "0.2 base->laser plus 1.0 motion"; } +namespace { + +/// 21 rays of 5 m over +-1 rad, swept in 0.5 s, from "laser" at @p stamp. +sensor_msgs::msg::LaserScan makeSweep(double stamp) +{ + sensor_msgs::msg::LaserScan msg; + msg.header.stamp = timestampToROS(stamp); + msg.header.frame_id = "laser"; + msg.angle_min = -1.0f; + msg.angle_max = 1.0f; + msg.angle_increment = 0.1f; + msg.time_increment = 0.5f / 20.0f; + msg.range_min = 0.1f; + msg.range_max = 30.0f; + msg.ranges.assign(21, 5.0f); + return msg; +} + +} // namespace + +/** + * With the odometry frame on TF, each ray is placed where the robot was when it was + * measured: at 1 m/s over a 0.5 s sweep, the last ray lands 0.5 m further than it would + * from the pose at the scan's stamp, the first one not at all. + */ +TEST(MsgConversion, convertScanMsgDeskewsWithOdometryTf) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "laser", rtabmap::Transform::getIdentity(), 1000.0); + addOdomMotion(*buffer); + const sensor_msgs::msg::LaserScan msg = makeSweep(1000.0); + + rtabmap::LaserScan skewed, deskewed; + ASSERT_TRUE(convertScanMsg(msg, "base_link", "", timestampToROS(1000.0), skewed, *buffer, 0.0)); + ASSERT_TRUE(convertScanMsg(msg, "base_link", "odom", timestampToROS(1000.0), deskewed, *buffer, 0.0)); + ASSERT_EQ(skewed.size(), deskewed.size()); + ASSERT_EQ(21, deskewed.size()); + + const float * first = deskewed.data().ptr(0, 0); + const float * firstSkewed = skewed.data().ptr(0, 0); + EXPECT_NEAR(firstSkewed[0], first[0], 1e-4); + EXPECT_NEAR(firstSkewed[1], first[1], 1e-4); + const float * last = deskewed.data().ptr(0, 20); + const float * lastSkewed = skewed.data().ptr(0, 20); + EXPECT_NEAR(lastSkewed[0] + 0.5f, last[0], 1e-3) << "moved by the robot's 0.5 m during the sweep"; + EXPECT_NEAR(lastSkewed[1], last[1], 1e-3); +} + +/** + * Without the odometry frame on TF -- odometry published as a topic only -- the scan is + * still converted, as it would be without an odometry frame: not deskewed, but not refused + * either. + */ +TEST(MsgConversion, convertScanMsgUsesTheScanAsItIsWithoutOdometryTf) +{ + const std::shared_ptr buffer = makeTfBuffer(); + const rtabmap::Transform baseToLaser(0.2f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f); + addTf(*buffer, "base_link", "laser", baseToLaser, 1000.0); + const sensor_msgs::msg::LaserScan msg = makeSweep(1000.0); + + rtabmap::LaserScan withoutOdom, withMissingOdom; + ASSERT_TRUE(convertScanMsg(msg, "base_link", "", timestampToROS(1000.0), withoutOdom, *buffer, 0.0)); + ASSERT_TRUE(convertScanMsg(msg, "base_link", "odom", timestampToROS(1000.0), withMissingOdom, *buffer, 0.0)); + + expectTransformNear(withMissingOdom.localTransform(), baseToLaser, 1e-4f); + ASSERT_EQ(withoutOdom.data().size(), withMissingOdom.data().size()); + EXPECT_EQ(0.0, cv::norm(withoutOdom.data(), withMissingOdom.data(), cv::NORM_INF)); +} + TEST(MsgConversion, convertRGBDMsgsRejectsBadEncoding) { const std::shared_ptr buffer = makeTfBuffer(); diff --git a/rtabmap_launch/launch/rtabmap.launch.py b/rtabmap_launch/launch/rtabmap.launch.py index 416688ab..e54c5b21 100644 --- a/rtabmap_launch/launch/rtabmap.launch.py +++ b/rtabmap_launch/launch/rtabmap.launch.py @@ -508,7 +508,7 @@ def generate_launch_description(): DeclareLaunchArgument('odom_tf_angular_variance', default_value='0.01', description='If TF is used to get odometry, this is the default angular variance'), DeclareLaunchArgument('odom_tf_linear_variance', default_value='0.001', description='If TF is used to get odometry, this is the default linear variance'), DeclareLaunchArgument('odom_args', default_value='', description='More arguments for odometry (overwrite same parameters in rtabmap_args).'), - DeclareLaunchArgument('odom_sensor_sync', default_value='false', description=''), + DeclareLaunchArgument('odom_sensor_sync', default_value='true', description='Correct each sensor\'s position for the motion between its stamp and the odometry\'s, using TF.'), DeclareLaunchArgument('odom_guess_frame_id', default_value='', description=''), DeclareLaunchArgument('odom_guess_min_translation', default_value='0.0', description=''), DeclareLaunchArgument('odom_guess_min_rotation', default_value='0.0', description=''), diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 40ec056f..6a44b869 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -508,14 +508,14 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg) imus_.erase(imus_.begin()); } } - if(dataMutex_.lockTry() == 0) + UScopeMutex dataLock(dataMutex_, false); + if(dataLock.lockTry() == 0) { if(bufferedDataToProcess_ && rtabmap_conversions::timestampFromROS(dataHeaderToProcess_.stamp) <= stamp) { bufferedDataToProcess_ = false; dataReady_.release(); } - dataMutex_.unlock(); } } } @@ -524,7 +524,8 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h { //RCLCPP_WARN(get_logger(), "Received image: %f delay=%f", data.stamp(), (now() - header.stamp).seconds()); double clockNow = rtabmap_conversions::timestampFromROS(now()); - if(dataMutex_.lockTry() == 0) + UScopeMutex dataLock(dataMutex_, false); + if(dataLock.lockTry() == 0) { if(bufferedDataToProcess_) { RCLCPP_ERROR(this->get_logger(), "We didn't receive IMU newer than previous image/scan (%f) and we just received a new image/scan (%f). The previous image/scan is dropped! Make sure IMU is published faster and with less delay than the image/scan.", @@ -537,7 +538,7 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h if(alwaysProcessMostRecentFrame_) { dataReady_.release(); } - dataMutex_.unlock(); + dataLock.unlock(); // processData() below must run unlocked ++processedMsgs_; if(!alwaysProcessMostRecentFrame_) { processData(); diff --git a/rtabmap_slam/CMakeLists.txt b/rtabmap_slam/CMakeLists.txt index 6107b23a..86e168a6 100644 --- a/rtabmap_slam/CMakeLists.txt +++ b/rtabmap_slam/CMakeLists.txt @@ -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() diff --git a/rtabmap_slam/README.md b/rtabmap_slam/README.md new file mode 100644 index 00000000..2c6ee36d --- /dev/null +++ b/rtabmap_slam/README.md @@ -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["
synchronized
RGB-D camera(s)
Stereo camera(s)
2D LiDAR
3D LiDAR
Odometry
"] + ASYNC["
asynchronous
IMU
GPS
Landmarks (markers, tags, fiducials)
"] + RTAB(["rtabmap"]) + GRAPH["Graph"] + INFO["Info"] + MAPS["
maps
2D occupancy grid
OctoMap
Elevation map
3D point cloud
"] + 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
map_frame_id"]) + ODOM(["odom
odometry frame"]) + BASE(["base_link
frame_id"]) + 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). diff --git a/rtabmap_slam/doc/rtabmap.md b/rtabmap_slam/doc/rtabmap.md new file mode 100644 index 00000000..9a04db83 --- /dev/null +++ b/rtabmap_slam/doc/rtabmap.md @@ -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
depth/image
rgb/camera_info"]) + ODOM(["odom
or TF's odom_frame_id"]) + R["rtabmap"] + 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
left/camera_info
right/image_rect
right/camera_info"]) + ODOM(["odom
or TF's odom_frame_id"]) + R["rtabmap"] + 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
or stereo_sync"] + SCAN(["scan or scan_cloud"]) + ODOM(["odom
or TF's odom_frame_id"]) + R["rtabmap"] + 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
or stereo_sync"] + S1["rgbd_sync
or stereo_sync"] + S2["rgbd_sync
or stereo_sync"] + S3["rgbd_sync
or stereo_sync"] + ODOM(["odom
or TF's odom_frame_id"]) + R["rtabmap"] + 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
or stereo_sync"] + S1["rgbd_sync
or stereo_sync"] + S2["rgbd_sync
or stereo_sync"] + S3["rgbd_sync
or stereo_sync"] + X["rgbdx_sync"] + SCAN(["scan or scan_cloud"]) + ODOM(["odom
or TF's odom_frame_id"]) + R["rtabmap"] + 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
or TF's odom_frame_id"]) + R["rtabmap"] + 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
rgb/camera_info"]) + ODOM(["odom
or TF's odom_frame_id"]) + R["rtabmap"] + 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["rtabmap"] + 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 `.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
or set_goal"]) + RTAB["rtabmap
global plan on the graph"] + NAV2["nav2
planner and controller"] + GOAL --> RTAB + RTAB -->|"map (occupancy grid)"| NAV2 + RTAB -->|"next node: navigate_to_pose action
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. diff --git a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h index ca41388c..4dc26b4e 100644 --- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h +++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h @@ -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(); diff --git a/rtabmap_slam/package.xml b/rtabmap_slam/package.xml index 6d2c94e2..4c67821c 100644 --- a/rtabmap_slam/package.xml +++ b/rtabmap_slam/package.xml @@ -34,9 +34,13 @@ rtabmap_msgs rtabmap_util rtabmap_sync - + + ament_cmake_gtest + rtabmap_conversions + ament_cmake + rosdoc2.yaml diff --git a/rtabmap_slam/rosdoc2.yaml b/rtabmap_slam/rosdoc2.yaml new file mode 100644 index 00000000..0b0420b9 --- /dev/null +++ b/rtabmap_slam/rosdoc2.yaml @@ -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: '' + } diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index efa27be9..94fd5517 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -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("info", 1); mapDataPub_ = this->create_publisher("mapData", 1); - mapGraphPub_ = this->create_publisher("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("mapGraph", rclcpp::QoS(1).reliable().durability(mapsManager_.isLatching()?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), latchedPubOptions); odomCachePub_ = this->create_publisher("mapOdomCache", 1); landmarksPub_ = this->create_publisher("landmarks", 1); labelsPub_ = this->create_publisher("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::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(); } 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(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 ", 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 >::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()) diff --git a/rtabmap_slam/test/core_wrapper_fixture.hpp b/rtabmap_slam/test/core_wrapper_fixture.hpp new file mode 100644 index 00000000..382337a4 --- /dev/null +++ b/rtabmap_slam/test/core_wrapper_fixture.hpp @@ -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 + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include + +#include "msg_builders.hpp" +#include "node_test_utils.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +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 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 defaultParameters( + const std::vector & params = {}) const + { + std::vector 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 makeNode( + const std::vector & params = {}, + const std::vector & arguments = {}) + { + rclcpp::NodeOptions options; + options.parameter_overrides(defaultParameters(params)); + if(!arguments.empty()) + { + options.arguments(arguments); + } + node_ = addNode(std::make_shared(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 makeMultiThreadedNode( + const std::vector & params = {}) + { + rclcpp::NodeOptions options; + options.parameter_overrides(defaultParameters(params)); + node_ = std::make_shared(options); + nodeExecutor_ = std::make_shared( + 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(*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("/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::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::SharedPtr odomPublisher() + { + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("odom", 10); + EXPECT_TRUE(waitForSubscriber(pub)); + return pub; + } + + /// Subscribes to `info`, which the node publishes once per processed update. + std::shared_ptr> collectInfo() + { + std::shared_ptr> info = + collect("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::SharedPtr & pub, + const std::shared_ptr> & info, + int count, double step = 0.5, double firstStamp = 1.0, double firstX = 0.0) + { + for(int i=0; isize(); + 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::Response::SharedPtr call( + const std::string & service, + typename SrvT::Request::SharedPtr request = std::make_shared(), + std::chrono::milliseconds timeout = std::chrono::milliseconds(10000)) + { + // The node advertises its services under its own name: /rtabmap/reset, not /reset. + typename rclcpp::Client::SharedPtr client = + helper()->create_client("/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(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(); + req->global_map = global; + req->optimized = optimized; + req->graph_only = true; + rtabmap_msgs::srv::GetMap::Response::SharedPtr res = + call("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(); + 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("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 mapIds() + { + std::vector ids; + rtabmap_msgs::srv::GetMap::Request::SharedPtr req = + std::make_shared(); + req->global_map = true; + req->optimized = false; + req->graph_only = false; + rtabmap_msgs::srv::GetMap::Response::SharedPtr res = + call("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 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(node_); // never destroyed + new std::shared_ptr(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 staticTf_; + rclcpp::Publisher::SharedPtr tfPub_; +}; + +} // namespace rtabmap_slam_test + +#endif /* RTABMAP_SLAM_CORE_WRAPPER_FIXTURE_HPP_ */ diff --git a/rtabmap_slam/test/msg_builders.hpp b/rtabmap_slam/test/msg_builders.hpp new file mode 100644 index 00000000..88707647 --- /dev/null +++ b/rtabmap_slam/test/msg_builders.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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#ifdef PRE_ROS_IRON +#include +#else +#include +#endif + +#include +#include +#include +#include +#include + +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::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 points; + for(double fx=kRoomXMin + 0.025; fx + +#include + +#include +#include +#include +#include +#include +#include + +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(); + helper_ = std::make_shared("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 + std::shared_ptr addNode(const std::shared_ptr & 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 & 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 + 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 + 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 + struct Collector + { + typename rclcpp::Subscription::SharedPtr subscription; + std::vector 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 + std::shared_ptr> collect( + const std::string & topic, const rclcpp::QoS & qos = rclcpp::QoS(10)) + { + std::shared_ptr> collector = std::make_shared>(); + std::weak_ptr> weak = collector; + collector->subscription = helper_->create_subscription( + topic, qos, + [weak](const typename MsgT::ConstSharedPtr msg) { + if(std::shared_ptr> collector = weak.lock()) + { + collector->messages.push_back(msg); + } + }); + return collector; + } + +private: + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + rclcpp::Node::SharedPtr helper_; + std::vector nodes_; +}; + +} // namespace rtabmap_slam_test + +#endif /* RTABMAP_SLAM_NODE_TEST_UTILS_HPP_ */ diff --git a/rtabmap_slam/test/test_core_wrapper_inputs.cpp b/rtabmap_slam/test/test_core_wrapper_inputs.cpp new file mode 100644 index 00000000..2f09c2dd --- /dev/null +++ b/rtabmap_slam/test/test_core_wrapper_inputs.cpp @@ -0,0 +1,1551 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include "core_wrapper_fixture.hpp" + +namespace rtabmap_slam_test { + +namespace { + +::testing::Environment * const kRclcppEnv = registerRclcppEnvironment(); + +using rtabmap::Parameters; + +class CoreWrapperInputsTest : public CoreWrapperTest +{ +protected: + /// Whether @p graph has a link of @p type between @p from and @p to, either way. + static bool hasLink(const rtabmap_msgs::msg::MapGraph & graph, int type, int from, int to) + { + for(const rtabmap_msgs::msg::Link & l : graph.links) + { + if(l.type == type && ((l.from_id == from && l.to_id == to) || + (l.from_id == to && l.to_id == from))) + { + return true; + } + } + return false; + } + + /// The link of @p type between @p from and @p to in @p graph, either way, or null. + static const rtabmap_msgs::msg::Link * findLink( + const rtabmap_msgs::msg::MapGraph & graph, int type, int from, int to) + { + for(const rtabmap_msgs::msg::Link & l : graph.links) + { + if(l.type == type && ((l.from_id == from && l.to_id == to) || + (l.from_id == to && l.to_id == from))) + { + return &l; + } + } + return nullptr; + } + + static bool hasPose(const rtabmap_msgs::msg::MapGraph & graph, int id) + { + return std::find(graph.poses_id.begin(), graph.poses_id.end(), id) != graph.poses_id.end(); + } + + /// The occupancy at (@p x, @p y) in the map frame: 100, 0, or -1 if unknown or off the grid. + static int occupancyAt(const nav_msgs::msg::OccupancyGrid & grid, double x, double y) + { + const int cx = int(std::floor((x - grid.info.origin.position.x) / grid.info.resolution)); + const int cy = int(std::floor((y - grid.info.origin.position.y) / grid.info.resolution)); + if(cx < 0 || cy < 0 || cx >= int(grid.info.width) || cy >= int(grid.info.height)) + { + return -1; + } + return grid.data[size_t(cy) * grid.info.width + cx]; + } + + /// The highest occupancy within one cell of (@p x, @p y): a wall falls on a cell boundary. + static int occupancyAround(const nav_msgs::msg::OccupancyGrid & grid, double x, double y) + { + int best = -1; + for(int dx=-1; dx<=1; ++dx) + { + for(int dy=-1; dy<=1; ++dy) + { + best = std::max(best, occupancyAt(grid, x + dx*grid.info.resolution, y + dy*grid.info.resolution)); + } + } + return best; + } + + static std::string where(double x, double y) + { + std::stringstream s; + s << "(" << x << ", " << y << ")"; + return s.str(); + } + + /** + * @brief Checks @p grid against the room, cell by cell, in the map frame. + * + * Along the walls, 0.3 m clear of the corners: occupied. Inside, 0.3 m clear of the + * walls: free -- walls from scans that did not move with the robot would show up here + * as occupied cells. 0.5 m outside: unknown. + */ + static void expectRoomGrid(const nav_msgs::msg::OccupancyGrid & grid) + { + ASSERT_GT(grid.info.resolution, 0.0f); + ASSERT_EQ(size_t(grid.info.width) * grid.info.height, grid.data.size()); + + int missingWalls = 0; + std::string firstMissing; + const auto checkWall = [&](double x, double y) { + if(occupancyAround(grid, x, y) != 100) + { + if(missingWalls++ == 0) { firstMissing = where(x, y); } + } + }; + for(double t=kRoomXMin + 0.3; t<=kRoomXMax - 0.3; t+=0.1) + { + checkWall(t, kRoomYMin); + checkWall(t, kRoomYMax); + } + for(double t=kRoomYMin + 0.3; t<=kRoomYMax - 0.3; t+=0.1) + { + checkWall(kRoomXMin, t); + checkWall(kRoomXMax, t); + } + EXPECT_EQ(0, missingWalls) << "wall cells not occupied, the first at " << firstMissing; + + int wrongInside = 0; + std::string firstWrong; + int firstValue = 0; + for(double x=kRoomXMin + 0.3; x<=kRoomXMax - 0.3; x+=0.1) + { + for(double y=kRoomYMin + 0.3; y<=kRoomYMax - 0.3; y+=0.1) + { + const int v = occupancyAt(grid, x, y); + if(v != 0) + { + if(wrongInside++ == 0) { firstWrong = where(x, y); firstValue = v; } + } + } + } + EXPECT_EQ(0, wrongInside) << "cells inside the room not free, the first at " + << firstWrong << " = " << firstValue; + + EXPECT_EQ(-1, occupancyAt(grid, kRoomXMax + 0.5, 0.0)); + EXPECT_EQ(-1, occupancyAt(grid, kRoomXMin - 0.5, 0.0)); + EXPECT_EQ(-1, occupancyAt(grid, 1.0, kRoomYMax + 0.5)); + EXPECT_EQ(-1, occupancyAt(grid, 1.0, kRoomYMin - 0.5)); + } + + /** + * @brief Sends @p count scans of the room with matching odometry, @p step meters apart + * along x, from a laser @p laserX meters ahead of base_link. + */ + void driveWithScans( + const rclcpp::Publisher::SharedPtr & odom, + const rclcpp::Publisher::SharedPtr & scan, + const std::shared_ptr> & info, + int count, double laserX, double step = 0.5) + { + for(int i=0; isize(); + const double stamp = 1.0 + double(i); + if(odom) + { + sendOdom(odom, stamp, step*i); + } + else + { + publishTf(makeTransform("odom", "base_link", stamp, step*i)); + } + scan->publish(makeRoomScan("laser", stamp, step*i + laserX)); + ASSERT_TRUE(spinUntil([&]() { return info->size() > before; })) + << "scan " << i << " was not processed"; + } + } +}; + +//========================================================================================== +// Sensor inputs, synchronized with odometry +//========================================================================================== + +/** + * A 2D lidar: each node stores its scan, and the occupancy grid is built from the scans. + * The scan is converted into base_link, so the lidar must be in TF. Driven across the + * room, the grid is the room: occupied walls, free inside, unknown beyond. + */ +TEST_F(CoreWrapperInputsTest, maps_a_laser_scan) +{ + publishStaticTf("laser", 0.1); + makeNode({rclcpp::Parameter("subscribe_scan", true)}); + std::shared_ptr> grid = + collect("map", rclcpp::QoS(1).reliable().transient_local()); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr scan = + helper()->create_publisher("scan", 10); + ASSERT_TRUE(waitForSubscriber(scan)); + ASSERT_TRUE(waitForPublisher(grid->subscription)); + + driveWithScans(odom, scan, info, 3, 0.1); + + rtabmap_msgs::msg::Node node = getNode(1); + EXPECT_FALSE(node.data.laser_scan_compressed.empty()); + EXPECT_NEAR(0.1, node.data.laser_scan_local_transform.translation.x, 1e-4); + + ASSERT_TRUE(spinUntil([&]() { return !grid->empty(); })); + EXPECT_EQ("map", grid->back().header.frame_id); + { + SCOPED_TRACE("map topic"); + expectRoomGrid(grid->back()); + } + + nav_msgs::srv::GetMap::Response::SharedPtr map = call("get_map"); + ASSERT_TRUE(map.get() != nullptr); + { + SCOPED_TRACE("get_map"); + expectRoomGrid(map->map); + } + nav_msgs::srv::GetMap::Response::SharedPtr prob = call("get_prob_map"); + ASSERT_TRUE(prob.get() != nullptr); + EXPECT_GT(prob->map.info.width, 0u); +} + +/** + * odom_sensor_sync, on by default, deskews scans through the odometry frame in TF. With + * odometry published only as a topic, there is no such frame: the scans are then used as + * they are, with a warning, rather than refused. + */ +TEST_F(CoreWrapperInputsTest, maps_scans_as_they_are_without_odometry_on_tf) +{ + publishStaticTf("laser", 0.1); + makeNode({rclcpp::Parameter("subscribe_scan", true)}); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr scan = + helper()->create_publisher("scan", 10); + ASSERT_TRUE(waitForSubscriber(scan)); + + for(int i=0; i<3; ++i) + { + const size_t before = info->size(); + odom->publish(makeOdometry(1.0 + i, 0.5*i)); // on the topic, not on TF + scan->publish(makeRoomScan("laser", 1.0 + i, 0.5*i + 0.1)); + ASSERT_TRUE(spinUntil([&]() { return info->size() > before; })) << "scan " << i << " was dropped"; + } + EXPECT_EQ(3u, getGraph().graph.poses_id.size()); +} + +/// A scan whose frame is not in TF cannot be placed on the robot and is dropped. +TEST_F(CoreWrapperInputsTest, drops_a_scan_without_its_tf) +{ + makeNode({rclcpp::Parameter("subscribe_scan", true), + rclcpp::Parameter("wait_for_transform", 0.05)}); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr scan = + helper()->create_publisher("scan", 10); + ASSERT_TRUE(waitForSubscriber(scan)); + + sendOdom(odom, 1.0, 0.0); + scan->publish(makeRoomScan("laser", 1.0, 0.0)); + spinFor(std::chrono::milliseconds(500)); + + EXPECT_TRUE(info->empty()); +} + +/** + * A scan that cannot be converted is dropped, but must not block the next ones: once the + * lidar's TF is there, the following scans are mapped. The conversion failure used to + * return with the synchronization mutex still locked, and every later update was then + * silently skipped. The node runs on a multi-threaded executor, as in the `rtabmap` + * executable: on a single thread, the recursive mutex would just be taken again. + */ +TEST_F(CoreWrapperInputsTest, maps_the_next_scans_after_one_without_its_tf) +{ + makeMultiThreadedNode({rclcpp::Parameter("subscribe_scan", true), + rclcpp::Parameter("wait_for_transform", 0.05)}); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr scan = + helper()->create_publisher("scan", 10); + ASSERT_TRUE(waitForSubscriber(scan)); + + sendOdom(odom, 1.0, 0.0); + scan->publish(makeRoomScan("laser", 1.0, 0.0)); + spinFor(std::chrono::milliseconds(500)); + ASSERT_TRUE(info->empty()) << "the scan without TF should have been dropped"; + + publishStaticTf("laser"); + for(int i=1; i<=3; ++i) + { + const size_t before = info->size(); + sendOdom(odom, 1.0 + i, 0.5*i); + scan->publish(makeRoomScan("laser", 1.0 + i, 0.5*i)); + ASSERT_TRUE(spinUntil([&]() { return info->size() > before; })) + << "scan " << i << " was not processed after the one without TF"; + } + EXPECT_EQ(3u, getGraph().graph.poses_id.size()); +} + +/// The same with a 3D lidar, whose conversion fails the same way without its TF. +TEST_F(CoreWrapperInputsTest, maps_the_next_clouds_after_one_without_its_tf) +{ + makeMultiThreadedNode({rclcpp::Parameter("subscribe_scan_cloud", true), + rclcpp::Parameter("wait_for_transform", 0.05)}); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr cloud = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(cloud)); + + sendOdom(odom, 1.0, 0.0); + cloud->publish(makeCloud("lidar", 1.0, roomScan3d(0.0, 0.0, 0.5))); + spinFor(std::chrono::milliseconds(500)); + ASSERT_TRUE(info->empty()) << "the cloud without TF should have been dropped"; + + publishStaticTf("lidar", 0.0, 0.0, 0.5); + for(int i=1; i<=3; ++i) + { + const size_t before = info->size(); + sendOdom(odom, 1.0 + i, 0.5*i); + cloud->publish(makeCloud("lidar", 1.0 + i, roomScan3d(0.5*i, 0.0, 0.5))); + ASSERT_TRUE(spinUntil([&]() { return info->size() > before; })) + << "cloud " << i << " was not processed after the one without TF"; + } + EXPECT_EQ(3u, getGraph().graph.poses_id.size()); +} + +/** + * With odom_frame_id set, odometry is read from TF at each scan's stamp instead of from a + * topic, and subscribe_odom is ignored. + */ +TEST_F(CoreWrapperInputsTest, reads_odometry_from_tf_with_odom_frame_id) +{ + publishStaticTf("laser"); + makeNode({rclcpp::Parameter("subscribe_scan", true), + rclcpp::Parameter("odom_frame_id", "odom")}); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr scan = + helper()->create_publisher("scan", 10); + ASSERT_TRUE(waitForSubscriber(scan)); + EXPECT_EQ(0u, helper()->count_publishers("odom") + helper()->count_subscribers("odom")); + + driveWithScans(nullptr, scan, info, 3, 0.0); + + rtabmap_msgs::msg::MapData map = getGraph(); + ASSERT_EQ(3u, map.graph.poses.size()); + EXPECT_NEAR(1.0, map.graph.poses[2].position.x, 1e-4); +} + +/// A 3D lidar: each node stores its cloud, and the grid is built from it. +TEST_F(CoreWrapperInputsTest, maps_a_point_cloud) +{ + publishStaticTf("lidar", 0.0, 0.0, 0.5); + makeNode({rclcpp::Parameter("subscribe_scan_cloud", true)}); + std::shared_ptr> grid = + collect("map", rclcpp::QoS(1).reliable().transient_local()); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr cloud = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(cloud)); + ASSERT_TRUE(waitForPublisher(grid->subscription)); + + for(int i=0; i<3; ++i) + { + const size_t before = info->size(); + sendOdom(odom, 1.0 + i, 0.5*i); + cloud->publish(makeCloud("lidar", 1.0 + i, roomScan3d(0.5*i, 0.0, 0.5))); + ASSERT_TRUE(spinUntil([&]() { return info->size() > before; })); + } + + rtabmap_msgs::msg::Node node = getNode(1); + EXPECT_FALSE(node.data.laser_scan_compressed.empty()); + EXPECT_NEAR(0.5, node.data.laser_scan_local_transform.translation.z, 1e-4); + + // The walls are obstacles and the floor is ground, which the 2D grid shows as free: + // the same grid as from the 2D lidar. + ASSERT_TRUE(spinUntil([&]() { return !grid->empty(); })); + expectRoomGrid(grid->back()); +} + +/** + * An RGB-D camera, through rtabmap_msgs/RGBDImage as rgbd_sync publishes it: each node + * stores the images and the calibration. + */ +TEST_F(CoreWrapperInputsTest, maps_an_rgbd_image) +{ + publishStaticTf("camera"); + makeNode({rclcpp::Parameter("subscribe_rgbd", true)}); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr rgbd = + helper()->create_publisher("rgbd_image", 10); + ASSERT_TRUE(waitForSubscriber(rgbd)); + + for(int i=0; i<2; ++i) + { + const double stamp = 1.0 + i; + rtabmap_msgs::msg::RGBDImage msg; + msg.header.frame_id = "camera"; + msg.header.stamp = stampOf(stamp); + msg.rgb = makeTexturedImage("camera", stamp, 64, 48, 42 + i); + msg.depth = makeDepthImage("camera", stamp); + msg.rgb_camera_info = makeCameraInfo("camera", stamp); + msg.depth_camera_info = makeCameraInfo("camera", stamp); + const size_t before = info->size(); + sendOdom(odom, stamp, 0.5*i); + rgbd->publish(msg); + ASSERT_TRUE(spinUntil([&]() { return info->size() > before; })); + } + + rtabmap_msgs::msg::Node node = getNode(1); + EXPECT_FALSE(node.data.left_compressed.empty()); + EXPECT_FALSE(node.data.right_compressed.empty()); + ASSERT_EQ(1u, node.data.left_camera_info.size()); + EXPECT_EQ(64u, node.data.left_camera_info[0].width); +} + +/** + * gen_scan makes a 2D scan out of the depth image, for a robot with a depth camera and no + * lidar, so the grid can be built the way it would be from a lidar. Given the depth image + * of a known scan, the scan it makes is that scan: every point of it within 1 cm of the + * original. + */ +TEST_F(CoreWrapperInputsTest, gen_scan_derives_a_scan_from_depth) +{ + // A known scan of the room from the camera's position -- the robot is at the origin + // facing +x, the camera on it, 0.3 m up, looking forward -- projected into a depth + // image with util3d::projectCloudToCamera(). The scan is at the camera's height, so it + // lands on the middle row, the one gen_scan reads back. + const sensor_msgs::msg::LaserScan known = makeRoomScan("camera", 1.0, 0.0, 0.0, 0.0, 14400); + pcl::PointCloud::Ptr knownCloud(new pcl::PointCloud); + for(size_t i=0; ipush_back(pcl::PointXYZ(float(known.ranges[i] * std::cos(a)), float(known.ranges[i] * std::sin(a)), 0.0f)); + } + + // A projected point lands up to a pixel off its column's center ray, where gen_scan puts + // it back, so the focal length bounds the error: depth / fx, 5.5 mm on the far wall. + // Only the middle row matters: a wide, short image, 90 degrees across -- the far wall + // and both sides. + const int width = 1280, height = 20; + const double fx = 640.0; + const cv::Mat K = (cv::Mat_(3, 3) << fx, 0.0, width/2.0, 0.0, fx, height/2.0, 0.0, 0.0, 1.0); + const cv::Mat depth = rtabmap::util3d::projectCloudToCamera( + cv::Size(width, height), K, knownCloud, + rtabmap_conversions::transformFromGeometryMsg(opticalTransform("camera_optical").transform)); + const int filled = cv::countNonZero(depth.row(height/2)); + ASSERT_EQ(width, filled) << "some columns of the middle row got no point"; + + publishOpticalTf("camera_optical", 0.3); + makeNode({rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("gen_scan", true), + rclcpp::Parameter("gen_scan_max_depth", 0.0)}); // the far wall is 3.5 m away and beyond + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr rgbd = + helper()->create_publisher("rgbd_image", 10); + ASSERT_TRUE(waitForSubscriber(rgbd)); + + rtabmap_msgs::msg::RGBDImage msg; + msg.header.frame_id = "camera_optical"; + msg.header.stamp = stampOf(1.0); + msg.rgb = makeTexturedImage("camera_optical", 1.0, width, height); + msg.depth = makeImage("camera_optical", 1.0, depth, "32FC1"); + msg.rgb_camera_info = makeCameraInfo("camera_optical", 1.0, width, height, fx); + msg.depth_camera_info = msg.rgb_camera_info; + sendOdom(odom, 1.0, 0.0); + rgbd->publish(msg); + ASSERT_TRUE(spinUntil([&]() { return !info->empty(); })); + + // The generated scan, in base_link, against the known one: every point within 1 cm. + rtabmap::SensorData data = rtabmap_conversions::sensorDataFromROS(getNode(1).data); + rtabmap::LaserScan scan; + data.uncompressData(0, 0, &scan); + ASSERT_FALSE(scan.isEmpty()); + EXPECT_TRUE(scan.is2d()); + EXPECT_EQ(filled, scan.size()) << "one point per column of the middle row"; + pcl::PointCloud::Ptr generated = rtabmap::util3d::laserScanToPointCloud(scan, scan.localTransform()); + double variance = 0.0; + int correspondences = 0; + rtabmap::util3d::computeVarianceAndCorrespondences( + generated, knownCloud, 0.01, variance, correspondences, false); + EXPECT_EQ(int(generated->size()), correspondences) << "generated points farther than 1 cm from the known scan"; +} + +/** + * rtabmap_msgs/SensorData carries everything a node holds in one message, the way the + * odometry nodes republish what they processed. + */ +TEST_F(CoreWrapperInputsTest, maps_sensor_data) +{ + makeNode({rclcpp::Parameter("subscribe_sensor_data", true)}); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr data = + helper()->create_publisher("sensor_data", 10); + ASSERT_TRUE(waitForSubscriber(data)); + + for(int i=0; i<2; ++i) + { + const double stamp = 1.0 + i; + rtabmap_msgs::msg::SensorData msg; + msg.header.frame_id = "base_link"; + msg.header.stamp = stampOf(stamp); + msg.user_data = {1, 2, 3}; // opaque to the node, carried through as is + const size_t before = info->size(); + sendOdom(odom, stamp, 0.5*i); + data->publish(msg); + ASSERT_TRUE(spinUntil([&]() { return info->size() > before; })); + } + + EXPECT_EQ(2u, getGraph().graph.poses_id.size()); +} + +//========================================================================================== +// Synchronization of sensors that are not stamped together +//========================================================================================== + +/** + * A robot with odometry, a 2D lidar and four cameras, none of them stamped together, the + * way they are on a real robot: + * + * - odometry at 50 Hz, arriving 5 ms after its stamp, with its TF; + * - the lidar at 10 Hz, 7 ms out of phase with the odometry, arriving 40 ms after its stamp; + * - the cameras at 30 Hz, each triggered 5 ms after the previous one, packed in one + * rtabmap_msgs/RGBDImages -- each camera keeping its own stamp -- that arrives 60 ms + * after the last one. + * + * The robot drives an arc, 1 m/s turning at 0.5 rad/s, for two seconds, and every message is + * published at its arrival time, in arrival order. Each camera frame's depth is uniform, + * 1000 mm plus the frame number, and depth is stored losslessly, so a node's depth images + * tell which frame, and so which stamp, each camera contributed. Each odometry message + * has a different variance, so a link's information tells which message it came from. + */ +class CoreWrapperSyncTest : public CoreWrapperInputsTest +{ +protected: + static constexpr double kStart = 1.0; + static constexpr double kDuration = 2.0; + static constexpr double kSpeed = 1.0; // m/s + static constexpr double kTurnRate = 0.5; // rad/s + static constexpr int kCameras = 4; + static constexpr int kWidth = 160; // 90 degrees each at kFx: the four cover 360 + static constexpr int kHeight = 20; // gen_scan only reads the middle row + static constexpr double kFx = 80.0; + + static double odomStamp(int k) { return kStart + 0.02 * k; } + static double scanStamp(int j) { return kStart + 0.007 + 0.1 * j; } + static double cameraStamp(int frame, int camera) { return kStart + frame / 30.0 + 0.005 * camera; } + static double odomVariance(int k) { return 0.001 * (1.0 + k); } + + /// Where the robot is at @p t, in odom (and map): an arc from the origin. + static rtabmap::Transform robotPose(double t) + { + const double yaw = kTurnRate * (t - kStart); + return rtabmap::Transform( + float(kSpeed / kTurnRate * std::sin(yaw)), + float(kSpeed / kTurnRate * (1.0 - std::cos(yaw))), + 0.0f, 0.0f, 0.0f, float(yaw)); + } + + static rtabmap::Transform laserMount() { return rtabmap::Transform(0.1f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); } + + /// Camera @p c looks out at c x 90 degrees, 0.2 m from the center, 0.3 m up. + static rtabmap::Transform cameraMount(int c) + { + const double yaw = c * M_PI / 2.0; + return rtabmap::Transform(float(0.2 * std::cos(yaw)), float(0.2 * std::sin(yaw)), 0.3f, 0.0f, 0.0f, float(yaw)) * + rtabmap::Transform(0, 0, 0, -0.5f, 0.5f, -0.5f, 0.5f); // optical: z forward, x right + } + static std::string cameraFrame(int c) { return "camera" + std::to_string(c) + "_optical"; } + + /** + * @brief The depth image of the room seen by a level camera at @p camera, an optical + * frame in the world: each column holds the room's depth along its center ray. + */ + static cv::Mat renderRoomDepth(const rtabmap::Transform & camera) + { + const Eigen::Vector3f axis = camera.toEigen3f().rotation() * Eigen::Vector3f::UnitZ(); + const double axisYaw = std::atan2(axis.y(), axis.x()); + cv::Mat depth(kHeight, kWidth, CV_16UC1); + for(int u=0; u & params, bool withScan = true) + { + publishStaticTf(makeTransform("base_link", "laser", 0.0, laserMount().x())); + for(int c=0; cnow(); + rtabmap_conversions::transformToGeometryMsg(cameraMount(c), tf.transform); + publishStaticTf(tf); + } + + std::vector all = { + rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("rgbd_cameras", 0), + rclcpp::Parameter("subscribe_scan", withScan), + rclcpp::Parameter("approx_sync", true), + rclcpp::Parameter("topic_queue_size", 50), + rclcpp::Parameter("sync_queue_size", 50), + rclcpp::Parameter(Parameters::kKpMaxFeatures(), "-1")}; // what is checked here is not visual + all.insert(all.end(), params.begin(), params.end()); + makeNode(all); + info_ = collectInfo(); + odom_ = odomPublisher(); + rclcpp::Publisher::SharedPtr scan = + helper()->create_publisher("scan", 50); + rclcpp::Publisher::SharedPtr rgbd = + helper()->create_publisher("rgbd_images", 50); + if(withScan) + { + ASSERT_TRUE(waitForSubscriber(scan)); + } + ASSERT_TRUE(waitForSubscriber(rgbd)); + + enum Type { kOdom, kScan, kImages }; + struct Event { double arrival; Type type; int index; }; + std::vector events; + for(int k=0; odomStamp(k) <= kStart + kDuration + 1e-9; ++k) { events.push_back({odomStamp(k) + 0.005, kOdom, k}); } + for(int j=0; withScan && scanStamp(j) <= kStart + kDuration + 1e-9; ++j) { events.push_back({scanStamp(j) + 0.040, kScan, j}); } + for(int f=0; cameraStamp(f, kCameras-1) <= kStart + kDuration + 1e-9; ++f) { events.push_back({cameraStamp(f, kCameras-1) + 0.060, kImages, f}); } + std::stable_sort(events.begin(), events.end(), + [](const Event & a, const Event & b) { return a.arrival < b.arrival; }); + + for(const Event & e : events) + { + if(e.type == kOdom) + { + const rtabmap::Transform pose = robotPose(odomStamp(e.index)); + float x, y, z, roll, pitch, yaw; + pose.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw); + publishTf(makeTransform("odom", "base_link", odomStamp(e.index), x, y, yaw)); + odom_->publish(makeOdometry(odomStamp(e.index), x, y, yaw, odomVariance(e.index))); + } + else if(e.type == kScan) + { + const rtabmap::Transform laser = robotPose(scanStamp(e.index)) * laserMount(); + scan->publish(makeRoomScan("laser", scanStamp(e.index), laser.x(), laser.y(), laser.theta())); + } + else + { + rtabmap_msgs::msg::RGBDImages msg; + msg.header.stamp = stampOf(cameraStamp(e.index, 0)); + msg.header.frame_id = cameraFrame(0); + for(int c=0; cpublish(msg); + } + spinFor(std::chrono::milliseconds(2)); + } + spinFor(std::chrono::milliseconds(500)); + } + + /// The nodes, with their images and scans, in id order. + std::vector nodes() + { + rtabmap_msgs::srv::GetMap::Request::SharedPtr req = + std::make_shared(); + req->global_map = true; + req->optimized = false; + req->graph_only = false; + rtabmap_msgs::srv::GetMap::Response::SharedPtr res = + call("get_map_data", req); + EXPECT_TRUE(res.get() != nullptr); + return res ? res->data.nodes : std::vector(); + } + + /// The frame each camera of @p data contributed, read back from its depth. + static std::vector cameraFrames(const rtabmap::SensorData & data) + { + cv::Mat depth; + rtabmap::SensorData copy = data; + copy.uncompressData(0, &depth); + std::vector frames; + for(int c=0; c(kHeight/2, c*kWidth + kWidth/2)) - 1000); + } + return frames; + } + + /// The farthest any point of any node's generated scan is from the room's walls. + double farthestFromTheWalls(const std::vector & all, size_t & points) + { + double worst = 0.0; + points = 0; + for(const rtabmap_msgs::msg::Node & node : all) + { + rtabmap::SensorData data = rtabmap_conversions::sensorDataFromROS(node.data); + rtabmap::LaserScan scan; + data.uncompressData(0, 0, &scan); + EXPECT_FALSE(scan.isEmpty()) << "node " << node.id; + const rtabmap::Transform toMap = rtabmap_conversions::transformFromPoseMsg(node.pose) * scan.localTransform(); + for(int i=0; i(0, i); + const cv::Point3f pt = rtabmap::util3d::transformPoint(cv::Point3f(p[0], p[1], 0.0f), toMap); + const double d = std::min(std::min(std::fabs(pt.x - kRoomXMin), std::fabs(pt.x - kRoomXMax)), + std::min(std::fabs(pt.y - kRoomYMin), std::fabs(pt.y - kRoomYMax))); + worst = std::max(worst, d); + ++points; + } + } + return worst; + } + + /** + * @brief A 0.1 s sweep of the room from the lidar, each ray measured from where the + * laser is at that ray's own time while the robot drives its arc. + */ + static sensor_msgs::msg::LaserScan makeSweepingRoomScan(double stamp, size_t count = 720) + { + sensor_msgs::msg::LaserScan scan = makeRoomScan("laser", stamp, 0.0, 0.0, 0.0, count); + scan.time_increment = float(0.1 / double(count)); + for(size_t i=0; i & params) + { + publishStaticTf(makeTransform("base_link", "laser", 0.0, laserMount().x())); + std::vector all = {rclcpp::Parameter("subscribe_scan", true)}; + all.insert(all.end(), params.begin(), params.end()); + makeNode(all); + info_ = collectInfo(); + odom_ = odomPublisher(); + rclcpp::Publisher::SharedPtr scan = + helper()->create_publisher("scan", 10); + ASSERT_TRUE(waitForSubscriber(scan)); + + for(double stamp = kStart; stamp <= kStart + 1.0 + 1e-9; stamp += 0.2) + { + for(double t = stamp - 0.02; t <= stamp + 0.12 + 1e-9; t += 0.01) + { + const rtabmap::Transform pose = robotPose(std::max(t, kStart)); + publishTf(makeTransform("odom", "base_link", t, pose.x(), pose.y(), pose.theta())); + } + spinFor(std::chrono::milliseconds(20)); + const rtabmap::Transform pose = robotPose(stamp); + const size_t before = info_->size(); + odom_->publish(makeOdometry(stamp, pose.x(), pose.y(), pose.theta())); + scan->publish(makeSweepingRoomScan(stamp)); + ASSERT_TRUE(spinUntil([&]() { return info_->size() > before; })) << "scan at " << stamp; + } + } + + static double angleBetween(const rtabmap::Transform & a, const rtabmap::Transform & b) + { + const rtabmap::Transform d = a.inverse() * b; + return std::fabs(Eigen::AngleAxisd(d.getQuaterniond()).angle()); + } + + std::shared_ptr> info_; + rclcpp::Publisher::SharedPtr odom_; +}; + +/** + * Each node takes the lidar's stamp, and the odometry interpolated in TF at that stamp -- + * between two samples, the lidar being out of phase with the odometry. With + * odom_sensor_sync, each camera's local transform is moved by the robot's motion between + * the lidar's stamp and that camera's, so the images are placed where the robot really + * was when each was taken. + */ +TEST_F(CoreWrapperSyncTest, places_each_sensor_at_its_own_stamp_with_odom_sensor_sync) +{ + driveUnsynchronized({}); // odom_sensor_sync is on by default + + const std::vector all = nodes(); + ASSERT_GE(all.size(), 3u) << "not enough updates made it through"; + double largestCorrection = 0.0; + for(const rtabmap_msgs::msg::Node & node : all) + { + SCOPED_TRACE("node " + std::to_string(node.id)); + // The lidar's stamp. + const double stamp = node.stamp; + const int j = int(std::lround((stamp - kStart - 0.007) / 0.1)); + EXPECT_NEAR(scanStamp(j), stamp, 1e-6) << "not a lidar stamp"; + + // The odometry, interpolated at it. + const rtabmap::Transform pose = rtabmap_conversions::transformFromPoseMsg(node.pose); + const rtabmap::Transform expected = robotPose(stamp); + EXPECT_NEAR(expected.x(), pose.x(), 1e-3); + EXPECT_NEAR(expected.y(), pose.y(), 1e-3); + EXPECT_NEAR(expected.theta(), pose.theta(), 1e-3); + + rtabmap::SensorData data = rtabmap_conversions::sensorDataFromROS(node.data); + rtabmap::LaserScan scan; + data.uncompressData(0, 0, &scan); + EXPECT_NEAR(laserMount().x(), scan.localTransform().x(), 1e-4) << "the scan is at the node's stamp"; + + // Each camera where the robot was at its own stamp. + const std::vector frames = cameraFrames(data); + ASSERT_EQ(size_t(kCameras), frames.size()); + ASSERT_EQ(size_t(kCameras), data.cameraModels().size()); + for(int c=0; c stamps; + for(const rtabmap_msgs::msg::Node & node : all) + { + stamps[node.id] = node.stamp; + } + int links = 0; + for(const rtabmap_msgs::msg::Link & link : getGraph(true, false).graph.links) + { + if(link.type == rtabmap::Link::kNeighbor) + { + const int to = std::max(link.from_id, link.to_id); + ASSERT_TRUE(stamps.count(to)); + const int k = int(std::lround((stamps.at(to) - kStart) / 0.02)); + EXPECT_NEAR(odomVariance(k), 1.0 / link.information[0], 1e-6) << "link to node " << to; + ++links; + } + } + EXPECT_EQ(int(all.size()) - 1, links); +} + +/** + * gen_scan with four cameras and no lidar: each node takes the first camera's stamp, and + * with odom_sensor_sync the other three are moved to where the robot was when each was + * taken. The scans generated from all nodes then line up on the room's walls: every point, + * placed in the map by its node's pose, within 1 cm of a wall. + */ +TEST_F(CoreWrapperSyncTest, gen_scan_from_unsynchronized_cameras_lines_up_with_odom_sensor_sync) +{ + driveUnsynchronized({rclcpp::Parameter("gen_scan", true), // odom_sensor_sync on by default + rclcpp::Parameter("gen_scan_max_depth", 0.0)}, false); + + const std::vector all = nodes(); + ASSERT_GE(all.size(), 3u) << "not enough updates made it through"; + for(const rtabmap_msgs::msg::Node & node : all) + { + const int f = int(std::lround((node.stamp - kStart) * 30.0)); + EXPECT_NEAR(cameraStamp(f, 0), node.stamp, 1e-6) << "node " << node.id << " is not at the first camera's stamp"; + } + size_t points = 0; + const double worst = farthestFromTheWalls(all, points); + EXPECT_GE(points, all.size() * kCameras * kWidth * 9 / 10); + EXPECT_LT(worst, 0.01) << "a generated point is " << worst << " m from the walls"; +} + +/** + * Without odom_sensor_sync, the three cameras taken after the first are placed as if taken + * at its stamp, while the robot was turning: their part of the scan misses the walls. + */ +TEST_F(CoreWrapperSyncTest, gen_scan_from_unsynchronized_cameras_misses_without_odom_sensor_sync) +{ + driveUnsynchronized({rclcpp::Parameter("odom_sensor_sync", false), + rclcpp::Parameter("gen_scan", true), + rclcpp::Parameter("gen_scan_max_depth", 0.0)}, false); + + const std::vector all = nodes(); + ASSERT_GE(all.size(), 3u) << "not enough updates made it through"; + size_t points = 0; + EXPECT_GT(farthestFromTheWalls(all, points), 0.01); +} + +/** + * A 2D lidar sweeping while the robot drives: with odom_sensor_sync, on by default, each + * ray is placed where the robot was when it was measured, so every point of every node's + * scan lands on the room's walls. + */ +TEST_F(CoreWrapperSyncTest, deskews_laser_scans_with_odom_sensor_sync) +{ + driveSweepingLidar({}); + + size_t points = 0; + const double worst = farthestFromTheWalls(nodes(), points); + EXPECT_GT(points, 0u); + EXPECT_LT(worst, 0.01) << "a scan point is " << worst << " m from the walls"; +} + +/// Without it, the rays measured late in the sweep are placed from where the robot started. +TEST_F(CoreWrapperSyncTest, leaves_laser_scans_skewed_without_odom_sensor_sync) +{ + driveSweepingLidar({rclcpp::Parameter("odom_sensor_sync", false)}); + + size_t points = 0; + EXPECT_GT(farthestFromTheWalls(nodes(), points), 0.03); +} + +/// Without odom_sensor_sync, each camera stays where it is mounted, whatever its stamp. +TEST_F(CoreWrapperSyncTest, leaves_cameras_at_their_mount_without_odom_sensor_sync) +{ + driveUnsynchronized({rclcpp::Parameter("odom_sensor_sync", false)}); + + const std::vector all = nodes(); + ASSERT_GE(all.size(), 3u) << "not enough updates made it through"; + for(const rtabmap_msgs::msg::Node & node : all) + { + SCOPED_TRACE("node " + std::to_string(node.id)); + rtabmap::SensorData data = rtabmap_conversions::sensorDataFromROS(node.data); + ASSERT_EQ(size_t(kCameras), data.cameraModels().size()); + for(int c=0; c> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr userData = + helper()->create_publisher("user_data_async", 1); + ASSERT_TRUE(waitForSubscriber(userData)); + + userData->publish(makeUserData(1.0)); + spinFor(std::chrono::milliseconds(100)); + driveStraight(odom, info, 2); + + EXPECT_FALSE(getNode(1).data.user_data.empty()); + EXPECT_TRUE(getNode(2).data.user_data.empty()); +} + +/// The first byte of the user data stored with @p node, or -1 if it has none. +int firstUserDataByte(const rtabmap_msgs::msg::Node & node) +{ + if(node.data.user_data.empty()) + { + return -1; + } + const cv::Mat userData = rtabmap::uncompressData( + rtabmap_conversions::compressedMatFromBytes(node.data.user_data)); + return userData.empty() ? -1 : int(userData.at(0, 0)); +} + +/** + * With subscribe_sensor_data too, user_data_async is attached to the next node. The + * SensorData message has its own user data field; when it is set, it wins, and the + * async user data is dropped with a warning rather than kept for a later node. + */ +TEST_F(CoreWrapperInputsTest, attaches_async_user_data_to_sensor_data_without_its_own) +{ + makeNode({rclcpp::Parameter("subscribe_sensor_data", true)}); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr data = + helper()->create_publisher("sensor_data", 10); + rclcpp::Publisher::SharedPtr userData = + helper()->create_publisher("user_data_async", 1); + ASSERT_TRUE(waitForSubscriber(data)); + ASSERT_TRUE(waitForSubscriber(userData)); + + const auto update = [&](double stamp, double x, int ownUserData) { + rtabmap::SensorData sensorData(cv::Mat(), 0, stamp, + ownUserData < 0 ? cv::Mat() : + rtabmap_conversions::userDataFromROS(makeUserData(stamp, uint8_t(ownUserData)))); + rtabmap_msgs::msg::SensorData msg; + rtabmap_conversions::sensorDataToROS(sensorData, msg, "base_link", true); + msg.header.stamp = stampOf(stamp); + const size_t before = info->size(); + sendOdom(odom, stamp, x); + data->publish(msg); + return spinUntil([&]() { return info->size() > before; }); + }; + + // No user data in the message: the async one is taken. + userData->publish(makeUserData(1.0, 7)); + spinFor(std::chrono::milliseconds(100)); + ASSERT_TRUE(update(1.0, 0.0, -1)); + + // User data in the message: it is kept, and the async one dropped... + userData->publish(makeUserData(2.0, 9)); + spinFor(std::chrono::milliseconds(100)); + ASSERT_TRUE(update(2.0, 0.5, 5)); + + // ...not carried over to the next node. + ASSERT_TRUE(update(3.0, 1.0, -1)); + + EXPECT_EQ(7, firstUserDataByte(getNode(1))); + EXPECT_EQ(5, firstUserDataByte(getNode(2))); + EXPECT_EQ(-1, firstUserDataByte(getNode(3))); +} + +/// A GPS fix is attached to the node closest in time, with its error from the covariance. +TEST_F(CoreWrapperInputsTest, attaches_gps_to_the_next_node) +{ + makeNode(); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr gps = + helper()->create_publisher("gps/fix", 1); + ASSERT_TRUE(waitForSubscriber(gps)); + + gps->publish(makeGpsFix(1.0, 45.3786, -71.9277, 250.0, 4.0)); + spinFor(std::chrono::milliseconds(100)); + driveStraight(odom, info, 1); + + rtabmap_msgs::msg::Node node = getNode(1); + EXPECT_NEAR(45.3786, node.data.gps.latitude, 1e-6); + EXPECT_NEAR(-71.9277, node.data.gps.longitude, 1e-6); + EXPECT_NEAR(250.0, node.data.gps.altitude, 1e-6); + EXPECT_NEAR(2.0, node.data.gps.error, 1e-6) << "sqrt of the largest variance"; +} + +/** + * The IMU orientation, interpolated at the node's stamp, is attached to it, and RTAB-Map + * turns it into a gravity constraint on the node -- a link from the node to itself that + * holds the graph's roll and pitch to gravity. The IMU messages here are a quarter and + * three quarters of the way around the node's stamp, so the orientation the link carries + * is interpolated, not the nearest one, nor their average. + */ +TEST_F(CoreWrapperInputsTest, attaches_imu_orientation_to_the_next_node) +{ + publishStaticTf("imu_link"); + makeNode(); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr imu = + helper()->create_publisher("imu", 10); + ASSERT_TRUE(waitForSubscriber(imu)); + + imu->publish(makeImu("imu_link", 0.9, 0.0)); + imu->publish(makeImu("imu_link", 1.3, 0.4)); + spinFor(std::chrono::milliseconds(100)); + driveStraight(odom, info, 1); // stamped 1.0: a quarter of the way from 0.9 to 1.3 + + const rtabmap_msgs::msg::Link * gravity = findLink(getGraph().graph, rtabmap::Link::kGravity, 1, 1); + ASSERT_TRUE(gravity != nullptr); + float roll, pitch, yaw; + rtabmap_conversions::transformFromGeometryMsg(gravity->transform).getEulerAngles(roll, pitch, yaw); + EXPECT_NEAR(0.1, roll, 1e-4); + EXPECT_NEAR(0.0, pitch, 1e-4); +} + +/** + * The orientation is re-expressed in base_link: an IMU mounted turned 90 degrees reports + * a roll about its own x axis, which is the robot's y axis, so the robot pitches. + */ +TEST_F(CoreWrapperInputsTest, expresses_imu_orientation_in_the_robot_frame) +{ + publishStaticTf(makeTransform("base_link", "imu_link", 0.0, 0.0, 0.0, M_PI/2.0)); + makeNode(); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr imu = + helper()->create_publisher("imu", 10); + ASSERT_TRUE(waitForSubscriber(imu)); + + imu->publish(makeImu("imu_link", 0.9, 0.1)); + imu->publish(makeImu("imu_link", 1.1, 0.1)); + spinFor(std::chrono::milliseconds(100)); + driveStraight(odom, info, 1); + + const rtabmap_msgs::msg::Link * gravity = findLink(getGraph().graph, rtabmap::Link::kGravity, 1, 1); + ASSERT_TRUE(gravity != nullptr); + float roll, pitch, yaw; + rtabmap_conversions::transformFromGeometryMsg(gravity->transform).getEulerAngles(roll, pitch, yaw); + EXPECT_NEAR(0.0, roll, 1e-4); + EXPECT_NEAR(0.1, pitch, 1e-4); // +0.1 about the robot's y axis +} + +/// With no IMU message after the node's stamp, there is nothing to interpolate: no link. +TEST_F(CoreWrapperInputsTest, adds_no_gravity_link_without_imu_around_the_stamp) +{ + publishStaticTf("imu_link"); + makeNode(); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr imu = + helper()->create_publisher("imu", 10); + ASSERT_TRUE(waitForSubscriber(imu)); + + imu->publish(makeImu("imu_link", 0.8, 0.1)); + imu->publish(makeImu("imu_link", 0.9, 0.1)); + spinFor(std::chrono::milliseconds(100)); + driveStraight(odom, info, 1); + + EXPECT_TRUE(findLink(getGraph().graph, rtabmap::Link::kGravity, 1, 1) == nullptr); +} + +/// An IMU message without an orientation carries nothing the node uses and is ignored. +TEST_F(CoreWrapperInputsTest, ignores_an_imu_without_orientation) +{ + publishStaticTf("imu_link"); + makeNode(); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr imu = + helper()->create_publisher("imu", 10); + ASSERT_TRUE(waitForSubscriber(imu)); + + sensor_msgs::msg::Imu msg = makeImu("imu_link", 0.9); + msg.orientation.w = 0.0; + imu->publish(msg); + msg.header.stamp = stampOf(1.1); + imu->publish(msg); + spinFor(std::chrono::milliseconds(100)); + driveStraight(odom, info, 1); + + EXPECT_FALSE(hasLink(getGraph().graph, rtabmap::Link::kGravity, 1, 1)); +} + +/** + * A landmark detection -- a fiducial seen by a camera -- becomes a landmark in the graph, + * under the negative of its id, linked to the node that saw it. + */ +TEST_F(CoreWrapperInputsTest, adds_detected_landmarks_to_the_graph) +{ + publishStaticTf("camera"); + makeNode(); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr landmark = + helper()->create_publisher("landmark_detection", 1); + ASSERT_TRUE(waitForSubscriber(landmark)); + + publishTf(makeTransform("odom", "base_link", 1.0)); + landmark->publish(makeLandmark("camera", 1.0, 5, 1.5)); + spinFor(std::chrono::milliseconds(100)); + driveStraight(odom, info, 1); + + rtabmap_msgs::msg::MapData map = getGraph(); + ASSERT_TRUE(hasPose(map.graph, -5)); + EXPECT_TRUE(hasLink(map.graph, rtabmap::Link::kLandmark, 1, -5)); + for(size_t i=0; i> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr landmark = + helper()->create_publisher("landmark_detection", 1); + ASSERT_TRUE(waitForSubscriber(landmark)); + + // Driving at 1 m/s: x = 0 at 1.0 s, x = 0.4 at 1.4 s. The tag is seen at 1.1 s, 1.5 m + // ahead: the robot was at x = 0.1 then, so the tag is at x = 1.6. + publishTf(makeTransform("odom", "base_link", 1.0, 0.0)); + publishTf(makeTransform("odom", "base_link", 1.4, 0.4)); + landmark->publish(makeLandmark("camera", 1.1, 5, 1.5)); + spinFor(std::chrono::milliseconds(100)); + driveStraight(odom, info, 1, 0.5, 1.4, 0.4); // the node, at 1.4 s and x = 0.4 + + rtabmap_msgs::msg::MapData map = getGraph(); + const rtabmap_msgs::msg::Link * link = findLink(map.graph, rtabmap::Link::kLandmark, 1, -5); + ASSERT_TRUE(link != nullptr); + // The graph may hold the link either way; seen from the node, the tag is 1.2 m ahead. + const double ahead = link->from_id == 1 ? link->transform.translation.x : -link->transform.translation.x; + EXPECT_NEAR(1.2, ahead, 1e-3); + bool found = false; + for(size_t i=0; i> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr landmark = + helper()->create_publisher("landmark_detection", 1); + ASSERT_TRUE(waitForSubscriber(landmark)); + + for(int i=0; i<3; ++i) + { + publishTf(makeTransform("odom", "base_link", 1.0 + i, 0.5*i)); + landmark->publish(makeLandmark("camera", 1.0 + i, 5, 1.5 - 0.5*i)); + spinFor(std::chrono::milliseconds(100)); + driveStraight(odom, info, 1, 0.5, 1.0 + i, 0.5*i); + } + + std::map x; + const rtabmap_msgs::msg::MapData map = getGraph(); + for(size_t k=0; k> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr landmark = + helper()->create_publisher("landmark_detection", 1); + ASSERT_TRUE(waitForSubscriber(landmark)); + + publishTf(makeTransform("odom", "base_link", 1.0)); + landmark->publish(makeLandmark("camera", 1.0, 0)); + spinFor(std::chrono::milliseconds(100)); + driveStraight(odom, info, 1); + + EXPECT_EQ(std::vector({1}), getGraph().graph.poses_id); +} + +/** + * global_pose -- an absolute pose from outside, a motion capture system say -- is + * attached to the next node as a pose prior: a link from the node to itself. + */ +TEST_F(CoreWrapperInputsTest, adds_a_global_pose_as_a_prior) +{ + makeNode(); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr global = + helper()->create_publisher("global_pose", 1); + ASSERT_TRUE(waitForSubscriber(global)); + + publishTf(makeTransform("odom", "base_link", 1.0)); + global->publish(makePoseWithCovariance("base_link", 1.0, 3.0)); + spinFor(std::chrono::milliseconds(100)); + driveStraight(odom, info, 1); + + EXPECT_TRUE(hasLink(getGraph().graph, rtabmap::Link::kPosePrior, 1, 1)); +} + +/** + * Like a landmark, a global pose is rarely stamped with the node it ends up in. It is + * moved forward by the robot's motion from its stamp to the node's, from the odometry in + * TF, interpolated between samples. + */ +TEST_F(CoreWrapperInputsTest, corrects_a_global_pose_for_the_motion_since_its_stamp) +{ + makeNode(); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr global = + helper()->create_publisher("global_pose", 1); + ASSERT_TRUE(waitForSubscriber(global)); + + // Driving at 1 m/s: x = 0 at 1.0 s, x = 0.4 at 1.4 s in odom. At 1.1 s, the robot is + // at x = 5.0 in the world, say the global pose: by 1.4 s it has moved 0.3 m further. + publishTf(makeTransform("odom", "base_link", 1.0, 0.0)); + publishTf(makeTransform("odom", "base_link", 1.4, 0.4)); + global->publish(makePoseWithCovariance("base_link", 1.1, 5.0)); + spinFor(std::chrono::milliseconds(100)); + driveStraight(odom, info, 1, 0.5, 1.4, 0.4); // the node, at 1.4 s + + const rtabmap_msgs::msg::Link * prior = findLink(getGraph().graph, rtabmap::Link::kPosePrior, 1, 1); + ASSERT_TRUE(prior != nullptr); + EXPECT_NEAR(5.3, prior->transform.translation.x, 1e-3) + << "5.0 would be uncorrected, 5.0 or 5.4 the correction from the nearest sample"; + EXPECT_NEAR(0.0, prior->transform.translation.y, 1e-3); +} + +TEST_F(CoreWrapperInputsTest, attaches_env_sensors_to_the_next_node) +{ + makeNode(); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr env = + helper()->create_publisher("env_sensor", 1); + ASSERT_TRUE(waitForSubscriber(env)); + + rtabmap_msgs::msg::EnvSensor msg; + msg.header.stamp = stampOf(1.0); + msg.type = rtabmap_msgs::msg::EnvSensor::TYPE_AMBIENT_TEMPERATURE; + msg.value = 21.5; + env->publish(msg); + spinFor(std::chrono::milliseconds(100)); + driveStraight(odom, info, 1); + + rtabmap_msgs::msg::Node node = getNode(1); + ASSERT_EQ(1u, node.data.env_sensors.size()); + EXPECT_EQ(rtabmap_msgs::msg::EnvSensor::TYPE_AMBIENT_TEMPERATURE, node.data.env_sensors[0].type); + EXPECT_DOUBLE_EQ(21.5, node.data.env_sensors[0].value); +} + +/** + * inter_odom fills the gaps between nodes with intermediate poses, when intermediate + * nodes are enabled and the detection rate is 0 -- the node is then driven by its sensor + * topics, and inter_odom is a faster odometry to interpolate the trajectory with. + * + * The stamps of those messages are compared with the update's, both in ROS time: built + * from their seconds and nanoseconds instead, they would be in system time, and rclcpp + * throws on a comparison across clocks. + */ +TEST_F(CoreWrapperInputsTest, inter_odom_adds_intermediate_nodes) +{ + makeNode({rclcpp::Parameter(Parameters::kRtabmapCreateIntermediateNodes(), "true")}); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr inter = + helper()->create_publisher("inter_odom", 10); + ASSERT_TRUE(waitForSubscriber(inter)); + + driveStraight(odom, info, 1); + inter->publish(makeOdometry(1.3, 0.15)); + inter->publish(makeOdometry(1.6, 0.3)); + spinFor(std::chrono::milliseconds(100)); + driveStraight(odom, info, 1, 0.5, 2.0, 0.5); + + EXPECT_EQ(4u, getGraph().graph.poses_id.size()); +} + +/** + * With subscribe_inter_odom_info, inter_odom is synchronized with inter_odom_info by exact + * stamp, so each intermediate node also gets the statistics of the odometry that + * produced it. A message on only one of the two is not used. + */ +TEST_F(CoreWrapperInputsTest, inter_odom_info_adds_intermediate_nodes) +{ + makeNode({rclcpp::Parameter(Parameters::kRtabmapCreateIntermediateNodes(), "true"), + rclcpp::Parameter("subscribe_inter_odom_info", true)}); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr inter = + helper()->create_publisher("inter_odom", 10); + rclcpp::Publisher::SharedPtr interInfo = + helper()->create_publisher("inter_odom_info", 10); + ASSERT_TRUE(waitForSubscriber(inter)); + ASSERT_TRUE(waitForSubscriber(interInfo)); + + driveStraight(odom, info, 1); + for(double stamp : {1.3, 1.6}) + { + nav_msgs::msg::Odometry msg = makeOdometry(stamp, 0.5*(stamp-1.0)); + rtabmap_msgs::msg::OdomInfo odomInfo; + odomInfo.header = msg.header; + odomInfo.time_estimation = 0.01f; + odomInfo.interval = 0.3f; + odomInfo.transform.translation.x = 0.15; + odomInfo.transform.rotation.w = 1.0; + inter->publish(msg); + interInfo->publish(odomInfo); + } + // Alone on inter_odom, with no inter_odom_info to pair with: dropped. + inter->publish(makeOdometry(1.8, 0.4)); + spinFor(std::chrono::milliseconds(200)); + driveStraight(odom, info, 1, 0.5, 2.0, 0.5); + + EXPECT_EQ(4u, getGraph().graph.poses_id.size()); +} + +//========================================================================================== +// Localization +//========================================================================================== + +class CoreWrapperLocalizationTest : public CoreWrapperInputsTest +{ +protected: + /// Builds a 3-node map along x and restarts on it in localization mode. + void restartInLocalization(const std::vector & params = {}) + { + { + makeNode(); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + driveStraight(odom, info, 3); + } + destroyNode(); + std::vector all = { + rclcpp::Parameter(Parameters::kMemIncrementalMemory(), "false")}; + all.insert(all.end(), params.begin(), params.end()); + makeNode(all); + } +}; + +/** + * In localization mode on a saved map, the map is loaded and not extended, and the pose + * starts where initial_pose says: it is added to the odometry until a loop closure + * localizes the robot for real. Until then the covariance says it is not localized. + */ +TEST_F(CoreWrapperLocalizationTest, initial_pose_places_the_robot_in_the_map) +{ + restartInLocalization({rclcpp::Parameter("initial_pose", "1 0 0 0 0 0")}); + std::shared_ptr> pose = + collect("localization_pose"); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + ASSERT_TRUE(waitForPublisher(pose->subscription)); + + driveStraight(odom, info, 2, 0.3, 10.0); + + EXPECT_EQ(3u, getGraph().graph.poses_id.size()); + ASSERT_TRUE(spinUntil([&]() { return pose->size() >= 2; })); + EXPECT_NEAR(1.3, pose->back().pose.pose.position.x, 1e-3); + EXPECT_EQ(9999.0, pose->back().pose.covariance[0]) << "not localized by a loop closure yet"; +} + +/// initialpose does the same at runtime, as RViz's "2D Pose Estimate" tool publishes it. +TEST_F(CoreWrapperLocalizationTest, initialpose_topic_places_the_robot_in_the_map) +{ + restartInLocalization(); + std::shared_ptr> pose = + collect("localization_pose"); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr initial = + helper()->create_publisher("initialpose", 1); + ASSERT_TRUE(waitForPublisher(pose->subscription)); + ASSERT_TRUE(waitForSubscriber(initial)); + + initial->publish(makePoseWithCovariance("map", 0.0, 0.5)); + spinFor(std::chrono::milliseconds(200)); + driveStraight(odom, info, 2, 0.3, 10.0); + + ASSERT_TRUE(spinUntil([&]() { return pose->size() >= 2; })); + EXPECT_NEAR(0.8, pose->back().pose.pose.position.x, 1e-3); +} + +/** + * loc_thr adds a "Localization status" entry to /diagnostics: an error until the + * localization covariance falls under the threshold. + */ +TEST_F(CoreWrapperLocalizationTest, reports_localization_status_on_diagnostics) +{ + restartInLocalization({rclcpp::Parameter("loc_thr", 0.25)}); + std::shared_ptr> diagnostics = + collect("/diagnostics"); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + + driveStraight(odom, info, 1, 0.3, 10.0); + + const diagnostic_msgs::msg::DiagnosticStatus * status = nullptr; + ASSERT_TRUE(spinUntil([&]() { + for(const auto & msg : diagnostics->messages) + { + for(const diagnostic_msgs::msg::DiagnosticStatus & s : msg->status) + { + if(s.name.find("Localization status") != std::string::npos) + { + status = &s; + } + } + } + return status != nullptr; }, std::chrono::milliseconds(5000))); + EXPECT_EQ(diagnostic_msgs::msg::DiagnosticStatus::ERROR, status->level); +} + +} // namespace + +} // namespace rtabmap_slam_test diff --git a/rtabmap_slam/test/test_core_wrapper_mapping.cpp b/rtabmap_slam/test/test_core_wrapper_mapping.cpp new file mode 100644 index 00000000..803e8224 --- /dev/null +++ b/rtabmap_slam/test/test_core_wrapper_mapping.cpp @@ -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 + +#include +#include + +#include + +#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 xs(const rtabmap_msgs::msg::MapGraph & graph) + { + std::vector 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> & 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> info = collectInfo(); + rclcpp::Publisher::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 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> info = collectInfo(); + rclcpp::Publisher::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> info = collectInfo(); + rclcpp::Publisher::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> info = collectInfo(); + rclcpp::Publisher::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> info = collectInfo(); + rclcpp::Publisher::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({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> info = collectInfo(); + rclcpp::Publisher::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({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> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + + driveStraight(odom, info, 2); + driveStraight(odom, info, 2, 0.5, 6.0, 1.0); + + EXPECT_EQ(std::vector({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> info = collectInfo(); + rclcpp::Publisher::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> info = collectInfo(); + rclcpp::Publisher::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> info = collectInfo(); + rclcpp::Publisher::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> tf = + collect("/tf", rclcpp::QoS(100)); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::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> tf = + collect("/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> tf = + collect("/tf", rclcpp::QoS(100)); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::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> tf = + collect("/tf", rclcpp::QoS(100)); + std::shared_ptr> path = + collect("mapPath"); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::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> path = + collect("mapPath"); + std::shared_ptr> graph = + collect("mapGraph", + rclcpp::QoS(1).reliable().transient_local()); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::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> pose = + collect("localization_pose"); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::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> pose = + collect("localization_pose"); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::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> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + driveStraight(odom, info, 2); + } + destroyNode(); + + makeNode(); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + driveStraight(odom, info, 2, 0.5, 10.0); + + EXPECT_EQ(3, info->front().ref_id); + EXPECT_EQ(std::vector({0, 0, 1, 1}), mapIds()); +} + +} // namespace + +} // namespace rtabmap_slam_test diff --git a/rtabmap_slam/test/test_core_wrapper_parameters.cpp b/rtabmap_slam/test/test_core_wrapper_parameters.cpp new file mode 100644 index 00000000..285c113d --- /dev/null +++ b/rtabmap_slam/test/test_core_wrapper_parameters.cpp @@ -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 + +#include +#include + +#include + +#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(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(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 diff --git a/rtabmap_slam/test/test_core_wrapper_planning.cpp b/rtabmap_slam/test/test_core_wrapper_planning.cpp new file mode 100644 index 00000000..3101e495 --- /dev/null +++ b/rtabmap_slam/test/test_core_wrapper_planning.cpp @@ -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 + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#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("goal_out"); + goalReached_ = collect("goal_reached"); + globalPath_ = collect("global_path"); + globalPathNodes_ = collect("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(); + req->node_id = id; + req->node_label = label; + return call("set_goal", req); + } + + virtual std::vector 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> goalOut_; + std::shared_ptr> goalReached_; + std::shared_ptr> globalPath_; + std::shared_ptr> globalPathNodes_; + std::shared_ptr> info_; + rclcpp::Publisher::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(); + label->node_id = 2; + label->node_label = "kitchen"; + ASSERT_TRUE(call("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::SharedPtr goal = + helper()->create_publisher("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({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::SharedPtr goal = + helper()->create_publisher("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({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 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("/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::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::SharedPtr goal = + helper()->create_publisher("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> tf = + collect("/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::SharedPtr goal = + helper()->create_publisher("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::SharedPtr goal = + helper()->create_publisher("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(); + 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("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(); + req->goal_node = 2; + rtabmap_msgs::srv::GetPlan::Response::SharedPtr res = + call("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 diff --git a/rtabmap_slam/test/test_core_wrapper_services.cpp b/rtabmap_slam/test/test_core_wrapper_services.cpp new file mode 100644 index 00000000..f7ba4322 --- /dev/null +++ b/rtabmap_slam/test/test_core_wrapper_services.cpp @@ -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 + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#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 & 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("list_labels"); + } + + bool setLabel(int id, const std::string & label) + { + rtabmap_msgs::srv::SetLabel::Request::SharedPtr req = + std::make_shared(); + req->node_id = id; + req->node_label = label; + return call("set_label", req).get() != nullptr; + } + + std::shared_ptr> info_; + rclcpp::Publisher::SharedPtr odom_; +}; + +/// Every service the node offers is advertised under its own name, /rtabmap/. +TEST_F(CoreWrapperServicesTest, advertises_its_services_under_its_name) +{ + makeNode(); + const std::vector 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> 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({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(); + req->label = "kitchen"; + ASSERT_TRUE(call("remove_label", req).get() != nullptr); + + rtabmap_msgs::srv::ListLabels::Response::SharedPtr labels = listLabels(); + ASSERT_TRUE(labels.get() != nullptr); + EXPECT_EQ(std::vector({"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({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("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 & extra = {}) + { + publishStaticTf("laser", 0.1); + publishOpticalTf("camera", kCameraHeight); + + std::vector 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::SharedPtr rgbd; + rclcpp::Publisher::SharedPtr scan; + rclcpp::Publisher::SharedPtr userData; + rclcpp::Publisher::SharedPtr sensorData; + if(input == MapInput::RgbdAndScan) + { + rgbd = helper()->create_publisher("rgbd_image", 10); + scan = helper()->create_publisher("scan", 10); + userData = helper()->create_publisher("user_data_async", 1); + ASSERT_TRUE(waitForSubscriber(rgbd)); + ASSERT_TRUE(waitForSubscriber(scan)); + ASSERT_TRUE(waitForSubscriber(userData)); + } + else + { + sensorData = helper()->create_publisher("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( + 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(); + 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("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(); + req->global_map = true; + req->optimized = true; + req->graph_only = graphOnly; + rtabmap_msgs::srv::GetMap::Response::SharedPtr res = + call("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 +{ +protected: + void SetUp() override + { + CoreWrapperMapPayloadsBase::SetUp(); + buildMap(GetParam()); + } +}; + +INSTANTIATE_TEST_SUITE_P(Inputs, CoreWrapperMapPayloadsTest, + ::testing::Values(MapInput::RgbdAndScan, MapInput::SensorData), + [](const ::testing::TestParamInfo & 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> 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(); + req->ids = {1}; + req->images = images; + req->scan = scan; + req->grid = grid; + req->user_data = userData; + rtabmap_msgs::srv::GetNodeData::Response::SharedPtr res = + call("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> keypoints(const rtabmap_msgs::msg::Node & node) + { + std::vector> 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> points(const rtabmap_msgs::msg::Node & node) + { + std::vector> 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(); + req->node_id = 1; + req->radius = 0.6f; + rtabmap_msgs::srv::GetNodesInRadius::Response::SharedPtr res = + call("get_nodes_in_radius", req); + ASSERT_TRUE(res.get() != nullptr); + std::vector ids = res->ids; + EXPECT_EQ(std::vector({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("get_nodes_in_radius", req); + ASSERT_TRUE(res.get() != nullptr); + ids = res->ids; + std::sort(ids.begin(), ids.end()); + EXPECT_EQ(std::vector({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({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 .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({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(); + req->database_path = dir() + "/other.db"; + req->clear = true; + ASSERT_TRUE(call("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("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(); + req->database_path = dir() + "/no/such/dir/other.db"; + ASSERT_TRUE(call("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> graph = + collect("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(); + req->global_map = true; + req->optimized = true; + req->graph_only = true; + ASSERT_TRUE(call("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(); + 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("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 diff --git a/rtabmap_util/README.md b/rtabmap_util/README.md index 4e86b566..db6badd6 100644 --- a/rtabmap_util/README.md +++ b/rtabmap_util/README.md @@ -8,6 +8,7 @@ Every node is a [composable node](https://docs.ros.org/en/jazzy/Tutorials/Interm - [Nodes](#nodes) - [Library](#library) + - [MapsManager](#mapsmanager) - [Conventions](#conventions) ## Nodes @@ -51,7 +52,58 @@ One page per node. The package also installs a small C++ library, whose API is documented in the [C++ API reference](https://docs.ros.org/en/jazzy/p/rtabmap_util/generated/index.html) generated from the headers. -`MapsManager` is the piece worth knowing about: it turns a pose graph plus per-node occupancy grids into the assembled clouds, occupancy grid, octomap and elevation map, and publishes them. Both [map_assembler](doc/map_assembler.md) and `rtabmap_slam`'s `rtabmap` node use it, which is why their map outputs and `Grid/*` parameters behave identically. +`MapsManager` is the piece worth knowing about: it turns a pose graph plus per-node occupancy grids into the assembled clouds, occupancy grid, octomap and elevation map, and publishes them. Both [map_assembler](doc/map_assembler.md) and [`rtabmap_slam`](../rtabmap_slam/README.md)'s `rtabmap` node use it, which is why their map outputs and `Grid/*` parameters behave identically. It is described below. + +### MapsManager + +**Published topics.** Everything is published only when subscribed, and -- by default -- **latched**, so a subscriber joining late immediately receives the current map. + +In a component container with intra-process communication enabled (`use_intra_process_comms`), these publishers automatically opt out of it when `latch` is on, since intra-process communication does not support transient local durability. With `latch` off, they keep the container's setting. + +| Topic | Type | Description | +|---|---|---| +| `cloud_map` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Ground and obstacles together. | +| `cloud_ground` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Ground only, colored green. | +| `cloud_obstacles` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Obstacles only, colored red. | +| `map` | [`nav_msgs/msg/OccupancyGrid`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/OccupancyGrid.html) | The 2D occupancy grid, the one navigation wants. | +| `grid_prob_map` | [`nav_msgs/msg/OccupancyGrid`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/OccupancyGrid.html) | The same grid as occupancy probabilities rather than free/occupied/unknown. | +| `octomap_occupied_space`, `octomap_obstacles`, `octomap_ground`, `octomap_empty_space`, `octomap_global_frontier_space` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Octomap contents, one cloud per category. Requires RTAB-Map built with OctoMap. | +| `octomap_grid` | [`nav_msgs/msg/OccupancyGrid`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/OccupancyGrid.html) | The octomap projected to 2D. | +| `octomap_binary`, `octomap_full` | [`octomap_msgs/msg/Octomap`](https://docs.ros.org/en/jazzy/p/octomap_msgs/msg/Octomap.html) | The tree itself, for `octovis` or other octomap consumers. Serialized as a **`ColorOcTree`**, see [Octomap tree type](#octomap-tree-type). | +| `elevation_map` | [`grid_map_msgs/msg/GridMap`](https://github.com/ANYbotics/grid_map/blob/master/grid_map_msgs/msg/GridMap.msg) | Elevation map. Requires RTAB-Map built with `grid_map`. | + +**Parameters.** + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `latch` | `bool` | `true` | Publish with transient-local durability so late subscribers get the current map. | +| `map_filter_radius` | `double` | `0.0` | Skip nodes closer together than this, in meters. A cheap way to thin a dense graph. `0` disables. | +| `map_filter_angle` | `double` | `30.0` | With `map_filter_radius`, nodes are only merged if they also differ by less than this angle, in degrees. | +| `map_always_update` | `bool` | `false` | Also assemble the latest sensor data, not yet a node, so the maps update even when the robot stands still and no node is added. | +| `map_empty_ray_tracing` | `bool` | `true` | For that latest data, fill the 2D scan's rays with empty cells (`Grid/Scan2dUnknownSpaceFilled`). | +| `map_cleanup` | `bool` | `true` | Free the cached clouds when nobody is subscribed. | +| `cloud_output_voxelized` | `bool` | `true` | Voxelize the assembled clouds at `Grid/CellSize`. | +| `cloud_subtract_filtering` | `bool` | `false` | Drop points that duplicate ones already in the map. Slower, smaller output. | +| `cloud_subtract_filtering_min_neighbors` | `int` | `2` | Neighbors needed for a point to count as a duplicate. | +| `octomap_tree_depth` | `int` | `16` | Depth the octomap clouds are generated at. Lower means coarser and faster. Maximum 16. | + +`map_always_update` and `map_empty_ray_tracing` only apply to the latest sensor data, not yet committed as a node, which only the `rtabmap` node has: they do nothing in `map_assembler`. + +Every RTAB-Map **`Grid/*`**, **`GridGlobal/*`**, **`StereoBM/*`** and **`StereoSGBM/*`** parameter is also exposed, all documented in RTAB-Map's [parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html). The split between the first two is worth knowing: **`Grid/*`** decides how each node's local grid is built from its sensor data -- the same segmentation [obstacles_detection](doc/obstacles_detection.md#parameters) does, and the parameters listed there apply here too -- while **`GridGlobal/*`** decides how those local grids are merged into the global map, so it covers the map's minimum size, its occupancy threshold, and how far the graph must move before the whole map is rebuilt. + +#### Octomap tree type + +RTAB-Map keeps a color per voxel, so the tree it publishes on `octomap_binary` and `octomap_full` reports its `id` as **`ColorOcTree`**, not the plain `OcTree` many examples assume. + +That is deliberate and interoperable: `octomap_msgs::binaryMsgToMap()` and `fullMsgToMap()` branch on that `id` and hand you back an `octomap::ColorOcTree`, and `octovis` opens it without complaint. What does break is code that assumes the other branch: + +```cpp +octomap::AbstractOcTree * tree = octomap_msgs::binaryMsgToMap(msg); +octomap::OcTree * octree = dynamic_cast(tree); // null +octomap::ColorOcTree * octree = dynamic_cast(tree); // ok +``` + +`ColorOcTree` does not derive from `OcTree` -- both derive from `OccupancyOcTreeBase` -- so cast to `ColorOcTree`, or to `octomap::OccupancyOcTreeBase<...>` if you only need occupancy and want to accept either. ## Conventions diff --git a/rtabmap_util/doc/map_assembler.md b/rtabmap_util/doc/map_assembler.md index ee7208a5..2fd4e96f 100644 --- a/rtabmap_util/doc/map_assembler.md +++ b/rtabmap_util/doc/map_assembler.md @@ -6,7 +6,7 @@ RTAB-Map publishes its graph and the per-node sensor data on `mapData`; turning It also lets you produce maps RTAB-Map is not currently configured to publish, or several differently-configured maps at once, without restarting SLAM. -The assembling itself is done by `MapsManager`, which is shared with `rtabmap_slam` — the outputs and every `Grid/*` parameter behave identically in both. +The assembling itself is done by [`MapsManager`](../README.md#mapsmanager), which is shared with `rtabmap_slam` — the outputs and every `Grid/*` parameter behave identically in both. ## Contents @@ -15,7 +15,6 @@ The assembling itself is done by `MapsManager`, which is shared with `rtabmap_sl - [Published Topics](#published-topics) - [Services](#services) - [Parameters](#parameters) -- [Octomap tree type](#octomap-tree-type) - [Start-up](#start-up) - [Notes](#notes) @@ -35,6 +34,8 @@ ComposableNode( 'cloud_output_voxelized': True}]) ``` +In a component container with intra-process communication enabled (`use_intra_process_comms`), the map publishers automatically opt out of it when `latch` is on (the default), since intra-process communication does not support transient local durability. With `latch` off, they keep the container's setting. See [`MapsManager`](../README.md#mapsmanager). + The graph comes from the SLAM node; the maps are built here, off its critical path. Nothing forces the split across machines — a second process on the robot works too — but only `mapData` crosses the boundary, so putting the assembling on a workstation keeps the heavy topics off the link as well as off the robot's CPU: ```mermaid @@ -60,19 +61,7 @@ flowchart LR ## Published Topics -Everything is published only when subscribed, and — by default — **latched**, so a subscriber joining late immediately receives the current map. - -| Topic | Type | Description | -|---|---|---| -| `cloud_map` | [`PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Ground and obstacles together. | -| `cloud_ground` | `PointCloud2` | Ground only, colored green. | -| `cloud_obstacles` | `PointCloud2` | Obstacles only, colored red. | -| `map` | [`OccupancyGrid`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/OccupancyGrid.html) | The 2D occupancy grid, the one navigation wants. | -| `grid_prob_map` | `OccupancyGrid` | The same grid as occupancy probabilities rather than free/occupied/unknown. | -| `octomap_occupied_space`, `octomap_obstacles`, `octomap_ground`, `octomap_empty_space`, `octomap_global_frontier_space` | `PointCloud2` | Octomap contents, one cloud per category. Requires RTAB-Map built with OctoMap. | -| `octomap_grid` | `OccupancyGrid` | The octomap projected to 2D. | -| `octomap_binary`, `octomap_full` | [`Octomap`](https://docs.ros.org/en/jazzy/p/octomap_msgs/msg/Octomap.html) | The tree itself, for `octovis` or other octomap consumers. Serialized as a **`ColorOcTree`**, see [Octomap tree type](#octomap-tree-type). | -| `elevation_map` | [`GridMap`](https://github.com/ANYbotics/grid_map/blob/master/grid_map_msgs/msg/GridMap.msg) | Elevation map. Requires RTAB-Map built with `grid_map`. | +The maps assembled by [`MapsManager`](../README.md#mapsmanager): point clouds, occupancy grids, octomap and elevation map, published only when subscribed and latched by default. The topics are listed [there](../README.md#mapsmanager). ## Services @@ -91,38 +80,7 @@ Everything is published only when subscribed, and — by default — **latched** | `regenerate_local_grids` | `bool` | `false` | Discard the occupancy grids stored with each node and rebuild them from the raw sensor data. Use it to change `Grid/*` parameters on an existing map without re-running SLAM. Costs CPU per node. | | `config_path` | `string` | `""` | An RTAB-Map `.ini` file to load parameters from, instead of listing them individually. | -**Map assembly**, from `MapsManager` - -| Parameter | Type | Default | Description | -|---|---|---|---| -| `latch` | `bool` | `true` | Publish with transient-local durability so late subscribers get the current map. | -| `map_filter_radius` | `double` | `0.0` | Skip nodes closer together than this, in meters. A cheap way to thin a dense graph. `0` disables. | -| `map_filter_angle` | `double` | `30.0` | With `map_filter_radius`, nodes are only merged if they also differ by less than this angle, in degrees. | -| `map_always_update` | `bool` | `false` | **No effect here**, see below. | -| `map_empty_ray_tracing` | `bool` | `true` | **No effect here**, see below. | -| `map_cleanup` | `bool` | `true` | Free the cached clouds when nobody is subscribed. | -| `cloud_output_voxelized` | `bool` | `true` | Voxelize the assembled clouds at `Grid/CellSize`. | -| `cloud_subtract_filtering` | `bool` | `false` | Drop points that duplicate ones already in the map. Slower, smaller output. | -| `cloud_subtract_filtering_min_neighbors` | `int` | `2` | Neighbors needed for a point to count as a duplicate. | -| `octomap_tree_depth` | `int` | `16` | Depth the octomap clouds are generated at. Lower means coarser and faster. Maximum 16. | - -`map_always_update` and `map_empty_ray_tracing` are declared because they come with `MapsManager`, but neither does anything in this node. Both only apply to the *current*, not-yet-committed node, which `MapsManager` identifies by the pose id `0`. That node is assembled inside `rtabmap_slam`'s `rtabmap` node from its live sensor data and is never published on `mapData`, so the graph reaching `map_assembler` only ever contains committed nodes. Set them on the `rtabmap` node instead, where they do apply. - -Every RTAB-Map **`Grid/*`**, **`GridGlobal/*`**, **`StereoBM/*`** and **`StereoSGBM/*`** parameter is also exposed, all documented in RTAB-Map's [parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html). The split between the first two is worth knowing: **`Grid/*`** decides how each node's local grid is built from its sensor data — the same segmentation [obstacles_detection](obstacles_detection.md#parameters) does, and the parameters listed there apply here too — while **`GridGlobal/*`** decides how those local grids are merged into the global map, so it covers the map's minimum size, its occupancy threshold, and how far the graph must move before the whole map is rebuilt. - -## Octomap tree type - -RTAB-Map keeps a color per voxel, so the tree it publishes on `octomap_binary` and `octomap_full` reports its `id` as **`ColorOcTree`**, not the plain `OcTree` many examples assume. - -That is deliberate and interoperable: `octomap_msgs::binaryMsgToMap()` and `fullMsgToMap()` branch on that `id` and hand you back an `octomap::ColorOcTree`, and `octovis` opens it without complaint. What does break is code that assumes the other branch: - -```cpp -octomap::AbstractOcTree * tree = octomap_msgs::binaryMsgToMap(msg); -octomap::OcTree * octree = dynamic_cast(tree); // null -octomap::ColorOcTree * octree = dynamic_cast(tree); // ok -``` - -`ColorOcTree` does not derive from `OcTree` — both derive from `OccupancyOcTreeBase` — so cast to `ColorOcTree`, or to `octomap::OccupancyOcTreeBase<...>` if you only need occupancy and want to accept either. +**Map assembly**: the parameters of [`MapsManager`](../README.md#mapsmanager), and every RTAB-Map `Grid/*`, `GridGlobal/*`, `StereoBM/*` and `StereoSGBM/*` parameter, as described there. Two of them do nothing here: `map_always_update` and `map_empty_ray_tracing`. ## Start-up diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index a6fa55f2..848676da 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -136,40 +136,47 @@ void MapsManager::init(rclcpp::Node & node, const std::string & name, bool) // mapping topics latched_.clear(); - gridMapPub_ = node.create_publisher("map", rclcpp::QoS(1).reliable().durability(latching_?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 these publishers, otherwise keep the node's setting. + rclcpp::PublisherOptions pubOptions; + if(latching_) + { + pubOptions.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + } + gridMapPub_ = node.create_publisher("map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions); latched_.insert(std::make_pair((void*)&gridMapPub_, false)); - gridProbMapPub_ = node.create_publisher("grid_prob_map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); + gridProbMapPub_ = node.create_publisher("grid_prob_map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions); latched_.insert(std::make_pair((void*)&gridProbMapPub_, false)); - cloudMapPub_ = node.create_publisher("cloud_map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); + cloudMapPub_ = node.create_publisher("cloud_map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions); latched_.insert(std::make_pair((void*)&cloudMapPub_, false)); - cloudObstaclesPub_ = node.create_publisher("cloud_obstacles", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); + cloudObstaclesPub_ = node.create_publisher("cloud_obstacles", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions); latched_.insert(std::make_pair((void*)&cloudObstaclesPub_, false)); - cloudGroundPub_ = node.create_publisher("cloud_ground", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); + cloudGroundPub_ = node.create_publisher("cloud_ground", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions); latched_.insert(std::make_pair((void*)&cloudGroundPub_, false)); #ifdef RTABMAP_OCTOMAP #ifdef WITH_OCTOMAP_MSGS - octoMapPubBin_ = node.create_publisher("octomap_binary", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); + octoMapPubBin_ = node.create_publisher("octomap_binary", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions); latched_.insert(std::make_pair((void*)&octoMapPubBin_, false)); - octoMapPubFull_ = node.create_publisher("octomap_full", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); + octoMapPubFull_ = node.create_publisher("octomap_full", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions); latched_.insert(std::make_pair((void*)&octoMapPubFull_, false)); #endif - octoMapCloud_ = node.create_publisher("octomap_occupied_space", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); // FIXME latching option in ROS2? + octoMapCloud_ = node.create_publisher("octomap_occupied_space", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions); // FIXME latching option in ROS2? latched_.insert(std::make_pair((void*)&octoMapCloud_, false)); - octoMapFrontierCloud_ = node.create_publisher("octomap_global_frontier_space", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); + octoMapFrontierCloud_ = node.create_publisher("octomap_global_frontier_space", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions); latched_.insert(std::make_pair((void*)&octoMapFrontierCloud_, false)); - octoMapObstacleCloud_ = node.create_publisher("octomap_obstacles", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); + octoMapObstacleCloud_ = node.create_publisher("octomap_obstacles", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions); latched_.insert(std::make_pair((void*)&octoMapObstacleCloud_, false)); - octoMapGroundCloud_ = node.create_publisher("octomap_ground", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); + octoMapGroundCloud_ = node.create_publisher("octomap_ground", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions); latched_.insert(std::make_pair((void*)&octoMapGroundCloud_, false)); - octoMapEmptySpace_ = node.create_publisher("octomap_empty_space", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); + octoMapEmptySpace_ = node.create_publisher("octomap_empty_space", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions); latched_.insert(std::make_pair((void*)&octoMapEmptySpace_, false)); - octoMapProj_ = node.create_publisher("octomap_grid", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); + octoMapProj_ = node.create_publisher("octomap_grid", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions); latched_.insert(std::make_pair((void*)&octoMapProj_, false)); #endif #if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) - elevationMapPub_ = node.create_publisher("elevation_map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); + elevationMapPub_ = node.create_publisher("elevation_map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions); latched_.insert(std::make_pair((void*)&elevationMapPub_, false)); #endif } diff --git a/rtabmap_viz/src/GuiWrapper.cpp b/rtabmap_viz/src/GuiWrapper.cpp index 9190794b..61fd4f72 100644 --- a/rtabmap_viz/src/GuiWrapper.cpp +++ b/rtabmap_viz/src/GuiWrapper.cpp @@ -71,7 +71,7 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) : frameId_("base_link"), odomFrameId_(""), waitForTransform_(0.2), // 200 ms - odomSensorSync_(false), + odomSensorSync_(true), maxOdomUpdateRate_(10) { tfBuffer_ = std::make_shared(this->get_clock());