From 38b45dca161cea224702e5afcb9d9475911b2b1d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 6 Aug 2026 14:50:22 -0700 Subject: [PATCH 01/17] Update for rtabmap 0.23.10 (#1442) * Update for rtabmap 0.23.10 * fixing min version * Fixed PythonInterface compilation error * CI Added workflow_dispatch --- .github/workflows/docker.yml | 4 ++++ .github/workflows/noetic-pr.yml | 1 + rtabmap_conversions/CMakeLists.txt | 2 +- rtabmap_odom/src/RGBDICPOdometryNode.cpp | 4 +++- rtabmap_odom/src/RGBDOdometryNode.cpp | 4 +++- rtabmap_odom/src/StereoOdometryNode.cpp | 4 +++- rtabmap_slam/src/CoreNode.cpp | 9 +++++++++ rtabmap_slam/src/CoreWrapper.cpp | 4 ++-- 8 files changed, 26 insertions(+), 6 deletions(-) diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index ab73b32e..0222175d 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -4,9 +4,13 @@ on: push: branches: - 'master' + workflow_dispatch: jobs: docker: + # Built and pushed only from master (push or manual dispatch), since it + # pushes the introlab3it/rtabmap_ros tags to Docker Hub. + if: github.ref == 'refs/heads/master' runs-on: ubuntu-latest strategy: diff --git a/.github/workflows/noetic-pr.yml b/.github/workflows/noetic-pr.yml index b984137f..710247cc 100644 --- a/.github/workflows/noetic-pr.yml +++ b/.github/workflows/noetic-pr.yml @@ -4,6 +4,7 @@ on: pull_request: branches: - 'master' + workflow_dispatch: concurrency: group: ${{ github.workflow }}-${{ github.ref }} diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index 01882399..69b06910 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/rtabmap_conversions/CMakeLists.txt @@ -7,7 +7,7 @@ find_package(catkin REQUIRED COMPONENTS image_geometry rtabmap_msgs ) -find_package(RTABMap 0.23.5 REQUIRED) +find_package(RTABMap 0.23.10 REQUIRED) catkin_package( INCLUDE_DIRS include diff --git a/rtabmap_odom/src/RGBDICPOdometryNode.cpp b/rtabmap_odom/src/RGBDICPOdometryNode.cpp index e86c8139..fe6bb7ac 100644 --- a/rtabmap_odom/src/RGBDICPOdometryNode.cpp +++ b/rtabmap_odom/src/RGBDICPOdometryNode.cpp @@ -73,7 +73,9 @@ int main(int argc, char **argv) } #ifdef RTABMAP_PYTHON - rtabmap::PythonInterface pythonInterface; + // Initialize the embedded python interpreter on the main thread, as + // the nodelet below is loaded in a worker thread. + rtabmap::PythonInterface::instance("rgbdicp_odometry"); #endif nodelet::Loader nodelet; diff --git a/rtabmap_odom/src/RGBDOdometryNode.cpp b/rtabmap_odom/src/RGBDOdometryNode.cpp index b3d9462e..60c081c3 100644 --- a/rtabmap_odom/src/RGBDOdometryNode.cpp +++ b/rtabmap_odom/src/RGBDOdometryNode.cpp @@ -73,7 +73,9 @@ int main(int argc, char **argv) } #ifdef RTABMAP_PYTHON - rtabmap::PythonInterface pythonInterface; + // Initialize the embedded python interpreter on the main thread, as + // the nodelet below is loaded in a worker thread. + rtabmap::PythonInterface::instance("rgbd_odometry"); #endif nodelet::Loader nodelet; diff --git a/rtabmap_odom/src/StereoOdometryNode.cpp b/rtabmap_odom/src/StereoOdometryNode.cpp index 8d6fc219..4d468e12 100644 --- a/rtabmap_odom/src/StereoOdometryNode.cpp +++ b/rtabmap_odom/src/StereoOdometryNode.cpp @@ -74,7 +74,9 @@ int main(int argc, char **argv) } #ifdef RTABMAP_PYTHON - rtabmap::PythonInterface pythonInterface; + // Initialize the embedded python interpreter on the main thread, as + // the nodelet below is loaded in a worker thread. + rtabmap::PythonInterface::instance("stereo_odometry"); #endif nodelet::Loader nodelet; diff --git a/rtabmap_slam/src/CoreNode.cpp b/rtabmap_slam/src/CoreNode.cpp index 0d47805e..bdaf58ca 100644 --- a/rtabmap_slam/src/CoreNode.cpp +++ b/rtabmap_slam/src/CoreNode.cpp @@ -32,6 +32,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#ifdef RTABMAP_PYTHON +#include +#endif #include "nodelet/loader.h" int main(int argc, char** argv) @@ -88,6 +91,12 @@ int main(int argc, char** argv) nargv.push_back(argv[i]); } +#ifdef RTABMAP_PYTHON + // Initialize the embedded python interpreter on the main thread, as + // the nodelet below is loaded in a worker thread. + rtabmap::PythonInterface::instance("rtabmap"); +#endif + nodelet::Loader nodelet; nodelet::M_string remap(ros::names::getRemappings()); std::string nodelet_name = ros::this_node::getName(); diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index f6eeeae6..85a0fb75 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -3332,10 +3332,10 @@ bool CoreWrapper::globalBundleAdjustmentCallback(rtabmap_msgs::GlobalBundleAdjus UTimer timer; int optimizer = (int)Optimizer::kTypeG2O; // g2o int iterations = Parameters::defaultOptimizerIterations(); - float pixelVariance = Parameters::defaultg2oPixelVariance(); + float pixelVariance = Parameters::defaultOptimizerPixelVariance(); bool rematchFeatures = true; Parameters::parse(parameters_, Parameters::kOptimizerIterations(), iterations); - Parameters::parse(parameters_, Parameters::kg2oPixelVariance(), pixelVariance); + Parameters::parse(parameters_, Parameters::kOptimizerPixelVariance(), pixelVariance); if(req.type == 1.0f) { optimizer = (int)Optimizer::kTypeCVSBA; From accefde0fbe49932b78eacf8842c8d6a0d82955c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 6 Aug 2026 15:02:30 -0700 Subject: [PATCH 02/17] CI: added dispatch_workflow for ros2 jobs --- .github/workflows/docker-ros2.yml | 3 +++ .github/workflows/ros2.yml | 1 + 2 files changed, 4 insertions(+) diff --git a/.github/workflows/docker-ros2.yml b/.github/workflows/docker-ros2.yml index 3c280a56..1f55e19e 100644 --- a/.github/workflows/docker-ros2.yml +++ b/.github/workflows/docker-ros2.yml @@ -5,6 +5,7 @@ on: branches: [ ros2 ] pull_request: branches: [ ros2 ] + workflow_dispatch: concurrency: group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} @@ -12,6 +13,8 @@ concurrency: jobs: docker: + # A manual dispatch is honored only on ros2, the only ref we push from. + if: ${{ github.event_name != 'workflow_dispatch' || github.ref == 'refs/heads/ros2' }} runs-on: ubuntu-latest strategy: diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index de8d33a9..f09cda7c 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -6,6 +6,7 @@ on: branches: [ ros2 ] pull_request: branches: [ ros2 ] + workflow_dispatch: env: BUILD_TYPE: Release From 65de131a814aa0386c01314f0b2d96ac0ee7b5ad Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 7 Aug 2026 11:11:30 -0700 Subject: [PATCH 03/17] fixing rolling ci apt source, changed name of docker workflow on ros2 to docker-ros2 --- .github/workflows/docker-ros2.yml | 2 +- .github/workflows/ros2.yml | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/docker-ros2.yml b/.github/workflows/docker-ros2.yml index 1f55e19e..6c9f7957 100644 --- a/.github/workflows/docker-ros2.yml +++ b/.github/workflows/docker-ros2.yml @@ -1,4 +1,4 @@ -name: docker +name: docker-ros2 on: push: diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index f09cda7c..d4807d5f 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -38,6 +38,7 @@ jobs: packages: 'rtabmap_launch rtabmap_demos rtabmap_python rtabmap_examples rtabmap_rviz_plugins' - ros_distro: rolling skip_keys: 'nav2_bringup nav2_msgs velodyne' + use_ros2_testing: true # Rolling is using ros2-testing (nightly) # rtabmap_costmap_plugins cannot be built, missing nav2 on rolling, build other packages: packages: 'rtabmap_launch rtabmap_demos rtabmap_python rtabmap_examples rtabmap_rviz_plugins' fail-fast: false @@ -48,6 +49,7 @@ jobs: - uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: ${{ matrix.ros_distro }} + use-ros2-testing: ${{ matrix.use_ros2_testing || false }} - run: | DEBIAN_FRONTEND=noninteractive sudo apt update From 8f35accc416d2f12103fa63e46e5285e15e9f99a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 7 Aug 2026 11:21:02 -0700 Subject: [PATCH 04/17] rolling ci: skip not available dep packages --- .github/workflows/ros2.yml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index d4807d5f..f0be82e9 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -37,7 +37,9 @@ jobs: # rtabmap_costmap_plugins cannot be built, missing nav2 on lyrical, build other packages: packages: 'rtabmap_launch rtabmap_demos rtabmap_python rtabmap_examples rtabmap_rviz_plugins' - ros_distro: rolling - skip_keys: 'nav2_bringup nav2_msgs velodyne' + # libpointmatcher and gtsam are rtabmap deps not yet available on rolling + # (same skip keys as introlab/rtabmap's cmake-ros workflow) + skip_keys: 'nav2_bringup nav2_msgs velodyne libpointmatcher gtsam' use_ros2_testing: true # Rolling is using ros2-testing (nightly) # rtabmap_costmap_plugins cannot be built, missing nav2 on rolling, build other packages: packages: 'rtabmap_launch rtabmap_demos rtabmap_python rtabmap_examples rtabmap_rviz_plugins' From 99436c6e875b52d043b45e9502dc20d3cb5383a5 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 7 Aug 2026 11:34:47 -0700 Subject: [PATCH 05/17] CI not required probes for flaky builds (docker humble-latest) --- rtabmap_msgs/CMakeLists.txt | 5 +++-- rtabmap_sync/CMakeLists.txt | 7 ++++--- rtabmap_viz/CMakeLists.txt | 9 +++++---- 3 files changed, 12 insertions(+), 9 deletions(-) diff --git a/rtabmap_msgs/CMakeLists.txt b/rtabmap_msgs/CMakeLists.txt index 01e7cebb..d40b0c34 100644 --- a/rtabmap_msgs/CMakeLists.txt +++ b/rtabmap_msgs/CMakeLists.txt @@ -11,11 +11,12 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") - # issues #1285 #1288 + # issues #1285 #1288 (best-effort probes: not REQUIRED, a phantom miss under + # emulated arm64 should not fail the build) find_library( rcutils_LIB NAMES rcutils PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib" - NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH ) endif() diff --git a/rtabmap_sync/CMakeLists.txt b/rtabmap_sync/CMakeLists.txt index 106d23cf..ceda4040 100644 --- a/rtabmap_sync/CMakeLists.txt +++ b/rtabmap_sync/CMakeLists.txt @@ -2,16 +2,17 @@ cmake_minimum_required(VERSION 3.5) project(rtabmap_sync) if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") - # issues #1285 #1288 + # issues #1285 #1288 (best-effort probes: not REQUIRED, a phantom miss under + # emulated arm64 should not fail the build) find_library( builtin_interfaces__rosidl_generator_c_LIB NAMES builtin_interfaces__rosidl_generator_c PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib" - NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH ) find_library( crypto_LIB NAMES crypto PATHS "/usr/lib/${CMAKE_SYSTEM_PROCESSOR}-linux-gnu" - NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH ) endif() diff --git a/rtabmap_viz/CMakeLists.txt b/rtabmap_viz/CMakeLists.txt index add21b02..a5e043d3 100644 --- a/rtabmap_viz/CMakeLists.txt +++ b/rtabmap_viz/CMakeLists.txt @@ -6,21 +6,22 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") - # issues #1285 #1288 + # issues #1285 #1288 (best-effort probes: not REQUIRED, a phantom miss under + # emulated arm64 should not fail the build) find_library( builtin_interfaces__rosidl_generator_c_LIB NAMES builtin_interfaces__rosidl_generator_c PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib" - NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH ) find_library( rcutils_LIB NAMES rcutils PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib" - NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH ) find_library( crypto_LIB NAMES crypto PATHS "/usr/lib/${CMAKE_SYSTEM_PROCESSOR}-linux-gnu" - NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH ) endif() From 0b60e529145f2c878224a9c830185ca1d242299a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 7 Aug 2026 14:03:56 -0700 Subject: [PATCH 06/17] Fixing TransformBroadcaster *this on rolling (#1443) --- rtabmap_odom/src/OdometryROS.cpp | 2 +- rtabmap_slam/src/CoreWrapper.cpp | 2 +- rtabmap_util/src/nodelets/db_player.cpp | 2 +- rtabmap_util/src/nodelets/imu_to_tf.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index bc34b042..5a270c75 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -127,7 +127,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o tfBuffer_ = std::make_shared(get_clock()); tfListener_ = std::make_shared(*tfBuffer_); - tfBroadcaster_ = std::make_shared(this); + tfBroadcaster_ = std::make_shared(*this); std::string initialPoseStr; frameId_ = this->declare_parameter("frame_id", frameId_); diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index d7b8aad0..e36b40aa 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -160,7 +160,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : tfBuffer_ = std::make_shared(this->get_clock()); tfListener_ = std::make_shared(*tfBuffer_); - tfBroadcaster_ = std::make_shared(this); + tfBroadcaster_ = std::make_shared(*this); bool publishTf = true; std::string initialPoseStr; diff --git a/rtabmap_util/src/nodelets/db_player.cpp b/rtabmap_util/src/nodelets/db_player.cpp index a78ca5fa..167af087 100644 --- a/rtabmap_util/src/nodelets/db_player.cpp +++ b/rtabmap_util/src/nodelets/db_player.cpp @@ -159,7 +159,7 @@ DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) : resumeSrv_ = this->create_service(servicePrefix + "resume", std::bind(&DbPlayer::resumeCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); if(publishTf) { - tfBroadcaster_ = std::make_shared(this); + tfBroadcaster_ = std::make_shared(*this); } if(publishClock) diff --git a/rtabmap_util/src/nodelets/imu_to_tf.cpp b/rtabmap_util/src/nodelets/imu_to_tf.cpp index 48c1db40..9234e03c 100644 --- a/rtabmap_util/src/nodelets/imu_to_tf.cpp +++ b/rtabmap_util/src/nodelets/imu_to_tf.cpp @@ -41,7 +41,7 @@ ImuToTF::ImuToTF(const rclcpp::NodeOptions & options) : { tfBuffer_ = std::make_shared(this->get_clock()); tfListener_ = std::make_shared(*tfBuffer_); - tfBroadcaster_ = std::make_shared(this); + tfBroadcaster_ = std::make_shared(*this); int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_); From 16f6f05d70b299b2856b777c1a3db4ca0256fa6d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 9 Aug 2026 17:06:23 -0700 Subject: [PATCH 07/17] map_assembler : cleanup nodes_ from latest node if it has been removed --- .../include/rtabmap_util/map_assembler.hpp | 1 + rtabmap_util/src/MapsManager.cpp | 8 ++- rtabmap_util/src/nodelets/map_assembler.cpp | 50 ++++++++++++++----- 3 files changed, 46 insertions(+), 13 deletions(-) diff --git a/rtabmap_util/include/rtabmap_util/map_assembler.hpp b/rtabmap_util/include/rtabmap_util/map_assembler.hpp index 8c87adeb..3c874ee0 100644 --- a/rtabmap_util/include/rtabmap_util/map_assembler.hpp +++ b/rtabmap_util/include/rtabmap_util/map_assembler.hpp @@ -79,6 +79,7 @@ private: private: MapsManager mapsManager_; std::map nodes_; + int lastNodeAdded_; std::map optimizedPoses_; std::string mapFrameId_; std::string rtabmapNodeName_; diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 827d43c1..19c1a9a2 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -381,7 +381,7 @@ std::map MapsManager::getFilteredPoses(const std::map(); + return poses; } std::map MapsManager::updateMapCaches( @@ -443,6 +443,12 @@ std::map MapsManager::updateMapCaches( return std::map(); } + if(posesIn.empty()) + { + UERROR("Poses are empty, cannot update map caches!"); + return std::map(); + } + // process only nodes (exclude landmarks) std::map poses; if(posesIn.begin()->first < 0) diff --git a/rtabmap_util/src/nodelets/map_assembler.cpp b/rtabmap_util/src/nodelets/map_assembler.cpp index f966533c..07e91ad7 100644 --- a/rtabmap_util/src/nodelets/map_assembler.cpp +++ b/rtabmap_util/src/nodelets/map_assembler.cpp @@ -52,6 +52,7 @@ namespace rtabmap_util { MapAssembler::MapAssembler(const rclcpp::NodeOptions & options) : Node("map_assembler", options), + lastNodeAdded_(-1), rtabmapNodeName_("rtabmap"), localGridsRegenerated_(false) { @@ -256,12 +257,28 @@ void MapAssembler::mapDataReceivedCallback(const rtabmap_msgs::msg::MapData::Con } void MapAssembler::processMapData(const rtabmap_msgs::msg::MapData & msg) { + if(msg.graph.poses.empty() && msg.nodes.empty()) + { + // empty map, nothing to update + return; + } + UTimer timer; std::map poses; std::multimap constraints; rtabmap::Transform mapOdom; rtabmap_conversions::mapGraphFromROS(msg.graph, poses, constraints, mapOdom); + + // If the last node added to the cache is not in the graph anymore, it has been + // discarded by rtabmap (e.g., too small motion), so remove it from the cache. + if(lastNodeAdded_>0 && poses.find(lastNodeAdded_) == poses.end()) + { + RCLCPP_DEBUG(get_logger(), "map_assembler: Removing node %d from cache (discarded by rtabmap)", lastNodeAdded_); + nodes_.erase(lastNodeAdded_); + } + lastNodeAdded_ = -1; + for(unsigned int i=0; i lastNodeAdded_) + { + lastNodeAdded_ = msg.nodes[i].id; + } } } - // create a tmp signature with latest sensory data - if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end()) - { - rtabmap::Signature tmpS = nodes_.at(poses.rbegin()->first); - rtabmap::SensorData tmpData = tmpS.sensorData(); - tmpData.setId(0); - uInsert(nodes_, std::make_pair(0, rtabmap::Signature(0, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), rtabmap::Transform(), tmpData))); - poses.insert(std::make_pair(0, poses.rbegin()->second)); - } - // Update maps if(!nodes_.empty()) { @@ -297,6 +308,12 @@ void MapAssembler::processMapData(const rtabmap_msgs::msg::MapData & msg) false, nodes_); } + else + { + // No data cached, republish the maps already assembled, applying + // the same pose filtering than updateMapCaches() would do. + poses = mapsManager_.getFilteredPoses(poses); + } double updateTime = timer.ticks(); mapFrameId_ = msg.header.frame_id; @@ -313,6 +330,9 @@ void MapAssembler::reset(const std::shared_ptr, { RCLCPP_INFO(this->get_logger(), "map_assembler: reset!"); mapsManager_.clear(); + nodes_.clear(); + lastNodeAdded_ = -1; + optimizedPoses_.clear(); } #ifdef WITH_OCTOMAP_MSGS @@ -326,7 +346,10 @@ void MapAssembler::octomapBinaryCallback( res->map.header.frame_id = mapFrameId_; res->map.header.stamp = now(); - mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_); + if(!optimizedPoses_.empty() && !nodes_.empty()) + { + mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_); + } const rtabmap::OctoMap * octomap = mapsManager_.getOctomap(); if(octomap->octree()->size()) @@ -342,7 +365,10 @@ void MapAssembler::octomapFullCallback( res->map.header.frame_id = mapFrameId_; res->map.header.stamp = now(); - mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_); + if(!optimizedPoses_.empty() && !nodes_.empty()) + { + mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_); + } const rtabmap::OctoMap * octomap = mapsManager_.getOctomap(); if(octomap->octree()->size()) From 1e6edb45798358a93481ab58ddabe2dd7e758155 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 26 Aug 2026 20:26:20 -0700 Subject: [PATCH 08/17] Updated 2d scan turtlebot3 demo with deskewing option --- .../turtlebot3/turtlebot3_scan.launch.py | 25 ++++++++++++++++--- .../turtlebot3_sim_scan_demo.launch.py | 6 +++++ 2 files changed, 28 insertions(+), 3 deletions(-) diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py index a66b55ec..22b9633a 100644 --- a/rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py @@ -27,13 +27,17 @@ def launch_setup(context, *args, **kwargs): localization = localization == 'True' or localization == 'true' icp_odometry = LaunchConfiguration('icp_odometry').perform(context) icp_odometry = icp_odometry == 'True' or icp_odometry == 'true' + deskewing = LaunchConfiguration('deskewing').perform(context) + deskewing = deskewing == 'True' or deskewing == 'true' parameters={ 'frame_id':'base_footprint', 'use_sim_time':use_sim_time, 'subscribe_depth':False, 'subscribe_rgb':False, - 'subscribe_scan':True, + 'subscribe_scan':not deskewing, + 'subscribe_scan_cloud':deskewing, + 'scan_cloud_is_2d': True, 'approx_sync':True, 'use_action_for_goal':True, 'Reg/Strategy':'1', @@ -50,13 +54,24 @@ def launch_setup(context, *args, **kwargs): arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db) remappings=[ - ('scan', '/scan')] + ('scan', '/scan' if not deskewing else 'scan_not_used'), + ('scan_cloud', '/scan/deskewed' if deskewing else 'scan_cloud_not_used' )] if icp_odometry: remappings.append(('odom', 'icp_odom')) return [ # Nodes to launch - + + # Lidar deskewing (optional, useful with real lidar) + Node( + condition=IfCondition(LaunchConfiguration('deskewing')), + package='rtabmap_util', executable='lidar_deskewing', output='screen', + parameters=[{'wait_for_transform': 0.1, + 'fixed_frame_id': 'odom', + 'slerp': False, + 'use_sim_time':use_sim_time}], + remappings=[('input_scan', '/scan')]), + # ICP odometry (optional) Node( condition=IfCondition(LaunchConfiguration('icp_odometry')), @@ -97,5 +112,9 @@ def generate_launch_description(): 'icp_odometry', default_value='false', description='Launch ICP odometry on top of wheel odometry.'), + DeclareLaunchArgument( + 'deskewing', default_value='false', + description='Do lidar scan deskewing based on wheel odometry.'), + OpaqueFunction(function=launch_setup) ]) diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py index 3fecc353..3c48bab0 100644 --- a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py @@ -182,6 +182,12 @@ def generate_launch_description(): 'icp_odometry', default_value='false', description='Launch ICP odometry on top of wheel odometry.'), + DeclareLaunchArgument( + 'deskewing', default_value='false', + description='Do lidar scan deskewing based on wheel odometry. In simulation it ' + 'doesn\'t matter because laser scans are not skewed, but the option ' + 'is there for the demo.'), + DeclareLaunchArgument( 'x_pose', default_value='-2.0', description='Initial position of the robot in the simulator.'), From f77dda2b583e5acded2034e1f532ddedccdfb83e Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 6 Sep 2026 17:23:42 -0700 Subject: [PATCH 09/17] rtabmap_conversions tests and doc (#1449) * Initial tests * more tests * More in-depth deskew() testing * slightly less verbose clamping corruption warning * added tf buffer related tests * added remaining tests * Added rosdoc2, improve tests when we require sync of odom stamp and sensor stamp * cleanup doc * fixing ci --- .github/workflows/ros2.yml | 10 +- .gitignore | 3 + rtabmap_conversions/CMakeLists.txt | 17 + rtabmap_conversions/README.md | 83 + .../rtabmap_conversions/MsgConversion.h | 689 +++- rtabmap_conversions/package.xml | 3 + rtabmap_conversions/rosdoc2.yaml | 35 + rtabmap_conversions/src/MsgConversion.cpp | 216 +- .../test/test_msg_conversion.cpp | 3456 +++++++++++++++++ rtabmap_odom/src/nodelets/icp_odometry.cpp | 6 +- rtabmap_viz/src/rgbd_image_viewer.cpp | 15 + 11 files changed, 4456 insertions(+), 77 deletions(-) create mode 100644 rtabmap_conversions/README.md create mode 100644 rtabmap_conversions/rosdoc2.yaml create mode 100644 rtabmap_conversions/test/test_msg_conversion.cpp diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index f0be82e9..0723b173 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -25,24 +25,24 @@ jobs: include: - ros_distro: humble skip_keys: '' - packages: 'rtabmap_ros' + packages: 'rtabmap_ros rtabmap_conversions' - ros_distro: jazzy skip_keys: '' - packages: 'rtabmap_ros' + packages: 'rtabmap_ros rtabmap_conversions' - ros_distro: kilted skip_keys: 'grid_map_ros' - packages: 'rtabmap_ros' + packages: 'rtabmap_ros rtabmap_conversions' - ros_distro: lyrical skip_keys: 'nav2_bringup nav2_msgs velodyne grid_map_ros realsense2_camera libpointmatcher' # rtabmap_costmap_plugins cannot be built, missing nav2 on lyrical, build other packages: - packages: 'rtabmap_launch rtabmap_demos rtabmap_python rtabmap_examples rtabmap_rviz_plugins' + packages: 'rtabmap_launch rtabmap_demos rtabmap_python rtabmap_examples rtabmap_rviz_plugins rtabmap_conversions' - ros_distro: rolling # libpointmatcher and gtsam are rtabmap deps not yet available on rolling # (same skip keys as introlab/rtabmap's cmake-ros workflow) skip_keys: 'nav2_bringup nav2_msgs velodyne libpointmatcher gtsam' use_ros2_testing: true # Rolling is using ros2-testing (nightly) # rtabmap_costmap_plugins cannot be built, missing nav2 on rolling, build other packages: - packages: 'rtabmap_launch rtabmap_demos rtabmap_python rtabmap_examples rtabmap_rviz_plugins' + packages: 'rtabmap_launch rtabmap_demos rtabmap_python rtabmap_examples rtabmap_rviz_plugins rtabmap_conversions' fail-fast: false container: image: osrf/ros:${{ matrix.ros_distro }}-desktop-full diff --git a/.gitignore b/.gitignore index 5fb548e9..8f56a6c7 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,6 @@ .pydevproject .settings __pycache__ +# rosdoc2 build artifacts +docs_build +cross_reference diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index bb6aa06e..7300b3ea 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/rtabmap_conversions/CMakeLists.txt @@ -122,5 +122,22 @@ install(DIRECTORY include/ FILES_MATCHING PATTERN "*.h" ) +############# +## Testing ## +############# +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(test_msg_conversion test/test_msg_conversion.cpp) + if(TARGET test_msg_conversion) + target_link_libraries(test_msg_conversion rtabmap_conversions) + if("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(test_msg_conversion ${AmentLibraries}) + else() + target_link_libraries(test_msg_conversion ${Libraries} ${PublicLibraries}) + endif() + endif() +endif() + ament_package() diff --git a/rtabmap_conversions/README.md b/rtabmap_conversions/README.md new file mode 100644 index 00000000..9b187a02 --- /dev/null +++ b/rtabmap_conversions/README.md @@ -0,0 +1,83 @@ +# rtabmap_conversions + +Conversions between [RTAB-Map](https://github.com/introlab/rtabmap) library types and ROS 2 messages. + +This package is a library only — it contains no nodes, no launch files and no parameters. Every other `rtabmap_ros` package that touches a message goes through it: [`rtabmap_slam`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_slam), [`rtabmap_odom`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_odom), [`rtabmap_sync`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_sync), [`rtabmap_util`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_util), [`rtabmap_viz`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_viz) and [`rtabmap_rviz_plugins`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_rviz_plugins). + +You only need it directly if you are writing your own node against RTAB-Map's C++ API and want to publish or subscribe to `rtabmap_msgs`. + +## Usage + +Add the dependency to your `package.xml` and `CMakeLists.txt`: + +```xml +rtabmap_conversions +``` + +```cmake +find_package(rtabmap_conversions REQUIRED) +target_link_libraries(my_node rtabmap_conversions::rtabmap_conversions) +``` + +Everything lives in a single header and the `rtabmap_conversions` namespace: + +```cpp +#include + +// A pose message to an rtabmap::Transform and back. +rtabmap::Transform pose = rtabmap_conversions::transformFromPoseMsg(msg.pose); + +geometry_msgs::msg::Pose out; +rtabmap_conversions::transformToPoseMsg(pose, out); +``` + +The naming is uniform: `xxxFromROS()` converts a message into an RTAB-Map type, `xxxToROS()` goes the other way. `ToROS()` functions write through a reference parameter so the message can be reused; `FromROS()` functions return by value. + +## What it covers + +| Group | Functions | +|---|---| +| Transforms | `transformFromTF`, `transformToTF`, `transformFromGeometryMsg`, `transformToGeometryMsg`, `transformFromPoseMsg`, `transformToPoseMsg` | +| TF lookups | `getTransform`, `getMovingTransform` | +| Camera models | `cameraModelFromROS`, `cameraModelToROS`, `stereoCameraModelFromROS` | +| Images | `toCvCopy`, `toCvShare`, `rgbdImageFromROS`, `rgbdImageToROS`, `convertRGBDMsgs`, `convertStereoMsg` | +| Laser scans | `convertScanMsg`, `convertScan3dMsg`, `deskew`, `transformPointCloud`, `sizeOfPointField` | +| Features | `keypointFromROS`, `point2fFromROS`, `point3fFromROS`, `globalDescriptorFromROS` (+ vector and `ToROS` variants) | +| Graph | `mapDataFromROS`, `mapGraphFromROS`, `nodeFromROS`, `linkFromROS`, `sensorDataFromROS` (+ `ToROS` variants) | +| Misc | `infoFromROS`, `odomInfoFromROS`, `odomInfoToStatistics`, `imuFromROS`, `userDataFromROS`, `envSensorFromROS`, `landmarksFromROS`, `timestampFromROS`, `timestampToROS` | + +Full signatures and per-function notes are in the [API documentation](https://docs.ros.org/en/rolling/p/rtabmap_conversions/) and in [`MsgConversion.h`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h). + +## Conventions worth knowing + +These cut across the whole API and are not obvious from the signatures. Per-function caveats — object lifetimes, which fields a given `ToROS()` fills — are documented on the functions themselves. + +**Null transforms.** RTAB-Map distinguishes a *null* transform (unknown) from identity. Over the wire this is encoded as an all-zero quaternion, so `transformFromGeometryMsg()` and `transformFromPoseMsg()` return a null `rtabmap::Transform` for one. Always check `isNull()` before using a result. `tf2::Transform` cannot represent this — it stores rotation as a basis matrix — so `transformToTF()` returns a `bool` instead. + +**`CameraInfo` matrices are fixed-size arrays.** `k`, `r` and `p` are `std::array`, so they are never "empty" — an unset matrix is all zeros. `cameraModelFromROS()` treats a zero `k[0]`/`p[0]` (the focal length) as absent. + +## Building and testing + +```bash +colcon build --packages-select rtabmap_conversions +colcon test --packages-select rtabmap_conversions +colcon test-result --verbose +``` + +## Documentation + +API documentation is generated with [rosdoc2](https://github.com/ros-infrastructure/rosdoc2) from the Doxygen comments in the public header, and published to [docs.ros.org](https://docs.ros.org/en/rolling/p/rtabmap_conversions/). To build it locally: + +```bash +rosdoc2 build --package-path rtabmap_conversions --output-directory doc_output +``` + +Besides `doc_output`, rosdoc2 writes `docs_build/` and `cross_reference/` scratch directories into the current directory. `docs_build/` contains a copy of the package manifest, so colcon then sees two packages of the same name and every later build fails with `Duplicate package names not supported`. Mark it once and the problem goes away for good — rosdoc2 leaves an existing marker in place on subsequent runs: + +```bash +touch docs_build/COLCON_IGNORE +``` + +## License + +BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license). diff --git a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h index 4e82e388..a87aa2a3 100644 --- a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h +++ b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h @@ -71,76 +71,364 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define RCLCPP_QOS(queueSize, qos) rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos) #endif +/** + * @namespace rtabmap_conversions + * @brief Conversions between RTAB-Map library types and ROS 2 messages. + * + * Naming is uniform throughout: `xxxFromROS()` converts a message into an RTAB-Map + * type and returns it by value, `xxxToROS()` writes an RTAB-Map type into a message + * passed by reference so the message can be reused. + * + * @note RTAB-Map distinguishes a *null* transform (unknown) from an identity one. On + * the wire a null transform is encoded as an all-zero quaternion, so results of + * the `transformFromXxx()` functions should be checked with + * rtabmap::Transform::isNull() before use. + */ namespace rtabmap_conversions { -void transformToTF(const rtabmap::Transform & transform, tf2::Transform & tfTransform); +//============================================================================ +// Transforms +// Conversions between rtabmap::Transform and the tf2 / geometry_msgs representations. +//============================================================================ + +/** + * @brief Convert a rtabmap::Transform into a tf2::Transform. + * @param[in] transform the transform to convert + * @param[out] tfTransform the converted transform, or filled with NaN if @p transform is null + * @return false if @p transform is null, true otherwise + * + * @note tf2::Transform stores its rotation as a basis matrix and so cannot represent + * the all-zero quaternion used elsewhere to mean "null". The null case is + * reported through the return value instead, and the output is poisoned with + * NaN so that ignoring that return value fails loudly rather than silently + * proceeding with a plausible-looking identity. + * @see transformFromTF() + */ +bool transformToTF(const rtabmap::Transform & transform, tf2::Transform & tfTransform); + +/** + * @brief Convert a tf2::Transform into a rtabmap::Transform. + * @param transform the transform to convert + * @return the converted transform, or a null transform if @p transform contains NaN + * (which is how transformToTF() reports a null transform) + * @see transformToTF() + */ rtabmap::Transform transformFromTF(const tf2::Transform & transform); +/** + * @brief Convert a rtabmap::Transform into a geometry_msgs Transform. + * + * The quaternion is normalized. A null @p transform is encoded as an all-zero + * quaternion, which transformFromGeometryMsg() decodes back to null. + * + * @param[in] transform the transform to convert + * @param[out] msg the converted message + */ void transformToGeometryMsg(const rtabmap::Transform & transform, geometry_msgs::msg::Transform & msg); + +/** + * @brief Convert a geometry_msgs Transform into a rtabmap::Transform. + * @param msg the message to convert + * @return the converted transform, or a null transform if the quaternion is all zeros + */ rtabmap::Transform transformFromGeometryMsg(const geometry_msgs::msg::Transform & msg); +/** + * @brief Convert a rtabmap::Transform into a geometry_msgs Pose. + * @param[in] transform the transform to convert + * @param[out] msg the converted message; a null @p transform gives an all-zero orientation + */ void transformToPoseMsg(const rtabmap::Transform & transform, geometry_msgs::msg::Pose & msg); + +/** + * @brief Convert a geometry_msgs Pose into a rtabmap::Transform. + * @param msg the message to convert + * @param ignoreRotationIfNotSet if true, an all-zero orientation yields a + * translation-only transform instead of a null one + * @return the converted transform, or a null transform if the orientation is all zeros + * and @p ignoreRotationIfNotSet is false + * + * @warning geometry_msgs::msg::Quaternion defaults to `w = 1`, not all zeros, so a + * default-constructed Pose is a valid identity rotation rather than "unset". + */ rtabmap::Transform transformFromPoseMsg(const geometry_msgs::msg::Pose & msg, bool ignoreRotationIfNotSet = false); + +//============================================================================ +// Images +// Extracting OpenCV images from RGBDImage messages, and building them back. +//============================================================================ + +/** + * @brief Extract the RGB and depth images of an RGBDImage message, copying the pixels. + * + * Handles both the raw (`rgb`, `depth`) and compressed (`rgb_compressed`, + * `depth_compressed`) fields. Both output pointers are always valid; they hold an + * empty image when the corresponding field is not set. + * + * @param[in] image the message to read + * @param[out] rgb the RGB image + * @param[out] depth the depth image + * @see toCvShare() to avoid the copy + */ void toCvCopy(const rtabmap_msgs::msg::RGBDImage & image, cv_bridge::CvImagePtr & rgb, cv_bridge::CvImagePtr & depth); + +/** + * @brief Extract the RGB and depth images of an RGBDImage message without copying. + * + * The returned images alias the message's buffers, so @p image must outlive them. + * + * @param[in] image the message to read; its shared pointer keeps the buffers alive + * @param[out] rgb the RGB image + * @param[out] depth the depth image + */ void toCvShare(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth); + +/** + * @brief Extract the RGB and depth images of an RGBDImage message without copying. + * @param[in] image the message to read + * @param[in] trackedObject object whose lifetime keeps the message buffers alive + * @param[out] rgb the RGB image + * @param[out] depth the depth image + */ void toCvShare(const rtabmap_msgs::msg::RGBDImage & image, const std::shared_ptr& trackedObject, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth); + +/** + * @brief Fill an RGBDImage message from a SensorData. + * + * Supports a single RGB-D camera or a single stereo pair; multi-camera data cannot be + * represented by this message and is rejected with an error. + * + * @param[in] data the sensor data to convert + * @param[out] msg the converted message, stamped with @p data's stamp + * @param[in] sensorFrameId frame id stamped on the message and its sub-messages + * + * @note rtabmap::SensorData holds its stamp as a double, so the stamp written here is + * only accurate to a few hundred nanoseconds at current epoch times and will not + * compare equal to the ROS stamp the data originally came from. Callers that need + * the exact original stamp assign `msg.header` after this call. + * @note Unlike infoToROS(), an already-stamped `msg.header` is overwritten rather than + * kept: the same header is applied to every sub-message here, so preserving only + * the top-level one would leave the message internally inconsistent. + */ void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::msg::RGBDImage & msg, const std::string & sensorFrameId); + +/** + * @brief Build a SensorData from an RGBDImage message. + * + * The stamp is taken from the top-level `image->header`, and the camera's local + * transform is not carried by the message (callers resolve it from TF). + * + * @param image the message to convert + * @return the converted sensor data + * + * @warning The returned SensorData does **not** copy the pixels: it points into the + * message's own buffers. @p image must therefore outlive it and must not be + * modified meanwhile. Deep-copy the images before letting the SensorData + * escape a subscription callback, because the queue recycles the message as + * soon as the callback returns. + */ rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr & image); -// copy data + +//============================================================================ +// Compressed data +//============================================================================ + +/** + * @brief Copy an already-compressed cv::Mat into a byte vector. + * @param[in] compressed a 1xN CV_8UC1 matrix of compressed bytes, or an empty matrix + * @param[out] bytes the bytes; cleared when @p compressed is empty + */ void compressedMatToBytes(const cv::Mat & compressed, std::vector & bytes); + +/** + * @brief Wrap a byte vector as a 1xN CV_8UC1 cv::Mat of compressed data. + * @param bytes the bytes to wrap + * @param copy if false, the returned matrix aliases @p bytes, which must then outlive it + * @return the matrix, empty when @p bytes is empty + */ cv::Mat compressedMatFromBytes(const std::vector & bytes, bool copy = true); + +//============================================================================ +// Statistics +//============================================================================ + +/** + * @brief Read an Info message into RTAB-Map statistics. + * @param[in] info the message to convert + * @param[out] stat the statistics, marked as extended + * @note The stamp comes from `info.header`, which infoToROS() does not set. + */ void infoFromROS(const rtabmap_msgs::msg::Info & info, rtabmap::Statistics & stat); + +/** + * @brief Fill an Info message from RTAB-Map statistics. + * @param[in] stats the statistics to convert + * @param[out] info the converted message + * @note If the caller left `info.header.stamp` unset it is filled from @p stats, so that + * infoFromROS() recovers a stamp. An already-stamped header is never overwritten: + * rtabmap::Statistics holds its stamp as a double, so the value derived from it is + * only accurate to a few hundred nanoseconds at current epoch times and will not + * compare equal to the ROS stamp the data came from. Callers wanting the exact + * input stamp — or a publication time unrelated to the data — stamp the header + * themselves before or after this call. + * @warning The frame id is never set: rtabmap::Statistics does not carry one, so the + * caller must always fill `info.header.frame_id` itself. + */ void infoToROS(const rtabmap::Statistics & stats, rtabmap_msgs::msg::Info & info); + +//============================================================================ +// Features and landmarks +// Keypoints, 2D/3D points, descriptors and environmental sensors. +//============================================================================ + +/** @brief Convert a Link message into a rtabmap::Link, including its 6x6 information matrix. */ rtabmap::Link linkFromROS(const rtabmap_msgs::msg::Link & msg); +/** @brief Fill a Link message from a rtabmap::Link. */ void linkToROS(const rtabmap::Link & link, rtabmap_msgs::msg::Link & msg); +/** @brief Convert a KeyPoint message into a cv::KeyPoint. */ cv::KeyPoint keypointFromROS(const rtabmap_msgs::msg::KeyPoint & msg); +/** @brief Fill a KeyPoint message from a cv::KeyPoint. */ void keypointToROS(const cv::KeyPoint & kpt, rtabmap_msgs::msg::KeyPoint & msg); +/** @brief Convert keypoint messages into a new vector of cv::KeyPoint. */ std::vector keypointsFromROS(const std::vector & msg); + +/** + * @brief Append keypoint messages to an existing vector. + * @param[in] msg the messages to convert + * @param[in,out] kpts vector the keypoints are appended to; existing content is kept + * @param[in] xShift offset added to the x coordinate of every appended keypoint, + * used when several camera images are laid out side by side + */ void keypointsFromROS(const std::vector & msg, std::vector & kpts, int xShift=0); + +/** @brief Fill keypoint messages from a vector of cv::KeyPoint. */ void keypointsToROS(const std::vector & kpts, std::vector & msg); +/** @brief Convert a GlobalDescriptor message, decompressing its data and info matrices. */ rtabmap::GlobalDescriptor globalDescriptorFromROS(const rtabmap_msgs::msg::GlobalDescriptor & msg); +/** @brief Fill a GlobalDescriptor message, compressing its data and info matrices. */ void globalDescriptorToROS(const rtabmap::GlobalDescriptor & desc, rtabmap_msgs::msg::GlobalDescriptor & msg); +/** @brief Convert global descriptor messages into RTAB-Map descriptors. */ std::vector globalDescriptorsFromROS(const std::vector & msg); +/** @brief Fill global descriptor messages; @p msg is cleared first. */ void globalDescriptorsToROS(const std::vector & desc, std::vector & msg); +/** @brief Convert an EnvSensor message into a rtabmap::EnvSensor. */ rtabmap::EnvSensor envSensorFromROS(const rtabmap_msgs::msg::EnvSensor & msg); +/** @brief Fill an EnvSensor message from a rtabmap::EnvSensor. */ void envSensorToROS(const rtabmap::EnvSensor & sensor, rtabmap_msgs::msg::EnvSensor & msg); +/** @brief Convert EnvSensor messages into a map keyed by sensor type. */ rtabmap::EnvSensors envSensorsFromROS(const std::vector & msg); +/** @brief Fill EnvSensor messages from a map of sensors; @p msg is cleared first. */ void envSensorsToROS(const rtabmap::EnvSensors & sensors, std::vector & msg); +/** @brief Convert a Point2f message into a cv::Point2f. */ cv::Point2f point2fFromROS(const rtabmap_msgs::msg::Point2f & msg); +/** @brief Fill a Point2f message from a cv::Point2f. */ void point2fToROS(const cv::Point2f & kpt, rtabmap_msgs::msg::Point2f & msg); +/** @brief Convert Point2f messages into a vector of cv::Point2f. */ std::vector points2fFromROS(const std::vector & msg); +/** @brief Fill Point2f messages from a vector of cv::Point2f. */ void points2fToROS(const std::vector & kpts, std::vector & msg); +/** @brief Convert a Point3f message into a cv::Point3f. */ cv::Point3f point3fFromROS(const rtabmap_msgs::msg::Point3f & msg); +/** @brief Fill a Point3f message from a cv::Point3f. */ void point3fToROS(const cv::Point3f & kpt, rtabmap_msgs::msg::Point3f & msg); +/** + * @brief Convert Point3f messages into a vector of cv::Point3f. + * @param msg the messages to convert + * @param transform applied to every point; ignored when null or identity + * @return the converted points + */ std::vector points3fFromROS(const std::vector & msg, const rtabmap::Transform & transform = rtabmap::Transform()); + +/** + * @brief Append Point3f messages to an existing vector. + * @param[in] msg the messages to convert + * @param[in,out] points3 vector the points are appended to; existing content is kept + * @param[in] transform applied to every appended point; ignored when null or identity + */ void points3fFromROS(const std::vector & msg, std::vector & points3, const rtabmap::Transform & transform = rtabmap::Transform()); + +/** + * @brief Fill Point3f messages from a vector of cv::Point3f. + * @param[in] kpts the points to convert + * @param[out] msg the converted messages + * @param[in] transform applied to every point; ignored when null or identity + */ void points3fToROS(const std::vector & kpts, std::vector & msg, const rtabmap::Transform & transform = rtabmap::Transform()); + +//============================================================================ +// Camera models +//============================================================================ + +/** + * @brief Convert a CameraInfo message into a rtabmap::CameraModel. + * + * Fisheye/equidistant distortion (4 coefficients) is repacked into RTAB-Map's 1x6 + * layout. A projection matrix means the model describes an already-rectified image. + * + * @param camInfo the message to convert + * @param localTransform transform from the base frame to the optical frame + * @return the converted model + * + * @note `k`, `r` and `p` are fixed-size arrays and so are never empty. An unset matrix + * is all zeros, which is detected through the focal length (`k[0]` / `p[0]`). + */ rtabmap::CameraModel cameraModelFromROS( const sensor_msgs::msg::CameraInfo & camInfo, const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity()); + +/** + * @brief Fill a CameraInfo message from a rtabmap::CameraModel. + * + * A model carrying a projection matrix describes a rectified image, so zero distortion + * is reported for it. Without one, `P` is synthesized as `[K | 0]` and the raw + * distortion coefficients are emitted (`equidistant` for a 1x6 fisheye matrix, + * `rational_polynomial` above 5 coefficients, `plumb_bob` otherwise). + * + * @param[in] model the model to convert + * @param[out] camInfo the converted message; the header is not set + */ void cameraModelToROS( const rtabmap::CameraModel & model, sensor_msgs::msg::CameraInfo & camInfo); +/** + * @brief Build a stereo model from a pair of CameraInfo messages. + * @param leftCamInfo left camera info + * @param rightCamInfo right camera info; the baseline is read from its `P(0,3)` + * @param localTransform transform from the base frame to the left optical frame + * @param stereoTransform explicit left-to-right transform, when not encoded in `P` + * @return the converted model + */ rtabmap::StereoCameraModel stereoCameraModelFromROS( const sensor_msgs::msg::CameraInfo & leftCamInfo, const sensor_msgs::msg::CameraInfo & rightCamInfo, const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity(), const rtabmap::Transform & stereoTransform = rtabmap::Transform()); + +/** + * @brief Build a stereo model, resolving the local transform from TF. + * @param leftCamInfo left camera info + * @param rightCamInfo right camera info + * @param frameId base frame the model's local transform is expressed in + * @param tfBuffer must contain @p frameId -> the left camera info's frame at + * its stamp + * @param waitForTransform seconds to wait for TF, 0 to not wait + * @return the converted model, invalid if the transform could not be resolved + */ rtabmap::StereoCameraModel stereoCameraModelFromROS( const sensor_msgs::msg::CameraInfo & leftCamInfo, const sensor_msgs::msg::CameraInfo & rightCamInfo, @@ -148,12 +436,34 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS( tf2_ros::Buffer & tfBuffer, double waitForTransform); + +//============================================================================ +// Map graph +// Poses, links, nodes and sensor data — the map serialization path. +//============================================================================ + +/** + * @brief Read a MapData message into poses, links and signatures. + * @param[in] msg the message to convert + * @param[out] poses optimized poses by node id + * @param[out] links constraints, keyed by their originating node id + * @param[out] signatures node data by node id + * @param[out] mapToOdom transform from the map frame to the odometry frame + */ void mapDataFromROS( const rtabmap_msgs::msg::MapData & msg, std::map & poses, std::multimap & links, std::map & signatures, rtabmap::Transform & mapToOdom); +/** + * @brief Fill a MapData message from poses, links and signatures. + * @param[in] poses optimized poses by node id + * @param[in] links constraints + * @param[in] signatures node data by node id + * @param[in] mapToOdom transform from the map frame to the odometry frame + * @param[out] msg the converted message; the header is not set + */ void mapDataToROS( const std::map & poses, const std::multimap & links, @@ -161,40 +471,159 @@ void mapDataToROS( const rtabmap::Transform & mapToOdom, rtabmap_msgs::msg::MapData & msg); +/** + * @brief Read a MapGraph message into poses and links. + * @param[in] msg the message to convert + * @param[out] poses optimized poses by node id + * @param[out] links constraints, keyed by their originating node id + * @param[out] mapToOdom transform from the map frame to the odometry frame + */ void mapGraphFromROS( const rtabmap_msgs::msg::MapGraph & msg, std::map & poses, std::multimap & links, rtabmap::Transform & mapToOdom); +/** + * @brief Fill a MapGraph message from poses and links. + * @param[in] poses optimized poses by node id + * @param[in] links constraints + * @param[in] mapToOdom transform from the map frame to the odometry frame + * @param[out] msg the converted message; the header is not set + */ void mapGraphToROS( const std::map & poses, const std::multimap & links, const rtabmap::Transform & mapToOdom, rtabmap_msgs::msg::MapGraph & msg); +/** + * @brief Convert a SensorData message into a rtabmap::SensorData. + * @param msg the message to convert + * @return the converted sensor data + * @note `ground_truth_pose` is not read here; nodeFromROS() owns that field. + */ rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::msg::SensorData & msg); + +/** + * @brief Fill a SensorData message from a rtabmap::SensorData. + * @param[in] signature the sensor data to convert + * @param[out] msg the converted message + * @param[in] frameId frame id stamped on the message + * @param[in] copyRawData also serialize the uncompressed images and laser scan, which + * is significantly larger on the wire + */ void sensorDataToROS(const rtabmap::SensorData & signature, rtabmap_msgs::msg::SensorData & msg, const std::string & frameId = "base_link", bool copyRawData = false); +/** + * @brief Convert a Node message into a rtabmap::Signature, with its data and visual words. + * @param msg the message to convert + * @return the converted signature + */ rtabmap::Signature nodeFromROS(const rtabmap_msgs::msg::Node & msg); + +/** + * @brief Fill a Node message from a rtabmap::Signature. + * @param[in] signature the signature to convert + * @param[out] msg the converted message + */ void nodeToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node & msg); -// DEPRECATED +/** @deprecated Use nodeFromROS() instead. */ rtabmap::Signature nodeDataFromROS(const rtabmap_msgs::msg::Node & msg); +/** @deprecated Use nodeToROS() instead. */ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node & msg); +/** @brief Convert only the node's metadata (id, map id, weight, stamp, label, pose). */ rtabmap::Signature nodeInfoFromROS(const rtabmap_msgs::msg::Node & msg); +/** @brief Fill only the node's metadata (id, map id, weight, stamp, label, pose). */ void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node & msg); + +//============================================================================ +// Odometry +//============================================================================ + +/** + * @brief Format odometry info as the `Odometry/...` statistics published with the map. + * @param info the odometry info to summarize + * @return statistic name (with its unit) to value + * @note The covariance-derived entries are omitted when `info.reg.covariance` is not a + * 6x6 CV_64FC1 matrix, which is the case for a default-constructed OdometryInfo. + */ std::map odomInfoToStatistics(const rtabmap::OdometryInfo & info); + +/** + * @brief Convert an OdomInfo message into a rtabmap::OdometryInfo. + * @param msg the message to convert + * @param ignoreData skip the heavy members (words, local map, correspondences) + * @return the converted odometry info + */ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_msgs::msg::OdomInfo & msg, bool ignoreData = false); + +/** + * @brief Fill an OdomInfo message from a rtabmap::OdometryInfo. + * @param[in] info the odometry info to convert + * @param[out] msg the converted message + * @param[in] ignoreData skip the heavy members (words, local map, correspondences) + */ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_msgs::msg::OdomInfo & msg, bool ignoreData = false); + +//============================================================================ +// User data, IMU and landmarks +//============================================================================ + +/** + * @brief Extract the payload of a UserData message. + * @param dataMsg the message to read + * @return the payload; still compressed when the message was written with compression, + * in which case the caller applies rtabmap::uncompressData() + */ cv::Mat userDataFromROS(const rtabmap_msgs::msg::UserData & dataMsg); + +/** + * @brief Fill a UserData message. + * @param[in] data the payload + * @param[out] dataMsg the converted message + * @param[in] compress compress the payload, which is then carried as a 1xN byte blob + */ void userDataToROS(const cv::Mat & data, rtabmap_msgs::msg::UserData & dataMsg, bool compress); +/** + * @brief Convert an Imu message into a rtabmap::IMU. + * @param msg the message to convert + * @param localTransform transform from the base frame to the IMU frame + * @return the converted IMU sample, with its three covariance matrices + */ rtabmap::IMU imuFromROS(const sensor_msgs::msg::Imu & msg, const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity()); + +/** + * @brief Fill an Imu message from a rtabmap::IMU. + * @param[in] imu the IMU sample to convert + * @param[out] msg the converted message; the header is not set + */ void imuToROS(const rtabmap::IMU & imu, sensor_msgs::msg::Imu & msg); +/** + * @brief Convert tag/landmark detections into RTAB-Map landmarks, expressed in @p frameId. + * + * Each detection is transformed from its own frame into @p frameId, then corrected for + * the odometry motion between @p odomStamp and the detection's stamp. + * + * @param tags detections by landmark id, each paired with its tag size; + * ids must be > 0, others are dropped with an error + * @param frameId base frame the landmarks are expressed in + * @param odomFrameId fixed frame used for the odometry correction; when empty + * no correction is applied + * @param odomStamp stamp the landmarks should be synchronized to + * @param tfBuffer must contain @p frameId -> each detection's frame at that + * detection's stamp, and, when @p odomFrameId is set, + * @p odomFrameId -> @p frameId covering both stamps + * @param waitForTransform seconds to wait for TF, 0 to not wait + * @param defaultLinVariance linear variance used when a detection carries no covariance + * @param defaultAngVariance angular variance used when a detection carries no covariance + * @return the landmarks, keyed by id + */ rtabmap::Landmarks landmarksFromROS( const std::map > & tags, const std::string & frameId, @@ -205,10 +634,43 @@ rtabmap::Landmarks landmarksFromROS( double defaultLinVariance, double defaultAngVariance); -inline double timestampFromROS(const rclcpp::Time & stamp) {return stamp.seconds();} -inline rclcpp::Time timestampToROS(const double & t) {int32_t sec= (int32_t)floor(t); return rclcpp::Time(sec, (uint32_t)std::round((t-sec) * 1e9));} -// common stuff +//============================================================================ +// Timestamps +//============================================================================ + +/** + * @brief Convert a ROS time into seconds. + * @note A double holds about 15-16 significant digits, so at current epoch times + * (~1.7e9 s) it resolves to roughly 400 ns. Converting back with timestampToROS() + * therefore does not reproduce the original stamp exactly, and the rounding can + * carry into the seconds field. Compare converted stamps with a tolerance, and + * keep the original rclcpp::Time whenever exactness matters. + */ +inline double timestampFromROS(const rclcpp::Time & stamp) {return stamp.seconds();} +/** + * @brief Convert seconds into a ROS time. + * @note The result uses RCL_ROS_TIME, matching how message header stamps convert. The + * rclcpp::Time(sec, nsec) constructor defaults to RCL_SYSTEM_TIME instead, and + * comparing times of different clock types throws. + */ +inline rclcpp::Time timestampToROS(const double & t) {int32_t sec= (int32_t)floor(t); return rclcpp::Time(sec, (uint32_t)std::round((t-sec) * 1e9), RCL_ROS_TIME);} + + +//============================================================================ +// TF lookups +//============================================================================ + +/** + * @brief Look a static relationship between two frames up in TF. + * @param fromFrameId the reference frame + * @param toFrameId the target frame + * @param stamp time of the lookup + * @param tfBuffer buffer to query + * @param waitForTransform seconds to wait for TF, 0 to not wait + * @return the transform, or a null transform if the lookup failed (which is logged + * rather than thrown) + */ rtabmap::Transform getTransform( const std::string & fromFrameId, const std::string & toFrameId, @@ -216,9 +678,19 @@ rtabmap::Transform getTransform( tf2_ros::Buffer & tfBuffer, double waitForTransform); - -// get moving transform accordingly to a fixed frame. For example get -// transform of /base_link between two stamps accordingly to /odom frame. +/** + * @brief Measure how a frame moved between two stamps, relative to a fixed frame. + * + * For example, the motion of `base_link` between two stamps as seen from `odom`. + * + * @param movingFrame the frame whose motion is measured + * @param fixedFrame the frame the motion is measured against + * @param stampFrom start of the interval + * @param stampTo end of the interval + * @param tfBuffer buffer to query + * @param waitForTransform seconds to wait for TF, 0 to not wait + * @return the motion, or a null transform if the lookup failed + */ rtabmap::Transform getMovingTransform( const std::string & movingFrame, const std::string & fixedFrame, @@ -227,6 +699,54 @@ rtabmap::Transform getMovingTransform( tf2_ros::Buffer & tfBuffer, double waitForTransform); + +//============================================================================ +// Sensor message conversion +// Assembling RGB-D, stereo and laser scan messages into RTAB-Map inputs. +//============================================================================ + +/** + * @brief Assemble one or more RGB-D (or RGB + right) camera streams into RTAB-Map inputs. + * + * With several cameras the images are concatenated horizontally into a single wide + * image and one model is produced per camera. Whether the second image is treated as a + * depth map or as the right image of a stereo pair is inferred from its encoding, and + * for `mono16` from whether the camera infos carry a baseline in `P(0,3)`. + * + * @param imageMsgs RGB (or left) images, one per camera; may be empty + * @param depthMsgs depth (or right) images, one per camera; may be empty + * @param cameraInfoMsgs camera infos, one per camera; must not be empty + * @param depthCameraInfoMsgs camera infos of the depth/right cameras; may be empty + * @param frameId base frame the local transforms are expressed in + * @param odomFrameId fixed frame the robot motion is measured against, used to + * re-express each camera pose relative to the base frame at + * @p odomStamp; empty to skip that correction entirely + * @param odomStamp stamp the data is synchronized to + * @param[out] rgb the assembled RGB (or left) image + * @param[out] depth the assembled depth (or right) image + * @param[out] cameraModels one model per camera, when the input is RGB-D + * @param[out] stereoCameraModels one model per camera, when the input is stereo + * @param tfBuffer must contain @p frameId -> each camera's optical frame at + * that camera's stamp, and, when @p odomFrameId is set, + * @p odomFrameId -> @p frameId covering both @p odomStamp + * and the camera stamps + * @param waitForTransform seconds to wait for TF, 0 to not wait + * @param alreadRectifiedImages whether the images are already rectified + * @param localKeyPointsMsgs optional per-camera keypoints to merge + * @param localPoints3dMsgs optional per-camera 3D points to merge + * @param localDescriptorsMsgs optional per-camera descriptors to merge + * @param[out] localKeyPoints merged keypoints, shifted to the concatenated image + * @param[out] localPoints3d merged 3D points + * @param[out] localDescriptors merged descriptors + * @return false on an unsupported encoding or a missing camera local transform + * + * @note The odometry correction is applied per camera, using each camera's own stamp, + * and only when it differs from @p odomStamp. If that lookup fails the function + * warns and carries on with an uncorrected pose — unlike a missing camera local + * transform, which is fatal and returns false. + * @note A camera's RGB and depth stamps are assumed to be equal. Should they differ, + * the depth stamp is the one used, since the geometry is what gets synchronized. + */ bool convertRGBDMsgs( const std::vector & imageMsgs, const std::vector & depthMsgs, @@ -249,6 +769,34 @@ bool convertRGBDMsgs( std::vector * localPoints3d = 0, cv::Mat * localDescriptors = 0); +/** + * @brief Convert a stereo pair into RTAB-Map inputs. + * + * The left image keeps its colour; the right image is always reduced to mono. + * + * @param leftImageMsg left image + * @param rightImageMsg right image + * @param leftCamInfoMsg left camera info + * @param rightCamInfoMsg right camera info; the baseline is read from its `P(0,3)` + * @param frameId base frame the local transform is expressed in + * @param odomFrameId fixed frame the robot motion is measured against, used to + * re-express the camera pose relative to the base frame at + * @p odomStamp; empty to skip that correction entirely + * @param odomStamp stamp the data is synchronized to + * @param[out] left the left image + * @param[out] right the right image, as mono + * @param[out] stereoModel the stereo model + * @param tfBuffer must contain @p frameId -> the left image's frame at the left + * image stamp, and, when @p odomFrameId is set, + * @p odomFrameId -> @p frameId covering both stamps + * @param waitForTransform seconds to wait for TF, 0 to not wait + * @param alreadyRectified whether the images are already rectified + * @return false on an unsupported encoding or a missing local transform + * + * @note The odometry correction is applied only when the left image stamp differs from + * @p odomStamp. A failed correction lookup warns and leaves the pose uncorrected; + * a missing local transform is fatal and returns false. + */ bool convertStereoMsg( const cv_bridge::CvImageConstPtr& leftImageMsg, const cv_bridge::CvImageConstPtr& rightImageMsg, @@ -264,6 +812,34 @@ bool convertStereoMsg( double waitForTransform, bool alreadyRectified); +/** + * @brief Convert a 2D LaserScan into a rtabmap::LaserScan. + * @param scan2dMsg the scan to convert + * @param frameId base frame the scan's local transform is expressed in + * @param odomFrameId fixed frame the robot motion is measured against, used to + * re-express the scan pose relative to the base frame at + * @p odomStamp; empty to skip that correction entirely + * @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 + * @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 + * bounds) or if a required transform is missing + * + * @note Unlike convertScan3dMsg(), this deskews the scan itself: the points are + * projected with laser_geometry, which transforms each ray at its own time using + * @p scan2dMsg.time_increment. That only corrects for motion if the projection + * 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. + * @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. + */ bool convertScanMsg( const sensor_msgs::msg::LaserScan & scan2dMsg, const std::string & frameId, @@ -274,6 +850,32 @@ bool convertScanMsg( double waitForTransform, bool outputInFrameId = false); +/** + * @brief Convert a PointCloud2 into a rtabmap::LaserScan. + * @param scan3dMsg the cloud to convert + * @param frameId base frame the scan's local transform is expressed in + * @param odomFrameId fixed frame the robot motion is measured against, used to + * re-express the scan pose relative to the base frame at + * @p odomStamp; empty to skip that correction entirely + * @param odomStamp stamp the scan is synchronized to + * @param[out] scan the converted scan + * @param tfBuffer must contain @p frameId -> the cloud's frame at the cloud + * stamp, and, when @p odomFrameId is set, @p odomFrameId -> + * @p frameId covering both stamps + * @param waitForTransform seconds to wait for TF, 0 to not wait + * @param maxPoints downsample to at most this many points, 0 for no limit + * @param maxRange drop points beyond this range, 0 for no limit + * @param is2D treat the cloud as planar + * @return false if the local transform could not be resolved + * + * @note The cloud is assumed to be already deskewed. A single rigid transform is applied + * to the whole cloud, so any motion during the sweep is preserved as-is; call + * deskew() on the message first if the sensor was moving. This is unlike + * convertScanMsg(), which deskews 2D scans itself through laser_geometry. + * @note The odometry correction is applied only when the cloud stamp differs from + * @p odomStamp; a failed correction lookup warns and leaves the pose uncorrected. + * @see deskew() + */ bool convertScan3dMsg( const sensor_msgs::msg::PointCloud2 & scan3dMsg, const std::string & frameId, @@ -286,6 +888,29 @@ bool convertScan3dMsg( float maxRange = 0.0f, bool is2D = false); + +//============================================================================ +// Point cloud utilities +//============================================================================ + +/** + * @brief Deskew a point cloud using TF. + * + * Corrects each point for the sensor motion during the sweep, using the per-point time + * channel (`t`, `time`, `stamps` or `timestamp`). See the other overload for how that + * channel is interpreted. + * + * @param input the cloud to deskew + * @param[out] output the deskewed cloud, expressed in the frame at input's header stamp + * @param fixedFrameId frame the sensor motion is measured against + * @param tfBuffer must contain the cloud's own frame relative to + * @p fixedFrameId across the whole sweep + * @param waitForTransform seconds to wait for TF, 0 to not wait + * @param slerp interpolate between the sweep's two end poses instead of + * looking TF up for every point; one query instead of N, at the + * cost of linearizing the motion across the sweep + * @return false if the cloud has no usable time channel or a lookup failed + */ bool deskew( const sensor_msgs::msg::PointCloud2 & input, sensor_msgs::msg::PointCloud2 & output, @@ -294,21 +919,49 @@ bool deskew( double waitForTransform, bool slerp = false); +/** + * @brief Deskew a point cloud using a constant velocity model. + * + * The per-point time channel may be named `t`, `time`, `stamps` or `timestamp`. Its + * datatype decides how it is read: `UINT32` (nanoseconds) and `FLOAT32` (seconds) are + * *offsets from the message header stamp*, while `FLOAT64` carries *absolute* stamps, + * with milliseconds/microseconds/nanoseconds detected automatically by magnitude. + * + * On success the channel is zeroed to mark the cloud as deskewed, so calling this again + * on the same cloud is a no-op that returns true rather than an error. + * + * @param input cloud with a per-point time channel + * @param[out] output deskewed cloud, expressed in the frame at input's header stamp + * @param velocity twist of the sensor frame (m/s and rad/s) + * @return false if the cloud has no usable time channel or @p velocity is null + */ bool deskew( const sensor_msgs::msg::PointCloud2 & input, sensor_msgs::msg::PointCloud2 & output, - double previousStamp, const rtabmap::Transform & velocity); -// Missing function in ros2 (from old pcl_ros) +/** + * @brief Apply a rigid transform to the XYZ fields of a point cloud. + * + * Missing function in ROS 2, taken from the old pcl_ros. + * + * @param transform the transform to apply + * @param in the cloud to transform + * @param[out] out the transformed cloud; all other fields are copied unchanged + */ void transformPointCloud ( const Eigen::Matrix4f &transform, const sensor_msgs::msg::PointCloud2 &in, sensor_msgs::msg::PointCloud2 &out); -/** Return the size of a datatype (which is an enum of sensor_msgs::PointField::) in bytes - * @param datatype one of the enums of sensor_msgs::PointField:: - * Note: Missing function in ros2 (from old pcl_ros) +/** + * @brief Return the size in bytes of a PointField datatype. + * + * Missing function in ROS 2, taken from the old pcl_ros. + * + * @param datatype one of the sensor_msgs::msg::PointField enums + * @return the size in bytes + * @throws std::runtime_error if @p datatype is not a known PointField type */ inline int sizeOfPointField(int datatype) { @@ -330,6 +983,13 @@ inline int sizeOfPointField(int datatype) return -1; } +/** + * @brief Find the entry of a map whose key is closest to @p key. + * @param buffer the map to search; must not be empty + * @param key the key to look for + * @return iterator to the closest entry, clamped to the first or last one when @p key + * falls outside the map's range + */ template typename std::map::const_iterator getClosestIterator( const std::map & buffer, @@ -365,6 +1025,7 @@ typename std::map::const_iterator getClosestIterator( return iterB; } + } #endif /* MSGCONVERSION_H_ */ diff --git a/rtabmap_conversions/package.xml b/rtabmap_conversions/package.xml index 9ad6cadd..9484d9e8 100644 --- a/rtabmap_conversions/package.xml +++ b/rtabmap_conversions/package.xml @@ -30,7 +30,10 @@ tf2_geometry_msgs tf2_ros + ament_cmake_gtest + ament_cmake + rosdoc2.yaml diff --git a/rtabmap_conversions/rosdoc2.yaml b/rtabmap_conversions/rosdoc2.yaml new file mode 100644 index 00000000..bcd0f339 --- /dev/null +++ b/rtabmap_conversions/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_conversions +## Build the docs locally with: +## rosdoc2 build --package-path rtabmap_conversions --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_conversions 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_conversions', + doxygen_xml_directory: 'generated/doxygen/xml', + output_dir: '' + } diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 39dabb6e..e252a617 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -27,6 +27,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap_conversions/MsgConversion.h" +#include +#include + #include #include #include "rclcpp/rclcpp.hpp" @@ -60,21 +63,46 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap_conversions { -void transformToTF(const rtabmap::Transform & transform, tf2::Transform & tfTransform) +bool transformToTF(const rtabmap::Transform & transform, tf2::Transform & tfTransform) { - if(!transform.isNull()) + if(transform.isNull()) { - geometry_msgs::msg::TransformStamped gm = tf2::eigenToTransform(transform.toEigen3d()); - //tf2::fromMsg(gm, tfTransform); - } - else - { - tfTransform = tf2::Transform(tf2::Quaternion(0,0,0,0)); + // tf2::Transform cannot represent a null transform: it stores its rotation as a + // basis matrix, so there is no equivalent of the all-zero quaternion used by the + // geometry_msgs conversions. Fill it with NaN so that a caller ignoring the + // return value corrupts its results loudly instead of silently carrying on with + // an identity that looks legitimate. + const tf2Scalar nan = std::numeric_limits::quiet_NaN(); + tfTransform = tf2::Transform( + tf2::Matrix3x3(nan, nan, nan, nan, nan, nan, nan, nan, nan), + tf2::Vector3(nan, nan, nan)); + return false; } + + geometry_msgs::msg::Transform msg; + transformToGeometryMsg(transform, msg); + tf2::fromMsg(msg, tfTransform); + return true; } rtabmap::Transform transformFromTF(const tf2::Transform & transform) { + // transformToTF() poisons its output with NaN for a null transform, as tf2::Transform + // has no null representation of its own. Map that back to a null transform here so the + // two functions round-trip, and so a NaN coming from anywhere else does not silently + // propagate into the rest of the pipeline. + const tf2::Vector3 & origin = transform.getOrigin(); + const tf2::Matrix3x3 & basis = transform.getBasis(); + bool nan = std::isnan(origin.x()) || std::isnan(origin.y()) || std::isnan(origin.z()); + for(int i=0; !nan && i<3; ++i) + { + nan = std::isnan(basis[i].x()) || std::isnan(basis[i].y()) || std::isnan(basis[i].z()); + } + if(nan) + { + return rtabmap::Transform(); + } + Eigen::Isometry3d eigenTf; geometry_msgs::msg::Transform gm = tf2::toMsg(transform); eigenTf = tf2::transformToEigen(gm); @@ -244,6 +272,7 @@ void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::msg::RGBDIma UERROR("Cannot convert multi-camera data to rgbd image"); return; } + msg.header = header; if(data.cameraModels().size() == 1) { //rgb+depth @@ -558,6 +587,13 @@ void infoFromROS(const rtabmap_msgs::msg::Info & info, rtabmap::Statistics & sta void infoToROS(const rtabmap::Statistics & stats, rtabmap_msgs::msg::Info & info) { + // Fall back to the statistics' own stamp when the caller left the header unstamped. + // Callers that already stamped it keep their value, which may be a publication time + // unrelated to the data, or the exact input stamp rather than this double-derived one. + if(info.header.stamp.sec == 0 && info.header.stamp.nanosec == 0) + { + info.header.stamp = timestampToROS(stats.stamp()); + } info.ref_id = stats.refImageId(); info.loop_closure_id = stats.loopClosureId(); info.proximity_detection_id = stats.proximityDetectionId(); @@ -829,9 +865,12 @@ rtabmap::CameraModel cameraModelFromROS( const sensor_msgs::msg::CameraInfo & camInfo, const rtabmap::Transform & localTransform) { + // Note: k, r and p are fixed-size arrays in the ROS message, so they are never + // empty and their size is always right. An unset matrix is signalled by all-zero + // content instead: k[0] and p[0] hold the focal length, which is always non-zero + // for a valid calibration, and an unset rectification matrix is all zeros. cv:: Mat K; - UASSERT(camInfo.k.empty() || camInfo.k.size() == 9); - if(!camInfo.k.empty()) + if(camInfo.k[0] != 0.0) { K = cv::Mat(3, 3, CV_64FC1); memcpy(K.data, camInfo.k.data(), 9*sizeof(double)); @@ -858,17 +897,22 @@ rtabmap::CameraModel cameraModelFromROS( } } + // R is a rotation matrix, so any of its elements can legitimately be zero: only + // an entirely zero matrix means "not set". cv:: Mat R; - UASSERT(camInfo.r.empty() || camInfo.r.size() == 9); - if(!camInfo.r.empty()) + bool rIsSet = false; + for(size_t i=0; !rIsSet && i odomInfoToStatistics(const rtabmap::OdometryInfo & stats.insert(std::make_pair("Odometry/ICPStructuralComplexity/", info.reg.icpStructuralComplexity)); stats.insert(std::make_pair("Odometry/ICPStructuralDistribution/", info.reg.icpStructuralDistribution)); stats.insert(std::make_pair("Odometry/ICPCorrespondences/", info.reg.icpCorrespondences)); - stats.insert(std::make_pair("Odometry/StdDevLin/", sqrt((float)info.reg.covariance.at(0,0)))); - stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)info.reg.covariance.at(5,5)))); - stats.insert(std::make_pair("Odometry/VarianceLin/", (float)info.reg.covariance.at(0,0))); - stats.insert(std::make_pair("Odometry/VarianceAng/", (float)info.reg.covariance.at(5,5))); + // RegistrationInfo leaves covariance empty by default, so only read it when the + // expected 6x6 matrix is actually there. + if(info.reg.covariance.type() == CV_64FC1 && + info.reg.covariance.rows == 6 && + info.reg.covariance.cols == 6) + { + stats.insert(std::make_pair("Odometry/StdDevLin/", sqrt((float)info.reg.covariance.at(0,0)))); + stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)info.reg.covariance.at(5,5)))); + stats.insert(std::make_pair("Odometry/VarianceLin/", (float)info.reg.covariance.at(0,0))); + stats.insert(std::make_pair("Odometry/VarianceAng/", (float)info.reg.covariance.at(5,5))); + } stats.insert(std::make_pair("Odometry/TimeEstimation/ms", info.timeEstimation*1000.0f)); stats.insert(std::make_pair("Odometry/TimeFiltering/ms", info.timeParticleFiltering*1000.0f)); stats.insert(std::make_pair("Odometry/LocalMapSize/", info.localMapSize)); @@ -2832,8 +2887,7 @@ bool deskew_impl( tf2_ros::Buffer * tfBuffer, double waitForTransform, bool slerp, - const rtabmap::Transform & velocity, - double previousStamp) + const rtabmap::Transform & velocity) { if(tfBuffer != 0) { @@ -2857,12 +2911,6 @@ bool deskew_impl( return false; } - if(previousStamp <= 0.0) - { - UERROR("previousStamp should be >0 when constant velocity model is used!"); - return false; - } - if(velocity.isNull()) { UERROR("velocity should be valid when constant velocity model is used!"); @@ -3133,8 +3181,23 @@ bool deskew_impl( } else if(lastStamp == firstStamp) { - UERROR("First and last stamps in the scan are the same (%f) (header=%f)!", timestampFromROS(lastStamp), timestampFromROS(input.header.stamp)); - return false; + // There is no time spread across the scan, so there is nothing to correct. This + // happens when the driver doesn't fill the per-point time channel, and also when + // the cloud has already been deskewed: deskewing zeroes that channel to mark it. + // Pass the cloud through unchanged so that deskewing twice is a no-op rather than + // a failure that makes the caller drop the frame. + static bool warned = false; + if(!warned) + { + UWARN("First and last stamps in the scan are the same (%f) (header=%f), the " + "cloud is returned unchanged. Either the time channel is not filled by " + "the driver, or the cloud has already been deskewed. This warning is " + "only shown once.", + timestampFromROS(lastStamp), timestampFromROS(input.header.stamp)); + warned = true; + } + output = input; + return true; } std::string errorMsg; if(tfBuffer != 0 && @@ -3184,23 +3247,19 @@ bool deskew_impl( float vx,vy,vz, vroll,vpitch,vyaw; velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw); - // We need three poses: - // 1- The pose of base frame in odom frame at first stamp - // 2- The pose of base frame in odom frame at msg stamp - // 3- The pose of base frame in odom frame at last stamp - UASSERT(timestampFromROS(firstStamp) >= previousStamp); - UASSERT(timestampFromROS(lastStamp) > previousStamp); - double dt1 = timestampFromROS(firstStamp) - previousStamp; - double dt2 = timestampFromROS(input.header.stamp) - previousStamp; - double dt3 = timestampFromROS(lastStamp) - previousStamp; - - rtabmap::Transform p1(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1); - rtabmap::Transform p2(vx*dt2, vy*dt2, vz*dt2, vroll*dt2, vpitch*dt2, vyaw*dt2); - rtabmap::Transform p3(vx*dt3, vy*dt3, vz*dt3, vroll*dt3, vpitch*dt3, vyaw*dt3); + // Integrate the velocity directly from the stamp of the msg, which is the + // frame the deskewed cloud is expressed in. Going through a third, earlier + // reference pose and composing it away would give the same answer for a pure + // translation, but not for a rotation: Transform() scales roll/pitch/yaw + // linearly instead of using the twist exponential, so the composition only + // cancels in the small-angle limit. Keeping dt bounded by the scan duration + // is where that approximation is at its best. + double dt1 = timestampFromROS(firstStamp) - timestampFromROS(input.header.stamp); + double dt3 = timestampFromROS(lastStamp) - timestampFromROS(input.header.stamp); // First and last poses are relative to stamp of the msg - firstPose = p2.inverse() * p1; - lastPose = p2.inverse() * p3; + firstPose = rtabmap::Transform(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1); + lastPose = rtabmap::Transform(vx*dt3, vy*dt3, vz*dt3, vroll*dt3, vpitch*dt3, vyaw*dt3); } if(firstPose.isNull()) @@ -3227,6 +3286,7 @@ bool deskew_impl( output = input; rclcpp::Time stamp; + bool clampWarned = false; // reported once per cloud, see the clamp below UTimer processingTime; if(timeOnColumns) { @@ -3272,7 +3332,27 @@ bool deskew_impl( rtabmap::Transform transform; if(slerp) { - transform = firstPose.interpolate((stamp-firstStamp).seconds() / scanTime, lastPose); + // The ordering check only compares the first and last samples, so a stamp + // outside [firstStamp, lastStamp] can slip through. Clamp it: extrapolating + // would throw the point far beyond the sweep. + double ratio = (stamp-firstStamp).seconds() / scanTime; + if(ratio < 0.0 || ratio > 1.0) + { + // Warned once per cloud rather than once per process: the timestamp + // channel is corrupted, which is a serious upstream problem worth + // reporting on every affected scan, but not once per point. + if(!clampWarned) + { + UWARN("A point has a stamp (%f) outside the first (%f) and last (%f) " + "stamps of the scan, its correction is clamped to the closest end " + "of the sweep. The timestamp channel of the input cloud is likely " + "corrupted. Only the first such point of this cloud is reported.", + timestampFromROS(stamp), timestampFromROS(firstStamp), timestampFromROS(lastStamp)); + clampWarned = true; + } + ratio = ratio<0.0?0.0:1.0; + } + transform = firstPose.interpolate(float(ratio), lastPose); } else { @@ -3366,7 +3446,27 @@ bool deskew_impl( rtabmap::Transform transform; if(slerp) { - transform = firstPose.interpolate((stamp-firstStamp).seconds() / scanTime, lastPose); + // The ordering check only compares the first and last samples, so a stamp + // outside [firstStamp, lastStamp] can slip through. Clamp it: extrapolating + // would throw the point far beyond the sweep. + double ratio = (stamp-firstStamp).seconds() / scanTime; + if(ratio < 0.0 || ratio > 1.0) + { + // Warned once per cloud rather than once per process: the timestamp + // channel is corrupted, which is a serious upstream problem worth + // reporting on every affected scan, but not once per point. + if(!clampWarned) + { + UWARN("A point has a stamp (%f) outside the first (%f) and last (%f) " + "stamps of the scan, its correction is clamped to the closest end " + "of the sweep. The timestamp channel of the input cloud is likely " + "corrupted. Only the first such point of this cloud is reported.", + timestampFromROS(stamp), timestampFromROS(firstStamp), timestampFromROS(lastStamp)); + clampWarned = true; + } + ratio = ratio<0.0?0.0:1.0; + } + transform = firstPose.interpolate(float(ratio), lastPose); } else { @@ -3428,16 +3528,15 @@ bool deskew( double waitForTransform, bool slerp) { - return deskew_impl(input, output, fixedFrameId, &tfBuffer, waitForTransform, slerp, rtabmap::Transform(), 0); + return deskew_impl(input, output, fixedFrameId, &tfBuffer, waitForTransform, slerp, rtabmap::Transform()); } bool deskew( const sensor_msgs::msg::PointCloud2 & input, sensor_msgs::msg::PointCloud2 & output, - double previousStamp, const rtabmap::Transform & velocity) { - return deskew_impl(input, output, "", 0, 0, true, velocity, previousStamp); + return deskew_impl(input, output, "", 0, 0, true, velocity); } @@ -3493,8 +3592,15 @@ transformPointCloud ( Eigen::Vector4f pt_out; bool max_range_point = false; - int distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset; - float* distance_ptr = (dist_idx < 0 ? NULL : (float*)(&in.data[distance_ptr_offset])); + // Only touch in.fields[dist_idx] when the "distance" field actually exists: + // indexing with -1 is out of bounds and aborts on a hardened libstdc++. + int distance_ptr_offset = 0; + float* distance_ptr = NULL; + if (dist_idx >= 0) + { + distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset; + distance_ptr = (float*)(&in.data[distance_ptr_offset]); + } if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2])) { if (distance_ptr==NULL || !std::isfinite(*distance_ptr)) // Invalid point diff --git a/rtabmap_conversions/test/test_msg_conversion.cpp b/rtabmap_conversions/test/test_msg_conversion.cpp new file mode 100644 index 00000000..0b911720 --- /dev/null +++ b/rtabmap_conversions/test/test_msg_conversion.cpp @@ -0,0 +1,3456 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include + +#include +#include +#include + +#include + +#include +#include +#include + +using namespace rtabmap_conversions; + +namespace { + +// A transform with translation and rotation on all three axes, so that a +// round-trip that drops or swaps a component cannot pass by accident. +rtabmap::Transform sampleTransform() +{ + return rtabmap::Transform(1.0f, -2.0f, 3.0f, 0.1f, -0.2f, 0.3f); +} + +void expectTransformNear( + const rtabmap::Transform & actual, + const rtabmap::Transform & expected, + float epsilon = 1e-5f) +{ + ASSERT_FALSE(actual.isNull()) << "expected " << expected.prettyPrint(); + for(int i=0; i<12; ++i) + { + EXPECT_NEAR(actual.data()[i], expected.data()[i], epsilon) + << "at index " << i + << "\n actual: " << actual.prettyPrint() + << "\n expected: " << expected.prettyPrint(); + } +} + +} // namespace + +///////////////////////// +// Transform <-> geometry_msgs +///////////////////////// + +TEST(MsgConversion, transformGeometryMsgRoundTrip) +{ + const rtabmap::Transform in = sampleTransform(); + + geometry_msgs::msg::Transform msg; + transformToGeometryMsg(in, msg); + + expectTransformNear(transformFromGeometryMsg(msg), in); +} + +TEST(MsgConversion, transformGeometryMsgQuaternionIsNormalized) +{ + geometry_msgs::msg::Transform msg; + transformToGeometryMsg(sampleTransform(), msg); + + const double norm = std::sqrt( + msg.rotation.x * msg.rotation.x + + msg.rotation.y * msg.rotation.y + + msg.rotation.z * msg.rotation.z + + msg.rotation.w * msg.rotation.w); + EXPECT_NEAR(norm, 1.0, 1e-9); +} + +TEST(MsgConversion, transformGeometryMsgNullRoundTrip) +{ + geometry_msgs::msg::Transform msg; + transformToGeometryMsg(rtabmap::Transform(), msg); + + // A null transform is encoded as an all-zero quaternion. + EXPECT_EQ(msg.rotation.x, 0.0); + EXPECT_EQ(msg.rotation.y, 0.0); + EXPECT_EQ(msg.rotation.z, 0.0); + EXPECT_EQ(msg.rotation.w, 0.0); + EXPECT_TRUE(transformFromGeometryMsg(msg).isNull()); +} + +TEST(MsgConversion, transformGeometryMsgIdentityIsNotNull) +{ + geometry_msgs::msg::Transform msg; + transformToGeometryMsg(rtabmap::Transform::getIdentity(), msg); + + const rtabmap::Transform out = transformFromGeometryMsg(msg); + EXPECT_FALSE(out.isNull()); + EXPECT_TRUE(out.isIdentity()); +} + +///////////////////////// +// Transform <-> tf2 +///////////////////////// + +TEST(MsgConversion, transformTFRoundTrip) +{ + const rtabmap::Transform in = sampleTransform(); + + tf2::Transform tf; + EXPECT_TRUE(transformToTF(in, tf)); + + expectTransformNear(transformFromTF(tf), in); +} + +TEST(MsgConversion, transformTFIdentityRoundTrip) +{ + tf2::Transform tf; + EXPECT_TRUE(transformToTF(rtabmap::Transform::getIdentity(), tf)) + << "an identity transform is not a null transform"; + + const rtabmap::Transform out = transformFromTF(tf); + EXPECT_FALSE(out.isNull()); + EXPECT_TRUE(out.isIdentity()); +} + +TEST(MsgConversion, transformToTFWritesTranslationAndRotation) +{ + // Guards against the output being left untouched: seed it with a value that + // differs from the expected result, then check it was actually overwritten. + tf2::Transform tf(tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(99, 99, 99)); + EXPECT_TRUE(transformToTF(sampleTransform(), tf)); + + EXPECT_NEAR(tf.getOrigin().x(), 1.0, 1e-5); + EXPECT_NEAR(tf.getOrigin().y(), -2.0, 1e-5); + EXPECT_NEAR(tf.getOrigin().z(), 3.0, 1e-5); + + geometry_msgs::msg::Transform expected; + transformToGeometryMsg(sampleTransform(), expected); + EXPECT_NEAR(tf.getRotation().x(), expected.rotation.x, 1e-5); + EXPECT_NEAR(tf.getRotation().y(), expected.rotation.y, 1e-5); + EXPECT_NEAR(tf.getRotation().z(), expected.rotation.z, 1e-5); + EXPECT_NEAR(tf.getRotation().w(), expected.rotation.w, 1e-5); +} + +TEST(MsgConversion, transformToTFNullReturnsFalseAndNaN) +{ + tf2::Transform tf(tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(99, 99, 99)); + + EXPECT_FALSE(transformToTF(rtabmap::Transform(), tf)); + + // tf2::Transform cannot carry a null sentinel, so the output is poisoned with NaN + // on purpose: a caller that ignores the return value must fail loudly rather than + // silently proceed with a plausible-looking identity. + for(int i=0; i<3; ++i) + { + EXPECT_TRUE(std::isnan(tf.getBasis()[i].x())) << "basis row " << i; + EXPECT_TRUE(std::isnan(tf.getBasis()[i].y())) << "basis row " << i; + EXPECT_TRUE(std::isnan(tf.getBasis()[i].z())) << "basis row " << i; + } + EXPECT_TRUE(std::isnan(tf.getOrigin().x())); + EXPECT_TRUE(std::isnan(tf.getOrigin().y())); + EXPECT_TRUE(std::isnan(tf.getOrigin().z())); + + const tf2::Quaternion q = tf.getRotation(); + EXPECT_TRUE(std::isnan(q.x())); + EXPECT_TRUE(std::isnan(q.y())); + EXPECT_TRUE(std::isnan(q.z())); + EXPECT_TRUE(std::isnan(q.w())); +} + +TEST(MsgConversion, transformToTFNullPoisonsComposition) +{ + // The point of the NaN: it propagates through downstream math instead of + // quietly producing a wrong-but-finite answer. + tf2::Transform tf; + EXPECT_FALSE(transformToTF(rtabmap::Transform(), tf)); + + const tf2::Transform composed = + tf * tf2::Transform(tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(1, 2, 3)); + + EXPECT_TRUE(std::isnan(composed.getOrigin().x())); + EXPECT_TRUE(std::isnan(composed.getOrigin().y())); + EXPECT_TRUE(std::isnan(composed.getOrigin().z())); +} + +TEST(MsgConversion, transformFromTFDetectsNaN) +{ + const tf2Scalar nan = std::numeric_limits::quiet_NaN(); + + // NaN anywhere in the rotation basis... + EXPECT_TRUE(transformFromTF(tf2::Transform( + tf2::Matrix3x3(nan, nan, nan, nan, nan, nan, nan, nan, nan), + tf2::Vector3(0, 0, 0))).isNull()) << "NaN basis"; + + // ...or in the translation alone must yield a null transform. + EXPECT_TRUE(transformFromTF(tf2::Transform( + tf2::Quaternion(0, 0, 0, 1), + tf2::Vector3(nan, 0, 0))).isNull()) << "NaN origin"; +} + +TEST(MsgConversion, transformTFNullRoundTrip) +{ + // The pair round-trips a null transform: toTF poisons with NaN and reports + // false, fromTF maps that back to null. + tf2::Transform tf; + EXPECT_FALSE(transformToTF(rtabmap::Transform(), tf)); + EXPECT_TRUE(transformFromTF(tf).isNull()); +} + +TEST(MsgConversion, transformFromTFAcceptsValidTransforms) +{ + // The NaN guard must not reject legitimate values, including zeros. + EXPECT_FALSE(transformFromTF(tf2::Transform( + tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(0, 0, 0))).isNull()); + EXPECT_FALSE(transformFromTF(tf2::Transform( + tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(-1, 2, -3))).isNull()); +} + +///////////////////////// +// Transform <-> Pose +///////////////////////// + +TEST(MsgConversion, transformPoseMsgRoundTrip) +{ + const rtabmap::Transform in = sampleTransform(); + + geometry_msgs::msg::Pose msg; + transformToPoseMsg(in, msg); + + expectTransformNear(transformFromPoseMsg(msg), in); +} + +TEST(MsgConversion, transformPoseMsgNullIsNull) +{ + geometry_msgs::msg::Pose msg; + transformToPoseMsg(rtabmap::Transform(), msg); + + EXPECT_TRUE(transformFromPoseMsg(msg).isNull()); +} + +TEST(MsgConversion, transformPoseMsgIgnoreRotationIfNotSet) +{ + // Note: geometry_msgs::msg::Quaternion defaults to w=1.0, so an "unset" + // orientation has to be zeroed explicitly to reach the branch under test. + geometry_msgs::msg::Pose msg; + msg.position.x = 1.0; + msg.position.y = 2.0; + msg.position.z = 3.0; + msg.orientation.w = 0.0; + + // An all-zero orientation normally yields a null transform... + + EXPECT_TRUE(transformFromPoseMsg(msg, false).isNull()); + + // ...but with ignoreRotationIfNotSet the translation is kept with no rotation. + expectTransformNear( + transformFromPoseMsg(msg, true), + rtabmap::Transform(1.0f, 2.0f, 3.0f, 0.0f, 0.0f, 0.0f)); +} + +///////////////////////// +// Points and keypoints +///////////////////////// + +TEST(MsgConversion, point2fRoundTrip) +{ + const cv::Point2f in(1.5f, -2.5f); + + rtabmap_msgs::msg::Point2f msg; + point2fToROS(in, msg); + const cv::Point2f out = point2fFromROS(msg); + + EXPECT_FLOAT_EQ(out.x, in.x); + EXPECT_FLOAT_EQ(out.y, in.y); +} + +TEST(MsgConversion, points2fVectorRoundTrip) +{ + const std::vector in = {{1.0f, 2.0f}, {-3.0f, 4.5f}}; + + std::vector msg; + points2fToROS(in, msg); + const std::vector out = points2fFromROS(msg); + + ASSERT_EQ(out.size(), in.size()); + for(size_t i=0; i in = {{1.0f, 2.0f, 3.0f}, {-4.0f, 5.0f, -6.0f}}; + + std::vector msg; + points3fToROS(in, msg); + const std::vector out = points3fFromROS(msg); + + ASSERT_EQ(out.size(), in.size()); + for(size_t i=0; i in = {{1.0f, 2.0f, 3.0f}}; + const rtabmap::Transform t = sampleTransform(); + + // Applying t on the way out and t.inverse() on the way in must cancel. + std::vector msg; + points3fToROS(in, msg, t); + const std::vector out = points3fFromROS(msg, t.inverse()); + + ASSERT_EQ(out.size(), in.size()); + EXPECT_NEAR(out[0].x, in[0].x, 1e-4); + EXPECT_NEAR(out[0].y, in[0].y, 1e-4); + EXPECT_NEAR(out[0].z, in[0].z, 1e-4); + + // ...and the intermediate message really is the transformed point. + const cv::Point3f expected = rtabmap::util3d::transformPoint(in[0], t); + EXPECT_NEAR(msg[0].x, expected.x, 1e-4); + EXPECT_NEAR(msg[0].y, expected.y, 1e-4); + EXPECT_NEAR(msg[0].z, expected.z, 1e-4); +} + +TEST(MsgConversion, points3fFromROSAppendsToExistingVector) +{ + std::vector msg(2); + msg[0].x = 1.0f; + msg[1].x = 2.0f; + + std::vector points = {{9.0f, 9.0f, 9.0f}}; + points3fFromROS(msg, points); + + ASSERT_EQ(points.size(), 3u); + EXPECT_FLOAT_EQ(points[0].x, 9.0f) << "existing content must be preserved"; + EXPECT_FLOAT_EQ(points[1].x, 1.0f); + EXPECT_FLOAT_EQ(points[2].x, 2.0f); +} + +TEST(MsgConversion, keypointRoundTrip) +{ + const cv::KeyPoint in(cv::Point2f(10.0f, 20.0f), 7.0f, 45.0f, 0.5f, 2, 3); + + rtabmap_msgs::msg::KeyPoint msg; + keypointToROS(in, msg); + const cv::KeyPoint out = keypointFromROS(msg); + + EXPECT_FLOAT_EQ(out.pt.x, in.pt.x); + EXPECT_FLOAT_EQ(out.pt.y, in.pt.y); + EXPECT_FLOAT_EQ(out.size, in.size); + EXPECT_FLOAT_EQ(out.angle, in.angle); + EXPECT_FLOAT_EQ(out.response, in.response); + EXPECT_EQ(out.octave, in.octave); + EXPECT_EQ(out.class_id, in.class_id); +} + +TEST(MsgConversion, keypointsFromROSAppendsAndAppliesXShift) +{ + const std::vector in = { + cv::KeyPoint(cv::Point2f(10.0f, 20.0f), 7.0f), + cv::KeyPoint(cv::Point2f(30.0f, 40.0f), 7.0f)}; + + std::vector msg; + keypointsToROS(in, msg); + ASSERT_EQ(msg.size(), in.size()); + + std::vector kpts = {cv::KeyPoint(cv::Point2f(1.0f, 1.0f), 1.0f)}; + keypointsFromROS(msg, kpts, /*xShift=*/100); + + ASSERT_EQ(kpts.size(), 3u); + EXPECT_FLOAT_EQ(kpts[0].pt.x, 1.0f) << "existing content must be preserved"; + EXPECT_FLOAT_EQ(kpts[1].pt.x, 110.0f); + EXPECT_FLOAT_EQ(kpts[2].pt.x, 130.0f); + EXPECT_FLOAT_EQ(kpts[1].pt.y, 20.0f) << "xShift must not touch y"; +} + +///////////////////////// +// Timestamps +///////////////////////// + +TEST(MsgConversion, timestampRoundTrip) +{ + // Not exact on purpose: a double resolves to a few hundred nanoseconds at this + // magnitude, so the round trip is only good to about a microsecond. + const double in = 1234567890.123456; + EXPECT_NEAR(timestampFromROS(timestampToROS(in)), in, 1e-6); +} + +TEST(MsgConversion, timestampToROSUsesRosClock) +{ + // Message header stamps convert to RCL_ROS_TIME, while rclcpp::Time(sec, nsec) + // defaults to RCL_SYSTEM_TIME. Comparing two different clock types throws, so a + // timestamp built here must be comparable with one taken from a message -- several + // conversions do exactly that when syncing to an odometry stamp. + const rclcpp::Time built = timestampToROS(1000.0); + EXPECT_EQ(built.get_clock_type(), RCL_ROS_TIME); + + builtin_interfaces::msg::Time asMsg = timestampToROS(1000.5); + const rclcpp::Time fromMsg(asMsg); + EXPECT_EQ(fromMsg.get_clock_type(), RCL_ROS_TIME); + + EXPECT_NO_THROW({ volatile bool differ = (built != fromMsg); (void)differ; }) + << "a built stamp must be comparable with a message-derived one"; +} + +TEST(MsgConversion, timestampZeroRoundTrip) +{ + EXPECT_EQ(timestampFromROS(timestampToROS(0.0)), 0.0); +} + +///////////////////////// +// sizeOfPointField +///////////////////////// + +TEST(MsgConversion, sizeOfPointField) +{ + EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::INT8), 1); + EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::UINT8), 1); + EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::INT16), 2); + EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::UINT16), 2); + EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::INT32), 4); + EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::UINT32), 4); + EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::FLOAT32), 4); + EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::FLOAT64), 8); +} + +TEST(MsgConversion, sizeOfPointFieldThrowsOnUnknownType) +{ + EXPECT_THROW(sizeOfPointField(42), std::runtime_error); +} + +///////////////////////// +// getClosestIterator +///////////////////////// + +TEST(MsgConversion, getClosestIterator) +{ + const std::map buffer = {{1.0, 10}, {2.0, 20}, {3.0, 30}}; + + EXPECT_EQ(getClosestIterator(buffer, 1.0)->second, 10) << "exact match"; + EXPECT_EQ(getClosestIterator(buffer, 2.0)->second, 20) << "exact match"; + EXPECT_EQ(getClosestIterator(buffer, 1.4)->second, 10) << "closer to lower"; + EXPECT_EQ(getClosestIterator(buffer, 1.6)->second, 20) << "closer to upper"; + EXPECT_EQ(getClosestIterator(buffer, 0.0)->second, 10) << "clamped below range"; + EXPECT_EQ(getClosestIterator(buffer, 99.0)->second, 30) << "clamped above range"; +} + +TEST(MsgConversion, getClosestIteratorSingleEntry) +{ + const std::map buffer = {{5.0, 50}}; + + EXPECT_EQ(getClosestIterator(buffer, 0.0)->second, 50); + EXPECT_EQ(getClosestIterator(buffer, 99.0)->second, 50); +} + +///////////////////////// +// compressedMat <-> bytes +///////////////////////// + +TEST(MsgConversion, compressedMatRoundTrip) +{ + const cv::Mat in = (cv::Mat_(1, 5) << 1, 2, 3, 250, 255); + + std::vector bytes; + compressedMatToBytes(in, bytes); + ASSERT_EQ(bytes.size(), 5u); + + const cv::Mat out = compressedMatFromBytes(bytes); + ASSERT_EQ(out.type(), CV_8UC1); + ASSERT_EQ(out.total(), in.total()); + EXPECT_EQ(cv::countNonZero(out.reshape(1, 1) != in.reshape(1, 1)), 0); +} + +TEST(MsgConversion, compressedMatEmptyRoundTrip) +{ + std::vector bytes = {1, 2, 3}; + compressedMatToBytes(cv::Mat(), bytes); + + EXPECT_TRUE(bytes.empty()) << "output must be cleared"; + EXPECT_TRUE(compressedMatFromBytes(bytes).empty()); +} + +TEST(MsgConversion, compressedMatFromBytesCopyFlag) +{ + std::vector bytes = {1, 2, 3}; + + const cv::Mat shared = compressedMatFromBytes(bytes, /*copy=*/false); + const cv::Mat copied = compressedMatFromBytes(bytes, /*copy=*/true); + + bytes[0] = 99; + EXPECT_EQ(shared.at(0, 0), 99) << "copy=false must alias the input"; + EXPECT_EQ(copied.at(0, 0), 1) << "copy=true must be independent"; +} + +///////////////////////// +// EnvSensor +///////////////////////// + +TEST(MsgConversion, envSensorRoundTrip) +{ + const rtabmap::EnvSensor in( + rtabmap::EnvSensor::kAmbientTemperature, 21.5, 1234567890.5); + + rtabmap_msgs::msg::EnvSensor msg; + envSensorToROS(in, msg); + const rtabmap::EnvSensor out = envSensorFromROS(msg); + + EXPECT_EQ(out.type(), in.type()); + EXPECT_DOUBLE_EQ(out.value(), in.value()); + EXPECT_NEAR(out.stamp(), in.stamp(), 1e-6); +} + +TEST(MsgConversion, envSensorsRoundTripKeyedByType) +{ + rtabmap::EnvSensors in; + in.insert(std::make_pair( + rtabmap::EnvSensor::kAmbientTemperature, + rtabmap::EnvSensor(rtabmap::EnvSensor::kAmbientTemperature, 21.5, 1.0))); + in.insert(std::make_pair( + rtabmap::EnvSensor::kAmbientLight, + rtabmap::EnvSensor(rtabmap::EnvSensor::kAmbientLight, 300.0, 2.0))); + + std::vector msg; + envSensorsToROS(in, msg); + ASSERT_EQ(msg.size(), in.size()); + + const rtabmap::EnvSensors out = envSensorsFromROS(msg); + ASSERT_EQ(out.size(), in.size()); + for(rtabmap::EnvSensors::const_iterator iter=in.begin(); iter!=in.end(); ++iter) + { + rtabmap::EnvSensors::const_iterator found = out.find(iter->first); + ASSERT_NE(found, out.end()) << "missing type " << iter->first; + EXPECT_DOUBLE_EQ(found->second.value(), iter->second.value()); + } +} + +///////////////////////// +// Link +///////////////////////// + +TEST(MsgConversion, linkRoundTrip) +{ + cv::Mat information = cv::Mat::eye(6, 6, CV_64FC1) * 3.0; + const rtabmap::Link in( + 1, 2, rtabmap::Link::kGlobalClosure, sampleTransform(), information); + + rtabmap_msgs::msg::Link msg; + linkToROS(in, msg); + const rtabmap::Link out = linkFromROS(msg); + + EXPECT_EQ(out.from(), in.from()); + EXPECT_EQ(out.to(), in.to()); + EXPECT_EQ(out.type(), in.type()); + expectTransformNear(out.transform(), in.transform()); + + ASSERT_EQ(out.infMatrix().rows, 6); + ASSERT_EQ(out.infMatrix().cols, 6); + for(int i=0; i<6; ++i) + { + for(int j=0; j<6; ++j) + { + EXPECT_DOUBLE_EQ( + out.infMatrix().at(i, j), + in.infMatrix().at(i, j)) << "at " << i << "," << j; + } + } +} + +///////////////////////// +// CameraModel +///////////////////////// + +TEST(MsgConversion, cameraModelFromROSReadsIntrinsics) +{ + sensor_msgs::msg::CameraInfo in; + in.width = 640; + in.height = 480; + in.distortion_model = "plumb_bob"; + in.d = {0.1, 0.2, 0.3, 0.4, 0.5}; + in.k = {525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0}; + in.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + in.p = {525.0, 0.0, 320.0, 0.0, 0.0, 525.0, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0}; + + const rtabmap::Transform localTransform(0.0f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f); + const rtabmap::CameraModel model = cameraModelFromROS(in, localTransform); + + EXPECT_EQ(model.imageWidth(), 640); + EXPECT_EQ(model.imageHeight(), 480); + EXPECT_NEAR(model.fx(), 525.0, 1e-9); + EXPECT_NEAR(model.fy(), 525.0, 1e-9); + EXPECT_NEAR(model.cx(), 320.0, 1e-9); + EXPECT_NEAR(model.cy(), 240.0, 1e-9); + expectTransformNear(model.localTransform(), localTransform); + + // The raw distortion coefficients are kept verbatim. + ASSERT_EQ(model.D_raw().cols, 5); + for(size_t i=0; i(0, i), in.d[i], 1e-9) << "d at " << i; + } +} + +TEST(MsgConversion, cameraModelToROSRectifiedHasNoDistortion) +{ + sensor_msgs::msg::CameraInfo in; + in.width = 640; + in.height = 480; + in.distortion_model = "plumb_bob"; + in.d = {0.1, 0.2, 0.3, 0.4, 0.5}; + in.k = {525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0}; + in.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + in.p = {525.0, 0.0, 320.0, 0.0, 0.0, 525.0, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0}; + + sensor_msgs::msg::CameraInfo out; + cameraModelToROS(cameraModelFromROS(in, rtabmap::Transform::getIdentity()), out); + + EXPECT_EQ(out.width, in.width); + EXPECT_EQ(out.height, in.height); + + // A model carrying a projection matrix describes an already-rectified image, + // so cameraModelToROS deliberately emits zero distortion rather than echoing + // back the raw coefficients. K and P do round-trip unchanged. + EXPECT_EQ(out.distortion_model, "plumb_bob"); + ASSERT_EQ(out.d.size(), 5u); + for(size_t i=0; i(3, 3) << + 525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0); + cv::Mat D = cv::Mat::zeros(1, 6, CV_64FC1); + D.at(0, 0) = 0.1; + D.at(0, 1) = 0.2; + D.at(0, 4) = 0.3; + D.at(0, 5) = 0.4; + + const rtabmap::CameraModel model( + "fisheye", cv::Size(640, 480), K, D, cv::Mat(), cv::Mat(), + rtabmap::Transform::getIdentity()); + + sensor_msgs::msg::CameraInfo out; + cameraModelToROS(model, out); + + EXPECT_EQ(out.distortion_model, "equidistant"); + ASSERT_EQ(out.d.size(), 4u); + EXPECT_NEAR(out.d[0], 0.1, 1e-9); + EXPECT_NEAR(out.d[1], 0.2, 1e-9); + EXPECT_NEAR(out.d[2], 0.3, 1e-9); + EXPECT_NEAR(out.d[3], 0.4, 1e-9); +} + +TEST(MsgConversion, cameraModelToROSRationalPolynomialDistortion) +{ + cv::Mat K = (cv::Mat_(3, 3) << + 525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0); + cv::Mat D = (cv::Mat_(1, 8) << + 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8); + + const rtabmap::CameraModel model( + "rational", cv::Size(640, 480), K, D, cv::Mat(), cv::Mat(), + rtabmap::Transform::getIdentity()); + + sensor_msgs::msg::CameraInfo out; + cameraModelToROS(model, out); + + EXPECT_EQ(out.distortion_model, "rational_polynomial"); + ASSERT_EQ(out.d.size(), 8u); + for(size_t i=0; i model -> message round trip. + sensor_msgs::msg::CameraInfo in; + in.width = 640; + in.height = 480; + in.distortion_model = "equidistant"; + in.d = {0.1, 0.2, 0.3, 0.4}; + in.k = {525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0}; + + sensor_msgs::msg::CameraInfo out; + cameraModelToROS(cameraModelFromROS(in, rtabmap::Transform::getIdentity()), out); + + EXPECT_EQ(out.distortion_model, "equidistant"); + ASSERT_EQ(out.d.size(), 4u); + for(size_t i=0; i identity = {1., 0., 0., 0., 1., 0., 0., 0., 1.}; + for(size_t i=0; i(3, 3) << + 525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0); + + const rtabmap::CameraModel model( + "raw", cv::Size(640, 480), K, cv::Mat(), cv::Mat(), cv::Mat(), + rtabmap::Transform::getIdentity()); + + sensor_msgs::msg::CameraInfo out; + cameraModelToROS(model, out); + + const std::array expected = { + 525.0, 0.0, 320.0, 0.0, + 0.0, 525.0, 240.0, 0.0, + 0.0, 0.0, 1.0, 0.0}; + for(size_t i=0; i(1, 4) << 1.0f, 2.0f, 3.0f, 4.0f); + cv::Mat info = (cv::Mat_(1, 2) << 9.0f, 8.0f); + const rtabmap::GlobalDescriptor in(7, data, info); + + rtabmap_msgs::msg::GlobalDescriptor msg; + globalDescriptorToROS(in, msg); + const rtabmap::GlobalDescriptor out = globalDescriptorFromROS(msg); + + EXPECT_EQ(out.type(), in.type()); + ASSERT_EQ(out.data().total(), in.data().total()); + for(size_t i=0; i(0, i), in.data().at(0, i)) << "data at " << i; + } + ASSERT_EQ(out.info().total(), in.info().total()); + for(size_t i=0; i(0, i), in.info().at(0, i)) << "info at " << i; + } +} + +TEST(MsgConversion, globalDescriptorsVectorRoundTrip) +{ + std::vector in; + in.push_back(rtabmap::GlobalDescriptor(1, (cv::Mat_(1, 2) << 1.0f, 2.0f))); + in.push_back(rtabmap::GlobalDescriptor(2, (cv::Mat_(1, 2) << 3.0f, 4.0f))); + + std::vector msg; + globalDescriptorsToROS(in, msg); + ASSERT_EQ(msg.size(), in.size()); + + const std::vector out = globalDescriptorsFromROS(msg); + ASSERT_EQ(out.size(), in.size()); + for(size_t i=0; i(0, 0), in[i].data().at(0, 0)) << "at " << i; + } +} + +TEST(MsgConversion, globalDescriptorsEmptyRoundTrip) +{ + std::vector msg(3); + globalDescriptorsToROS(std::vector(), msg); + + EXPECT_TRUE(msg.empty()) << "output must be cleared"; + EXPECT_TRUE(globalDescriptorsFromROS(msg).empty()); +} + +///////////////////////// +// UserData +///////////////////////// + +TEST(MsgConversion, userDataUncompressedRoundTrip) +{ + const cv::Mat in = (cv::Mat_(2, 3) << 1, 2, 3, 4, 5, 6); + + rtabmap_msgs::msg::UserData msg; + userDataToROS(in, msg, /*compress=*/false); + + EXPECT_EQ(msg.rows, in.rows); + EXPECT_EQ(msg.cols, in.cols); + EXPECT_EQ(msg.type, in.type()); + + const cv::Mat out = userDataFromROS(msg); + ASSERT_EQ(out.rows, in.rows); + ASSERT_EQ(out.cols, in.cols); + ASSERT_EQ(out.type(), in.type()); + EXPECT_EQ(cv::countNonZero(out != in), 0); +} + +TEST(MsgConversion, userDataCompressedRoundTrip) +{ + const cv::Mat in = (cv::Mat_(2, 3) << 1, 2, 3, 4, 5, 6); + + rtabmap_msgs::msg::UserData msg; + userDataToROS(in, msg, /*compress=*/true); + + // Compressed payloads travel as a 1xN byte blob. + EXPECT_EQ(msg.rows, 1); + EXPECT_EQ(msg.type, CV_8UC1); + EXPECT_EQ((size_t)msg.cols, msg.data.size()); + + // userDataFromROS hands back the still-compressed blob; the caller uncompresses. + const cv::Mat blob = userDataFromROS(msg); + ASSERT_FALSE(blob.empty()); + const cv::Mat out = rtabmap::uncompressData(blob); + + ASSERT_EQ(out.rows, in.rows); + ASSERT_EQ(out.cols, in.cols); + ASSERT_EQ(out.type(), in.type()); + EXPECT_EQ(cv::countNonZero(out != in), 0); +} + +TEST(MsgConversion, userDataEmpty) +{ + rtabmap_msgs::msg::UserData msg; + userDataToROS(cv::Mat(), msg, /*compress=*/false); + EXPECT_TRUE(msg.data.empty()); + EXPECT_TRUE(userDataFromROS(msg).empty()); +} + +///////////////////////// +// StereoCameraModel +///////////////////////// + +TEST(MsgConversion, stereoCameraModelFromROS) +{ + const double fx = 525.0; + const double baseline = 0.12; + + sensor_msgs::msg::CameraInfo left; + left.width = 640; + left.height = 480; + left.k = {fx, 0.0, 320.0, 0.0, fx, 240.0, 0.0, 0.0, 1.0}; + left.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + left.p = {fx, 0.0, 320.0, 0.0, 0.0, fx, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0}; + + // The right camera carries the baseline in P(0,3) = -fx * baseline. + sensor_msgs::msg::CameraInfo right = left; + right.p[3] = -fx * baseline; + + const rtabmap::StereoCameraModel model = stereoCameraModelFromROS( + left, right, rtabmap::Transform::getIdentity()); + + EXPECT_NEAR(model.left().fx(), fx, 1e-9); + EXPECT_NEAR(model.right().fx(), fx, 1e-9); + EXPECT_NEAR(model.baseline(), baseline, 1e-9); + EXPECT_TRUE(model.isValidForProjection()); +} + +///////////////////////// +// OdometryInfo +///////////////////////// + +TEST(MsgConversion, odomInfoRoundTrip) +{ + rtabmap::OdometryInfo in; + in.lost = false; + in.features = 500; + in.localMapSize = 1000; + in.localScanMapSize = 2000; + in.localKeyFrames = 5; + in.keyFrameAdded = true; + in.timeEstimation = 0.02f; + in.interval = 0.033; + in.distanceTravelled = 12.5f; + in.reg.matches = 300; + in.reg.inliers = 250; + in.transform = sampleTransform(); + + rtabmap_msgs::msg::OdomInfo msg; + odomInfoToROS(in, msg); + const rtabmap::OdometryInfo out = odomInfoFromROS(msg); + + EXPECT_EQ(out.lost, in.lost); + EXPECT_EQ(out.features, in.features); + EXPECT_EQ(out.localMapSize, in.localMapSize); + EXPECT_EQ(out.localScanMapSize, in.localScanMapSize); + EXPECT_EQ(out.localKeyFrames, in.localKeyFrames); + EXPECT_EQ(out.keyFrameAdded, in.keyFrameAdded); + EXPECT_FLOAT_EQ(out.timeEstimation, in.timeEstimation); + EXPECT_NEAR(out.interval, in.interval, 1e-6); + EXPECT_FLOAT_EQ(out.distanceTravelled, in.distanceTravelled); + EXPECT_EQ(out.reg.matches, in.reg.matches); + EXPECT_EQ(out.reg.inliers, in.reg.inliers); + expectTransformNear(out.transform, in.transform); +} + +TEST(MsgConversion, odomInfoIgnoreDataDropsHeavyMembers) +{ + rtabmap::OdometryInfo in; + in.features = 500; + in.reg.inliers = 250; + in.words.insert(std::make_pair(1, cv::KeyPoint(cv::Point2f(1, 2), 3))); + in.localMap.insert(std::make_pair(1, cv::Point3f(1, 2, 3))); + + rtabmap_msgs::msg::OdomInfo full; + odomInfoToROS(in, full, /*ignoreData=*/false); + EXPECT_FALSE(full.words_keys.empty()); + + rtabmap_msgs::msg::OdomInfo light; + odomInfoToROS(in, light, /*ignoreData=*/true); + EXPECT_TRUE(light.words_keys.empty()) << "heavy members must be dropped"; + + // The scalar statistics survive either way. + EXPECT_EQ(odomInfoFromROS(light).features, in.features); + EXPECT_EQ(odomInfoFromROS(light).reg.inliers, in.reg.inliers); +} + +TEST(MsgConversion, odomInfoToStatistics) +{ + rtabmap::OdometryInfo info; + info.features = 400; + info.reg.inliers = 100; + info.reg.matches = 200; + info.localMapSize = 1234; + + const std::map stats = odomInfoToStatistics(info); + + ASSERT_TRUE(stats.find("Odometry/Features/") != stats.end()); + EXPECT_FLOAT_EQ(stats.at("Odometry/Features/"), 400.0f); + EXPECT_FLOAT_EQ(stats.at("Odometry/Matches/"), 200.0f); + EXPECT_FLOAT_EQ(stats.at("Odometry/Inliers/"), 100.0f); + EXPECT_FLOAT_EQ(stats.at("Odometry/LocalMapSize/"), 1234.0f); + // MatchesRatio is inliers/features, and must not divide by zero. + EXPECT_FLOAT_EQ(stats.at("Odometry/MatchesRatio/"), 100.0f/400.0f); +} + +TEST(MsgConversion, odomInfoToStatisticsEmptyCovariance) +{ + // RegistrationInfo does not initialize covariance, so a plain OdometryInfo has + // an empty matrix. Reading it must not be attempted. + rtabmap::OdometryInfo info; + ASSERT_TRUE(info.reg.covariance.empty()) << "precondition"; + + const std::map stats = odomInfoToStatistics(info); + + EXPECT_TRUE(stats.find("Odometry/StdDevLin/") == stats.end()) + << "covariance-derived stats must be omitted, not read out of bounds"; + EXPECT_TRUE(stats.find("Odometry/VarianceAng/") == stats.end()); + // The rest of the statistics are still produced. + EXPECT_TRUE(stats.find("Odometry/Features/") != stats.end()); +} + +TEST(MsgConversion, odomInfoToStatisticsWithCovariance) +{ + rtabmap::OdometryInfo info; + info.reg.covariance = cv::Mat::eye(6, 6, CV_64FC1) * 4.0; + + const std::map stats = odomInfoToStatistics(info); + + ASSERT_TRUE(stats.find("Odometry/VarianceLin/") != stats.end()); + EXPECT_FLOAT_EQ(stats.at("Odometry/VarianceLin/"), 4.0f); + EXPECT_FLOAT_EQ(stats.at("Odometry/StdDevLin/"), 2.0f); + EXPECT_FLOAT_EQ(stats.at("Odometry/VarianceAng/"), 4.0f); + EXPECT_FLOAT_EQ(stats.at("Odometry/StdDevAng/"), 2.0f); +} + +TEST(MsgConversion, odomInfoToStatisticsNoFeatures) +{ + rtabmap::OdometryInfo info; + info.features = 0; + info.reg.inliers = 10; + + EXPECT_FLOAT_EQ(odomInfoToStatistics(info).at("Odometry/MatchesRatio/"), 0.0f) + << "must not divide by zero"; +} + +///////////////////////// +// MapGraph / MapData +///////////////////////// + +TEST(MsgConversion, mapGraphRoundTrip) +{ + std::map poses; + poses.insert(std::make_pair(1, rtabmap::Transform(1, 0, 0, 0, 0, 0))); + poses.insert(std::make_pair(2, sampleTransform())); + + std::multimap links; + links.insert(std::make_pair(1, rtabmap::Link( + 1, 2, rtabmap::Link::kNeighbor, sampleTransform(), + cv::Mat::eye(6, 6, CV_64FC1) * 2.0))); + links.insert(std::make_pair(2, rtabmap::Link( + 2, 1, rtabmap::Link::kGlobalClosure, rtabmap::Transform::getIdentity(), + cv::Mat::eye(6, 6, CV_64FC1)))); + + const rtabmap::Transform mapToOdom(0.5f, -0.5f, 0.0f, 0.0f, 0.0f, 0.1f); + + rtabmap_msgs::msg::MapGraph msg; + mapGraphToROS(poses, links, mapToOdom, msg); + ASSERT_EQ(msg.poses.size(), poses.size()); + ASSERT_EQ(msg.poses_id.size(), poses.size()); + ASSERT_EQ(msg.links.size(), links.size()); + + std::map outPoses; + std::multimap outLinks; + rtabmap::Transform outMapToOdom; + mapGraphFromROS(msg, outPoses, outLinks, outMapToOdom); + + ASSERT_EQ(outPoses.size(), poses.size()); + for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + ASSERT_TRUE(outPoses.find(iter->first) != outPoses.end()) << "missing pose " << iter->first; + expectTransformNear(outPoses.at(iter->first), iter->second); + } + + ASSERT_EQ(outLinks.size(), links.size()); + for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) + { + std::multimap::const_iterator found = outLinks.find(iter->first); + ASSERT_TRUE(found != outLinks.end()) << "missing link from " << iter->first; + EXPECT_EQ(found->second.from(), iter->second.from()); + EXPECT_EQ(found->second.to(), iter->second.to()); + EXPECT_EQ(found->second.type(), iter->second.type()); + } + + expectTransformNear(outMapToOdom, mapToOdom); +} + +TEST(MsgConversion, mapGraphEmptyRoundTrip) +{ + rtabmap_msgs::msg::MapGraph msg; + mapGraphToROS(std::map(), std::multimap(), + rtabmap::Transform(), msg); + + EXPECT_TRUE(msg.poses.empty()); + EXPECT_TRUE(msg.links.empty()); + + std::map poses; + std::multimap links; + rtabmap::Transform mapToOdom; + mapGraphFromROS(msg, poses, links, mapToOdom); + + EXPECT_TRUE(poses.empty()); + EXPECT_TRUE(links.empty()); + EXPECT_TRUE(mapToOdom.isNull()) << "a null map_to_odom must survive as null"; +} + +TEST(MsgConversion, mapDataRoundTrip) +{ + std::map poses; + poses.insert(std::make_pair(1, sampleTransform())); + + std::multimap links; + links.insert(std::make_pair(1, rtabmap::Link( + 1, 2, rtabmap::Link::kNeighbor, sampleTransform()))); + + std::map signatures; + rtabmap::Signature sig(1, 0, 3, 1234.5, "my_label", sampleTransform()); + signatures.insert(std::make_pair(1, sig)); + + const rtabmap::Transform mapToOdom = rtabmap::Transform::getIdentity(); + + rtabmap_msgs::msg::MapData msg; + mapDataToROS(poses, links, signatures, mapToOdom, msg); + ASSERT_EQ(msg.nodes.size(), signatures.size()); + ASSERT_EQ(msg.graph.poses.size(), poses.size()); + + std::map outPoses; + std::multimap outLinks; + std::map outSignatures; + rtabmap::Transform outMapToOdom; + mapDataFromROS(msg, outPoses, outLinks, outSignatures, outMapToOdom); + + EXPECT_EQ(outPoses.size(), poses.size()); + EXPECT_EQ(outLinks.size(), links.size()); + ASSERT_EQ(outSignatures.size(), signatures.size()); + ASSERT_TRUE(outSignatures.find(1) != outSignatures.end()); + EXPECT_EQ(outSignatures.at(1).id(), sig.id()); + EXPECT_EQ(outSignatures.at(1).getLabel(), sig.getLabel()); + EXPECT_EQ(outSignatures.at(1).getWeight(), sig.getWeight()); + EXPECT_NEAR(outSignatures.at(1).getStamp(), sig.getStamp(), 1e-6); +} + +///////////////////////// +// Node / Signature +///////////////////////// + +namespace { + +rtabmap::Signature sampleSignature() +{ + rtabmap::Signature s(7, 2, 3, 1234.5, "node_label", sampleTransform()); + + std::multimap words; + std::vector kpts; + std::vector pts3; + cv::Mat descriptors(2, 4, CV_32FC1); + for(int i=0; i<2; ++i) + { + words.insert(std::make_pair(100 + i, i)); + kpts.push_back(cv::KeyPoint(cv::Point2f(10.0f * i, 20.0f * i), 7.0f)); + pts3.push_back(cv::Point3f(1.0f * i, 2.0f * i, 3.0f * i)); + for(int j=0; j<4; ++j) + { + descriptors.at(i, j) = float(i * 4 + j); + } + } + s.setWords(words, kpts, pts3, descriptors); + return s; +} + +} // namespace + +TEST(MsgConversion, nodeRoundTrip) +{ + const rtabmap::Signature in = sampleSignature(); + + rtabmap_msgs::msg::Node msg; + nodeToROS(in, msg); + const rtabmap::Signature out = nodeFromROS(msg); + + EXPECT_EQ(out.id(), in.id()); + EXPECT_EQ(out.mapId(), in.mapId()); + EXPECT_EQ(out.getWeight(), in.getWeight()); + EXPECT_NEAR(out.getStamp(), in.getStamp(), 1e-6); + EXPECT_EQ(out.getLabel(), in.getLabel()); + expectTransformNear(out.getPose(), in.getPose()); + + // Visual words: ids, keypoints, 3D points and descriptors. + ASSERT_EQ(out.getWords().size(), in.getWords().size()); + EXPECT_TRUE(std::equal(out.getWords().begin(), out.getWords().end(), in.getWords().begin())); + + ASSERT_EQ(out.getWordsKpts().size(), in.getWordsKpts().size()); + for(size_t i=0; i(3, 3) << + 525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0); + const rtabmap::CameraModel model( + "cam", cv::Size(640, 480), K, cv::Mat(), cv::Mat(), cv::Mat(), + rtabmap::Transform(0.0f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f)); + + rtabmap::SensorData in(cv::Mat(), cv::Mat(), model, 42, 1234.5); + in.setGroundTruth(sampleTransform()); + in.setGPS(rtabmap::GPS(1234.5, -71.9, 45.4, 100.0, 5.0, 90.0)); + + rtabmap_msgs::msg::SensorData msg; + sensorDataToROS(in, msg, "base_link"); + EXPECT_EQ(msg.header.frame_id, "base_link"); + + const rtabmap::SensorData out = sensorDataFromROS(msg); + + EXPECT_NEAR(out.stamp(), in.stamp(), 1e-6); + + // sensorDataToROS writes ground_truth_pose into the message, but sensorDataFromROS + // deliberately does not read it back: the ground truth is owned by the enclosing + // Node conversion (nodeFromROS feeds it to the Signature constructor). See + // nodeGroundTruthRoundTrip for the round trip that does preserve it. + EXPECT_FALSE(transformFromPoseMsg(msg.ground_truth_pose).isNull()) + << "the message must still carry the ground truth for nodeFromROS"; + EXPECT_TRUE(out.groundTruth().isNull()) + << "sensorDataFromROS does not restore the ground truth"; + + ASSERT_EQ(out.cameraModels().size(), 1u); + EXPECT_NEAR(out.cameraModels()[0].fx(), 525.0, 1e-9); + EXPECT_NEAR(out.cameraModels()[0].cx(), 320.0, 1e-9); + expectTransformNear( + out.cameraModels()[0].localTransform(), model.localTransform()); + + EXPECT_NEAR(out.gps().longitude(), in.gps().longitude(), 1e-9); + EXPECT_NEAR(out.gps().latitude(), in.gps().latitude(), 1e-9); + EXPECT_NEAR(out.gps().altitude(), in.gps().altitude(), 1e-9); + EXPECT_NEAR(out.gps().bearing(), in.gps().bearing(), 1e-9); +} + +TEST(MsgConversion, sensorDataUserDataRoundTrip) +{ + rtabmap::SensorData in; + in.setStamp(10.0); + in.setUserData((cv::Mat_(1, 3) << 7, 8, 9)); + + rtabmap_msgs::msg::SensorData msg; + sensorDataToROS(in, msg); + const rtabmap::SensorData out = sensorDataFromROS(msg); + + const cv::Mat data = out.userDataRaw().empty() + ? rtabmap::uncompressData(out.userDataCompressed()) + : out.userDataRaw(); + ASSERT_FALSE(data.empty()); + ASSERT_EQ(data.cols, 3); + EXPECT_EQ(data.at(0, 0), 7); + EXPECT_EQ(data.at(0, 2), 9); +} + +///////////////////////// +// Statistics / Info +///////////////////////// + +TEST(MsgConversion, infoRoundTrip) +{ + rtabmap::Statistics in; + in.setExtended(true); + in.setRefImageId(5); + in.setLoopClosureId(9); + in.setProximityDetectionId(11); + in.setStamp(1234.5); + in.setLoopClosureTransform(sampleTransform()); + in.setWmState(std::vector{1, 2, 3}); + + std::map posterior; + posterior.insert(std::make_pair(1, 0.25f)); + posterior.insert(std::make_pair(2, 0.75f)); + in.setPosterior(posterior); + + std::map weights; + weights.insert(std::make_pair(1, 10)); + in.setWeights(weights); + + std::map labels; + labels.insert(std::make_pair(1, "kitchen")); + in.setLabels(labels); + + in.addStatistic("Some/Stat/", 3.5f); + + rtabmap_msgs::msg::Info msg; + infoToROS(in, msg); + + // An unstamped header is filled from the statistics, so infoFromROS recovers the + // stamp without the caller doing anything. Only to double precision, though. + EXPECT_NEAR(timestampFromROS(msg.header.stamp), in.stamp(), 1e-6); + EXPECT_TRUE(msg.header.frame_id.empty()) << "the frame id is always the caller's job"; + + rtabmap::Statistics out; + infoFromROS(msg, out); + + EXPECT_EQ(out.refImageId(), in.refImageId()); + EXPECT_EQ(out.loopClosureId(), in.loopClosureId()); + EXPECT_EQ(out.proximityDetectionId(), in.proximityDetectionId()); + EXPECT_NEAR(out.stamp(), in.stamp(), 1e-6); + expectTransformNear(out.loopClosureTransform(), in.loopClosureTransform()); + EXPECT_EQ(out.wmState(), in.wmState()); + + ASSERT_EQ(out.posterior().size(), in.posterior().size()); + EXPECT_FLOAT_EQ(out.posterior().at(1), 0.25f); + EXPECT_FLOAT_EQ(out.posterior().at(2), 0.75f); + + ASSERT_EQ(out.weights().size(), in.weights().size()); + EXPECT_EQ(out.weights().at(1), 10); + + ASSERT_EQ(out.labels().size(), in.labels().size()); + EXPECT_EQ(out.labels().at(1), "kitchen"); + + ASSERT_TRUE(out.data().find("Some/Stat/") != out.data().end()); + EXPECT_FLOAT_EQ(out.data().at("Some/Stat/"), 3.5f); +} + +///////////////////////// +// PointCloud2 helpers +///////////////////////// + +namespace { + +/// Builds a dense, unorganized XYZ float cloud from the given points. +sensor_msgs::msg::PointCloud2 makeXYZCloud(const std::vector & points) +{ + sensor_msgs::msg::PointCloud2 cloud; + cloud.height = 1; + cloud.width = points.size(); + cloud.is_bigendian = false; + cloud.is_dense = true; + cloud.fields.resize(3); + const char * names[3] = {"x", "y", "z"}; + for(int i=0; i<3; ++i) + { + cloud.fields[i].name = names[i]; + cloud.fields[i].offset = 4 * i; + cloud.fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32; + cloud.fields[i].count = 1; + } + cloud.point_step = 12; + cloud.row_step = cloud.point_step * cloud.width; + cloud.data.resize(cloud.row_step * cloud.height); + for(size_t i=0; i(&cloud.data[i * cloud.point_step]); + p[0] = points[i].x; + p[1] = points[i].y; + p[2] = points[i].z; + } + return cloud; +} + +cv::Point3f readXYZ(const sensor_msgs::msg::PointCloud2 & cloud, size_t index) +{ + const float * p = reinterpret_cast(&cloud.data[index * cloud.point_step]); + return cv::Point3f(p[0], p[1], p[2]); +} + +} // namespace + +TEST(MsgConversion, transformPointCloudTranslation) +{ + const std::vector points = {{1.0f, 2.0f, 3.0f}, {-1.0f, 0.0f, 1.0f}}; + const sensor_msgs::msg::PointCloud2 in = makeXYZCloud(points); + + Eigen::Matrix4f t = Eigen::Matrix4f::Identity(); + t(0, 3) = 10.0f; + t(1, 3) = 20.0f; + t(2, 3) = 30.0f; + + sensor_msgs::msg::PointCloud2 out; + transformPointCloud(t, in, out); + + ASSERT_EQ(out.width, in.width); + ASSERT_EQ(out.point_step, in.point_step); + for(size_t i=0; i points = {{1.0f, 0.0f, 0.0f}}; + const sensor_msgs::msg::PointCloud2 in = makeXYZCloud(points); + + const Eigen::Matrix4f t = + rtabmap::Transform(0, 0, 0, 0, 0, M_PI/2.0).toEigen4f(); + + sensor_msgs::msg::PointCloud2 out; + transformPointCloud(t, in, out); + + const cv::Point3f p = readXYZ(out, 0); + EXPECT_NEAR(p.x, 0.0f, 1e-5); + EXPECT_NEAR(p.y, 1.0f, 1e-5); + EXPECT_NEAR(p.z, 0.0f, 1e-5); +} + +TEST(MsgConversion, transformPointCloudIdentityPreservesMetadata) +{ + const sensor_msgs::msg::PointCloud2 in = makeXYZCloud({{1.0f, 2.0f, 3.0f}}); + + sensor_msgs::msg::PointCloud2 out; + transformPointCloud(Eigen::Matrix4f::Identity(), in, out); + + EXPECT_EQ(out.height, in.height); + EXPECT_EQ(out.width, in.width); + EXPECT_EQ(out.point_step, in.point_step); + EXPECT_EQ(out.row_step, in.row_step); + EXPECT_EQ(out.is_dense, in.is_dense); + ASSERT_EQ(out.fields.size(), in.fields.size()); + for(size_t i=0; i last +constexpr float kWallDistance = 5.0f; // m, distance to the wall at the first point +constexpr float kSpeed = 1.0f; // m/s forward (+x) + +/// How the per-point time channel is encoded. deskew() accepts three datatypes, and +/// FLOAT64 differs from the other two: it carries ABSOLUTE stamps (with an automatic +/// ms/us/ns unit guess), while UINT32 and FLOAT32 carry offsets from the header stamp. +enum TimeEncoding +{ + kOffsetSecFloat32, ///< FLOAT32 seconds, relative to header.stamp + kOffsetNsecUint32, ///< UINT32 nanoseconds, relative to header.stamp + kAbsoluteSecFloat64, ///< FLOAT64 absolute seconds + kAbsoluteMsecFloat64 ///< FLOAT64 absolute milliseconds (auto-scaled by deskew) +}; + +/// Organized-cloud layout. deskew() picks its traversal from width>height, so the two +/// orderings exercise different loops. +enum ScanLayout +{ + kTimeOnColumns, ///< Ouster style: width=time samples, height=rings + kTimeOnRows ///< Velodyne style: height=time samples, width=rings +}; + +/** + * Builds the raw (skewed) scan of a flat wall captured while moving forward. + * + * Each time sample is taken 1 ms after the previous one, by which time the robot has + * closed in on the wall by kSpeed * elapsed. Expressed in the sensor frame at capture + * time, the wall therefore appears to slide towards the robot: a straight wall is + * recorded as a slanted line. Deskewing must undo exactly that. + * + * @param headerStamp absolute stamp put in the message header + * @param firstPointOffset time of the first sample relative to the header stamp + * @param encoding how to write the time channel + * @param layout whether time runs along columns or rows + * @param rings number of rings (the non-time dimension) + * @param fieldName name of the time channel + * @param descendingTime emit the samples newest-first, which deskew has to detect + * @param displacement distance travelled as a function of time since the first + * sample; defaults to the constant-velocity kSpeed * elapsed + */ +sensor_msgs::msg::PointCloud2 makeSkewedWallScan( + double headerStamp, + double firstPointOffset, + TimeEncoding encoding = kOffsetSecFloat32, + ScanLayout layout = kTimeOnColumns, + size_t rings = 1, + const std::string & fieldName = "t", + bool descendingTime = false, + const std::function & displacement = nullptr) +{ + const bool timeIs64Bit = + encoding == kAbsoluteSecFloat64 || encoding == kAbsoluteMsecFloat64; + // Keep the 8-byte time channel aligned: x,y,z then 4 bytes of padding. + const uint32_t timeOffset = timeIs64Bit ? 16 : 12; + const uint32_t pointStep = timeIs64Bit ? 24 : 16; + + sensor_msgs::msg::PointCloud2 cloud; + cloud.header.stamp = timestampToROS(headerStamp); + cloud.header.frame_id = "base_link"; + cloud.is_bigendian = false; + cloud.is_dense = true; + if(layout == kTimeOnColumns) + { + cloud.width = kScanPoints; + cloud.height = rings; + } + else + { + cloud.width = rings; + cloud.height = kScanPoints; + } + + cloud.fields.resize(4); + const char * xyz[3] = {"x", "y", "z"}; + for(int i=0; i<3; ++i) + { + cloud.fields[i].name = xyz[i]; + cloud.fields[i].offset = 4 * i; + cloud.fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32; + cloud.fields[i].count = 1; + } + cloud.fields[3].name = fieldName; + cloud.fields[3].offset = timeOffset; + cloud.fields[3].datatype = + encoding == kOffsetNsecUint32 ? sensor_msgs::msg::PointField::UINT32 : + timeIs64Bit ? sensor_msgs::msg::PointField::FLOAT64 : + sensor_msgs::msg::PointField::FLOAT32; + cloud.fields[3].count = 1; + + cloud.point_step = pointStep; + cloud.row_step = cloud.point_step * cloud.width; + cloud.data.resize(cloud.row_step * cloud.height); + + for(size_t i=0; i(base); + // The robot has closed in on the wall by this much when the sample was taken. + const double travelled = displacement ? displacement(elapsed) : kSpeed * elapsed; + p[0] = kWallDistance - float(travelled); // the skew + p[1] = -1.0f + 2.0f * float(sample) / float(kScanPoints - 1); + p[2] = 0.1f * float(r); // one plane per ring + + switch(encoding) + { + case kOffsetSecFloat32: + *reinterpret_cast(base + timeOffset) = float(offset); + break; + case kOffsetNsecUint32: + *reinterpret_cast(base + timeOffset) = + uint32_t(std::llround(offset * 1e9)); + break; + case kAbsoluteSecFloat64: + *reinterpret_cast(base + timeOffset) = absolute; + break; + case kAbsoluteMsecFloat64: + *reinterpret_cast(base + timeOffset) = absolute * 1e3; + break; + } + } + } + return cloud; +} + +/// Reads x of the point at (time sample, ring) for the given layout. +float readWallX(const sensor_msgs::msg::PointCloud2 & cloud, size_t sample, size_t ring, + ScanLayout layout) +{ + const size_t row = (layout == kTimeOnColumns) ? ring : sample; + const size_t col = (layout == kTimeOnColumns) ? sample : ring; + return *reinterpret_cast( + &cloud.data[row * cloud.row_step + col * cloud.point_step]); +} + +float readField(const sensor_msgs::msg::PointCloud2 & cloud, size_t index, size_t field) +{ + return *reinterpret_cast( + &cloud.data[index * cloud.point_step + cloud.fields[field].offset]); +} + +} // namespace + +TEST(MsgConversion, deskewConstantVelocityHeaderAtFirstPoint) +{ + const double firstPointStamp = 1000.0; + + // Header stamped at the first point, so "t" runs 0 .. +0.100 s. + const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(firstPointStamp, 0.0); + ASSERT_NEAR(readField(in, 0, 0), kWallDistance, 1e-4) << "first point is unskewed"; + ASSERT_NEAR(readField(in, kScanPoints-1, 0), kWallDistance - float(kScanSpan), 1e-4) + << "last point is skewed by v*0.099s = 9.9 cm"; + + sensor_msgs::msg::PointCloud2 out; + ASSERT_TRUE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0))); + + // Everything collapses back onto the wall at its original distance. + for(size_t i=0; i height=4 rings. + expectDeskewRecoversWall(kOffsetSecFloat32, kTimeOnColumns, 4, 1000.0); +} + +TEST(MsgConversion, deskewTimeOnRowsWithMultipleRings) +{ + // Velodyne layout: height=101 samples > width=4 rings, which takes the other loop. + expectDeskewRecoversWall(kOffsetSecFloat32, kTimeOnRows, 4, 1000.0); +} + +TEST(MsgConversion, deskewLayoutsAgree) +{ + // The same scan expressed in either layout must deskew to the same geometry. + const double headerStamp = 1000.0; + const size_t rings = 4; + + sensor_msgs::msg::PointCloud2 byColumns, byRows; + ASSERT_TRUE(deskew(makeSkewedWallScan(headerStamp, 0.0, kOffsetSecFloat32, kTimeOnColumns, rings), + byColumns, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0))); + ASSERT_TRUE(deskew(makeSkewedWallScan(headerStamp, 0.0, kOffsetSecFloat32, kTimeOnRows, rings), + byRows, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0))); + + for(size_t i=0; i(&c.data[i * c.point_step]); + return std::make_pair(p[0], p[1]); + }; + + for(size_t i=0; i a = xy(in, i); + const std::pair b = xy(out, i); + const double dt = double(i) * kScanStep; // sample 0 sits at the header stamp + + EXPECT_NEAR(std::hypot(b.first, b.second), std::hypot(a.first, a.second), 1e-4) + << "a rotation must preserve the range of sample " << i; + EXPECT_NEAR(std::atan2(b.second, b.first) - std::atan2(a.second, a.first), + yawRate * dt, 1e-4) + << "sample " << i << " must be rotated by yawRate*dt"; + } + + // Spelling out the i=0 case: dt is zero there, so that sample is untouched. + EXPECT_FLOAT_EQ(xy(out, 0).first, xy(in, 0).first); + EXPECT_FLOAT_EQ(xy(out, 0).second, xy(in, 0).second); +} + +TEST(MsgConversion, deskewPassesThroughWhenThereIsNoTimeSpread) +{ + // A driver that leaves the time channel at zero gives a scan with no time spread. + // There is nothing to correct, so the cloud must come back unchanged rather than + // being reported as a failure -- callers abort the frame on false. + sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(1000.0, 0.0); + for(size_t i=0; i(&in.data[i * in.point_step + in.fields[3].offset]) = 0.0f; + } + + sensor_msgs::msg::PointCloud2 out; + ASSERT_TRUE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0))); + EXPECT_EQ(out.data, in.data) << "the cloud must be returned untouched"; +} + +TEST(MsgConversion, deskewIsIdempotent) +{ + // Deskewing zeroes the time channel to mark the cloud as done, so running deskew a + // second time (e.g. lidar_deskewing feeding icp_odometry) must be a silent no-op. + const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(1000.0, 0.0); + const rtabmap::Transform velocity(kSpeed, 0, 0, 0, 0, 0); + + sensor_msgs::msg::PointCloud2 once; + ASSERT_TRUE(deskew(in, once, velocity)); + for(size_t i=0; i( + &once.data[i * once.point_step + once.fields[3].offset]), 0.0f) + << "deskewing must zero the time channel, sample " << i; + } + + sensor_msgs::msg::PointCloud2 twice; + ASSERT_TRUE(deskew(once, twice, velocity)) << "a second pass must not fail"; + EXPECT_EQ(twice.data, once.data) << "a second pass must change nothing"; +} + +TEST(MsgConversion, deskewClampsSamplesOutsideTheSweep) +{ + // The ordering check only inspects the first and last samples, so a corrupt stamp in + // the middle is not detected. It must be clamped to the end of the sweep rather than + // extrapolated, which would fling the point far past the wall. + const double headerStamp = 1000.0; + sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(headerStamp, 0.0); + const size_t corrupt = kScanPoints / 2; + *reinterpret_cast( + &in.data[corrupt * in.point_step + in.fields[3].offset]) = 0.5f; // 5x the sweep + + sensor_msgs::msg::PointCloud2 out; + ASSERT_TRUE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0))); + + // Clamped to the last sample's correction, so it lands within the sweep's own range + // rather than metres away. Every other sample is unaffected. + const float x = readWallX(out, corrupt, 0, kTimeOnColumns); + EXPECT_GE(x, kWallDistance - 1e-3f); + EXPECT_LE(x, kWallDistance + float(kSpeed * kScanSpan) + 1e-3f) + << "an unclamped ratio of ~5 would put this point ~0.45 m past the wall"; + + for(size_t i=0; i(3, 3) << + 525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0); + const rtabmap::CameraModel model( + "cam", cv::Size(4, 4), K, cv::Mat(), cv::Mat(), cv::Mat(), + rtabmap::Transform(0.0f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f)); + + cv::Mat rgb(4, 4, CV_8UC3, cv::Scalar(10, 20, 30)); + cv::Mat depth(4, 4, CV_16UC1, cv::Scalar(1000)); + rtabmap::SensorData in(rgb, depth, model, 1, 1234.5); + + rtabmap_msgs::msg::RGBDImage msg; + rgbdImageToROS(in, msg, "camera_link"); + + EXPECT_EQ(msg.rgb_camera_info.header.frame_id, "camera_link"); + EXPECT_NEAR(timestampFromROS(msg.rgb_camera_info.header.stamp), 1234.5, 1e-6); + + // The top-level header is stamped too, so rgbdImageFromROS recovers the stamp + // without the caller having to fill it in. + EXPECT_EQ(msg.header.frame_id, "camera_link"); + EXPECT_NEAR(timestampFromROS(msg.header.stamp), 1234.5, 1e-6); + + // The returned SensorData shallow-references the message buffers, so the message + // must outlive it -- see rgbdImageFromROSAliasesTheMessage. + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr held = + std::make_shared(msg); + const rtabmap::SensorData out = rgbdImageFromROS(held); + + EXPECT_NEAR(out.stamp(), in.stamp(), 1e-6); + ASSERT_EQ(out.cameraModels().size(), 1u); + EXPECT_NEAR(out.cameraModels()[0].fx(), 525.0, 1e-9); + + // The local transform is not carried by the message (CameraInfo has no such + // field); callers resolve it from TF, so it comes back as the default identity. + EXPECT_TRUE(out.cameraModels()[0].localTransform().isIdentity()) + << out.cameraModels()[0].localTransform().prettyPrint(); + + ASSERT_FALSE(out.imageRaw().empty()); + EXPECT_EQ(out.imageRaw().type(), CV_8UC3); + EXPECT_EQ(cv::countNonZero(out.imageRaw().reshape(1) != rgb.reshape(1)), 0); + + ASSERT_FALSE(out.depthRaw().empty()); + EXPECT_EQ(out.depthRaw().type(), CV_16UC1); + EXPECT_EQ(cv::countNonZero(out.depthRaw() != depth), 0); +} + +TEST(MsgConversion, rgbdImageFromROSAliasesTheMessage) +{ + // rgbdImageFromROS deliberately avoids copying the pixels: the SensorData it returns + // points into the message's own buffers. Mutating the message is visible through the + // SensorData. Callers must therefore keep the message alive and unchanged for as long + // as they use the result -- and must deep-copy before letting the SensorData outlive + // the subscription callback, since the ROS queue recycles the message once it + // returns. + cv::Mat rgb(4, 4, CV_8UC3, cv::Scalar(10, 20, 30)); + cv::Mat depth(4, 4, CV_16UC1, cv::Scalar(1000)); + + auto msg = std::make_shared(); + msg->rgb_camera_info.width = 4; + msg->rgb_camera_info.height = 4; + msg->rgb_camera_info.k = {525.0, 0.0, 2.0, 0.0, 525.0, 2.0, 0.0, 0.0, 1.0}; + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb).toImageMsg(msg->rgb); + cv_bridge::CvImage(std_msgs::msg::Header(), "16UC1", depth).toImageMsg(msg->depth); + + const rtabmap::SensorData data = rgbdImageFromROS(msg); + ASSERT_FALSE(data.imageRaw().empty()); + ASSERT_EQ(data.imageRaw().at(0, 0), cv::Vec3b(10, 20, 30)); + + // Writing through the message is observable in the SensorData: no copy was made. + msg->rgb.data[0] = 99; + EXPECT_EQ(data.imageRaw().at(0, 0)[0], 99) + << "SensorData is expected to alias the message buffer"; +} + +TEST(MsgConversion, toCvCopyReadsRawImages) +{ + cv::Mat rgb(4, 4, CV_8UC3, cv::Scalar(10, 20, 30)); + cv::Mat depth(4, 4, CV_16UC1, cv::Scalar(1000)); + + rtabmap_msgs::msg::RGBDImage msg; + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb).toImageMsg(msg.rgb); + cv_bridge::CvImage(std_msgs::msg::Header(), "16UC1", depth).toImageMsg(msg.depth); + + cv_bridge::CvImagePtr rgbPtr, depthPtr; + toCvCopy(msg, rgbPtr, depthPtr); + + ASSERT_TRUE(rgbPtr && depthPtr); + EXPECT_EQ(cv::countNonZero(rgbPtr->image.reshape(1) != rgb.reshape(1)), 0); + EXPECT_EQ(cv::countNonZero(depthPtr->image != depth), 0); + + // The copy must be independent of the message buffer. + rgbPtr->image.at(0, 0) = cv::Vec3b(0, 0, 0); + EXPECT_EQ(rgb.at(0, 0), cv::Vec3b(10, 20, 30)); +} + +TEST(MsgConversion, toCvCopyEmptyImageYieldsEmptyPtr) +{ + rtabmap_msgs::msg::RGBDImage msg; + + cv_bridge::CvImagePtr rgbPtr, depthPtr; + toCvCopy(msg, rgbPtr, depthPtr); + + ASSERT_TRUE(rgbPtr && depthPtr) << "pointers must be valid even with no image"; + EXPECT_TRUE(rgbPtr->image.empty()); + EXPECT_TRUE(depthPtr->image.empty()); +} + +TEST(MsgConversion, toCvShareAliasesRawImages) +{ + cv::Mat rgb(4, 4, CV_8UC3, cv::Scalar(10, 20, 30)); + cv::Mat depth(4, 4, CV_16UC1, cv::Scalar(1000)); + + rtabmap_msgs::msg::RGBDImage msg; + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb).toImageMsg(msg.rgb); + cv_bridge::CvImage(std_msgs::msg::Header(), "16UC1", depth).toImageMsg(msg.depth); + + cv_bridge::CvImageConstPtr rgbPtr, depthPtr; + toCvShare(msg, std::shared_ptr(), rgbPtr, depthPtr); + + ASSERT_TRUE(rgbPtr && depthPtr); + ASSERT_FALSE(rgbPtr->image.empty()); + EXPECT_EQ(cv::countNonZero(rgbPtr->image.reshape(1) != rgb.reshape(1)), 0); + EXPECT_EQ(cv::countNonZero(depthPtr->image != depth), 0); +} + +///////////////////////// +// Compressed images +///////////////////////// + +namespace { + +/// Builds a depth image compressed the way rtabmap does it (not a jpg/png CompressedImage). +sensor_msgs::msg::CompressedImage makeRtabmapCompressedDepth(const cv::Mat & depth) +{ + sensor_msgs::msg::CompressedImage msg; + msg.format = ""; // anything but "jpg" takes the rtabmap::uncompressImage path + msg.data = rtabmap::compressImage(depth, ".png"); + return msg; +} + +} // namespace + +TEST(MsgConversion, toCvCopyReadsCompressedDepth) +{ + const cv::Mat depth(4, 4, CV_16UC1, cv::Scalar(1234)); + + rtabmap_msgs::msg::RGBDImage msg; + msg.depth_compressed = makeRtabmapCompressedDepth(depth); + + cv_bridge::CvImagePtr rgbPtr, depthPtr; + toCvCopy(msg, rgbPtr, depthPtr); + + ASSERT_TRUE(depthPtr); + ASSERT_FALSE(depthPtr->image.empty()); + EXPECT_EQ(depthPtr->image.type(), CV_16UC1); + EXPECT_EQ(depthPtr->encoding, sensor_msgs::image_encodings::TYPE_16UC1); + EXPECT_EQ(cv::countNonZero(depthPtr->image != depth), 0); +} + +TEST(MsgConversion, toCvShareReadsCompressedDepth) +{ + const cv::Mat depth(4, 4, CV_32FC1, cv::Scalar(1.5f)); + + rtabmap_msgs::msg::RGBDImage msg; + msg.depth_compressed = makeRtabmapCompressedDepth(depth); + + cv_bridge::CvImageConstPtr rgbPtr, depthPtr; + toCvShare(msg, std::shared_ptr(), rgbPtr, depthPtr); + + ASSERT_TRUE(depthPtr); + ASSERT_FALSE(depthPtr->image.empty()); + EXPECT_EQ(depthPtr->image.type(), CV_32FC1); + EXPECT_EQ(depthPtr->encoding, sensor_msgs::image_encodings::TYPE_32FC1); + EXPECT_EQ(cv::countNonZero(depthPtr->image != depth), 0); +} + +TEST(MsgConversion, toCvCopyReadsCompressedRgb) +{ + const cv::Mat rgb(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + + rtabmap_msgs::msg::RGBDImage msg; + msg.rgb_compressed.format = "png"; + msg.rgb_compressed.data = rtabmap::compressImage(rgb, ".png"); + + cv_bridge::CvImagePtr rgbPtr, depthPtr; + toCvCopy(msg, rgbPtr, depthPtr); + + ASSERT_TRUE(rgbPtr); + ASSERT_FALSE(rgbPtr->image.empty()); + EXPECT_EQ(rgbPtr->image.type(), CV_8UC3); + EXPECT_EQ(cv::countNonZero(rgbPtr->image.reshape(1) != rgb.reshape(1)), 0); +} + +///////////////////////// +// SensorData: raw copies, laser scans, stereo +///////////////////////// + +TEST(MsgConversion, sensorDataToROSCopyRawDataCarriesImages) +{ + cv::Mat K = (cv::Mat_(3, 3) << + 525.0, 0.0, 4.0, 0.0, 525.0, 4.0, 0.0, 0.0, 1.0); + const rtabmap::CameraModel model("cam", cv::Size(8, 8), K, cv::Mat(), cv::Mat(), cv::Mat()); + + const cv::Mat rgb(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + const cv::Mat depth(8, 8, CV_16UC1, cv::Scalar(2000)); + rtabmap::SensorData in(rgb, depth, model, 1, 1000.0); + + // Without copyRawData the raw images are not serialized... + rtabmap_msgs::msg::SensorData without; + sensorDataToROS(in, without, "base_link", /*copyRawData=*/false); + EXPECT_TRUE(without.left.data.empty()); + EXPECT_TRUE(without.right.data.empty()); + + // ...with it, they are. + rtabmap_msgs::msg::SensorData with; + sensorDataToROS(in, with, "base_link", /*copyRawData=*/true); + ASSERT_FALSE(with.left.data.empty()); + ASSERT_FALSE(with.right.data.empty()); + EXPECT_EQ(with.left.encoding, sensor_msgs::image_encodings::BGR8); + EXPECT_EQ(with.right.encoding, sensor_msgs::image_encodings::TYPE_16UC1); + + const rtabmap::SensorData out = sensorDataFromROS(with); + ASSERT_FALSE(out.imageRaw().empty()); + EXPECT_EQ(cv::countNonZero(out.imageRaw().reshape(1) != rgb.reshape(1)), 0); + ASSERT_FALSE(out.depthRaw().empty()); + EXPECT_EQ(cv::countNonZero(out.depthRaw() != depth), 0); +} + +TEST(MsgConversion, sensorDataLaserScanRoundTrip) +{ + cv::Mat points(1, 3, CV_32FC3); + points.at(0, 0) = cv::Vec3f(1.0f, 0.0f, 0.0f); + points.at(0, 1) = cv::Vec3f(0.0f, 2.0f, 0.0f); + points.at(0, 2) = cv::Vec3f(0.0f, 0.0f, 3.0f); + + const rtabmap::Transform localTransform(0.0f, 0.0f, 0.3f, 0.0f, 0.0f, 0.0f); + const rtabmap::LaserScan scan(points, /*maxPoints=*/100, /*maxRange=*/40.0f, + rtabmap::LaserScan::kXYZ, localTransform); + + rtabmap::SensorData in; + in.setStamp(1000.0); + in.setLaserScan(scan); + + rtabmap_msgs::msg::SensorData msg; + sensorDataToROS(in, msg, "base_link", /*copyRawData=*/true); + + EXPECT_EQ(msg.laser_scan_max_pts, 100); + EXPECT_FLOAT_EQ(msg.laser_scan_max_range, 40.0f); + EXPECT_EQ(msg.laser_scan_format, (int)rtabmap::LaserScan::kXYZ); + expectTransformNear(transformFromGeometryMsg(msg.laser_scan_local_transform), + localTransform, 1e-4f); + + const rtabmap::SensorData out = sensorDataFromROS(msg); + const rtabmap::LaserScan & outScan = out.laserScanRaw().empty() + ? out.laserScanCompressed() : out.laserScanRaw(); + EXPECT_EQ(outScan.size(), scan.size()); + EXPECT_EQ(outScan.maxPoints(), scan.maxPoints()); + EXPECT_FLOAT_EQ(outScan.rangeMax(), scan.rangeMax()); + expectTransformNear(outScan.localTransform(), localTransform, 1e-4f); +} + +TEST(MsgConversion, sensorDataStereoModelRoundTrip) +{ + const double fx = 525.0; + const double baseline = 0.12; + const rtabmap::Transform localTransform(0.0f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f); + + const rtabmap::StereoCameraModel stereo( + fx, fx, 320.0, 240.0, baseline, localTransform, cv::Size(640, 480)); + ASSERT_TRUE(stereo.isValidForProjection()) << "precondition"; + + rtabmap::SensorData in; + in.setStamp(1000.0); + in.setStereoImage(cv::Mat(), cv::Mat(), stereo); + + rtabmap_msgs::msg::SensorData msg; + sensorDataToROS(in, msg, "base_link"); + + // The stereo branch fills BOTH camera infos, unlike the monocular one. + ASSERT_EQ(msg.left_camera_info.size(), 1u); + ASSERT_EQ(msg.right_camera_info.size(), 1u); + + const rtabmap::SensorData out = sensorDataFromROS(msg); + ASSERT_EQ(out.stereoCameraModels().size(), 1u); + EXPECT_TRUE(out.cameraModels().empty()) << "must not be read back as monocular"; + EXPECT_NEAR(out.stereoCameraModels()[0].left().fx(), fx, 1e-9); + EXPECT_NEAR(out.stereoCameraModels()[0].baseline(), baseline, 1e-6); + expectTransformNear(out.stereoCameraModels()[0].localTransform(), localTransform, 1e-4f); +} + +TEST(MsgConversion, nodeWithStereoModelRoundTrip) +{ + const rtabmap::StereoCameraModel stereo( + 525.0, 525.0, 320.0, 240.0, 0.12, + rtabmap::Transform::getIdentity(), cv::Size(640, 480)); + + rtabmap::Signature in(3, 0, 1, 1000.0, "stereo_node", sampleTransform()); + in.sensorData().setStereoImage(cv::Mat(), cv::Mat(), stereo); + + rtabmap_msgs::msg::Node msg; + nodeToROS(in, msg); + const rtabmap::Signature out = nodeFromROS(msg); + + EXPECT_EQ(out.id(), in.id()); + ASSERT_EQ(out.sensorData().stereoCameraModels().size(), 1u); + EXPECT_NEAR(out.sensorData().stereoCameraModels()[0].baseline(), 0.12, 1e-6); +} + +TEST(MsgConversion, infoToROSKeepsACallerSuppliedStamp) +{ + // CoreWrapper stamps the message before calling infoToROS, sometimes with a + // publication time unrelated to the data. That must not be overwritten. + rtabmap::Statistics in; + in.setExtended(true); + in.setStamp(1234.5); + + rtabmap_msgs::msg::Info msg; + msg.header.stamp = timestampToROS(9999.0); + msg.header.frame_id = "map"; + infoToROS(in, msg); + + EXPECT_NEAR(timestampFromROS(msg.header.stamp), 9999.0, 1e-6) + << "a caller-supplied stamp must win over the statistics stamp"; + EXPECT_EQ(msg.header.frame_id, "map"); +} + +TEST(MsgConversion, infoOdomCacheRoundTrip) +{ + // Statistics carries a whole MapGraph for the odometry cache in localization mode. + std::map poses; + poses.insert(std::make_pair(1, sampleTransform())); + poses.insert(std::make_pair(2, rtabmap::Transform(1, 2, 3, 0, 0, 0))); + + std::multimap links; + links.insert(std::make_pair(1, rtabmap::Link( + 1, 2, rtabmap::Link::kNeighbor, sampleTransform()))); + + rtabmap::Statistics in; + in.setExtended(true); + in.setOdomCachePoses(poses); + in.setOdomCacheConstraints(links); + + rtabmap_msgs::msg::Info msg; + infoToROS(in, msg); + ASSERT_EQ(msg.odom_cache.poses.size(), poses.size()); + ASSERT_EQ(msg.odom_cache.links.size(), links.size()); + + rtabmap::Statistics out; + infoFromROS(msg, out); + + ASSERT_EQ(out.odomCachePoses().size(), poses.size()); + expectTransformNear(out.odomCachePoses().at(1), poses.at(1)); + expectTransformNear(out.odomCachePoses().at(2), poses.at(2)); + EXPECT_EQ(out.odomCacheConstraints().size(), links.size()); +} + +///////////////////////// +// TF-based conversions +///////////////////////// + +namespace { + +/// A tf2 buffer needs a clock, but neither a node nor a listener: transforms can be +/// injected directly, which makes every TF-based conversion an ordinary unit test. +std::shared_ptr makeTfBuffer() +{ + std::shared_ptr buffer = + std::make_shared(std::make_shared(RCL_ROS_TIME)); + // Transforms are injected synchronously before the lookups, so tell tf2 not to warn + // about waiting for a listener thread that will never exist. + buffer->setUsingDedicatedThread(true); + return buffer; +} + +void addTf(tf2_ros::Buffer & buffer, + const std::string & parent, const std::string & child, + const rtabmap::Transform & t, double stamp, bool isStatic = true) +{ + geometry_msgs::msg::TransformStamped msg; + msg.header.stamp = timestampToROS(stamp); + msg.header.frame_id = parent; + msg.child_frame_id = child; + transformToGeometryMsg(t, msg.transform); + ASSERT_TRUE(buffer.setTransform(msg, "unit_test", isStatic)); +} + +} // namespace + +TEST(MsgConversion, getTransformReadsTheBuffer) +{ + const std::shared_ptr buffer = makeTfBuffer(); + const rtabmap::Transform baseToCamera(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f); + addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0); + + const rtabmap::Transform out = + getTransform("base_link", "camera_link", timestampToROS(1000.0), *buffer, 0.0); + + expectTransformNear(out, baseToCamera); +} + +TEST(MsgConversion, getTransformReturnsNullWhenUnknown) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "camera_link", rtabmap::Transform::getIdentity(), 1000.0); + + // An unrelated frame must not throw; it must come back as a null transform. + EXPECT_TRUE(getTransform("base_link", "lidar_link", timestampToROS(1000.0), *buffer, 0.0) + .isNull()); +} + +TEST(MsgConversion, getTransformIsInvertedByFrameOrder) +{ + const std::shared_ptr buffer = makeTfBuffer(); + const rtabmap::Transform baseToCamera(0.1f, 0.2f, 0.3f, 0.0f, 0.0f, 0.5f); + addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0); + + const rtabmap::Transform forward = + getTransform("base_link", "camera_link", timestampToROS(1000.0), *buffer, 0.0); + const rtabmap::Transform backward = + getTransform("camera_link", "base_link", timestampToROS(1000.0), *buffer, 0.0); + + expectTransformNear(backward, forward.inverse(), 1e-4f); +} + +TEST(MsgConversion, getMovingTransformMeasuresMotionBetweenStamps) +{ + // base_link drives 1 m along x of odom between t=1000 and t=1001. + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "odom", "base_link", rtabmap::Transform(0, 0, 0, 0, 0, 0), 1000.0, false); + addTf(*buffer, "odom", "base_link", rtabmap::Transform(1, 0, 0, 0, 0, 0), 1001.0, false); + + // Motion of base_link from t=1000 to t=1001, seen in the fixed odom frame. + const rtabmap::Transform motion = getMovingTransform( + "base_link", "odom", timestampToROS(1000.0), timestampToROS(1001.0), *buffer, 0.0); + + ASSERT_FALSE(motion.isNull()); + EXPECT_NEAR(motion.x(), 1.0, 1e-4); + EXPECT_NEAR(motion.y(), 0.0, 1e-4); + EXPECT_NEAR(motion.z(), 0.0, 1e-4); +} + +TEST(MsgConversion, getMovingTransformInterpolatesBetweenStamps) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "odom", "base_link", rtabmap::Transform(0, 0, 0, 0, 0, 0), 1000.0, false); + addTf(*buffer, "odom", "base_link", rtabmap::Transform(1, 0, 0, 0, 0, 0), 1001.0, false); + + // Halfway through, so half the motion. + const rtabmap::Transform half = getMovingTransform( + "base_link", "odom", timestampToROS(1000.0), timestampToROS(1000.5), *buffer, 0.0); + + ASSERT_FALSE(half.isNull()); + EXPECT_NEAR(half.x(), 0.5, 1e-4); +} + +TEST(MsgConversion, getMovingTransformIsNullWithoutAFixedFrame) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "odom", "base_link", rtabmap::Transform::getIdentity(), 1000.0, false); + + EXPECT_TRUE(getMovingTransform("base_link", "map", + timestampToROS(1000.0), timestampToROS(1001.0), *buffer, 0.0).isNull()); +} + +TEST(MsgConversion, convertScanMsgProducesALaserScan) +{ + 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); + + sensor_msgs::msg::LaserScan msg; + msg.header.stamp = timestampToROS(1000.0); + msg.header.frame_id = "laser"; + msg.angle_min = -1.0f; + msg.angle_max = 1.0f; + msg.angle_increment = 0.1f; + msg.time_increment = 0.0f; + msg.range_min = 0.1f; + msg.range_max = 30.0f; + msg.ranges.assign(21, 5.0f); + + rtabmap::LaserScan scan; + ASSERT_TRUE(convertScanMsg(msg, "base_link", "", timestampToROS(1000.0), + scan, *buffer, 0.0)); + + EXPECT_FALSE(scan.empty()); + EXPECT_EQ(scan.size(), (int)msg.ranges.size()); + expectTransformNear(scan.localTransform(), baseToLaser, 1e-4f); +} + +TEST(MsgConversion, convertScanMsgRejectsMalformedScans) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "laser", rtabmap::Transform::getIdentity(), 1000.0); + + sensor_msgs::msg::LaserScan base; + base.header.stamp = timestampToROS(1000.0); + base.header.frame_id = "laser"; + base.angle_min = -1.0f; + base.angle_max = 1.0f; + base.angle_increment = 0.1f; + base.range_min = 0.1f; + base.range_max = 30.0f; + base.ranges.assign(21, 5.0f); + + rtabmap::LaserScan scan; + + sensor_msgs::msg::LaserScan zeroIncrement = base; + zeroIncrement.angle_increment = 0.0f; + EXPECT_FALSE(convertScanMsg(zeroIncrement, "base_link", "", timestampToROS(1000.0), + scan, *buffer, 0.0)) << "angle_increment of 0 would divide by zero"; + + sensor_msgs::msg::LaserScan invertedRange = base; + invertedRange.range_min = 40.0f; + EXPECT_FALSE(convertScanMsg(invertedRange, "base_link", "", timestampToROS(1000.0), + scan, *buffer, 0.0)) << "range_min > range_max"; + + sensor_msgs::msg::LaserScan invertedAngle = base; + invertedAngle.angle_min = 1.0f; + invertedAngle.angle_max = -1.0f; + EXPECT_FALSE(convertScanMsg(invertedAngle, "base_link", "", timestampToROS(1000.0), + scan, *buffer, 0.0)) << "positive increment with angle_max < angle_min"; +} + +TEST(MsgConversion, convertScanMsgFailsWithoutTf) +{ + const std::shared_ptr buffer = makeTfBuffer(); // empty + + sensor_msgs::msg::LaserScan msg; + msg.header.stamp = timestampToROS(1000.0); + msg.header.frame_id = "laser"; + msg.angle_min = -1.0f; + msg.angle_max = 1.0f; + msg.angle_increment = 0.1f; + msg.range_min = 0.1f; + msg.range_max = 30.0f; + msg.ranges.assign(21, 5.0f); + + rtabmap::LaserScan scan; + EXPECT_FALSE(convertScanMsg(msg, "base_link", "", timestampToROS(1000.0), + scan, *buffer, 0.0)); +} + +TEST(MsgConversion, convertScan3dMsgKeepsLocalTransformAndLimits) +{ + const std::shared_ptr buffer = makeTfBuffer(); + const rtabmap::Transform baseToLidar(0.0f, 0.0f, 0.5f, 0.0f, 0.0f, 0.0f); + addTf(*buffer, "base_link", "lidar", baseToLidar, 1000.0); + + sensor_msgs::msg::PointCloud2 msg = + makeXYZCloud({{1.0f, 0.0f, 0.0f}, {2.0f, 0.0f, 0.0f}, {3.0f, 0.0f, 0.0f}}); + msg.header.stamp = timestampToROS(1000.0); + msg.header.frame_id = "lidar"; + + rtabmap::LaserScan scan; + ASSERT_TRUE(convertScan3dMsg(msg, "base_link", "", timestampToROS(1000.0), + scan, *buffer, 0.0)); + + EXPECT_EQ(scan.size(), 3); + expectTransformNear(scan.localTransform(), baseToLidar, 1e-4f); + EXPECT_EQ(scan.rangeMax(), 0.0f) << "no max range requested"; + + rtabmap::LaserScan limited; + ASSERT_TRUE(convertScan3dMsg(msg, "base_link", "", timestampToROS(1000.0), + limited, *buffer, 0.0, /*maxPoints=*/10, /*maxRange=*/2.5f)); + EXPECT_EQ(limited.maxPoints(), 10); + EXPECT_FLOAT_EQ(limited.rangeMax(), 2.5f); +} + +TEST(MsgConversion, convertScan3dMsgFailsWithoutTf) +{ + const std::shared_ptr buffer = makeTfBuffer(); // empty + + sensor_msgs::msg::PointCloud2 msg = makeXYZCloud({{1.0f, 0.0f, 0.0f}}); + msg.header.stamp = timestampToROS(1000.0); + msg.header.frame_id = "lidar"; + + rtabmap::LaserScan scan; + EXPECT_FALSE(convertScan3dMsg(msg, "base_link", "", timestampToROS(1000.0), + scan, *buffer, 0.0)); +} + +TEST(MsgConversion, landmarksFromROSAppliesTfAndDefaultVariance) +{ + const std::shared_ptr buffer = makeTfBuffer(); + const rtabmap::Transform baseToCamera(0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); + addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0); + + geometry_msgs::msg::PoseWithCovarianceStamped tag; + tag.header.stamp = timestampToROS(1000.0); + tag.header.frame_id = "camera_link"; + tag.pose.pose.position.x = 2.0; // 2 m in front of the camera + tag.pose.pose.orientation.w = 1.0; + // covariance left at zero -> the defaults must be substituted + + std::map > tags; + tags.insert(std::make_pair(7, std::make_pair(tag, 0.15f))); + + const rtabmap::Landmarks landmarks = landmarksFromROS( + tags, "base_link", "", timestampToROS(1000.0), *buffer, 0.0, + /*defaultLinVariance=*/0.01, /*defaultAngVariance=*/0.02); + + ASSERT_EQ(landmarks.size(), 1u); + ASSERT_TRUE(landmarks.find(7) != landmarks.end()); + + // The tag pose must come back in base_link: 0.5 (base->camera) + 2.0 (camera->tag). + EXPECT_NEAR(landmarks.at(7).pose().x(), 2.5, 1e-4); + + const cv::Mat cov = landmarks.at(7).covariance(); + ASSERT_EQ(cov.rows, 6); + EXPECT_NEAR(cov.at(0,0), 0.01, 1e-9) << "linear default"; + EXPECT_NEAR(cov.at(3,3), 0.02, 1e-9) << "angular default"; +} + +TEST(MsgConversion, landmarksFromROSCorrectsForOdometryMotion) +{ + // The tag is seen 1 s after the odometry stamp, during which the robot drives 1 m. + // landmarksFromROS must fold that motion in, otherwise the landmark is placed where + // the robot would have seen it had it not moved. + const std::shared_ptr buffer = makeTfBuffer(); + const rtabmap::Transform baseToCamera(0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); + addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0); + addTf(*buffer, "odom", "base_link", rtabmap::Transform(0, 0, 0, 0, 0, 0), 1000.0, false); + addTf(*buffer, "odom", "base_link", rtabmap::Transform(1, 0, 0, 0, 0, 0), 1001.0, false); + + geometry_msgs::msg::PoseWithCovarianceStamped tag; + tag.header.stamp = timestampToROS(1001.0); // observed at t=1001 + tag.header.frame_id = "camera_link"; + tag.pose.pose.position.x = 2.0; + tag.pose.pose.orientation.w = 1.0; + + std::map > tags; + tags.insert(std::make_pair(7, std::make_pair(tag, 0.15f))); + + // odomStamp is 1000, one second BEFORE the observation. + const rtabmap::Landmarks corrected = landmarksFromROS( + tags, "base_link", "odom", timestampToROS(1000.0), *buffer, 0.0, 0.01, 0.02); + + ASSERT_EQ(corrected.size(), 1u); + // 0.5 (base->camera) + 2.0 (camera->tag) + 1.0 (odometry motion since odomStamp). + EXPECT_NEAR(corrected.at(7).pose().x(), 3.5, 1e-3); + + // Without an odom frame the correction cannot be looked up, and the landmark stays + // in the frame at the observation stamp. + const rtabmap::Landmarks uncorrected = landmarksFromROS( + tags, "base_link", "", timestampToROS(1000.0), *buffer, 0.0, 0.01, 0.02); + ASSERT_EQ(uncorrected.size(), 1u); + EXPECT_NEAR(uncorrected.at(7).pose().x(), 2.5, 1e-3); +} + +TEST(MsgConversion, landmarksFromROSKeepsProvidedCovariance) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "camera_link", rtabmap::Transform::getIdentity(), 1000.0); + + geometry_msgs::msg::PoseWithCovarianceStamped tag; + tag.header.stamp = timestampToROS(1000.0); + tag.header.frame_id = "camera_link"; + tag.pose.pose.position.x = 1.0; + tag.pose.pose.orientation.w = 1.0; + for(size_t i=0; i<6; ++i) + { + tag.pose.covariance[i*6 + i] = 0.5; // a real, finite covariance + } + + std::map > tags; + tags.insert(std::make_pair(1, std::make_pair(tag, 0.1f))); + + const rtabmap::Landmarks landmarks = landmarksFromROS( + tags, "base_link", "", timestampToROS(1000.0), *buffer, 0.0, + /*defaultLinVariance=*/0.01, /*defaultAngVariance=*/0.02); + + ASSERT_EQ(landmarks.size(), 1u); + EXPECT_NEAR(landmarks.at(1).covariance().at(0,0), 0.5, 1e-9) + << "a provided covariance must not be replaced by the default"; + EXPECT_NEAR(landmarks.at(1).covariance().at(3,3), 0.5, 1e-9); +} + +TEST(MsgConversion, landmarksFromROSRejectsNonPositiveIds) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "camera_link", rtabmap::Transform::getIdentity(), 1000.0); + + geometry_msgs::msg::PoseWithCovarianceStamped tag; + tag.header.stamp = timestampToROS(1000.0); + tag.header.frame_id = "camera_link"; + tag.pose.pose.orientation.w = 1.0; + + std::map > tags; + tags.insert(std::make_pair(0, std::make_pair(tag, 0.1f))); + tags.insert(std::make_pair(-3, std::make_pair(tag, 0.1f))); + tags.insert(std::make_pair(5, std::make_pair(tag, 0.1f))); + + const rtabmap::Landmarks landmarks = landmarksFromROS( + tags, "base_link", "", timestampToROS(1000.0), *buffer, 0.0, 0.01, 0.02); + + EXPECT_EQ(landmarks.size(), 1u) << "ids <= 0 must be dropped"; + EXPECT_TRUE(landmarks.find(5) != landmarks.end()); +} + +void expectTfDeskewRecoversWall(bool slerp) +{ + SCOPED_TRACE(slerp ? "slerp=true" : "slerp=false"); + + // base_link advances 0.1 m along odom over the sweep -- the same motion the constant + // velocity tests apply at 1 m/s. With slerp the correction is interpolated between + // the two end poses; without it, every sample gets its own TF lookup. + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "odom", "base_link", rtabmap::Transform(0, 0, 0, 0, 0, 0), 1000.0, false); + addTf(*buffer, "odom", "base_link", + rtabmap::Transform(float(kSpeed * kScanSpan), 0, 0, 0, 0, 0), + 1000.0 + kScanSpan, false); + + sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(1000.0, 0.0); + in.header.frame_id = "base_link"; + + sensor_msgs::msg::PointCloud2 out; + ASSERT_TRUE(deskew(in, out, "odom", *buffer, 0.0, slerp)); + + for(size_t i=0; i b = makeTfBuffer(); + geometry_msgs::msg::TransformStamped m; + m.header.frame_id = "odom"; + m.child_frame_id = "base_link"; + m.transform.rotation.w = 1.0; + for(double elapsed : {0.0, kneeTime, kScanSpan}) + { + m.header.stamp = timestampToROS(1000.0 + elapsed); + m.transform.translation.x = travelled(elapsed); + b->setTransform(m, "unit_test", false); + } + return b; + }; + + sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan( + 1000.0, 0.0, kOffsetSecFloat32, kTimeOnColumns, 1, "t", false, travelled); + in.header.frame_id = "base_link"; + + sensor_msgs::msg::PointCloud2 slerped, perPoint; + const std::shared_ptr b1 = buildBuffer(); + const std::shared_ptr b2 = buildBuffer(); + ASSERT_TRUE(deskew(in, slerped, "odom", *b1, 0.0, /*slerp=*/true)); + ASSERT_TRUE(deskew(in, perPoint, "odom", *b2, 0.0, /*slerp=*/false)); + + // Per-point lookups follow the real motion, so they reconstruct the wall exactly. + for(size_t i=0; i buffer = makeTfBuffer(); // empty + + sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(1000.0, 0.0); + in.header.frame_id = "base_link"; + + sensor_msgs::msg::PointCloud2 out; + EXPECT_FALSE(deskew(in, out, "odom", *buffer, 0.0, true)); +} + +///////////////////////// +// convertRGBDMsgs / convertStereoMsg +///////////////////////// + +namespace { + +/// A rectified pinhole CameraInfo. tx is P(0,3): 0 for the left/depth camera, and +/// -fx*baseline for the right camera of a stereo pair. +sensor_msgs::msg::CameraInfo makeCameraInfo( + const std::string & frameId, double stamp, int width, int height, + double tx = 0.0, double fx = 100.0) +{ + sensor_msgs::msg::CameraInfo info; + info.header.stamp = timestampToROS(stamp); + info.header.frame_id = frameId; + info.width = width; + info.height = height; + info.distortion_model = "plumb_bob"; + info.d = {0.0, 0.0, 0.0, 0.0, 0.0}; + info.k = {fx, 0.0, width/2.0, 0.0, fx, height/2.0, 0.0, 0.0, 1.0}; + info.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + info.p = {fx, 0.0, width/2.0, tx, 0.0, fx, height/2.0, 0.0, 0.0, 0.0, 1.0, 0.0}; + return info; +} + +cv_bridge::CvImageConstPtr makeImage( + const std::string & frameId, double stamp, + const cv::Mat & image, const std::string & encoding) +{ + std_msgs::msg::Header header; + header.stamp = timestampToROS(stamp); + header.frame_id = frameId; + return std::make_shared(header, encoding, image); +} + +} // namespace + +TEST(MsgConversion, convertRGBDMsgsSingleCamera) +{ + const std::shared_ptr buffer = makeTfBuffer(); + const rtabmap::Transform baseToCamera(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f); + addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0); + + const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + const cv::Mat depthImage(8, 8, CV_16UC1, cv::Scalar(1500)); + + const std::vector images = + {makeImage("camera_link", 1000.0, rgbImage, "bgr8")}; + const std::vector depths = + {makeImage("camera_link", 1000.0, depthImage, "16UC1")}; + const std::vector infos = + {makeCameraInfo("camera_link", 1000.0, 8, 8)}; + + cv::Mat rgb, depth; + std::vector models; + std::vector stereoModels; + ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "", + timestampToROS(1000.0), rgb, depth, models, stereoModels, + *buffer, 0.0, /*alreadyRectifiedImages=*/true)); + + EXPECT_TRUE(stereoModels.empty()) << "a depth image must not produce a stereo model"; + ASSERT_EQ(models.size(), 1u); + EXPECT_NEAR(models[0].fx(), 100.0, 1e-9); + expectTransformNear(models[0].localTransform(), baseToCamera, 1e-4f); + + ASSERT_EQ(rgb.cols, 8); + ASSERT_EQ(rgb.rows, 8); + EXPECT_EQ(depth.type(), CV_16UC1); + EXPECT_EQ(depth.at(0, 0), 1500); +} + +TEST(MsgConversion, convertRGBDMsgsMultiCameraSideBySide) +{ + // Two cameras are concatenated horizontally into one wide image, one model each. + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "cam0", rtabmap::Transform(0.1f, 0.1f, 0, 0, 0, 0), 1000.0); + addTf(*buffer, "base_link", "cam1", rtabmap::Transform(0.1f, -0.1f, 0, 0, 0, 0), 1000.0); + + const cv::Mat rgb0(8, 8, CV_8UC3, cv::Scalar(10, 0, 0)); + const cv::Mat rgb1(8, 8, CV_8UC3, cv::Scalar(0, 20, 0)); + const cv::Mat depth0(8, 8, CV_16UC1, cv::Scalar(1000)); + const cv::Mat depth1(8, 8, CV_16UC1, cv::Scalar(2000)); + + const std::vector images = { + makeImage("cam0", 1000.0, rgb0, "bgr8"), + makeImage("cam1", 1000.0, rgb1, "bgr8")}; + const std::vector depths = { + makeImage("cam0", 1000.0, depth0, "16UC1"), + makeImage("cam1", 1000.0, depth1, "16UC1")}; + const std::vector infos = { + makeCameraInfo("cam0", 1000.0, 8, 8), + makeCameraInfo("cam1", 1000.0, 8, 8)}; + + cv::Mat rgb, depth; + std::vector models; + std::vector stereoModels; + ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "", + timestampToROS(1000.0), rgb, depth, models, stereoModels, + *buffer, 0.0, true)); + + ASSERT_EQ(models.size(), 2u); + EXPECT_EQ(rgb.cols, 16) << "the two 8-wide images must be side by side"; + EXPECT_EQ(rgb.rows, 8); + EXPECT_EQ(depth.cols, 16); + + // Each half keeps its own camera's data. + EXPECT_EQ(depth.at(0, 0), 1000); + EXPECT_EQ(depth.at(0, 8), 2000); + EXPECT_NEAR(models[0].localTransform().y(), 0.1, 1e-4); + EXPECT_NEAR(models[1].localTransform().y(), -0.1, 1e-4); +} + +namespace { + +/// base_link sits at odom origin at t=1000 and 1 m along x at t=1001. +void addOdomMotion(tf2_ros::Buffer & buffer) +{ + geometry_msgs::msg::TransformStamped m; + m.header.frame_id = "odom"; + m.child_frame_id = "base_link"; + m.transform.rotation.w = 1.0; + m.header.stamp = timestampToROS(1000.0); + m.transform.translation.x = 0.0; + ASSERT_TRUE(buffer.setTransform(m, "unit_test", false)); + m.header.stamp = timestampToROS(1001.0); + m.transform.translation.x = 1.0; + ASSERT_TRUE(buffer.setTransform(m, "unit_test", false)); +} + +} // namespace + +TEST(MsgConversion, convertRGBDMsgsSyncsToOdomStamp) +{ + // The image is captured at t=1001 but must be expressed relative to the base frame + // at odomStamp=1000, one metre back. + const std::shared_ptr buffer = makeTfBuffer(); + const rtabmap::Transform baseToCamera(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f); + addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0); + addOdomMotion(*buffer); + + const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + const cv::Mat depthImage(8, 8, CV_16UC1, cv::Scalar(1500)); + const std::vector images = + {makeImage("camera_link", 1001.0, rgbImage, "bgr8")}; + const std::vector depths = + {makeImage("camera_link", 1001.0, depthImage, "16UC1")}; + const std::vector infos = + {makeCameraInfo("camera_link", 1001.0, 8, 8)}; + + cv::Mat rgb, depth; + std::vector corrected; + std::vector stereoModels; + ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "odom", + timestampToROS(1000.0), rgb, depth, corrected, stereoModels, *buffer, 0.0, true)); + ASSERT_EQ(corrected.size(), 1u); + EXPECT_NEAR(corrected[0].localTransform().x(), 1.1, 1e-3) + << "0.1 base->camera plus 1.0 of odometry motion"; + + // Without an odom frame the motion is not folded in. + std::vector uncorrected; + std::vector stereoModels2; + ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "", + timestampToROS(1000.0), rgb, depth, uncorrected, stereoModels2, *buffer, 0.0, true)); + ASSERT_EQ(uncorrected.size(), 1u); + EXPECT_NEAR(uncorrected[0].localTransform().x(), 0.1, 1e-3); +} + +TEST(MsgConversion, convertRGBDMsgsSyncsEachCameraAtItsOwnStamp) +{ + // Two cameras captured 1 s apart, on a robot moving 1 m/s along x. Each must be + // corrected by its OWN elapsed motion, not by a single shared one. + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "cam0", rtabmap::Transform(0.1f, 0.1f, 0, 0, 0, 0), 1000.0); + addTf(*buffer, "base_link", "cam1", rtabmap::Transform(0.1f, -0.1f, 0, 0, 0, 0), 1000.0); + addOdomMotion(*buffer); // x=0 @1000, x=1 @1001 + geometry_msgs::msg::TransformStamped m; // extend to x=2 @1002 + m.header.frame_id = "odom"; + m.child_frame_id = "base_link"; + m.transform.rotation.w = 1.0; + m.header.stamp = timestampToROS(1002.0); + m.transform.translation.x = 2.0; + ASSERT_TRUE(buffer->setTransform(m, "unit_test", false)); + + const cv::Mat rgb0(8, 8, CV_8UC3, cv::Scalar(10, 0, 0)); + const cv::Mat rgb1(8, 8, CV_8UC3, cv::Scalar(0, 20, 0)); + const cv::Mat depth0(8, 8, CV_16UC1, cv::Scalar(1000)); + const cv::Mat depth1(8, 8, CV_16UC1, cv::Scalar(2000)); + + const std::vector images = { + makeImage("cam0", 1001.0, rgb0, "bgr8"), + makeImage("cam1", 1002.0, rgb1, "bgr8")}; + const std::vector depths = { + makeImage("cam0", 1001.0, depth0, "16UC1"), + makeImage("cam1", 1002.0, depth1, "16UC1")}; + const std::vector infos = { + makeCameraInfo("cam0", 1001.0, 8, 8), + makeCameraInfo("cam1", 1002.0, 8, 8)}; + + cv::Mat rgb, depth; + std::vector models; + std::vector stereoModels; + ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "odom", + timestampToROS(1000.0), rgb, depth, models, stereoModels, *buffer, 0.0, true)); + + ASSERT_EQ(models.size(), 2u); + // cam0 is 1 s after odomStamp, cam1 is 2 s after. + EXPECT_NEAR(models[0].localTransform().x(), 1.1, 1e-3) << "0.1 + 1.0 of motion"; + EXPECT_NEAR(models[1].localTransform().x(), 2.1, 1e-3) << "0.1 + 2.0 of motion"; + // The corrections must differ, which is the whole point of per-camera stamps. + EXPECT_GT(models[1].localTransform().x() - models[0].localTransform().x(), 0.5); + // The lateral offsets are untouched by a purely forward motion. + EXPECT_NEAR(models[0].localTransform().y(), 0.1, 1e-3); + EXPECT_NEAR(models[1].localTransform().y(), -0.1, 1e-3); +} + +TEST(MsgConversion, convertRGBDMsgsPrefersTheDepthStampWhenTheyDiffer) +{ + // The RGB and depth stamps of a camera are assumed to be equal. This pins the + // tie-break for when they are not: the depth stamp prevails, since it is the one the + // geometry is synchronized to. Not a behaviour to rely on -- a camera whose two + // stamps disagree is already outside the contract. + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "camera_link", rtabmap::Transform(0.1f, 0, 0, 0, 0, 0), 1000.0); + addOdomMotion(*buffer); + + const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + const cv::Mat depthImage(8, 8, CV_16UC1, cv::Scalar(1500)); + + // RGB stamped at odomStamp (no motion), depth stamped 1 s later (1 m of motion). + const std::vector images = + {makeImage("camera_link", 1000.0, rgbImage, "bgr8")}; + const std::vector depths = + {makeImage("camera_link", 1001.0, depthImage, "16UC1")}; + const std::vector infos = + {makeCameraInfo("camera_link", 1000.0, 8, 8)}; + + cv::Mat rgb, depth; + std::vector models; + std::vector stereoModels; + ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "odom", + timestampToROS(1000.0), rgb, depth, models, stereoModels, *buffer, 0.0, true)); + + ASSERT_EQ(models.size(), 1u); + EXPECT_NEAR(models[0].localTransform().x(), 1.1, 1e-3) + << "the depth stamp (1001) prevails over the rgb stamp (1000)"; +} + +TEST(MsgConversion, convertRGBDMsgsMultiStereoBuildsOneModelPerPair) +{ + // mono8 "right" images make convertRGBDMsgs take the stereo branch and produce + // StereoCameraModels instead of CameraModels. The odometry sync is not re-tested + // here: it happens in the shared loop before the depth/stereo split, so + // convertRGBDMsgsSyncsEachCameraAtItsOwnStamp already covers it for both. + const double fx = 100.0; + const double baseline0 = 0.15; + const double baseline1 = 0.20; + + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "left0", rtabmap::Transform(0.1f, 0.1f, 0, 0, 0, 0), 1000.0); + addTf(*buffer, "base_link", "left1", rtabmap::Transform(0.1f, -0.1f, 0, 0, 0, 0), 1000.0); + + const cv::Mat left0(8, 8, CV_8UC1, cv::Scalar(40)); + const cv::Mat left1(8, 8, CV_8UC1, cv::Scalar(60)); + const cv::Mat right(8, 8, CV_8UC1, cv::Scalar(50)); + + const std::vector images = { + makeImage("left0", 1000.0, left0, "mono8"), + makeImage("left1", 1000.0, left1, "mono8")}; + const std::vector rights = { + makeImage("right0", 1000.0, right, "mono8"), + makeImage("right1", 1000.0, right, "mono8")}; + const std::vector leftInfos = { + makeCameraInfo("left0", 1000.0, 8, 8, 0.0, fx), + makeCameraInfo("left1", 1000.0, 8, 8, 0.0, fx)}; + const std::vector rightInfos = { + makeCameraInfo("right0", 1000.0, 8, 8, -fx*baseline0, fx), + makeCameraInfo("right1", 1000.0, 8, 8, -fx*baseline1, fx)}; + + cv::Mat rgb, depth; + std::vector models; + std::vector stereoModels; + ASSERT_TRUE(convertRGBDMsgs(images, rights, leftInfos, rightInfos, "base_link", "", + timestampToROS(1000.0), rgb, depth, models, stereoModels, *buffer, 0.0, true)); + + EXPECT_TRUE(models.empty()) << "mono8 right images must give stereo models"; + ASSERT_EQ(stereoModels.size(), 2u); + + // Each pair keeps its own baseline and its own local transform. + EXPECT_NEAR(stereoModels[0].baseline(), baseline0, 1e-6); + EXPECT_NEAR(stereoModels[1].baseline(), baseline1, 1e-6); + EXPECT_NEAR(stereoModels[0].localTransform().y(), 0.1, 1e-3); + EXPECT_NEAR(stereoModels[1].localTransform().y(), -0.1, 1e-3); + + // The two left images are laid out side by side, as in the RGB-D case. + EXPECT_EQ(rgb.cols, 16); + EXPECT_EQ(depth.cols, 16); +} + +TEST(MsgConversion, convertRGBDMsgsSurvivesAFailedOdomLookup) +{ + // A missing odom frame must only warn: the data is still converted, uncorrected. + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "camera_link", rtabmap::Transform(0.1f, 0, 0.2f, 0, 0, 0), 1001.0); + + const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + const std::vector images = + {makeImage("camera_link", 1001.0, rgbImage, "bgr8")}; + const std::vector infos = + {makeCameraInfo("camera_link", 1001.0, 8, 8)}; + + cv::Mat rgb, depth; + std::vector models; + std::vector stereoModels; + ASSERT_TRUE(convertRGBDMsgs(images, {}, infos, {}, "base_link", "odom", + timestampToROS(1000.0), rgb, depth, models, stereoModels, *buffer, 0.0, true)) + << "a failed odometry correction must not be fatal"; + ASSERT_EQ(models.size(), 1u); + EXPECT_NEAR(models[0].localTransform().x(), 0.1, 1e-3) << "left uncorrected"; +} + +TEST(MsgConversion, convertStereoMsgSyncsToOdomStamp) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "left_link", rtabmap::Transform(0.1f, 0, 0.2f, 0, 0, 0), 1000.0); + addOdomMotion(*buffer); + + const cv::Mat mono(8, 8, CV_8UC1, cv::Scalar(40)); + + cv::Mat left, right; + rtabmap::StereoCameraModel model; + ASSERT_TRUE(convertStereoMsg( + makeImage("left_link", 1001.0, mono, "mono8"), + makeImage("right_link", 1001.0, mono, "mono8"), + makeCameraInfo("left_link", 1001.0, 8, 8, 0.0), + makeCameraInfo("right_link", 1001.0, 8, 8, -15.0), + "base_link", "odom", timestampToROS(1000.0), + left, right, model, *buffer, 0.0, true)); + + EXPECT_NEAR(model.localTransform().x(), 1.1, 1e-3); +} + +TEST(MsgConversion, convertScan3dMsgSyncsToOdomStamp) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "lidar", rtabmap::Transform(0.0f, 0, 0.5f, 0, 0, 0), 1000.0); + addOdomMotion(*buffer); + + sensor_msgs::msg::PointCloud2 msg = makeXYZCloud({{1.0f, 0.0f, 0.0f}}); + msg.header.stamp = timestampToROS(1001.0); + msg.header.frame_id = "lidar"; + + rtabmap::LaserScan scan; + ASSERT_TRUE(convertScan3dMsg(msg, "base_link", "odom", timestampToROS(1000.0), + scan, *buffer, 0.0)); + + EXPECT_NEAR(scan.localTransform().x(), 1.0, 1e-3) << "0.0 base->lidar plus 1.0 motion"; + EXPECT_NEAR(scan.localTransform().z(), 0.5, 1e-3); +} + +TEST(MsgConversion, convertScanMsgSyncsToOdomStamp) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "laser", rtabmap::Transform(0.2f, 0, 0.1f, 0, 0, 0), 1000.0); + addOdomMotion(*buffer); + + sensor_msgs::msg::LaserScan msg; + msg.header.stamp = timestampToROS(1001.0); + msg.header.frame_id = "laser"; + msg.angle_min = -1.0f; + msg.angle_max = 1.0f; + msg.angle_increment = 0.1f; + msg.time_increment = 0.0f; + msg.range_min = 0.1f; + msg.range_max = 30.0f; + msg.ranges.assign(21, 5.0f); + + rtabmap::LaserScan scan; + ASSERT_TRUE(convertScanMsg(msg, "base_link", "odom", timestampToROS(1000.0), + scan, *buffer, 0.0)); + + EXPECT_NEAR(scan.localTransform().x(), 1.2, 1e-3) << "0.2 base->laser plus 1.0 motion"; +} + +TEST(MsgConversion, convertRGBDMsgsRejectsBadEncoding) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "camera_link", rtabmap::Transform::getIdentity(), 1000.0); + + // 32FC1 is a valid depth encoding but not a valid rgb/left one. + const cv::Mat bad(8, 8, CV_32FC1, cv::Scalar(1.0f)); + const std::vector images = + {makeImage("camera_link", 1000.0, bad, "32FC1")}; + const std::vector infos = + {makeCameraInfo("camera_link", 1000.0, 8, 8)}; + + cv::Mat rgb, depth; + std::vector models; + std::vector stereoModels; + EXPECT_FALSE(convertRGBDMsgs(images, {}, infos, {}, "base_link", "", + timestampToROS(1000.0), rgb, depth, models, stereoModels, + *buffer, 0.0, true)); +} + +TEST(MsgConversion, convertRGBDMsgsFailsWithoutTf) +{ + const std::shared_ptr buffer = makeTfBuffer(); // empty + + const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + const std::vector images = + {makeImage("camera_link", 1000.0, rgbImage, "bgr8")}; + const std::vector infos = + {makeCameraInfo("camera_link", 1000.0, 8, 8)}; + + cv::Mat rgb, depth; + std::vector models; + std::vector stereoModels; + EXPECT_FALSE(convertRGBDMsgs(images, {}, infos, {}, "base_link", "", + timestampToROS(1000.0), rgb, depth, models, stereoModels, + *buffer, 0.0, true)); +} + +TEST(MsgConversion, convertRGBDMsgsCarriesLocalFeatures) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "camera_link", rtabmap::Transform::getIdentity(), 1000.0); + + const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + const cv::Mat depthImage(8, 8, CV_16UC1, cv::Scalar(1500)); + const std::vector images = + {makeImage("camera_link", 1000.0, rgbImage, "bgr8")}; + const std::vector depths = + {makeImage("camera_link", 1000.0, depthImage, "16UC1")}; + const std::vector infos = + {makeCameraInfo("camera_link", 1000.0, 8, 8)}; + + std::vector kptMsgs(2); + kptMsgs[0].pt.x = 1.0f; kptMsgs[0].pt.y = 2.0f; kptMsgs[0].size = 7.0f; + kptMsgs[1].pt.x = 3.0f; kptMsgs[1].pt.y = 4.0f; kptMsgs[1].size = 7.0f; + std::vector ptMsgs(2); + ptMsgs[0].x = 1.0f; ptMsgs[1].x = 2.0f; + cv::Mat descriptors = cv::Mat::ones(2, 4, CV_32FC1); + + std::vector outKpts; + std::vector outPts; + cv::Mat outDescriptors; + + cv::Mat rgb, depth; + std::vector models; + std::vector stereoModels; + ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "", + timestampToROS(1000.0), rgb, depth, models, stereoModels, + *buffer, 0.0, true, + {kptMsgs}, {ptMsgs}, {descriptors}, + &outKpts, &outPts, &outDescriptors)); + + ASSERT_EQ(outKpts.size(), 2u); + EXPECT_FLOAT_EQ(outKpts[0].pt.x, 1.0f); + EXPECT_FLOAT_EQ(outKpts[1].pt.x, 3.0f); + ASSERT_EQ(outPts.size(), 2u); + EXPECT_FLOAT_EQ(outPts[1].x, 2.0f); + EXPECT_EQ(outDescriptors.rows, 2); +} + +TEST(MsgConversion, convertStereoMsgProducesAStereoModel) +{ + const std::shared_ptr buffer = makeTfBuffer(); + const rtabmap::Transform baseToLeft(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f); + addTf(*buffer, "base_link", "left_link", baseToLeft, 1000.0); + + const double fx = 100.0; + const double baseline = 0.15; + const cv::Mat leftImage(8, 8, CV_8UC1, cv::Scalar(40)); + const cv::Mat rightImage(8, 8, CV_8UC1, cv::Scalar(50)); + + cv::Mat left, right; + rtabmap::StereoCameraModel model; + ASSERT_TRUE(convertStereoMsg( + makeImage("left_link", 1000.0, leftImage, "mono8"), + makeImage("right_link", 1000.0, rightImage, "mono8"), + makeCameraInfo("left_link", 1000.0, 8, 8, /*tx=*/0.0, fx), + makeCameraInfo("right_link", 1000.0, 8, 8, /*tx=*/-fx*baseline, fx), + "base_link", "", timestampToROS(1000.0), + left, right, model, *buffer, 0.0, /*alreadyRectified=*/true)); + + EXPECT_NEAR(model.baseline(), baseline, 1e-6); + EXPECT_NEAR(model.left().fx(), fx, 1e-9); + expectTransformNear(model.localTransform(), baseToLeft, 1e-4f); + + ASSERT_EQ(left.type(), CV_8UC1); + ASSERT_EQ(right.type(), CV_8UC1); + EXPECT_EQ(left.at(0, 0), 40); + EXPECT_EQ(right.at(0, 0), 50); +} + +TEST(MsgConversion, convertStereoMsgConvertsColourToMono) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "left_link", rtabmap::Transform::getIdentity(), 1000.0); + + const cv::Mat colour(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + const cv::Mat mono(8, 8, CV_8UC1, cv::Scalar(50)); + + cv::Mat left, right; + rtabmap::StereoCameraModel model; + ASSERT_TRUE(convertStereoMsg( + makeImage("left_link", 1000.0, colour, "bgr8"), + makeImage("right_link", 1000.0, mono, "mono8"), + makeCameraInfo("left_link", 1000.0, 8, 8, 0.0), + makeCameraInfo("right_link", 1000.0, 8, 8, -15.0), + "base_link", "", timestampToROS(1000.0), + left, right, model, *buffer, 0.0, true)); + + // The left image is kept in colour; the right is always reduced to mono. + EXPECT_EQ(left.type(), CV_8UC3); + EXPECT_EQ(right.type(), CV_8UC1); +} + +TEST(MsgConversion, convertStereoMsgRejectsBadEncoding) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "left_link", rtabmap::Transform::getIdentity(), 1000.0); + + const cv::Mat bad(8, 8, CV_32FC1, cv::Scalar(1.0f)); + const cv::Mat mono(8, 8, CV_8UC1, cv::Scalar(50)); + + cv::Mat left, right; + rtabmap::StereoCameraModel model; + EXPECT_FALSE(convertStereoMsg( + makeImage("left_link", 1000.0, bad, "32FC1"), + makeImage("right_link", 1000.0, mono, "mono8"), + makeCameraInfo("left_link", 1000.0, 8, 8, 0.0), + makeCameraInfo("right_link", 1000.0, 8, 8, -15.0), + "base_link", "", timestampToROS(1000.0), + left, right, model, *buffer, 0.0, true)); +} + +TEST(MsgConversion, convertStereoMsgFailsWithoutTf) +{ + const std::shared_ptr buffer = makeTfBuffer(); // empty + const cv::Mat mono(8, 8, CV_8UC1, cv::Scalar(50)); + + cv::Mat left, right; + rtabmap::StereoCameraModel model; + EXPECT_FALSE(convertStereoMsg( + makeImage("left_link", 1000.0, mono, "mono8"), + makeImage("right_link", 1000.0, mono, "mono8"), + makeCameraInfo("left_link", 1000.0, 8, 8, 0.0), + makeCameraInfo("right_link", 1000.0, 8, 8, -15.0), + "base_link", "", timestampToROS(1000.0), + left, right, model, *buffer, 0.0, true)); +} + +///////////////////////// +// IMU +///////////////////////// + +TEST(MsgConversion, imuRoundTrip) +{ + sensor_msgs::msg::Imu in; + in.orientation.x = 0.0; + in.orientation.y = 0.0; + in.orientation.z = 0.0; + in.orientation.w = 1.0; + in.angular_velocity.x = 0.1; + in.angular_velocity.y = 0.2; + in.angular_velocity.z = 0.3; + in.linear_acceleration.x = 1.0; + in.linear_acceleration.y = 2.0; + in.linear_acceleration.z = 9.81; + for(size_t i=0; i<9; ++i) + { + in.orientation_covariance[i] = 0.01 * (i + 1); + in.angular_velocity_covariance[i] = 0.02 * (i + 1); + in.linear_acceleration_covariance[i] = 0.03 * (i + 1); + } + + const rtabmap::IMU imu = imuFromROS(in, rtabmap::Transform::getIdentity()); + + sensor_msgs::msg::Imu out; + imuToROS(imu, out); + + EXPECT_DOUBLE_EQ(out.orientation.w, in.orientation.w); + EXPECT_DOUBLE_EQ(out.angular_velocity.x, in.angular_velocity.x); + EXPECT_DOUBLE_EQ(out.angular_velocity.y, in.angular_velocity.y); + EXPECT_DOUBLE_EQ(out.angular_velocity.z, in.angular_velocity.z); + EXPECT_DOUBLE_EQ(out.linear_acceleration.x, in.linear_acceleration.x); + EXPECT_DOUBLE_EQ(out.linear_acceleration.y, in.linear_acceleration.y); + EXPECT_DOUBLE_EQ(out.linear_acceleration.z, in.linear_acceleration.z); + for(size_t i=0; i<9; ++i) + { + EXPECT_DOUBLE_EQ(out.orientation_covariance[i], in.orientation_covariance[i]) + << "orientation covariance at " << i; + EXPECT_DOUBLE_EQ(out.angular_velocity_covariance[i], in.angular_velocity_covariance[i]) + << "angular velocity covariance at " << i; + EXPECT_DOUBLE_EQ(out.linear_acceleration_covariance[i], in.linear_acceleration_covariance[i]) + << "linear acceleration covariance at " << i; + } +} diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp index 990976ec..581b5316 100644 --- a/rtabmap_odom/src/nodelets/icp_odometry.cpp +++ b/rtabmap_odom/src/nodelets/icp_odometry.cpp @@ -333,7 +333,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan { // deskew with constant velocity model (we are in frameId) sensor_msgs::msg::PointCloud2 scanOutDeskewed; - if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess())) + if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, velocityGuess())) { RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!"); return; @@ -362,7 +362,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan { // deskew with constant velocity model sensor_msgs::msg::PointCloud2 scanOutDeskewed; - if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess())) + if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, velocityGuess())) { RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!"); return; @@ -583,7 +583,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p } std::shared_ptr cloudDeskewed(new sensor_msgs::msg::PointCloud2); - if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, previousStamp(), velocityGuess())) + if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, velocityGuess())) { RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!"); return; diff --git a/rtabmap_viz/src/rgbd_image_viewer.cpp b/rtabmap_viz/src/rgbd_image_viewer.cpp index d9fd1953..0cc36c81 100644 --- a/rtabmap_viz/src/rgbd_image_viewer.cpp +++ b/rtabmap_viz/src/rgbd_image_viewer.cpp @@ -165,6 +165,21 @@ void RGBDImageViewer::callback( QMetaObject::invokeMethod(warningLabel_, "clear"); } + // rgbdImageFromROS() does not copy the pixels: the SensorData points into the ROS + // message buffers, which the subscription queue recycles as soon as this callback + // returns. The event below is posted asynchronously and so outlives the callback, + // therefore the images must be deep-copied first. + if(!data.imageRaw().empty() || !data.depthOrRightRaw().empty()) { + cv::Mat image = data.imageRaw().clone(); + cv::Mat depthOrRight = data.depthOrRightRaw().clone(); + if(!data.stereoCameraModels().empty()) { + data.setStereoImage(image, depthOrRight, data.stereoCameraModels()); + } + else { + data.setRGBDImage(image, depthOrRight, data.cameraModels()); + } + } + this->post(new rtabmap::SensorEvent(data)); } From 61edb4ee85e35f4cc967fa6b502b58d8e3bc6e4f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 7 Sep 2026 21:23:22 -0700 Subject: [PATCH 10/17] rtabmap_util tests and doc (#1450) * Initial tests * more tests * More in-depth deskew() testing * slightly less verbose clamping corruption warning * added tf buffer related tests * added remaining tests * Added rosdoc2, improve tests when we require sync of odom stamp and sensor stamp * cleanup doc * fixing ci * rtabmap_util tests and doc * Added db_player tests * Added MapsManager tests * Added map_assembler tests * Documenting node first draft * relative links * Fixed british->usa english style. Reviewed all md files. * added link to install ros1 * updated badges * added Iron * added ubuntu * added codecov * updated coverage ci * fixing rosdep * updated ci cov job * ci bump * fixing cov ci * small doc cleanup --- .github/workflows/coverage.yml | 156 +++ .gitignore | 2 + README.md | 167 +-- codecov.yml | 77 ++ rtabmap_conversions/README.md | 4 +- .../rtabmap_conversions/MsgConversion.h | 16 +- rtabmap_conversions/src/MsgConversion.cpp | 14 +- .../test/test_msg_conversion.cpp | 67 +- rtabmap_odom/src/nodelets/icp_odometry.cpp | 2 +- rtabmap_util/CMakeLists.txt | 37 + rtabmap_util/README.md | 66 + rtabmap_util/doc/db_player.md | 117 ++ rtabmap_util/doc/disparity_to_depth.md | 55 + rtabmap_util/doc/imu_to_tf.md | 197 +++ rtabmap_util/doc/lidar_deskewing.md | 95 ++ rtabmap_util/doc/map_assembler.md | 109 ++ rtabmap_util/doc/obstacles_detection.md | 140 +++ rtabmap_util/doc/point_cloud_aggregator.md | 97 ++ rtabmap_util/doc/point_cloud_assembler.md | 162 +++ rtabmap_util/doc/point_cloud_xyz.md | 95 ++ rtabmap_util/doc/point_cloud_xyzrgb.md | 121 ++ rtabmap_util/doc/pointcloud_to_depthimage.md | 96 ++ rtabmap_util/doc/rgbd_relay.md | 75 ++ rtabmap_util/doc/rgbd_split.md | 76 ++ .../include/rtabmap_util/MapsManager.h | 133 +++ .../include/rtabmap_util/map_assembler.hpp | 4 + .../rtabmap_util/point_cloud_aggregator.hpp | 4 +- .../rtabmap_util/point_cloud_assembler.hpp | 4 +- .../include/rtabmap_util/rgbd_split.hpp | 3 + rtabmap_util/package.xml | 3 + rtabmap_util/rosdoc2.yaml | 35 + rtabmap_util/src/DbPlayerNode.cpp | 16 +- rtabmap_util/src/MapsManager.cpp | 7 + rtabmap_util/src/nodelets/db_player.cpp | 21 +- .../src/nodelets/disparity_to_depth.cpp | 24 +- rtabmap_util/src/nodelets/lidar_deskewing.cpp | 2 +- rtabmap_util/src/nodelets/map_assembler.cpp | 48 +- .../src/nodelets/point_cloud_aggregator.cpp | 39 +- .../src/nodelets/point_cloud_assembler.cpp | 38 +- .../src/nodelets/pointcloud_to_depthimage.cpp | 7 +- rtabmap_util/src/nodelets/rgbd_relay.cpp | 53 +- rtabmap_util/src/nodelets/rgbd_split.cpp | 113 +- rtabmap_util/test/db_builders.hpp | 366 ++++++ rtabmap_util/test/msg_builders.hpp | 217 ++++ rtabmap_util/test/node_test_utils.hpp | 320 +++++ rtabmap_util/test/test_db_player.cpp | 809 +++++++++++++ rtabmap_util/test/test_disparity_to_depth.cpp | 248 ++++ rtabmap_util/test/test_imu_to_tf.cpp | 205 ++++ rtabmap_util/test/test_lidar_deskewing.cpp | 204 ++++ rtabmap_util/test/test_map_assembler.cpp | 549 +++++++++ rtabmap_util/test/test_maps_manager.cpp | 1060 +++++++++++++++++ .../test/test_obstacles_detection.cpp | 423 +++++++ .../test/test_point_cloud_aggregator.cpp | 194 +++ .../test/test_point_cloud_assembler.cpp | 399 +++++++ rtabmap_util/test/test_point_cloud_xyz.cpp | 344 ++++++ rtabmap_util/test/test_point_cloud_xyzrgb.cpp | 531 +++++++++ .../test/test_pointcloud_to_depthimage.cpp | 370 ++++++ rtabmap_util/test/test_rgbd_relay.cpp | 350 ++++++ rtabmap_util/test/test_rgbd_split.cpp | 397 ++++++ tools/set_doc_distro.sh | 52 + 60 files changed, 9443 insertions(+), 192 deletions(-) create mode 100644 .github/workflows/coverage.yml create mode 100644 codecov.yml create mode 100644 rtabmap_util/README.md create mode 100644 rtabmap_util/doc/db_player.md create mode 100644 rtabmap_util/doc/disparity_to_depth.md create mode 100644 rtabmap_util/doc/imu_to_tf.md create mode 100644 rtabmap_util/doc/lidar_deskewing.md create mode 100644 rtabmap_util/doc/map_assembler.md create mode 100644 rtabmap_util/doc/obstacles_detection.md create mode 100644 rtabmap_util/doc/point_cloud_aggregator.md create mode 100644 rtabmap_util/doc/point_cloud_assembler.md create mode 100644 rtabmap_util/doc/point_cloud_xyz.md create mode 100644 rtabmap_util/doc/point_cloud_xyzrgb.md create mode 100644 rtabmap_util/doc/pointcloud_to_depthimage.md create mode 100644 rtabmap_util/doc/rgbd_relay.md create mode 100644 rtabmap_util/doc/rgbd_split.md create mode 100644 rtabmap_util/rosdoc2.yaml create mode 100644 rtabmap_util/test/db_builders.hpp create mode 100644 rtabmap_util/test/msg_builders.hpp create mode 100644 rtabmap_util/test/node_test_utils.hpp create mode 100644 rtabmap_util/test/test_db_player.cpp create mode 100644 rtabmap_util/test/test_disparity_to_depth.cpp create mode 100644 rtabmap_util/test/test_imu_to_tf.cpp create mode 100644 rtabmap_util/test/test_lidar_deskewing.cpp create mode 100644 rtabmap_util/test/test_map_assembler.cpp create mode 100644 rtabmap_util/test/test_maps_manager.cpp create mode 100644 rtabmap_util/test/test_obstacles_detection.cpp create mode 100644 rtabmap_util/test/test_point_cloud_aggregator.cpp create mode 100644 rtabmap_util/test/test_point_cloud_assembler.cpp create mode 100644 rtabmap_util/test/test_point_cloud_xyz.cpp create mode 100644 rtabmap_util/test/test_point_cloud_xyzrgb.cpp create mode 100644 rtabmap_util/test/test_pointcloud_to_depthimage.cpp create mode 100644 rtabmap_util/test/test_rgbd_relay.cpp create mode 100644 rtabmap_util/test/test_rgbd_split.cpp create mode 100755 tools/set_doc_distro.sh diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml new file mode 100644 index 00000000..dd22c6ed --- /dev/null +++ b/.github/workflows/coverage.yml @@ -0,0 +1,156 @@ +name: Coverage + +on: + push: + branches: [ ros2 ] + pull_request: + branches: [ ros2 ] + workflow_dispatch: + +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: ${{ github.event_name == 'pull_request' }} + +permissions: + contents: read + id-token: write + +jobs: + coverage: + name: Coverage (humble) + runs-on: ubuntu-latest + container: + # RTAB-Map master, already built and installed on top of ROS Humble + # (jammy = 22.04) -- the same base the repo's own docker/humble image + # uses. Saves building the library here, and pins the coverage run to + # RTAB-Map master rather than whatever the released binaries carry. + image: introlab3it/rtabmap:jammy + env: + CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }} + + steps: + - uses: actions/checkout@v4 + + # The container runs as root, so no sudo (it is not installed). + # + # colcon lcov-result is not a built-in verb -- it comes from the + # colcon-lcov-result package, which action-ros-ci calls but does not + # install. ros2.yml gets it for free because it runs ros-tooling/setup-ros + # first; this job calls action-ros-ci directly, so install it here. + # Without it action-ros-ci logs "invalid choice: 'lcov-result'", ignores + # the failure, and the job goes green having measured nothing. + # Versions pinned to the ones setup-ros uses. + - run: | + export DEBIAN_FRONTEND=noninteractive + apt-get update + # lcov for genhtml (the browsable artifact below); colcon-lcov-result + # shells out to it too. + apt-get install -y lcov python3-pip + pip3 install -U \ + colcon-lcov-result==0.5.0 \ + colcon-coveragepy-result==0.0.8 + colcon lcov-result --help > /dev/null + # The image ships rosdep but has never initialized it -- the repo's + # own docker/humble Dockerfile runs `rosdep init` for the same reason. + # action-ros-ci only runs `rosdep update`, which fails with "no + # sources directory exists" until this has happened once. + rosdep init || true + + # Only the packages that have tests. The others are still built when a + # 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 + 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. + rosdep-skip-keys: rtabmap + # The coverage-gcc mixin only adds --coverage; it sets no build type, + # and neither action-ros-ci nor this repo's CMakeLists do. Without + # this the build type is empty, which happens to mean -O0 but is not + # guaranteed to stay that way -- and at -O2 inlining and dead-code + # elimination make gcov's line attribution unreliable. + extra-cmake-args: -DCMAKE_BUILD_TYPE=Debug + colcon-defaults: | + { + "build": { + "mixin": ["coverage-gcc"] + } + } + # Pinned so a change in the mixin repository cannot break this job. + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/b8436aa16c0bdbc01081b12caa253cbf16e0fb82/index.yaml + # Run the coverage passes ourselves, in the step below. The action's + # own baseline pass is a bare `colcon lcov-result --initial` with no + # package selection, so it walks every package in the workspace -- + # including the ones --packages-up-to never built -- and fails with + # "cannot read .../build/rtabmap_viz". + coverage-result: false + + # Both passes restricted to the packages that were actually built. + # + # lcov captures each package's whole build directory, so more than this + # repo's sources land in it. Filtered: + # */test/* the test sources -- the instrument, not the subject. A line + # there is uncovered only when the test skipped it, which says + # nothing about the code under test, and they are near-fully + # covered by construction. + # /usr/*, /opt/* everything outside the workspace. RTAB-Map's own .cpp + # files are never captured (the library is installed in the + # image, not built here), but its inline and template code is + # emitted into the objects of the packages that include it, + # as are PCL, Eigen and the standard library. + # *CompilerId*, */CMakeFiles/* CMake's compiler-probe translation + # units. colcon-lcov-result tries to delete their .gcno files + # but misses these, and they are recorded against a path that + # does not exist -- which kills the genhtml pass colcon runs + # at the end ("cannot read .../CMakeCCompilerId.c", exit 2). + # Filters run before that pass, so dropping them here is + # enough. + # Quoting them matters: action-ros-ci injects filters unquoted, so the + # shell can glob them away before colcon ever sees them. + - name: Coverage report + working-directory: ros_ws + run: | + # `.` not `source`: steps in this container run under sh (dash), where + # `source` does not exist. GitHub picks sh whenever it cannot find + # bash in the image's PATH, and says so in the log ("shell: sh -e"). + . /opt/ros/humble/setup.sh + PKGS="rtabmap_conversions rtabmap_util" + # 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 + # 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 \ + --filter '*/test/*' '/usr/*' '/opt/*' \ + '*CompilerId*' '*/CMakeFiles/*' || true + test -s lcov/total_coverage.info + + # Fails the job if the step above produced nothing -- the failure mode + # this workflow has hit twice already is a green run that measured zero. + - name: Coverage summary + run: lcov --summary ros_ws/lcov/total_coverage.info + + # colcon lcov-result runs genhtml itself, into the same lcov/ directory. + - name: Upload HTML coverage artifact + uses: actions/upload-artifact@v4 + with: + name: coverage-html + path: ros_ws/lcov + retention-days: 14 + + - name: Upload to Codecov + if: ${{ env.CODECOV_TOKEN != '' }} + uses: codecov/codecov-action@v5 + with: + files: ros_ws/lcov/total_coverage.info + # Upload ONLY the aggregated lcov file. By default the CLI also walks + # the tree and runs gcov over every .gcno it finds, which re-adds the + # test sources the --filter above just dropped. + disable_search: true + plugins: noop + token: ${{ env.CODECOV_TOKEN }} + fail_ci_if_error: false diff --git a/.gitignore b/.gitignore index 8f56a6c7..5d2ec8fa 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,8 @@ .pydevproject .settings +.vscode __pycache__ # rosdoc2 build artifacts docs_build cross_reference +doc_output diff --git a/README.md b/README.md index de5a2176..eaee4560 100644 --- a/README.md +++ b/README.md @@ -1,89 +1,86 @@ rtabmap_ros =========== -RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Humble minimum required**: currently most nodes are ported to ROS2. The interface is the same than on ROS1 (parameters and topic names should still match ROS1 documentation on [rtabmap_ros](http://wiki.ros.org/rtabmap_ros)). +ROS 2 wrapper for [RTAB-Map](https://github.com/introlab/rtabmap), a graph-based SLAM library with appearance-based loop closure detection. It builds and maintains a 3D map from RGB-D, stereo or lidar data, closes loops on revisited places and exports the result as an occupancy grid, a point cloud or an OctoMap. + +**ROS 2 Humble minimum required.** The interface matches ROS 1: parameters and topic names still follow the [ROS 1 documentation](http://wiki.ros.org/rtabmap_ros) for anything not yet covered by the package pages below. #### CI Latest - - - - - - - - - - - -
ROS 1Build Status
Build Status -
ROS 2Build Status -
- - #### ROS Binaries - - - - - - - - - - - - - - - - - - - - - - - - - - - -
ROS 1NoeticBuild Status
ROS 2HumbleBuild Status
JazzyBuild Status
RollingBuild Status
Docker - rtabmap_ros - Docker Pulls
+| | Build | Docker | +|---|---|---| +| ROS 1 | [![ROS 1](https://github.com/introlab/rtabmap_ros/actions/workflows/ros1.yml/badge.svg)](https://github.com/introlab/rtabmap_ros/actions/workflows/ros1.yml) | [![Docker](https://github.com/introlab/rtabmap_ros/actions/workflows/docker.yml/badge.svg)](https://github.com/introlab/rtabmap_ros/actions/workflows/docker.yml) | +| ROS 2 | [![ROS 2](https://github.com/introlab/rtabmap_ros/actions/workflows/ros2.yml/badge.svg)](https://github.com/introlab/rtabmap_ros/actions/workflows/ros2.yml) | [![Docker ROS 2](https://github.com/introlab/rtabmap_ros/actions/workflows/docker-ros2.yml/badge.svg)](https://github.com/introlab/rtabmap_ros/actions/workflows/docker-ros2.yml) | -# Usage +#### ROS Binaries -* For sensor integration examples (stereo and RGB-D cameras, 3D LiDAR), see [rtabmap_examples](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) sub-folder. +| | Distro | Ubuntu | Released | In apt | Build | +|---|---|---|---|---|---| +| ROS 1 | Noetic (EOL) | 20.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Fnoetic%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/noetic/distribution.yaml) | [![apt](https://img.shields.io/ros/v/noetic/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#noetic) | | +| ROS 2 | Humble | 22.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Fhumble%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/humble/distribution.yaml) | [![apt](https://img.shields.io/ros/v/humble/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#humble) | [![build](http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary)](http://build.ros2.org/job/Hbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary/) | +| ROS 2 | Iron (EOL) | 22.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Firon%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/iron/distribution.yaml) | [![apt](https://img.shields.io/ros/v/iron/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#iron) | | +| ROS 2 | Jazzy | 24.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Fjazzy%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/jazzy/distribution.yaml) | [![apt](https://img.shields.io/ros/v/jazzy/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#jazzy) | [![build](http://build.ros2.org/buildStatus/icon?job=Jbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary)](http://build.ros2.org/job/Jbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary/) | +| ROS 2 | Kilted | 24.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Fkilted%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/kilted/distribution.yaml) | [![apt](https://img.shields.io/ros/v/kilted/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#kilted) | [![build](http://build.ros2.org/buildStatus/icon?job=Kbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary)](http://build.ros2.org/job/Kbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary/) | +| ROS 2 | Lyrical | 26.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Flyrical%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/lyrical/distribution.yaml) | [![apt](https://img.shields.io/ros/v/lyrical/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#lyrical) | [![build](http://build.ros2.org/buildStatus/icon?job=Lbin_uR64__rtabmap_ros__ubuntu_resolute_amd64__binary)](http://build.ros2.org/job/Lbin_uR64__rtabmap_ros__ubuntu_resolute_amd64__binary/) | +| ROS 2 | Rolling | 26.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Frolling%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/rolling/distribution.yaml) | [![apt](https://img.shields.io/ros/v/rolling/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#rolling) | | +| Docker | [rtabmap_ros](https://hub.docker.com/r/introlab3it/rtabmap_ros) | | | ![Docker Pulls](https://img.shields.io/docker/pulls/introlab3it/rtabmap_ros.svg?label=pulls) | | -* For robot integration examples (turtlebot3 and turtlebot4, nav2 integration), see [rtabmap_demos](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_demos) sub-folder. +*Released* is the version bloomed into [rosdistro](https://github.com/ros/rosdistro); *In apt* is what `apt install` actually gives you today. They differ while a release is waiting on a buildfarm sync. -## Logging -To make RTAB-Map's logs appear ordered with RCLCPP's logs, set the following environment variables in your `.bashrc` (see official "[About Logging](https://docs.ros.org/en/humble/Concepts/Intermediate/About-Logging.html)" documentation for more info): -```bash -export RCUTILS_LOGGING_USE_STDOUT=1 -export RCUTILS_LOGGING_BUFFERED_STREAM=1 -# Optional, but if you like colored logs: -export RCUTILS_COLORIZED_OUTPUT=1 -``` +# Packages -## Recommended DDS -If RTAB-Map's GUI or topic frequency feel laggy (even if processing time looks fast enough), it may be caused by the DDS. I recommend to use [Cyclone DDS](https://docs.ros.org/en/foxy/Installation/DDS-Implementations/Working-with-Eclipse-CycloneDDS.html), you can try it by adding this before launching any nodes/launch files (or add to your `.bashrc`): -```bash -export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp -# Cyclone prefers multicast by default, if your router got too much spammed, -# disable multicast with (https://github.com/ros2/rmw_cyclonedds/issues/489): -export CYCLONEDDS_URI="0.0.0.0" -``` +The stack is split into small packages so a pipeline only pulls in what it uses. Package names link to their documentation where it exists; the rest are being written and will be linked as they land. -# Installation +### SLAM + +| Package | Description | +|---|---| +| `rtabmap_slam` | The `rtabmap` node itself: appearance-based loop closure detection, graph optimization, memory management and map assembly. | +| `rtabmap_odom` | Odometry nodes — `rgbd_odometry`, `stereo_odometry` and `icp_odometry`. Any external odometry can be used instead. | +| `rtabmap_sync` | Synchronizes camera and lidar topics into a single message so they reach the SLAM node together — `rgbd_sync`, `stereo_sync`, `rgbdx_sync`. | + +### Sensor processing + +| Package | Description | +|---|---| +| [`rtabmap_util`](rtabmap_util/README.md) | Utility nodes around the pipeline: format conversions, point cloud filtering and assembly, obstacle detection, map assembly, database replay. Most are useful on their own. | +| `rtabmap_costmap_plugins` | A variant of nav2's voxel layer that follows the robot along z, keeping the voxel grid centered on the base frame. For robots that change altitude, e.g. drones. | + +### Interfaces and libraries + +| Package | Description | +|---|---| +| `rtabmap_msgs` | Message, service and action definitions used across the stack. | +| [`rtabmap_conversions`](rtabmap_conversions/README.md) | C++ library converting between RTAB-Map library types and ROS 2 messages. | +| `rtabmap_python` | Python helpers, currently image compression matching RTAB-Map's own format. | + +### Visualization + +| Package | Description | +|---|---| +| `rtabmap_viz` | RTAB-Map's own GUI as a ROS 2 node: live graph, loop closures, feature matches and the parameter panel. | +| `rtabmap_rviz_plugins` | RViz displays for the map graph, the assembled cloud and the SLAM info. | + +### Launch files + +| Package | Description | +|---|---| +| `rtabmap_launch` | `rtabmap.launch.py`, the one-line way to bring up the whole stack. | +| [`rtabmap_examples`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) | Sensor integration examples: stereo and RGB-D cameras, 3D lidar. | +| [`rtabmap_demos`](rtabmap_demos/README.md) | Full robot demos: turtlebot3 and turtlebot4, nav2 integration, multi-session mapping. | + +# Installation + +These instructions are for ROS 2. For ROS 1, follow the [installation instructions](https://github.com/introlab/rtabmap_ros/tree/master#installation) on the [`master`](https://github.com/introlab/rtabmap_ros/tree/master) branch, which also carries the latest version for Noetic. ### Binaries + ```bash sudo apt install ros-$ROS_DISTRO-rtabmap-ros ``` ### From Source + * Make sure to uninstall any rtabmap binaries: ``` sudo apt remove ros-$ROS_DISTRO-rtabmap* @@ -103,3 +100,39 @@ sudo apt install ros-$ROS_DISTRO-rtabmap-ros colcon build --symlink-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DRTABMAP_SYNC_USER_DATA=ON -DCMAKE_BUILD_TYPE=Release ``` +# Usage + +* For sensor integration examples (stereo and RGB-D cameras, 3D LiDAR), see [rtabmap_examples](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) sub-folder. + +* For robot integration examples (turtlebot3 and turtlebot4, nav2 integration), see [rtabmap_demos](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_demos) sub-folder. + +## Logging +To make RTAB-Map's logs appear ordered with RCLCPP's logs, set the following environment variables in your `.bashrc` (see official "[About Logging](https://docs.ros.org/en/jazzy/Concepts/Intermediate/About-Logging.html)" documentation for more info): +```bash +export RCUTILS_LOGGING_USE_STDOUT=1 +export RCUTILS_LOGGING_BUFFERED_STREAM=1 +# Optional, but if you like colored logs: +export RCUTILS_COLORIZED_OUTPUT=1 +``` + +## Recommended DDS +If RTAB-Map's GUI or topic frequency feel laggy (even if processing time looks fast enough), it may be caused by the DDS. I recommend to use [Cyclone DDS](https://docs.ros.org/en/jazzy/Installation/RMW-Implementations/DDS-Implementations/Working-with-Eclipse-CycloneDDS.html), you can try it by adding this before launching any nodes/launch files (or add to your `.bashrc`): +```bash +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +# Cyclone prefers multicast by default, if your router got too much spammed, +# disable multicast with (https://github.com/ros2/rmw_cyclonedds/issues/489): +export CYCLONEDDS_URI="0.0.0.0" +``` + +# Documentation + +* **Package documentation** — the tables above, and the [API reference on docs.ros.org](https://docs.ros.org/en/jazzy/p/rtabmap_ros/). +* **Examples** — [rtabmap_examples](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) for sensors, [rtabmap_demos](rtabmap_demos/README.md) for full robots. +* **Parameters** — every `Rtabmap/*`, `Grid/*`, `Odom/*` and other core parameter is listed in the [RTAB-Map parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html). +* **Library API** — [RTAB-Map's own API documentation](https://introlab.github.io/rtabmap/api/latest/). +* **Papers and videos** — [introlab.github.io/rtabmap](https://introlab.github.io/rtabmap/). +* **Old tutorials** — the [ROS 1 wiki](http://wiki.ros.org/rtabmap_ros/Tutorials), for anything not covered above; parameters and topic names are unchanged. + +# License + +BSD-3-Clause, see [LICENSE](LICENSE). RTAB-Map itself may be built with components under other licenses; see the [rtabmap](https://github.com/introlab/rtabmap) repository. diff --git a/codecov.yml b/codecov.yml new file mode 100644 index 00000000..4fec39d4 --- /dev/null +++ b/codecov.yml @@ -0,0 +1,77 @@ +# Codecov configuration -- https://docs.codecov.com/docs/codecov-yaml +# +# Coverage data is produced by .github/workflows/coverage.yml (colcon's +# coverage-gcc mixin over the packages that have tests, aggregated by +# colcon-lcov-result) and uploaded by codecov/codecov-action; this file only +# controls what Codecov reports back on a pull request. Nothing is posted +# unless the Codecov GitHub App has access to the repository. + +# action-ros-ci checks the repository out into a colcon workspace, so every +# path in the lcov report is prefixed. Strip it, otherwise Codecov cannot match +# a file to the one in the diff and reports no coverage at all. +fixes: + - "ros_ws/src/rtabmap_ros/::" + +# Mark uncovered added lines inline in the "Files changed" tab. +github_checks: + annotations: true + +coverage: + precision: 2 + round: down + range: "10...90" # red/green scale: 10% is fully red, 90% fully green + + status: + # Catch a slow slide down without pinning an absolute number. + project: + default: + target: auto + threshold: 1% + + # Coverage of the lines this pull request touches. Advisory: reported, but + # does not block the merge -- drop "informational" to make it gate. + patch: + default: + informational: true + +# Per-package breakdown, computed from the same single upload -- no extra job +# and no separate flag upload per package. Each component gets its own line in +# the pull request comment and its own status check. +component_management: + default_rules: + statuses: + - type: project + target: auto + threshold: 1% + individual_components: + - component_id: rtabmap_conversions + name: rtabmap_conversions + paths: + - rtabmap_conversions/** + - component_id: rtabmap_util + name: rtabmap_util + paths: + - rtabmap_util/** + +comment: + layout: "condensed_header, diff, components, files" + behavior: default + require_changes: true # stay quiet when coverage doesn't move + +# Only rtabmap_conversions and rtabmap_util 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. +ignore: + - "rtabmap_costmap_plugins/**" + - "rtabmap_demos/**" + - "rtabmap_examples/**" + - "rtabmap_launch/**" + - "rtabmap_msgs/**" + - "rtabmap_odom/**" + - "rtabmap_python/**" + - "rtabmap_rviz_plugins/**" + - "rtabmap_slam/**" + - "rtabmap_sync/**" + - "rtabmap_viz/**" + - "**/test/**" diff --git a/rtabmap_conversions/README.md b/rtabmap_conversions/README.md index 9b187a02..98265a7b 100644 --- a/rtabmap_conversions/README.md +++ b/rtabmap_conversions/README.md @@ -46,7 +46,7 @@ The naming is uniform: `xxxFromROS()` converts a message into an RTAB-Map type, | Graph | `mapDataFromROS`, `mapGraphFromROS`, `nodeFromROS`, `linkFromROS`, `sensorDataFromROS` (+ `ToROS` variants) | | Misc | `infoFromROS`, `odomInfoFromROS`, `odomInfoToStatistics`, `imuFromROS`, `userDataFromROS`, `envSensorFromROS`, `landmarksFromROS`, `timestampFromROS`, `timestampToROS` | -Full signatures and per-function notes are in the [API documentation](https://docs.ros.org/en/rolling/p/rtabmap_conversions/) and in [`MsgConversion.h`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h). +Full signatures and per-function notes are in the [API documentation](https://docs.ros.org/en/jazzy/p/rtabmap_conversions/) and in [`MsgConversion.h`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h). ## Conventions worth knowing @@ -66,7 +66,7 @@ colcon test-result --verbose ## Documentation -API documentation is generated with [rosdoc2](https://github.com/ros-infrastructure/rosdoc2) from the Doxygen comments in the public header, and published to [docs.ros.org](https://docs.ros.org/en/rolling/p/rtabmap_conversions/). To build it locally: +API documentation is generated with [rosdoc2](https://github.com/ros-infrastructure/rosdoc2) from the Doxygen comments in the public header, and published to [docs.ros.org](https://docs.ros.org/en/jazzy/p/rtabmap_conversions/). To build it locally: ```bash rosdoc2 build --package-path rtabmap_conversions --output-directory doc_output diff --git a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h index a87aa2a3..dbaf94df 100644 --- a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h +++ b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h @@ -176,7 +176,9 @@ void toCvCopy(const rtabmap_msgs::msg::RGBDImage & image, cv_bridge::CvImagePtr /** * @brief Extract the RGB and depth images of an RGBDImage message without copying. * - * The returned images alias the message's buffers, so @p image must outlive them. + * The returned images alias the message's buffers, so @p image must outlive them. Both + * output pointers are always valid; they hold an empty image when the corresponding + * field is not set. * * @param[in] image the message to read; its shared pointer keeps the buffers alive * @param[out] rgb the RGB image @@ -186,6 +188,10 @@ void toCvShare(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr & image, cv_br /** * @brief Extract the RGB and depth images of an RGBDImage message without copying. + * + * Both output pointers are always valid; they hold an empty image when the corresponding + * field is not set. + * * @param[in] image the message to read * @param[in] trackedObject object whose lifetime keeps the message buffers alive * @param[out] rgb the RGB image @@ -219,8 +225,12 @@ void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::msg::RGBDIma * The stamp is taken from the top-level `image->header`, and the camera's local * transform is not carried by the message (callers resolve it from TF). * + * The depth image is optional: a message carrying only the color image and its camera + * info gives a SensorData with no depth, which is valid. + * * @param image the message to convert - * @return the converted sensor data + * @return the converted sensor data, empty (SensorData::isValid() false) if the message + * carries no color image or an unsupported encoding * * @warning The returned SensorData does **not** copy the pixels: it points into the * message's own buffers. @p image must therefore outlive it and must not be @@ -772,7 +782,7 @@ bool convertRGBDMsgs( /** * @brief Convert a stereo pair into RTAB-Map inputs. * - * The left image keeps its colour; the right image is always reduced to mono. + * The left image keeps its color; the right image is always reduced to mono. * * @param leftImageMsg left image * @param rightImageMsg right image diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index e252a617..1f2c4685 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -255,6 +255,11 @@ void toCvShare(const rtabmap_msgs::msg::RGBDImage & image, const std::shared_ptr depth = ptr; } } + else + { + // empty + depth = std::make_shared(); + } } catch(cv::Exception& e) { UFATAL("Fatal error while converting rgbd image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); @@ -432,7 +437,11 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::msg::RGBDImage::ConstSh int depthWidth = depthMsg->image.cols; int depthHeight = depthMsg->image.rows; + // The depth image is optional: a message can legitimately carry only the color + // image and its camera info. Compare the resolutions only when there is a depth + // image, otherwise the ratios divide by zero. UASSERT_MSG( + depthMsg->image.empty() || imageWidth/depthWidth == imageHeight/depthHeight, uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str()); @@ -448,7 +457,8 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::msg::RGBDImage::ConstSh imageMsg->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 || imageMsg->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 || imageMsg->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) || - !(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 || + !(depthMsg->image.empty() || + depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 || depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 || depthMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)) { @@ -2707,7 +2717,7 @@ bool convertScanMsg( 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.size()*scan2dMsg.time_increment), + 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); if(tmpT.isNull()) diff --git a/rtabmap_conversions/test/test_msg_conversion.cpp b/rtabmap_conversions/test/test_msg_conversion.cpp index 0b911720..e9033389 100644 --- a/rtabmap_conversions/test/test_msg_conversion.cpp +++ b/rtabmap_conversions/test/test_msg_conversion.cpp @@ -1756,7 +1756,7 @@ TEST(MsgConversion, deskewConstantVelocityHeaderAtLastPoint) EXPECT_NEAR(readField(out, i, 0), expected, 1e-4) << "x of point " << i; } - // The line is flat to well under a millimetre: that is the deskewing working, + // The line is flat to well under a millimeter: that is the deskewing working, // independently of which end of the scan the frame is anchored to. float minX = readField(out, 0, 0); float maxX = minX; @@ -2011,7 +2011,7 @@ TEST(MsgConversion, deskewClampsSamplesOutsideTheSweep) ASSERT_TRUE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0))); // Clamped to the last sample's correction, so it lands within the sweep's own range - // rather than metres away. Every other sample is unaffected. + // rather than meters away. Every other sample is unaffected. const float x = readWallX(out, corrupt, 0, kTimeOnColumns); EXPECT_GE(x, kWallDistance - 1e-3f); EXPECT_LE(x, kWallDistance + float(kSpeed * kScanSpan) + 1e-3f) @@ -2224,6 +2224,57 @@ TEST(MsgConversion, toCvShareReadsCompressedDepth) EXPECT_EQ(cv::countNonZero(depthPtr->image != depth), 0); } +TEST(MsgConversion, toCvShareOnAnEmptyMessageGivesEmptyImages) +{ + // Both pointers must be valid even when the message carries nothing: callers such as + // rgbdImageFromROS() dereference them unconditionally. + const rtabmap_msgs::msg::RGBDImage msg; + + cv_bridge::CvImageConstPtr rgbPtr, depthPtr; + toCvShare(msg, std::shared_ptr(), rgbPtr, depthPtr); + + ASSERT_TRUE(rgbPtr); + ASSERT_TRUE(depthPtr); + EXPECT_TRUE(rgbPtr->image.empty()); + EXPECT_TRUE(depthPtr->image.empty()); +} + +TEST(MsgConversion, rgbdImageFromROSOnAnEmptyMessageIsInvalid) +{ + rtabmap_msgs::msg::RGBDImage::SharedPtr msg = + std::make_shared(); + msg->header.frame_id = "camera_link"; + msg->header.stamp = rclcpp::Time(1000, 0, RCL_ROS_TIME); + + const rtabmap::SensorData data = rgbdImageFromROS(msg); + + EXPECT_FALSE(data.isValid()) << "an empty message must give empty data, not a crash"; +} + +TEST(MsgConversion, rgbdImageFromROSWithoutDepthKeepsTheColorImage) +{ + // The depth image is optional: color plus camera info is a valid message, and the + // resolution check must not divide by the zero depth width. + rtabmap_msgs::msg::RGBDImage::SharedPtr msg = + std::make_shared(); + msg->header.frame_id = "camera_link"; + msg->header.stamp = rclcpp::Time(1000, 0, RCL_ROS_TIME); + cv::Mat rgb(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb).toImageMsg(msg->rgb); + msg->rgb_camera_info.width = 8; + msg->rgb_camera_info.height = 8; + msg->rgb_camera_info.k = {525.0, 0.0, 4.0, 0.0, 525.0, 4.0, 0.0, 0.0, 1.0}; + + const rtabmap::SensorData data = rgbdImageFromROS(msg); + + EXPECT_TRUE(data.isValid()); + ASSERT_FALSE(data.imageRaw().empty()); + EXPECT_EQ(data.imageRaw().at(0, 0), cv::Vec3b(10, 20, 30)); + EXPECT_TRUE(data.depthRaw().empty()); + ASSERT_EQ(data.cameraModels().size(), 1u); + EXPECT_NEAR(data.cameraModels()[0].fx(), 525.0, 1e-9); +} + TEST(MsgConversion, toCvCopyReadsCompressedRgb) { const cv::Mat rgb(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); @@ -2990,7 +3041,7 @@ void addOdomMotion(tf2_ros::Buffer & buffer) TEST(MsgConversion, convertRGBDMsgsSyncsToOdomStamp) { // The image is captured at t=1001 but must be expressed relative to the base frame - // at odomStamp=1000, one metre back. + // at odomStamp=1000, one meter back. const std::shared_ptr buffer = makeTfBuffer(); const rtabmap::Transform baseToCamera(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f); addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0); @@ -3075,7 +3126,7 @@ TEST(MsgConversion, convertRGBDMsgsPrefersTheDepthStampWhenTheyDiffer) { // The RGB and depth stamps of a camera are assumed to be equal. This pins the // tie-break for when they are not: the depth stamp prevails, since it is the one the - // geometry is synchronized to. Not a behaviour to rely on -- a camera whose two + // geometry is synchronized to. Not a behavior to rely on -- a camera whose two // stamps disagree is already outside the contract. const std::shared_ptr buffer = makeTfBuffer(); addTf(*buffer, "base_link", "camera_link", rtabmap::Transform(0.1f, 0, 0, 0, 0, 0), 1000.0); @@ -3350,25 +3401,25 @@ TEST(MsgConversion, convertStereoMsgProducesAStereoModel) EXPECT_EQ(right.at(0, 0), 50); } -TEST(MsgConversion, convertStereoMsgConvertsColourToMono) +TEST(MsgConversion, convertStereoMsgConvertsColorToMono) { const std::shared_ptr buffer = makeTfBuffer(); addTf(*buffer, "base_link", "left_link", rtabmap::Transform::getIdentity(), 1000.0); - const cv::Mat colour(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + const cv::Mat color(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); const cv::Mat mono(8, 8, CV_8UC1, cv::Scalar(50)); cv::Mat left, right; rtabmap::StereoCameraModel model; ASSERT_TRUE(convertStereoMsg( - makeImage("left_link", 1000.0, colour, "bgr8"), + makeImage("left_link", 1000.0, color, "bgr8"), makeImage("right_link", 1000.0, mono, "mono8"), makeCameraInfo("left_link", 1000.0, 8, 8, 0.0), makeCameraInfo("right_link", 1000.0, 8, 8, -15.0), "base_link", "", timestampToROS(1000.0), left, right, model, *buffer, 0.0, true)); - // The left image is kept in colour; the right is always reduced to mono. + // The left image is kept in color; the right is always reduced to mono. EXPECT_EQ(left.type(), CV_8UC3); EXPECT_EQ(right.type(), CV_8UC1); } diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp index 581b5316..2281e7fb 100644 --- a/rtabmap_odom/src/nodelets/icp_odometry.cpp +++ b/rtabmap_odom/src/nodelets/icp_odometry.cpp @@ -313,7 +313,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan scanMsg->header.frame_id, guessFrameId().empty()?frameId():guessFrameId(), scanMsg->header.stamp, - rclcpp::Time(scanMsg->header.stamp.sec, scanMsg->header.stamp.nanosec) + rclcpp::Duration::from_seconds(scanMsg->ranges.size()*scanMsg->time_increment), + rclcpp::Time(scanMsg->header.stamp.sec, scanMsg->header.stamp.nanosec) + rclcpp::Duration::from_seconds((scanMsg->ranges.empty()?0:scanMsg->ranges.size()-1)*scanMsg->time_increment), this->tfBuffer(), this->waitForTransform()); if(tmpT.isNull()) diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt index 718e0456..9c287b4a 100644 --- a/rtabmap_util/CMakeLists.txt +++ b/rtabmap_util/CMakeLists.txt @@ -300,4 +300,41 @@ install(DIRECTORY include/ FILES_MATCHING PATTERN "*.h" ) +############# +## Testing ## +############# +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + # Each node gets its own test binary: a crash or a stuck executor in one node cannot + # take the others down, and every binary starts with a clean DDS graph. + macro(rtabmap_util_add_node_test test_name) + ament_add_gtest(${test_name} test/${test_name}.cpp) + if(TARGET ${test_name}) + target_include_directories(${test_name} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test) + target_link_libraries(${test_name} rtabmap_util_plugins rtabmap_util) + if("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(${test_name} ${AmentLibraries}) + else() + target_link_libraries(${test_name} ${Libraries} ${PublicLibraries}) + endif() + endif() + endmacro() + + rtabmap_util_add_node_test(test_imu_to_tf) + rtabmap_util_add_node_test(test_disparity_to_depth) + rtabmap_util_add_node_test(test_rgbd_relay) + rtabmap_util_add_node_test(test_rgbd_split) + rtabmap_util_add_node_test(test_lidar_deskewing) + rtabmap_util_add_node_test(test_obstacles_detection) + rtabmap_util_add_node_test(test_point_cloud_aggregator) + rtabmap_util_add_node_test(test_point_cloud_assembler) + rtabmap_util_add_node_test(test_point_cloud_xyz) + rtabmap_util_add_node_test(test_pointcloud_to_depthimage) + rtabmap_util_add_node_test(test_point_cloud_xyzrgb) + rtabmap_util_add_node_test(test_db_player) + rtabmap_util_add_node_test(test_maps_manager) + rtabmap_util_add_node_test(test_map_assembler) +endif() + ament_package() diff --git a/rtabmap_util/README.md b/rtabmap_util/README.md new file mode 100644 index 00000000..8eccf4b5 --- /dev/null +++ b/rtabmap_util/README.md @@ -0,0 +1,66 @@ +# rtabmap_util + +Standalone utility nodes for [RTAB-Map](https://github.com/introlab/rtabmap) pipelines: converting between sensor representations, cleaning up point clouds, assembling maps and replaying recorded sessions. + +Every node is a [composable node](https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Composition.html) as well as a standalone executable. Composing them into one process with their producer avoids copying images and clouds between processes, which is worth doing for anything on the sensor path. + +## Nodes + +One page per node. + +**Sensor conversion** + +| Node | Description | +|---|---| +| [disparity_to_depth](doc/disparity_to_depth.md) | Disparity image → depth image, in meters and in millimeters. | +| [pointcloud_to_depthimage](doc/pointcloud_to_depthimage.md) | Point cloud → depth image registered to an RGB camera. Lets a lidar feed an RGB-D pipeline. | +| [point_cloud_xyz](doc/point_cloud_xyz.md) | Depth or disparity image → point cloud, with filtering. | +| [point_cloud_xyzrgb](doc/point_cloud_xyzrgb.md) | RGB-D, stereo or disparity → colored point cloud. | +| [imu_to_tf](doc/imu_to_tf.md) | IMU orientation → TF. | + +**RGBDImage plumbing** + +| Node | Description | +|---|---| +| [rgbd_relay](doc/rgbd_relay.md) | Republishes an `RGBDImage`, compressing or decompressing on the way. | +| [rgbd_split](doc/rgbd_split.md) | Splits an `RGBDImage` back into standard `Image` and `CameraInfo` topics. | + +**Point cloud processing** + +| Node | Description | +|---|---| +| [lidar_deskewing](doc/lidar_deskewing.md) | Removes motion distortion from a lidar sweep. | +| [point_cloud_aggregator](doc/point_cloud_aggregator.md) | Merges one cloud from each of several sensors into one. | +| [point_cloud_assembler](doc/point_cloud_assembler.md) | Accumulates one sensor over time into a denser cloud. | +| [obstacles_detection](doc/obstacles_detection.md) | Segments a cloud into ground and obstacles. | + +**Maps and replay** + +| Node | Description | +|---|---| +| [map_assembler](doc/map_assembler.md) | Rebuilds the global maps from RTAB-Map's graph, off the SLAM node's critical path. | +| [db_player](doc/db_player.md) | Replays a recorded RTAB-Map database as live sensor topics. | + +## Library + +The package also installs a small C++ library, whose API is documented in the [C++ API reference](https://docs.ros.org/en/jazzy/p/rtabmap_util/generated/index.html) generated from the headers. + +`MapsManager` is the piece worth knowing about: it turns a pose graph plus per-node occupancy grids into the assembled clouds, occupancy grid, octomap and elevation map, and publishes them. Both [map_assembler](doc/map_assembler.md) and `rtabmap_slam`'s `rtabmap` node use it, which is why their map outputs and `Grid/*` parameters behave identically. + +## Conventions + +A few things recur across these nodes. + +**`qos` parameters.** Most nodes expose a `qos` integer selecting the reliability of their subscriptions: `0` system default, `1` reliable, `2` best effort. It has to be compatible with the publisher or **no messages arrive at all** and nothing says why. Sensor drivers commonly publish best effort. + +**`approx_sync`.** Nodes taking several inputs match them by nearest stamp by default. Set it to `false` when the inputs are hardware-synchronized and carry identical stamps: the exact policy is cheaper and cannot mismatch. With approximate sync, `approx_sync_max_interval` is worth setting as a guard against silently pairing stale data. + +**`fixed_frame_id`.** Where a node has to account for the robot moving between two stamps, it does so by asking TF how a frame moved relative to a fixed one — usually `odom`. Leaving it empty disables the compensation rather than erroring, so a moving robot then gets subtly misplaced data. + +**`Grid/*` parameters.** Nodes that segment or assemble maps use RTAB-Map's own [`LocalGridMaker`](https://introlab.github.io/rtabmap/api/latest/classrtabmap_1_1LocalGridMaker.html), and expose its parameters directly under their RTAB-Map names. Their meanings and defaults are in RTAB-Map's [parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html), which is the source of truth for them. One to know about: `Grid/RangeMax` is not unlimited by default, so distant points are dropped before anything else happens. + +## Building the documentation + +```bash +rosdoc2 build --package-path rtabmap_util --output-directory doc_output +``` diff --git a/rtabmap_util/doc/db_player.md b/rtabmap_util/doc/db_player.md new file mode 100644 index 00000000..1dfae23d --- /dev/null +++ b/rtabmap_util/doc/db_player.md @@ -0,0 +1,117 @@ +# db_player + +Replays a recorded RTAB-Map database as live sensor topics. + +Point it at a `.db` file and it publishes the images, scans, odometry and transforms that were recorded into it, at the rate they were captured. Everything downstream sees a running robot. + +That makes it the tool for offline work: re-run SLAM with different parameters on the same data, debug a failure you cannot reproduce on the robot, or develop a node without hardware. Unlike a rosbag, the database is what RTAB-Map itself wrote, so it is always available after a mapping session. + +> **The executable is named `data_player`**, not `db_player`. The composable node is `rtabmap_util::DbPlayer`. + +## Usage + +```bash +ros2 run rtabmap_util data_player --ros-args \ + -p database:=~/.ros/rtabmap.db \ + -p rate:=1.0 \ + -p frame_id:=base_link +``` + +```python +ComposableNode( + package='rtabmap_util', + plugin='rtabmap_util::DbPlayer', + name='db_player', + parameters=[{'database': '/path/to/rtabmap.db', 'rate': 1.0, + 'frame_id': 'base_link'}]) +``` + +## Published Topics + +**Which topics exist depends on what the database contains.** The node inspects the first frame and only advertises what it can actually publish, so a lidar-only database has no image topics at all. + +| Topic | Type | Published when | +|---|---|---| +| `rgb/image`, `rgb/camera_info` | [`Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html), [`CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Single RGB-D camera. | +| `depth/image`, `depth/camera_info` | `Image`, `CameraInfo` | Single RGB-D camera. | +| `left/image`, `left/camera_info` | `Image`, `CameraInfo` | Single stereo pair. | +| `right/image`, `right/camera_info` | `Image`, `CameraInfo` | Single stereo pair. | +| `image` | `Image` | Images with no calibration. | +| `rgbd_image0`, `rgbd_image1`, … | [`RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | Multiple RGB-D cameras, one topic each. | +| `stereo_image0`, `stereo_image1`, … | `RGBDImage` | Multiple stereo pairs, one topic each. | +| `scan` | [`LaserScan`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/LaserScan.html) | A 2D laser scan. | +| `scan_cloud` | [`PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | A 3D laser scan. | +| `odom` | [`Odometry`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Odometry.html) | Odometry poses, with their covariance. | +| `imu` | [`Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Gravity was recorded. Orientation only. | +| `global_pose` | [`PoseWithCovarianceStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseWithCovarianceStamped.html) | A prior pose was recorded. | +| `gps/fix` | [`NavSatFix`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/NavSatFix.html) | GPS was recorded. | +| `env_sensor` | [`EnvSensor`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/EnvSensor.html) | Environmental sensors were recorded. | +| `/clock` | [`Clock`](https://docs.ros.org/en/jazzy/p/rosgraph_msgs/msg/Clock.html) | `publish_clock` is set. See [Simulated time](#simulated-time). | + +Everything except `/tf` and `/clock` is published only when it has a subscriber. + +## Published Transforms + +Broadcast on every frame unless `publish_tf` is false. + +| Transform | Published when | +|---|---| +| `odom_frame_id` → `frame_id` | Odometry is available. | +| `frame_id` → `camera_frame_id` | A camera is calibrated. Multi-camera setups get a numeric suffix; stereo gets `left_`/`right_` prefixes, with the right frame offset by the baseline. | +| `frame_id` → `scan_frame_id` | A scan is present. | +| `frame_id` → `imu_frame_id` | An IMU is present. | +| `ground_truth_frame_id` → `ground_truth_base_frame_id` | Ground truth was recorded. | + +## Services + +| Service | Type | Description | +|---|---|---| +| `~/pause` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Pause playback. | +| `~/resume` | `std_srvs/srv/Empty` | Resume it. | + +When run as the standalone executable, the **space bar** toggles pause as well. + +## Parameters + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `database` | `string` | `""` | **Required.** Path to the `.db` file. `~` is expanded, relative paths resolve against the working directory. The node throws on start-up if it is unset or unreadable. | +| `rate` | `double` | `1.0` | Playback speed as a multiple of the recorded rate. `2.0` is twice as fast, `0.5` half. | +| `start_id` | `int` | `0` | Skip to this node id. `0` starts at the beginning. | +| `ignore_odom` | `bool` | `false` | Do not publish odometry or its transform, so you can run your own odometry against the raw sensor data. | +| `publish_tf` | `bool` | `true` | Broadcast the transforms above. Turn it off if a robot state publisher already provides them. | +| `publish_clock` | `bool` | `false` | Publish `/clock`. See [Simulated time](#simulated-time). | +| `frame_id` | `string` | `"base_link"` | Robot base frame. | +| `odom_frame_id` | `string` | `"odom"` | Odometry frame. | +| `camera_frame_id` | `string` | `"camera_optical_link"` | Camera optical frame. | +| `scan_frame_id` | `string` | `"base_laser_link"` | Lidar frame. | +| `imu_frame_id` | `string` | `"imu_link"` | IMU frame. | +| `ground_truth_frame_id` | `string` | `"world"` | Ground truth parent frame. | +| `ground_truth_base_frame_id` | `string` | `"base_link_gt"` | Ground truth child frame. | +| `qos` | `int` | `0` | Reliability of all publishers unless overridden below. | +| `qos_camera_info`, `qos_odom`, `qos_scan`, `qos_scan_cloud`, `qos_global_pose`, `qos_gps`, `qos_imu`, `qos_env_sensor` | `int` | value of `qos` | Per-topic overrides. | + +**2D scan geometry** — only used when the recorded scan has no angle metadata of its own, which happens for scans converted from a 3D lidar. + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `scan_angle_min` | `double` | `-π` | | +| `scan_angle_max` | `double` | `π` | | +| `scan_angle_increment` | `double` | `π/720` | | +| `scan_range_min` | `double` | `0.0` | | +| `scan_range_max` | `double` | `60.0` | | + +## Simulated time + +With `publish_clock` the node publishes `/clock` from the recorded stamps. Start every other node with `use_sim_time:=true` and the whole system runs on the database's timeline instead of the wall clock, so playback speed no longer affects behavior — a good idea when replaying faster than real time, and essential for reproducible runs. + +```bash +ros2 run rtabmap_util data_player --ros-args -p database:=map.db -p publish_clock:=true +ros2 launch rtabmap_launch rtabmap.launch.py use_sim_time:=true +``` + +## Notes + +Playback ends when the last node has been published, and the standalone executable exits at that point. + +The database is opened read-only as far as playback is concerned, so replaying the same file while RTAB-Map maps into another one is safe. diff --git a/rtabmap_util/doc/disparity_to_depth.md b/rtabmap_util/doc/disparity_to_depth.md new file mode 100644 index 00000000..f8cc9ea7 --- /dev/null +++ b/rtabmap_util/doc/disparity_to_depth.md @@ -0,0 +1,55 @@ +# disparity_to_depth + +Converts a disparity image into a depth image. + +Most of ROS handles depth, while a stereo pipeline produces disparity. This node bridges the two: for every pixel it computes `depth = baseline * focal / disparity`, taking the baseline and focal length from the incoming [`stereo_msgs/msg/DisparityImage`](https://docs.ros.org/en/jazzy/p/stereo_msgs/msg/DisparityImage.html) itself, so no camera info is needed. + +Pixels whose disparity falls outside the message's own `min_disparity`/`max_disparity` are written as zero, which is the ROS convention for "no reading". + +## Usage + +```bash +ros2 run rtabmap_util disparity_to_depth --ros-args \ + -r disparity:=/stereo/disparity \ + -r depth:=/stereo/depth +``` + +```python +ComposableNode( + package='rtabmap_util', + plugin='rtabmap_util::DisparityToDepth', + name='disparity_to_depth', + remappings=[('disparity', '/stereo/disparity'), + ('depth', '/stereo/depth')]) +``` + +## Subscribed Topics + +| Topic | Type | Description | +|---|---|---| +| `disparity` | [`stereo_msgs/msg/DisparityImage`](https://docs.ros.org/en/jazzy/p/stereo_msgs/msg/DisparityImage.html) | The disparity image must be `32FC1`; anything else is rejected with an error. | + +## Published Topics + +| Topic | Type | Description | +|---|---|---| +| `depth` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) (`32FC1`) | Depth in **meters**. | +| `depth_raw` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) (`16UC1`) | The same depth in **millimeters**, the compact form most RGB-D drivers publish. | + +Both are computed only if something is subscribed to them, so leaving one unused costs nothing. Both keep the header of the input disparity image. + +## Parameters + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `qos` | `int` | `0` | Reliability of both sides: `0` system default, `1` reliable, `2` best effort. | +| `qos_sub` | `int` | value of `qos` | Reliability of the `disparity` subscription alone. | +| `qos_pub` | `int` | value of `qos` | Reliability of the `depth` and `depth_raw` publishers alone. | +| `queue_sub` | `int` | `1` | Queue depth of the `disparity` subscription. Must be at least 1. | +| `queue_pub` | `int` | `1` | Queue depth of both publishers. Must be at least 1. | + +## Notes + +Depth beyond 65.535 m cannot be represented in the `16UC1` output and wraps around; use the `32FC1` `depth` topic for long-range stereo. + +This node performs no filtering or hole-filling. A noisy disparity image gives a noisy depth image. diff --git a/rtabmap_util/doc/imu_to_tf.md b/rtabmap_util/doc/imu_to_tf.md new file mode 100644 index 00000000..8802e830 --- /dev/null +++ b/rtabmap_util/doc/imu_to_tf.md @@ -0,0 +1,197 @@ +# imu_to_tf + +Broadcasts the orientation of an IMU as a TF transform. + +The node subscribes to a `sensor_msgs/msg/Imu` topic, takes the `orientation` field and broadcasts it on `/tf` as the rotation of `fixed_frame_id` → the IMU frame. Set `base_frame_id` and that frame becomes the child instead, with the orientation re-expressed in it from the IMU's mounting, so the transform says how the *robot* is oriented rather than how the sensor is. Either way `fixed_frame_id` is the parent, and nothing else of the message is used: the transform's translation is always zero, and the angular velocity and linear acceleration are ignored. + +It exists so that a consumer that needs an oriented frame — a lidar deskewing node, a point cloud assembler, RViz — can get one from an IMU alone, without running odometry. + +## Usage + +As a standalone node: + +```bash +ros2 run rtabmap_util imu_to_tf --ros-args \ + -r imu/data:=/imu \ + -p fixed_frame_id:=odom +``` + +As a composable node, in the same process as its producer or consumer: + +```python +ComposableNode( + package='rtabmap_util', + plugin='rtabmap_util::ImuToTF', + name='imu_to_tf', + parameters=[{'fixed_frame_id': 'odom'}], + remappings=[('imu/data', '/imu')]) +``` + +### When the IMU has no orientation + +The node reads `orientation` and nothing else, and many IMUs do not fill it in — they publish only angular velocity and linear acceleration. Fuse them into an orientation first, with a filter such as [`imu_filter_madgwick`](https://github.com/CCNYRoboticsLab/imu_tools), and point this node at the filter's output: + +```python +Node( + package='imu_filter_madgwick', executable='imu_filter_madgwick_node', + parameters=[{'use_mag': False, 'world_frame': 'enu', 'publish_tf': False}], + remappings=[('imu/data_raw', '/camera/imu')]), # publishes /imu/data + +Node( + package='rtabmap_util', executable='imu_to_tf', + parameters=[{'fixed_frame_id': 'odom'}], + remappings=[('imu/data', '/imu/data')]), +``` + +Set `publish_tf: False` on the filter. It can broadcast a transform of its own, and two nodes publishing orientation for the same frame is exactly the conflict described in [Notes](#notes). `use_mag: False` keeps it off the magnetometer, which is rarely trustworthy indoors or near motors. + +A quick way to tell whether you need the filter at all: + +```bash +ros2 topic echo /camera/imu --field orientation --once +``` + +All zeros, or an `orientation_covariance` whose first element is `-1`, means the driver is not estimating orientation and this node has nothing to publish. + +### A stabilized frame for lidar deskewing and odometry + +The way it is used in [`rtabmap_examples/launch/lidar3d.launch.py`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_examples/launch/lidar3d.launch.py). A 3D lidar needs a fixed frame to deskew against and ICP odometry benefits from a motion guess, but before odometry is running there is no `odom` frame to use. An IMU can supply one — for rotation. + +Point `fixed_frame_id` at a frame that does not exist anywhere else, named after the base frame: + +```python +Node( + package='rtabmap_util', executable='imu_to_tf', + parameters=[{'fixed_frame_id': 'base_link_stabilized', + 'base_frame_id': 'base_link', + 'wait_for_transform_duration': 0.001}], + remappings=[('imu/data', '/imu/data')]) +``` + +This publishes `base_link_stabilized` → `base_link` carrying the robot's orientation and nothing else. Because the node never publishes a translation, `base_link_stabilized` stays glued to the robot and only its *orientation* is meaningful over time: it is a gravity-leveled version of the base frame rather than a world frame. That is exactly what the two consumers need. + +[lidar_deskewing](lidar_deskewing.md) then corrects the rotation of each sweep: + +```python +Node( + package='rtabmap_util', executable='lidar_deskewing', + parameters=[{'fixed_frame_id': 'base_link_stabilized'}], + remappings=[('input_cloud', '/lidar/points')]) +``` + +and ICP odometry takes the same frame as its motion guess, with its own deskewing turned off since it is already done: + +```python +Node( + package='rtabmap_odom', executable='icp_odometry', + parameters=[{'frame_id': 'base_link', + 'odom_frame_id': 'icp_odom', + 'guess_frame_id': 'base_link_stabilized', + 'deskewing': False}], + remappings=[('scan_cloud', '/lidar/points/deskewed')]) +``` + +The three nodes chain into a single TF tree: + +```text +map rtabmap +└── icp_odom icp_odometry + └── base_link_stabilized imu_to_tf + └── base_link + ├── lidar_link robot description (static) + └── imu_link +``` + +| Edge | Published by | +|---|---| +| `map` → `icp_odom` | `rtabmap` | +| `icp_odom` → `base_link_stabilized` | `icp_odometry` | +| `base_link_stabilized` → `base_link` | **this node**, from the IMU orientation | +| `base_link` → `lidar_link`, `imu_link` | your robot description, static | + +Note what `icp_odometry` publishes: because `guess_frame_id` is set it broadcasts the *correction* `icp_odom` → `base_link_stabilized` rather than `icp_odom` → `base_link`. That is what makes the two nodes compose — the stabilized frame slots into the chain and every frame keeps exactly one parent. Without `guess_frame_id` the odometry would publish straight to `base_link` and fight this node over it. + +Only rotation is compensated, and the two errors behave differently over a sweep: + +| Error | How it scales | Worst when | +|---|---|---| +| Rotation, corrected here | grows with range | turning fast, looking far | +| Translation, left over | same at every range, grows with speed | driving fast, looking close | + +Moving slowly, or looking far, the leftover translation stays under the lidar's own range noise and can be ignored. Fast and close it is the bigger of the two, and it shifts the cloud rather than blurring it, so it turns into odometry drift. + +Once something publishes a real `odom` → `base_link` — wheel or visual odometry, or an EKF such as [`robot_localization`](https://github.com/cra-ros-pkg/robot_localization) fusing that same IMU with wheel odometry — point both `fixed_frame_id` and `guess_frame_id` at `odom` instead and drop this node. Translation then gets compensated too. + +### A rotation guess for visual odometry + +The same stabilized frame is useful to a camera, for a different reason. Visual odometry predicts where each feature from the previous frame should land in the current one and searches around that prediction; on a fast rotation the prediction is far off, matches are lost and odometry breaks exactly when the motion is hardest. + +An IMU fixes the prediction. Run this node as above to publish `base_link_stabilized` → `base_link`, then hand that frame to the odometry as its guess: + +```python +Node( + package='rtabmap_odom', executable='rgbd_odometry', + parameters=[{'frame_id': 'base_link', + 'guess_frame_id': 'base_link_stabilized'}], + remappings=[('rgb/image', '/camera/color/image_raw'), + ('depth/image', '/camera/depth/image_rect_raw'), + ('rgb/camera_info', '/camera/color/camera_info')]) +``` + +With [`rtabmap_launch`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_launch) the same thing is one argument: + +```bash +ros2 launch rtabmap_launch rtabmap.launch.py odom_guess_frame_id:=base_link_stabilized +``` + +The TF chain is the one from the previous section with `rgbd_odometry` in place of `icp_odometry`; it publishes the same `odom` → `base_link_stabilized` correction, so the frames still form one tree. + +A rotation-only guess is enough here, because feature matching cares about where things appear, not where they are. Turning the camera slides every feature across the image by the same amount, near or far. Moving it slides them too, but far less, and less the further away they are — generally little enough to stay inside the window the matcher searches. So rotation is the part a guess has to get right, and that is exactly what the IMU supplies. It is also why an IMU far too drifty to give you a *pose* still makes a good guess: only the rotation over a single frame interval is being used, long before drift has time to accumulate. + +## Subscribed Topics + +| Topic | Type | Description | +|---|---|---| +| `imu/data` | [`sensor_msgs/msg/Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Only `orientation` and `header` are read. The subscription has a queue depth of 1; its reliability comes from the `qos` parameter. | + +## Published Topics + +None. The node only broadcasts transforms. + +## Published Transforms + +| Transform | Description | +|---|---| +| `fixed_frame_id` → IMU frame | Broadcast when `base_frame_id` is empty. The child frame is the `header.frame_id` of the incoming message. | +| `fixed_frame_id` → `base_frame_id` | Broadcast when `base_frame_id` is set. The orientation is re-expressed in the base frame first, see [Mounting offset](#mounting-offset). | + +The transform carries a rotation only; its translation is always zero. It is stamped with the IMU message's stamp, not the current time. + +## Required Transforms + +| Transform | Description | +|---|---| +| `base_frame_id` → IMU frame | Only when `base_frame_id` is set and differs from the IMU's `header.frame_id`. This is the fixed mounting of the IMU on the robot, normally published by your robot description. | + +## Parameters + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `fixed_frame_id` | `string` | `"odom"` | Parent frame of the broadcast transform. | +| `base_frame_id` | `string` | `""` | Frame to report the orientation in. Empty broadcasts the IMU frame itself, which is the cheapest option when nothing else needs the base frame oriented. | +| `qos` | `int` | `0` | Reliability of the `imu/data` subscription: `0` system default, `1` reliable, `2` best effort. Must match the publisher, or no message arrives. | +| `wait_for_transform_duration` | `double` | `0.1` | Seconds to wait for the `base_frame_id` → IMU transform before giving up on a message. Only used when `base_frame_id` is set. | + +## Mounting offset + +When `base_frame_id` is set, the node looks up the mounting transform `base_frame_id` → IMU frame and re-expresses the orientation in the base frame. The **yaw of the mounting is deliberately discarded**: only its roll and pitch are applied. + +That is what you want from an absolute orientation source. An IMU bolted on facing sideways still measures the same absolute heading as one facing forward, so its yaw must reach the base frame untouched; its roll and pitch, on the other hand, do have to be rotated into the base frame to be meaningful. + +A message is **dropped** — logged as an error, nothing broadcast — if that mounting transform is not available within `wait_for_transform_duration`. + +## Notes + +Only one node may publish a given TF edge. If odometry is already publishing `odom` → `base_link`, do not point this node at the same pair — give it a frame of its own, as in [the stabilized frame above](#a-stabilized-frame-for-lidar-deskewing-and-odometry), or leave `base_frame_id` empty. Two publishers on one edge make the transform flicker between them. + +The node does not integrate or filter anything — whatever orientation the message carries is what gets broadcast. See [When the IMU has no orientation](#when-the-imu-has-no-orientation) if your driver does not estimate one. diff --git a/rtabmap_util/doc/lidar_deskewing.md b/rtabmap_util/doc/lidar_deskewing.md new file mode 100644 index 00000000..c2ecea94 --- /dev/null +++ b/rtabmap_util/doc/lidar_deskewing.md @@ -0,0 +1,95 @@ +# lidar_deskewing + +Removes the motion distortion from a lidar scan. + +A spinning lidar takes tens of milliseconds to complete a sweep, and on a moving robot every point in that sweep is measured from a slightly different pose. The result is a *skewed* cloud: straight walls come out bent, and registration against it drifts. + +This node uses TF to find where the sensor actually was when each point was taken, and moves every point into the pose at the start of the sweep. A straight wall comes back straight. + +## Usage + +```bash +ros2 run rtabmap_util lidar_deskewing --ros-args \ + -p fixed_frame_id:=odom \ + -r input_cloud:=/velodyne_points +``` + +```python +ComposableNode( + package='rtabmap_util', + plugin='rtabmap_util::LidarDeskewing', + name='lidar_deskewing', + parameters=[{'fixed_frame_id': 'odom'}], + remappings=[('input_cloud', '/velodyne_points')]) +``` + +## Subscribed Topics + +Connect one of the two. + +| Topic | Type | Description | +|---|---|---| +| `input_cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Must carry a **per-point time channel**, see [Requirements](#requirements). | +| `input_scan` | [`sensor_msgs/msg/LaserScan`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/LaserScan.html) | Per-point times come from `time_increment`. | + +## Published Topics + +Output names are derived from the **resolved** input names, so remapping the input moves the output with it. With `input_cloud` remapped to `/velodyne_points` the output is `/velodyne_points/deskewed`. + +| Topic | Type | Description | +|---|---|---| +| `/deskewed` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The deskewed cloud, same frame and stamp as the input. | +| `/deskewed` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | A `LaserScan` cannot represent a deskewed sweep — the points no longer lie on a regular angular grid — so the scan input also produces a cloud. | + +## Required Transforms + +| Transform | Description | +|---|---| +| `fixed_frame_id` → sensor frame, across the sweep | Must be available for the whole span of the sweep, at both its first and last stamp. | + +## Parameters + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `fixed_frame_id` | `string` | `""` | **Required.** Frame the motion is measured against, usually `odom`. | +| `wait_for_transform` | `double` | `0.01` | Seconds to wait for the transforms spanning the sweep. Raise it if odometry lags the lidar. | +| `slerp` | `bool` | `false` | Interpolate between the poses at the start and end of the sweep instead of looking up TF per point. Much cheaper, and accurate enough at constant velocity. | +| `queue_size` | `int` | `1` | Queue depth of the input subscriptions. | +| `qos` | `int` | `0` | Reliability of the input subscriptions: `0` system default, `1` reliable, `2` best effort. | + +## Requirements + +For `input_cloud`, the cloud **must have a per-point time field**. Without one the node cannot know when each point was taken and cannot deskew. + +The field has to be named `t`, `time`, `stamps` or `timestamp` — anything else is not recognized, whatever it contains. Its type decides how the value is read: + +| Type | Meaning | +|---|---| +| `uint32` | nanoseconds since the cloud's own stamp | +| `float32` | seconds since the cloud's own stamp | +| `float64` | an absolute timestamp; seconds, milliseconds, microseconds and nanoseconds are told apart by magnitude | + +Common drivers that satisfy this out of the box: **Ouster** (`t`), **Velodyne** (`time`), **RoboSense** (`timestamp`) and **Livox** (`timestamp`). Livox needs its PointCloud2 output rather than the default `CustomMsg` format, which this node cannot subscribe to at all. + +To check what your driver actually publishes: + +```bash +ros2 topic echo /your/points --field fields --once +``` + +If none of the four names is in that list, look for a driver option to add per-point timestamps before anything else. + +The `fixed_frame_id` → sensor transform must cover the whole sweep, which means **odometry has to be at least as recent as the lidar**. If it lags, raise `wait_for_transform`. + +## Behavior when TF is missing + +The two inputs deliberately differ: + +- A **cloud** is republished **unchanged** with a warning. Deskewing is an improvement, not a precondition, and dropping frames would break the pipeline behind it. +- A **scan** is **dropped**, because converting it to a cloud is only worth doing as part of deskewing. + +## Notes + +Deskewing matters most when rotating: at 1 rad/s a 100 ms sweep spans nearly 6°, and the far end of the scan is badly misplaced. Pure translation at walking speed is a few centimeters, which matters at close range. + +Put this node before ICP odometry or [point_cloud_assembler](point_cloud_assembler.md), not after. Anything registering against a skewed cloud has already paid for the distortion. diff --git a/rtabmap_util/doc/map_assembler.md b/rtabmap_util/doc/map_assembler.md new file mode 100644 index 00000000..fe0fb62b --- /dev/null +++ b/rtabmap_util/doc/map_assembler.md @@ -0,0 +1,109 @@ +# map_assembler + +Rebuilds the global maps from RTAB-Map's graph, in a separate process. + +RTAB-Map publishes its graph and the per-node sensor data on `mapData`; turning that into a point cloud, an occupancy grid or an octomap costs real CPU. This node does that work, so the SLAM node does not have to and the mapping loop stays responsive. + +It also lets you produce maps RTAB-Map is not currently configured to publish, or several differently-configured maps at once, without restarting SLAM. + +The assembling itself is done by `MapsManager`, which is shared with `rtabmap_slam` — the outputs and every `Grid/*` parameter behave identically in both. + +## Usage + +```bash +ros2 run rtabmap_util map_assembler --ros-args \ + -p Grid/CellSize:=0.05 -p Grid/RangeMax:=8.0 -p cloud_output_voxelized:=true +``` + +```python +ComposableNode( + package='rtabmap_util', + plugin='rtabmap_util::MapAssembler', + name='map_assembler', + parameters=[{'Grid/CellSize': '0.05', 'Grid/RangeMax': '8.0', + 'cloud_output_voxelized': True}]) +``` + +## Subscribed Topics + +| Topic | Type | Description | +|---|---|---| +| `mapData` | [`rtabmap_msgs/msg/MapData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/MapData.html) | The graph, plus the sensor data of any newly added node. Published by `rtabmap`. | + +## Published Topics + +Everything is published only when subscribed, and — by default — **latched**, so a subscriber joining late immediately receives the current map. + +| Topic | Type | Description | +|---|---|---| +| `cloud_map` | [`PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Ground and obstacles together. | +| `cloud_ground` | `PointCloud2` | Ground only, colored green. | +| `cloud_obstacles` | `PointCloud2` | Obstacles only, colored red. | +| `map` | [`OccupancyGrid`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/OccupancyGrid.html) | The 2D occupancy grid, the one navigation wants. | +| `grid_prob_map` | `OccupancyGrid` | The same grid as occupancy probabilities rather than free/occupied/unknown. | +| `octomap_occupied_space`, `octomap_obstacles`, `octomap_ground`, `octomap_empty_space`, `octomap_global_frontier_space` | `PointCloud2` | Octomap contents, one cloud per category. Requires RTAB-Map built with OctoMap. | +| `octomap_grid` | `OccupancyGrid` | The octomap projected to 2D. | +| `octomap_binary`, `octomap_full` | [`Octomap`](https://docs.ros.org/en/jazzy/p/octomap_msgs/msg/Octomap.html) | The tree itself, for `octovis` or other octomap consumers. Serialized as a **`ColorOcTree`**, see [Octomap tree type](#octomap-tree-type). | +| `elevation_map` | [`GridMap`](https://github.com/ANYbotics/grid_map/blob/master/grid_map_msgs/msg/GridMap.msg) | Elevation map. Requires RTAB-Map built with `grid_map`. | + +## Services + +| Service | Type | Description | +|---|---|---| +| `~/reset` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Drop the cached nodes and every assembled map. | +| `~/octomap_binary` | [`octomap_msgs/srv/GetOctomap`](https://docs.ros.org/en/jazzy/p/octomap_msgs/srv/GetOctomap.html) | Build and return the octomap on demand. | +| `~/octomap_full` | `octomap_msgs/srv/GetOctomap` | The same, with occupancy probabilities. | + +## Parameters + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `initialize_from_rtabmap_timeout` | `double` | `5.0` | Seconds to wait for rtabmap's `get_map_data` service on start-up, which is how the node catches up on a map that already exists. Set to `0` to skip the call and subscribe immediately, which is what you want when `map_assembler` starts *before* rtabmap. | +| `rtabmap` | `string` | `"rtabmap"` | Name of the rtabmap node whose `get_map_data` service to call. | +| `regenerate_local_grids` | `bool` | `false` | Discard the occupancy grids stored with each node and rebuild them from the raw sensor data. Use it to change `Grid/*` parameters on an existing map without re-running SLAM. Costs CPU per node. | +| `config_path` | `string` | `""` | An RTAB-Map `.ini` file to load parameters from, instead of listing them individually. | + +**Map assembly**, from `MapsManager` + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `latch` | `bool` | `true` | Publish with transient-local durability so late subscribers get the current map. | +| `map_filter_radius` | `double` | `0.0` | Skip nodes closer together than this, in meters. A cheap way to thin a dense graph. `0` disables. | +| `map_filter_angle` | `double` | `30.0` | With `map_filter_radius`, nodes are only merged if they also differ by less than this angle, in degrees. | +| `map_always_update` | `bool` | `false` | **No effect here**, see below. | +| `map_empty_ray_tracing` | `bool` | `true` | **No effect here**, see below. | +| `map_cleanup` | `bool` | `true` | Free the cached clouds when nobody is subscribed. | +| `cloud_output_voxelized` | `bool` | `true` | Voxelize the assembled clouds at `Grid/CellSize`. | +| `cloud_subtract_filtering` | `bool` | `false` | Drop points that duplicate ones already in the map. Slower, smaller output. | +| `cloud_subtract_filtering_min_neighbors` | `int` | `2` | Neighbors needed for a point to count as a duplicate. | +| `octomap_tree_depth` | `int` | `16` | Depth the octomap clouds are generated at. Lower means coarser and faster. Maximum 16. | + +`map_always_update` and `map_empty_ray_tracing` are declared because they come with `MapsManager`, but neither does anything in this node. Both only apply to the *current*, not-yet-committed node, which `MapsManager` identifies by the pose id `0`. That node is assembled inside `rtabmap_slam`'s `rtabmap` node from its live sensor data and is never published on `mapData`, so the graph reaching `map_assembler` only ever contains committed nodes. Set them on the `rtabmap` node instead, where they do apply. + +Every RTAB-Map **`Grid/*`**, **`GridGlobal/*`**, **`StereoBM/*`** and **`StereoSGBM/*`** parameter is also exposed, all documented in RTAB-Map's [parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html). The split between the first two is worth knowing: **`Grid/*`** decides how each node's local grid is built from its sensor data — the same segmentation [obstacles_detection](obstacles_detection.md#parameters) does, and the parameters listed there apply here too — while **`GridGlobal/*`** decides how those local grids are merged into the global map, so it covers the map's minimum size, its occupancy threshold, and how far the graph must move before the whole map is rebuilt. + +## Octomap tree type + +RTAB-Map keeps a color per voxel, so the tree it publishes on `octomap_binary` and `octomap_full` reports its `id` as **`ColorOcTree`**, not the plain `OcTree` many examples assume. + +That is deliberate and interoperable: `octomap_msgs::binaryMsgToMap()` and `fullMsgToMap()` branch on that `id` and hand you back an `octomap::ColorOcTree`, and `octovis` opens it without complaint. What does break is code that assumes the other branch: + +```cpp +octomap::AbstractOcTree * tree = octomap_msgs::binaryMsgToMap(msg); +octomap::OcTree * octree = dynamic_cast(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. + +## Start-up + +`map_assembler` normally starts alongside rtabmap and builds its maps from the `mapData` messages that follow. If it starts **after** rtabmap it would miss everything already mapped, so on start-up it calls rtabmap's `get_map_data` service once to fetch the existing map. + +That call blocks the subscription to `mapData` until it returns or times out, which is wasted time when rtabmap is not running yet. Set `initialize_from_rtabmap_timeout` to `0` in that case. + +If rtabmap is started later in localization mode, call its `publish_maps` service with `graph_only=false` so `map_assembler` receives the data it missed. + +## Notes + +`regenerate_local_grids` is the parameter to reach for when a recorded map's grids were built with settings you now want to change. Without it, `Grid/*` changes only affect nodes added from then on, because each node's grid is stored with it. diff --git a/rtabmap_util/doc/obstacles_detection.md b/rtabmap_util/doc/obstacles_detection.md new file mode 100644 index 00000000..4fcadbfa --- /dev/null +++ b/rtabmap_util/doc/obstacles_detection.md @@ -0,0 +1,140 @@ +# obstacles_detection + +Segments a point cloud into ground and obstacles. + +The node takes a cloud, works out which points belong to the floor and which stick up from it, and publishes the two apart. Downstream that feeds navigation: obstacles into a costmap, ground into a traversability check. + +The segmentation is RTAB-Map's own [`LocalGridMaker`](https://introlab.github.io/rtabmap/api/latest/classrtabmap_1_1LocalGridMaker.html), so it is configured through the same `Grid/*` parameters as RTAB-Map itself and produces the same result the SLAM node would. + +## Usage + +```bash +ros2 run rtabmap_util obstacles_detection --ros-args \ + -r cloud:=/camera/cloud \ + -p frame_id:=base_link \ + -p Grid/MaxObstacleHeight:=2.0 +``` + +```python +ComposableNode( + package='rtabmap_util', + plugin='rtabmap_util::ObstaclesDetection', + name='obstacles_detection', + parameters=[{'frame_id': 'base_link', 'Grid/MaxObstacleHeight': '2.0'}], + remappings=[('cloud', '/camera/cloud')]) +``` + +### Feeding a nav2 costmap + +The usual reason to run this node: nav2's costmap wants to be told separately what is floor and what is in the way. A depth camera gives neither directly, so the chain is depth image → cloud → segmented cloud → costmap. + +[point_cloud_xyz](point_cloud_xyz.md) projects the depth image, with `decimation` and `voxel_size` set to keep the cost down, and this node splits the result: + +```python +Node( + package='rtabmap_util', executable='point_cloud_xyz', + parameters=[{'decimation': 2, 'max_depth': 3.0, 'voxel_size': 0.02}], + remappings=[('depth/image', '/camera/depth/image_raw'), + ('depth/camera_info', '/camera/camera_info'), + ('cloud', '/camera/cloud')]), + +Node( + package='rtabmap_util', executable='obstacles_detection', + parameters=[{'frame_id': 'base_link'}], + remappings=[('cloud', '/camera/cloud'), + ('ground', '/camera/ground'), + ('obstacles', '/camera/obstacles')]), +``` + +The two outputs then become two observation sources on the costmap's voxel layer: + +```yaml +local_costmap: + local_costmap: + ros__parameters: + plugins: ["voxel_layer", "inflation_layer"] + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: ground obstacles + ground: + topic: /camera/ground + data_type: "PointCloud2" + max_obstacle_height: 0.4 + marking: False # the floor is not an obstacle... + clearing: True # ...but seeing it proves the space is free + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + obstacles: + topic: /camera/obstacles + data_type: "PointCloud2" + max_obstacle_height: 0.4 + marking: True + clearing: True + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 +``` + +The `marking`/`clearing` split is the whole point. Ground points only clear: they tell the costmap that the space the camera looked through is free, without writing an obstacle at floor level. Obstacle points do both, so an obstacle that moves away is cleared by the next observation instead of lingering. + +Feeding the raw cloud in as a single source cannot do this — every floor point would mark an obstacle and the robot would refuse to move. Working from [`turtlebot3_rgbd.launch.py`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py) and its [nav2 parameters](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/params/turtlebot3_rgbd_nav2_params.yaml) will save some time. + +## Subscribed Topics + +| Topic | Type | Description | +|---|---|---| +| `cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The cloud to segment, in any frame that TF can relate to `frame_id`. | + +## Published Topics + +| Topic | Type | Description | +|---|---|---| +| `ground` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Points classified as floor. In the **input** cloud's frame. | +| `obstacles` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Points classified as obstacles. In the **input** cloud's frame. | +| `proj_obstacles` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The obstacles flattened onto the ground plane, in `frame_id`. This is the 2D footprint a planar costmap wants. | + +Each output is computed only if something is subscribed to it. + +## Required Transforms + +| Transform | Description | +|---|---| +| `frame_id` → cloud frame | Where the sensor sits on the robot. Segmentation happens in `frame_id`, so this is what makes "up" meaningful. | +| `map_frame_id` → `frame_id` | Only when `map_frame_id` is set. See [Levelling on a slope](#levelling-on-a-slope). | + +## Parameters + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `frame_id` | `string` | `"base_link"` | The robot frame. Its xy plane is the ground plane the segmentation works against. | +| `map_frame_id` | `string` | `""` | See [Levelling on a slope](#levelling-on-a-slope). | +| `wait_for_transform` | `double` | `0.2` | Seconds to wait for a transform before dropping the cloud. | +| `qos` | `int` | `0` | Reliability of the subscription and the publishers: `0` system default, `1` reliable, `2` best effort. | + +Every RTAB-Map **`Grid/*`** parameter is also exposed as a ROS parameter of this node, and they are what actually control the segmentation. They are documented in RTAB-Map's [parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html). + +The one to decide first is `Grid/NormalsSegmentation`, which picks between two ways of finding the ground. Left on, it is segmented from surface normals, which copes with slopes and steps. Turned off, it is a plain height threshold: much cheaper, and exact when the floor really is flat, but `Grid/MaxGroundHeight` then has to be set, since it *is* that threshold. + +## Levelling on a slope + +Segmentation is done in `frame_id`, so if the robot is pitched or rolled — on a ramp, or with a suspension that dips — the ground plane tilts with it and the floor ahead can be classified as an obstacle. + +Setting `map_frame_id` makes the node take the robot's pose in that frame and apply its **roll and pitch**, so segmentation happens against a level plane rather than the robot's own tilt. + +Height is a separate matter: the robot's **z** in the map frame is ignored unless `Grid/MapFrameProjection` is also set to `true`. That is usually what you want — a height threshold should be measured from the robot, not from an arbitrary map origin — but if you are mapping a multi-level building and want the thresholds relative to the map, enable it. + +## Notes + +If `obstacles` comes back empty on an obviously cluttered scene, check `Grid/RangeMax` first. It is **not unlimited by default**, and everything beyond it is discarded before segmentation even runs. + +The second thing to check is `Grid/MinClusterSize` against your cloud density. A sparse lidar can produce clusters smaller than the default, in which case every obstacle is thrown away as noise. Either lower it or raise `Grid/ClusterRadius`. diff --git a/rtabmap_util/doc/point_cloud_aggregator.md b/rtabmap_util/doc/point_cloud_aggregator.md new file mode 100644 index 00000000..a310c448 --- /dev/null +++ b/rtabmap_util/doc/point_cloud_aggregator.md @@ -0,0 +1,97 @@ +# point_cloud_aggregator + +Merges one cloud from each of several sensors into a single cloud. + +A robot with two or three lidars, or a ring of depth cameras, produces one cloud per sensor. This node waits for a matching set, transforms them all into a common frame and publishes a single cloud, so everything downstream sees the robot's full field of view as one measurement. + +The sensors do not have to fire together: the clouds are matched by nearest stamp, and setting `fixed_frame_id` compensates for the robot having moved between them. See [Sensors that do not fire together](#sensors-that-do-not-fire-together). + +It combines **several sensors into one frame**. To combine **one sensor over many frames**, use [point_cloud_assembler](point_cloud_assembler.md). + +## Usage + +```bash +ros2 run rtabmap_util point_cloud_aggregator --ros-args \ + -p count:=3 -p frame_id:=base_link -p fixed_frame_id:=odom \ + -r cloud1:=/lidar_front/points/deskewed \ + -r cloud2:=/lidar_left/points/deskewed \ + -r cloud3:=/lidar_right/points/deskewed +``` + +```python +ComposableNode( + package='rtabmap_util', + plugin='rtabmap_util::PointCloudAggregator', + name='point_cloud_aggregator', + parameters=[{'count': 3, 'frame_id': 'base_link', 'fixed_frame_id': 'odom'}], + remappings=[('cloud1', '/lidar_front/points/deskewed'), + ('cloud2', '/lidar_left/points/deskewed'), + ('cloud3', '/lidar_right/points/deskewed')]) +``` + +With 2D or 3D lidars, feed the aggregator **deskewed** clouds: run a [lidar_deskewing](lidar_deskewing.md) node per sensor first, which is where the `/deskewed` topics above come from. For a 2D lidar publishing `LaserScan` that node is needed regardless — this one only takes `PointCloud2`, and `lidar_deskewing` converts to one as it deskews. + +The two nodes correct different motions and you generally want both. Deskewing removes the distortion *within* each sweep, point by point, because a spinning lidar measures each point from a slightly different pose. `fixed_frame_id` here places whole clouds relative to each other, because the sensors did not fire at the same instant. Merging raw sweeps only merges their distortions. + +## Subscribed Topics + +| Topic | Type | Description | +|---|---|---| +| `cloud1` … `cloud4` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Only the first `count` are subscribed. | + +## Published Topics + +| Topic | Type | Description | +|---|---|---| +| `combined_cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | In `frame_id`, or in `cloud1`'s frame if `frame_id` is empty. Stamped with `cloud1`. | + +Nothing is computed unless `combined_cloud` has a subscriber. + +## Required Transforms + +| Transform | Description | +|---|---| +| target frame → each cloud's frame | Where each sensor sits. The target is `frame_id`, or `cloud1`'s frame when that is empty. | +| `fixed_frame_id` → each cloud's frame, at each stamp | Only when `fixed_frame_id` is set. See [Sensors that do not fire together](#sensors-that-do-not-fire-together). | + +## Parameters + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `count` | `int` | `2` | How many clouds to combine, 2 to 4. Determines how many `cloudN` topics are subscribed. | +| `frame_id` | `string` | `""` | Frame to express the combined cloud in. Empty uses `cloud1`'s frame, which is the cheapest option since that cloud then needs no transform, but see [Converting back to a LaserScan](#converting-back-to-a-laserscan). | +| `fixed_frame_id` | `string` | `""` | Frame to compensate motion against, usually `odom`. See below. | +| `approx_sync` | `bool` | `true` | Match the clouds by nearest stamp. Set false when the sensors are hardware-triggered and share exact stamps. | +| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. A good guard against silently merging stale data. | +| `wait_for_transform` | `double` | `0.1` | Seconds to wait for a transform before dropping the set. | +| `xyz_output` | `bool` | `false` | Strip everything but XYZ from the output. Useful when the inputs disagree on their extra fields. | +| `topic_queue_size` | `int` | `1` | Queue depth of each input subscription. | +| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. | +| `qos` | `int` | `0` | Reliability of the cloud subscriptions: `0` system default, `1` reliable, `2` best effort. | + +## Converting back to a LaserScan + +Some consumers still want a 2D `LaserScan` — `slam_toolbox`, `amcl`, or a costmap layer configured for one. [`pointcloud_to_laserscan`](https://docs.ros.org/en/jazzy/p/pointcloud_to_laserscan/) flattens the combined cloud into one: + +```python +Node( + package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node', + parameters=[{'target_frame': 'base_link', 'min_height': -0.1, 'max_height': 0.5}], + remappings=[('cloud_in', '/combined_cloud')]) +``` + +**Set `frame_id` to the robot center when you do this.** A `LaserScan` is a set of ranges measured outward from one origin, so the conversion is only meaningful about a point the consumer thinks of as the robot. Leaving `frame_id` empty puts the combined cloud in `cloud1`'s frame — a sensor bolted somewhere on the edge of the robot — and every range then comes out measured from that corner. With three lidars merged, the result is a scan centerd on whichever one happened to be `cloud1`. + +One case where you should *not* combine first: if the clouds are only going into a nav2 costmap, give nav2 each sensor as its own observation source instead. A costmap clears free space by ray tracing outward from where the observation was made, and it takes that origin from the cloud's own frame. Merge everything into one cloud at `base_link` and every point looks as though it were seen from the robot center, so space gets cleared along lines no sensor ever looked down — including straight through whatever the other sensors can see. + +## Sensors that do not fire together + +With `approx_sync` the clouds carry different stamps, and on a moving robot each was captured from a different pose. Merging them by their static mounting transforms alone smears the result. + +Setting `fixed_frame_id` fixes that: the node asks TF where each sensor was at its own stamp, relative to that fixed frame, and places each cloud accordingly. Two lidars 30 ms apart on a robot turning at 1 rad/s are nearly 2° apart — clearly visible as a doubled wall. + +Leave it empty only when the sensors are genuinely synchronized, or when the robot is stationary. + +## Diagnostics + +The node publishes to `/diagnostics` and warns if no combined cloud has been produced for a while — usually a sign that one of the `cloudN` topics is silent, or that the stamps are too far apart to sync. diff --git a/rtabmap_util/doc/point_cloud_assembler.md b/rtabmap_util/doc/point_cloud_assembler.md new file mode 100644 index 00000000..55caefbf --- /dev/null +++ b/rtabmap_util/doc/point_cloud_assembler.md @@ -0,0 +1,162 @@ +# point_cloud_assembler + +Accumulates the clouds of one sensor over time into a denser cloud. + +A single sweep is sparse, or narrow, or both. This node keeps the recent ones, places each where the sensor was when it was captured, and publishes the union. + +That is used for two quite different things. With a **narrow field of view** — a depth camera reduced to a fake scan, say — accumulating a second's worth of sweeps as the robot moves is what makes the sensor usable for SLAM at all. With a 3D lidar it is about **enriching what already works**: each node gets a denser, less occluded cloud, which registers better and puts many more points in the database, so an offline export later has the resolution to be worth having. + +It combines **one sensor over many frames**. To combine **several sensors into one frame**, use [point_cloud_aggregator](point_cloud_aggregator.md) — or, if you want them merely accumulated rather than matched into sets, remap them all onto this node's `cloud` topic. Nothing stops several publishers sharing it, and each cloud is placed by its own stamp and frame like any other; the publish trigger then covers them together — `max_clouds` counts across all the sensors, and a given `assembling_time` gathers correspondingly more clouds. + +## Usage + +Assemble 10 sweeps, using TF for the poses: + +```bash +ros2 run rtabmap_util point_cloud_assembler --ros-args \ + -r cloud:=/velodyne_points/deskewed \ + -p max_clouds:=10 -p fixed_frame_id:=odom -p voxel_size:=0.05 +``` + +```python +ComposableNode( + package='rtabmap_util', + plugin='rtabmap_util::PointCloudAssembler', + name='point_cloud_assembler', + parameters=[{'max_clouds': 10, 'fixed_frame_id': 'odom', 'voxel_size': 0.05}], + remappings=[('cloud', '/velodyne_points/deskewed')]) +``` + +With a lidar, feed it **deskewed** clouds from a [lidar_deskewing](lidar_deskewing.md) node rather than the driver's raw output: accumulating skewed sweeps accumulates their distortion too. For a 2D lidar publishing `LaserScan` that node is needed regardless — this one only takes `PointCloud2`, and `lidar_deskewing` converts to one as it deskews. + +### Denser clouds for SLAM, and keeping every point + +From [`lidar3d_assemble.launch.py`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_examples/launch/lidar3d_assemble.launch.py). Here the input is a real 3D lidar, and assembling buys both resolution and coverage: a node built from a second of sweeps is denser between the rings and sees around what a single sweep was occluded by, so it registers better. + +It also decides how much of the lidar survives. RTAB-Map stores one cloud per node and updates at around 1 Hz, while the lidar and odometry run at 10 — and running the mapping node at 10 Hz is not practical. Nine sweeps in ten therefore never reach the database. The `assembling_time: 1.0` below hands each node the whole second instead, so **nothing is thrown away**: the database keeps every point the lidar returned, which is what makes this the approach for survey scanning and a full-resolution offline export. + +```python +Node( + package='rtabmap_util', executable='point_cloud_assembler', + parameters=[{'assembling_time': 1.0, + 'fixed_frame_id': ''}], # '' selects the odom topic + remappings=[('cloud', '/lidar/points/deskewed'), + ('odom', 'icp_odom')]), +``` + +Note `fixed_frame_id: ''`. Clearing it switches the node from TF to the `odom` topic, pairing each cloud with the exact odometry message that goes with it rather than an interpolated TF lookup — see [Where the poses come from](#where-the-poses-come-from). Feeding the result to `rtabmap` as `scan_cloud` means the assembled cloud, not the raw sweep, is what gets stored. + +### Widening a narrow field of view + +From [`turtlebot3_rgbd_fake_scan.launch.py`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_fake_scan.launch.py). A depth camera sees perhaps 60° across, and [`depthimage_to_laserscan`](https://docs.ros.org/en/jazzy/p/depthimage_to_laserscan/) reduces that to a fake scan thinner still. One of those is too little to localize against; twenty of them, accumulated as the robot drives and turns, cover a useful arc. + +```python +Node( + package='rtabmap_util', executable='point_cloud_assembler', + parameters=[{'max_clouds': 20, + 'circular_buffer': True, + 'linear_update': 0.3, + 'angular_update': 0.5, + 'voxel_size': 0.05, + 'frame_id': 'base_link'}], + remappings=[('cloud', '/camera/scan/deskewed')]), +``` + +`circular_buffer` is what makes this work as a live input: the window rolls, so every incoming scan produces a full assembled cloud rather than one per twenty. `linear_update` and `angular_update` stop a stationary robot from filling the buffer with twenty copies of the same view, which would leave it with nothing but the current scan the moment it moved off again. + +The cloud goes to `rtabmap` as `scan_cloud`, with `scan_cloud_is_2d` set since the points all came from one row of pixels. + +## Subscribed Topics + +| Topic | Type | Description | +|---|---|---| +| `cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The sweeps to accumulate. Ideally deskewed, see [Usage](#usage). | +| `odom` | [`nav_msgs/msg/Odometry`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Odometry.html) | Only when `fixed_frame_id` is empty. See [Where the poses come from](#where-the-poses-come-from). | +| `odom_info` | [`rtabmap_msgs/msg/OdomInfo`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/OdomInfo.html) | Only when `subscribe_odom_info` is true. | + +## Published Topics + +| Topic | Type | Description | +|---|---|---| +| `assembled_cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | In `frame_id` if set, otherwise the frame of the newest cloud. Stamped with the newest cloud. | + +Nothing is accumulated unless `assembled_cloud` has a subscriber. + +## Required Transforms + +| Transform | Description | +|---|---| +| `fixed_frame_id` → cloud frame, at each stamp | Only in TF mode, i.e. when `fixed_frame_id` is set. | +| `frame_id` → cloud frame | Only when `frame_id` is set. | + +## Parameters + +**What triggers a publish** — set exactly one of these + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `max_clouds` | `int` | `0` | Publish once this many clouds have been collected. | +| `assembling_time` | `double` | `0.0` | Publish once this many seconds have been collected. | + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `circular_buffer` | `bool` | `false` | Keep a rolling window instead of clearing after each publish, so a full assembled cloud is published for **every** input rather than one in `max_clouds`. Costs more, gives smooth output. | +| `skip_clouds` | `int` | `0` | Drop this many input clouds between the ones kept. | +| `linear_update` | `double` | `0.0` | Only accumulate a cloud if the sensor has moved this far, in meters, since the last one kept. `0` disables. | +| `angular_update` | `double` | `0.0` | Same for rotation, in radians. `0` disables. | + +**Poses** + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `fixed_frame_id` | `string` | `"odom"` | Frame the sweeps are placed in, via TF. **Set it to `""` to use the `odom` topic instead.** | +| `frame_id` | `string` | `""` | Frame to express the output in. Empty uses the newest cloud's frame. | +| `wait_for_transform` | `double` | `0.1` | Seconds to wait for a transform before dropping a cloud. | +| `subscribe_odom_info` | `bool` | `false` | Keep only the clouds odometry marked as keyframes. Needs the `odom` topic mode, see [Following odometry's keyframes](#following-odometrys-keyframes). | + +**Filtering** + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `range_min` | `double` | `0.0` | Drop points nearer than this to the sensor, in meters. Good for removing the robot itself. `0` disables. | +| `range_max` | `double` | `0.0` | Drop points further than this, in meters. `0` disables. | +| `voxel_size` | `double` | `0.0` | Downsample the assembled cloud to one point per voxel, in meters. `0` disables. Strongly recommended, otherwise the cloud grows linearly with `max_clouds`. | +| `noise_radius` | `double` | `0.0` | Radius outlier removal on the output, in meters. `0` disables. | +| `noise_min_neighbors` | `int` | `5` | Neighbors needed within `noise_radius`. | +| `remove_z` | `bool` | `false` | Flatten the output to 2D by zeroing z. | + +**Plumbing** + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `topic_queue_size` | `int` | `1` | Queue depth of each input subscription. | +| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer, in odom-topic mode. | +| `qos` | `int` | `0` | Reliability of the cloud subscription. | +| `qos_odom` | `int` | value of `qos` | Reliability of the `odom` and `odom_info` subscriptions. | + +## Where the poses come from + +Each sweep has to be placed where the sensor was when it was captured, and there are two ways to get that pose: + +- **TF** (default). `fixed_frame_id` is set, and the node looks the pose up per cloud. Simple, and it works with any odometry source. +- **The `odom` topic**. Set `fixed_frame_id` to `""` and the node synchronizes each cloud with an `Odometry` message instead. Use this when odometry is not published to TF, or when you need the pose that exactly matches the cloud rather than an interpolated one. + +Because `fixed_frame_id` **defaults to `"odom"`**, the `odom` topic is not subscribed unless you clear it explicitly. Setting `subscribe_odom_info` alone is not enough. + +## Following odometry's keyframes + +With `subscribe_odom_info` the node also takes `odom_info` and keeps a cloud only when that message reports a keyframe was added; the ones in between are dropped. + +This is a better-informed version of `linear_update` and `angular_update`. Those are fixed distances you have to guess at, whereas odometry decides a keyframe from how much of the current scan still matches the last one — `Odom/ScanKeyFrameThr` for ICP, `Odom/KeyFrameThr` for visual odometry. It therefore adapts to the scene, keeping more clouds where the geometry changes quickly and fewer down a featureless corridor, and the assembled cloud ends up built from exactly the frames odometry itself considered distinct. + +It only has an effect in the `odom` topic mode. With `fixed_frame_id` set the node subscribes to the cloud on its own and never sees `odom_info`, so clear `fixed_frame_id` as well — see [Where the poses come from](#where-the-poses-come-from). + +## Notes + +Set `voxel_size`. Without it the assembled cloud is the plain union of every sweep, points and all, and both memory and downstream cost grow with `max_clouds`. A voxel size near the sensor's resolution costs almost no fidelity. + +`circular_buffer` changes the output rate, not just the contents: without it you get one assembled cloud per `max_clouds` inputs, with it you get one per input. + +## Diagnostics + +The node publishes to `/diagnostics` and warns if no assembled cloud has been produced for a while — typically a missing transform or a silent input topic. diff --git a/rtabmap_util/doc/point_cloud_xyz.md b/rtabmap_util/doc/point_cloud_xyz.md new file mode 100644 index 00000000..280d3d9c --- /dev/null +++ b/rtabmap_util/doc/point_cloud_xyz.md @@ -0,0 +1,95 @@ +# point_cloud_xyz + +Projects a depth or disparity image into a point cloud. + +The node takes a depth image and its calibration and produces a [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html), with optional decimation, range limits, voxel and radius filtering, and normal estimation — the same preprocessing RTAB-Map would do internally, done once and shared. + +[`depth_image_proc`](https://docs.ros.org/en/jazzy/p/depth_image_proc/)'s own `point_cloud_xyz` does the bare projection; this node exists for the filtering, and for accepting disparity directly. + +See [point_cloud_xyzrgb](point_cloud_xyzrgb.md) for the colored equivalent. + +## Usage + +```bash +ros2 run rtabmap_util point_cloud_xyz --ros-args \ + -r depth/image:=/camera/depth/image_raw \ + -r depth/camera_info:=/camera/depth/camera_info \ + -p decimation:=4 -p max_depth:=5.0 -p voxel_size:=0.05 +``` + +```python +ComposableNode( + package='rtabmap_util', + plugin='rtabmap_util::PointCloudXYZ', + name='point_cloud_xyz', + parameters=[{'decimation': 4, 'max_depth': 5.0, 'voxel_size': 0.05}], + remappings=[('depth/image', '/camera/depth/image_raw'), + ('depth/camera_info', '/camera/depth/camera_info')]) +``` + +## Subscribed Topics + +The node listens on two independent input sets and uses whichever one is being published. Only one of them should be connected. + +**Depth** + +| Topic | Type | Description | +|---|---|---| +| `depth/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | `32FC1` (meters), `16UC1` (millimeters) or `mono16`. Goes through [`image_transport`](https://docs.ros.org/en/jazzy/p/image_transport/), see `depth_transport` parameter below. | +| `depth/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | | + +**Disparity** + +| Topic | Type | Description | +|---|---|---| +| `disparity/image` | [`stereo_msgs/msg/DisparityImage`](https://docs.ros.org/en/jazzy/p/stereo_msgs/msg/DisparityImage.html) | `32FC1` or `16SC1`. The 16-bit form is fixed point, 16 units per pixel of disparity. | +| `disparity/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | | + +## Published Topics + +| Topic | Type | Description | +|---|---|---| +| `cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | In the frame of the input image, stamped with it. Carries `normal_*` fields when normals are enabled. | + +Nothing is computed unless `cloud` has a subscriber. + +## Parameters + +**Synchronization** + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `approx_sync` | `bool` | `true` | Match image and camera info by nearest stamp. Set false when they are published with identical stamps, which is stricter and cheaper. | +| `approx_sync_max_interval` | `double` | `0.0` | With `approx_sync`, reject pairs further apart than this many seconds. `0` disables the check. | +| `topic_queue_size` | `int` | `1` | Queue depth of each input subscription. | +| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. | +| `qos` | `int` | `0` | Reliability of the image and disparity subscriptions: `0` system default, `1` reliable, `2` best effort. | +| `qos_camera_info` | `int` | value of `qos` | Reliability of the camera info subscriptions. | +| `depth_transport` | `string` | `"raw"` | `image_transport` plugin for `depth/image`, e.g. `compressedDepth`. | + +**Projection and filtering**, applied in this order + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `decimation` | `int` | `1` | Keep one pixel in `decimation`, in each direction. `2` gives a quarter of the points. The image dimensions must divide by it. | +| `roi_ratios` | `string` | `""` | Crop before projecting, as four ratios `"left right top bottom"`, e.g. `"0.1 0.1 0 0.2"`. | +| `min_depth` | `double` | `0.0` | Discard points nearer than this, in meters. `0` disables. | +| `max_depth` | `double` | `0.0` | Discard points further than this, in meters. `0` disables. | +| `voxel_size` | `double` | `0.0` | Downsample to one point per voxel of this size, in meters. `0` disables. | +| `noise_filter_radius` | `double` | `0.0` | Radius outlier removal, in meters. `0` disables. | +| `noise_filter_min_neighbors` | `int` | `5` | Neighbors a point needs within `noise_filter_radius` to survive. | +| `normal_k` | `int` | `0` | Estimate normals from this many nearest neighbors. `0` disables. | +| `normal_radius` | `double` | `0.0` | Estimate normals from all neighbors within this radius, in meters. `0` disables. | +| `filter_nans` | `bool` | `false` | See [Organized output](#organized-output). | + +## Organized output + +By default the cloud stays **organized**: one point per pixel, in image order, with out-of-range points set to NaN rather than removed. That layout is what lets consumers treat the cloud as an image, and it is why a cloud with `max_depth` set still reports the full point count. + +Set `filter_nans` to `true` to drop the invalid points instead. The cloud becomes unorganized and its size reflects what is actually in range — including being empty when nothing is. + +Voxel and radius filtering also produce unorganized clouds, since both remove points. + +## Notes + +`decimation` is by far the cheapest way to cut the cost of everything downstream, and on a depth image it loses very little: neighboring pixels of a surface are nearly redundant. Reach for it before `voxel_size`. diff --git a/rtabmap_util/doc/point_cloud_xyzrgb.md b/rtabmap_util/doc/point_cloud_xyzrgb.md new file mode 100644 index 00000000..158905fb --- /dev/null +++ b/rtabmap_util/doc/point_cloud_xyzrgb.md @@ -0,0 +1,121 @@ +# point_cloud_xyzrgb + +Projects an RGB-D frame, a stereo pair or a disparity image into a colored point cloud. + +The colored counterpart of [point_cloud_xyz](point_cloud_xyz.md): same filtering, same parameters, but every point carries the color of the pixel it came from. It accepts four different input sets, so it can sit at the end of an RGB-D, stereo or disparity pipeline without anything in between. + +## Usage + +```bash +ros2 run rtabmap_util point_cloud_xyzrgb --ros-args \ + -r rgb/image:=/camera/color/image_raw \ + -r depth/image:=/camera/aligned_depth_to_color/image_raw \ + -r rgb/camera_info:=/camera/color/camera_info \ + -p decimation:=4 -p voxel_size:=0.05 +``` + +```python +ComposableNode( + package='rtabmap_util', + plugin='rtabmap_util::PointCloudXYZRGB', + name='point_cloud_xyzrgb', + parameters=[{'decimation': 4, 'voxel_size': 0.05}], + remappings=[('rgb/image', '/camera/color/image_raw'), + ('depth/image', '/camera/aligned_depth_to_color/image_raw'), + ('rgb/camera_info', '/camera/color/camera_info')]) +``` + +## Subscribed Topics + +Four independent input sets; connect exactly one. + +**RGB-D** + +| Topic | Type | Description | +|---|---|---| +| `rgb/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | `mono8`, `mono16`, `bgr8`, `rgb8`, `bgra8`, `rgba8` or `bayer_grbg8`. | +| `depth/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | `32FC1`, `16UC1` or `mono16`, **registered to the color camera**. | +| `rgb/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | | + +**Stereo** + +| Topic | Type | Description | +|---|---|---| +| `left/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Rectified. | +| `right/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Rectified. | +| `left/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | | +| `right/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Its `P(0,3)` carries the baseline. | + +Dense matching is done on the fly with OpenCV's block matcher; see [Stereo matching](#stereo-matching). + +**Disparity** + +| Topic | Type | Description | +|---|---|---| +| `left/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Supplies the color. | +| `disparity` | [`stereo_msgs/msg/DisparityImage`](https://docs.ros.org/en/jazzy/p/stereo_msgs/msg/DisparityImage.html) | `32FC1` or `16SC1`. | +| `left/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | | + +**Bundled** + +| Topic | Type | Description | +|---|---|---| +| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | A whole frame in one message, RGB-D or stereo. No synchronization needed, so this is the most reliable input. | + +## Published Topics + +| Topic | Type | Description | +|---|---|---| +| `cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | `XYZRGB`, or `XYZRGBNormal` when normals are enabled. | + +Nothing is computed unless `cloud` has a subscriber. + +## Parameters + +Identical to [point_cloud_xyz](point_cloud_xyz.md#parameters), with [`image_transport`](https://docs.ros.org/en/jazzy/p/image_transport/) added and the `Stereo*` family below. + +**Synchronization** + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `approx_sync` | `bool` | `true` | Match the inputs by nearest stamp. Set false when they share exact stamps. | +| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. | +| `topic_queue_size` | `int` | `1` | Queue depth of each input subscription. | +| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. | +| `qos` | `int` | `0` | Reliability of the image and disparity subscriptions. | +| `qos_camera_info` | `int` | value of `qos` | Reliability of the camera info subscriptions. | +| `image_transport` | `string` | `"raw"` | `image_transport` plugin for the color, left and right images. | +| `depth_transport` | `string` | `"raw"` | `image_transport` plugin for `depth/image`. | + +**Projection and filtering**, applied in this order + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `decimation` | `int` | `1` | Keep one pixel in `decimation`, in each direction. | +| `roi_ratios` | `string` | `""` | Crop before projecting, `"left right top bottom"`. **Ignored for stereo input**, which warns if you set it. | +| `min_depth` | `double` | `0.0` | Discard points nearer than this, in meters. `0` disables. | +| `max_depth` | `double` | `0.0` | Discard points further than this, in meters. `0` disables. | +| `voxel_size` | `double` | `0.0` | Downsample to one point per voxel, in meters. `0` disables. | +| `noise_filter_radius` | `double` | `0.0` | Radius outlier removal, in meters. `0` disables. | +| `noise_filter_min_neighbors` | `int` | `5` | Neighbors needed within `noise_filter_radius`. | +| `normal_k` | `int` | `0` | Estimate normals from this many neighbors. `0` disables. | +| `normal_radius` | `double` | `0.0` | Estimate normals within this radius. `0` disables. | +| `filter_nans` | `bool` | `false` | Drop invalid points instead of leaving them NaN, giving an unorganized cloud. See [point_cloud_xyz](point_cloud_xyz.md#organized-output). | + +## Stereo matching + +The stereo and `rgbd_image`-with-stereo inputs run OpenCV's block matcher, configured through RTAB-Map's `StereoBM/*` parameters, which are exposed as ROS parameters of this node: + +```bash +-p StereoBM/NumDisparities:=64 -p StereoBM/BlockSize:=15 +``` + +The full list is in RTAB-Map's [parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html). The two that matter most are `StereoBM/NumDisparities` (must exceed the largest disparity you expect, and must not exceed the image width) and `StereoBM/BlockSize`. + +If you already have a disparity image, feed the disparity input instead — it skips the matching entirely. + +## Notes + +For RGB-D input the depth **must be registered to the color camera**: the node pairs pixel `(u,v)` of the color image with pixel `(u,v)` of the depth image and uses one calibration for both. Unregistered depth gives a cloud whose colors are offset from its geometry. Most drivers offer an aligned depth stream for this reason. + +An `rgbd_image` carrying only color and no depth is valid and yields an empty cloud rather than an error. diff --git a/rtabmap_util/doc/pointcloud_to_depthimage.md b/rtabmap_util/doc/pointcloud_to_depthimage.md new file mode 100644 index 00000000..106e0df3 --- /dev/null +++ b/rtabmap_util/doc/pointcloud_to_depthimage.md @@ -0,0 +1,96 @@ +# pointcloud_to_depthimage + +Projects a point cloud into a camera to make a depth image registered to it. + +Given a cloud (from a 3D lidar or a ToF camera) and the `camera_info` of an RGB camera, this node projects the points into that camera and outputs the depth image it would have produced if it were an RGB-D sensor: same intrinsics, same size, pixel `(u,v)` of the depth image lining up with pixel `(u,v)` of the color image. The result plugs into anything that consumes depth images: RTAB-Map's RGB-D pipeline, [`depth_image_proc`](https://docs.ros.org/en/jazzy/p/depth_image_proc/), obstacle avoidance built for depth cameras. + +Two typical setups: + +* **Lidar + one or more RGB cameras.** The natural way to feed a lidar into an RGB-D SLAM setup: the lidar supplies the geometry, the cameras the appearance. Run one instance per camera, each subscribing to the same cloud but to that camera's `camera_info`; a 3D lidar usually covers all of them at once. The resulting RGB-D streams can then be combined with [rtabmap_sync](https://docs.ros.org/en/jazzy/p/rtabmap_sync/)'s `rgbd_sync`/`rgbdx_sync` and given to RTAB-Map through its `rgbd_cameras` parameter. +* **ToF camera + RGB camera, not synchronized.** Two separate sensors, each with its own clock and its own pose, so their frames line up neither in time nor in space. Projecting the ToF cloud into the RGB camera registers the depth to the color image, and setting `fixed_frame_id` to a high-rate odometry frame — VIO, or an IMU-driven odometry running well above the camera rate — compensates the motion between the two stamps at the same time. See [Motion compensation](#motion-compensation). + +## Usage + +```bash +ros2 run rtabmap_util pointcloud_to_depthimage --ros-args \ + -r cloud:=/velodyne_points \ + -r camera_info:=/camera/color/camera_info \ + -p fixed_frame_id:=odom -p decimation:=4 -p fill_holes_size:=2 +``` + +```python +ComposableNode( + package='rtabmap_util', + plugin='rtabmap_util::PointCloudToDepthImage', + name='pointcloud_to_depthimage', + parameters=[{'fixed_frame_id': 'odom', 'decimation': 4, 'fill_holes_size': 2}], + remappings=[('cloud', '/velodyne_points'), + ('camera_info', '/camera/color/camera_info')]) +``` + +## Subscribed Topics + +| Topic | Type | Description | +|---|---|---| +| `cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The geometry to project. An empty cloud yields an all-zero image rather than nothing. | +| `camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Defines the target camera: its intrinsics, its size, and through its `frame_id` its pose. Normally the RGB camera the depth image is being registered to. | + +## Published Topics + +| Topic | Type | Description | +|---|---|---| +| `image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) (`32FC1`) | Depth in **meters**, in the camera info's frame. | +| `image_raw` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) (`16UC1`) | The same depth in **millimeters**. | +| `image/camera_info`, `image_raw/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | The input calibration, rescaled if `decimation` is set. | +| `cloud_transformed` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The input cloud in the camera frame. Debugging aid; only published when hole filling is on and something subscribes. | + +Nothing is computed unless one of the two image topics has a subscriber. + +## Required Transforms + +| Transform | Description | +|---|---| +| cloud frame → camera frame | Where the cloud's sensor sits relative to the camera. | +| `fixed_frame_id` → cloud frame, at both stamps | Only when `fixed_frame_id` is set, which is how motion between the two stamps is measured. See [Motion compensation](#motion-compensation). | + +## Parameters + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `fixed_frame_id` | `string` | `""` | Frame the sensor's motion is measured against, usually `odom`. **Required when `approx` is true.** See [Motion compensation](#motion-compensation). | +| `approx` | `bool` | `true` | Match cloud and camera info by nearest stamp. Set false when the two share exact stamps, in which case `fixed_frame_id` is unnecessary. | +| `wait_for_transform` | `double` | `0.1` | Seconds to wait for a transform before dropping the frame. | +| `decimation` | `int` | `1` | Render at 1/`decimation` of the camera info's resolution. The published camera info is scaled to match. Must divide both the width and the height exactly, otherwise it is ignored with an error and the image comes out full size. See [Hole filling](#hole-filling). | +| `fill_holes_size` | `int` | `0` | Radius, in pixels, for filling gaps between projected points. `0` disables. See [Hole filling](#hole-filling). | +| `fill_holes_error` | `double` | `0.1` | Largest depth difference, in meters, across which a hole may be filled. | +| `fill_iterations` | `int` | `1` | How many times to repeat the filling pass. | +| `upscale` | `bool` | `false` | Interpolate the depth image back to full resolution after rendering. Only has an effect when `decimation` is greater than 1, and only needed when the consumer requires full resolution. See [Hole filling](#hole-filling). | +| `upscale_depth_error_ratio` | `double` | `0.02` | Relative depth difference tolerated across a block when upscaling. Above it the block is left empty rather than interpolated across an edge. | +| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. | +| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. | +| `qos` | `int` | `0` | Reliability of the cloud subscription. | +| `qos_camera_info` | `int` | value of `qos` | Reliability of the camera info subscription. | + +## Motion compensation + +The cloud's sensor and the camera almost never fire at the same instant, and on a moving robot that offset matters: projecting a cloud captured 40 ms earlier into the camera's current pose puts everything in the wrong place. + +When `fixed_frame_id` is set, the node asks TF how the cloud's frame moved between the two stamps and folds that displacement into the projection, so the cloud is placed where the camera was **at its own stamp**. Driving forward at 1 m/s with a 40 ms offset moves everything 4 cm — enough to matter at close range. + +The lookup is only as good as the frame it measures against: TF interpolates between the samples it has, so the source publishing `fixed_frame_id` should run well above the sensor rate. A VIO or wheel odometry at 100+ Hz gives a meaningful displacement over a 40 ms gap; a 1 Hz SLAM output does not. + +Without `fixed_frame_id` the stamp difference is silently ignored, which is why the node logs a fatal error if `approx` is true and no fixed frame is given. If the transform cannot be found the frame is dropped rather than projected wrongly. + +## Hole filling + +A lidar cloud is far sparser than a camera image, so a direct projection is mostly gaps: individual pixels with depth, surrounded by zeros. + +**Start with `decimation`.** Rendering at a coarser resolution puts more points in each pixel, so the wide gaps between lidar rings largely disappear instead of having to be filled in afterwards. `decimation: 4` is a reasonable starting point for a 3D lidar against a full-resolution camera. The published camera info is scaled to match, so consumers that read it keep working at the smaller size. + +**Then close what is left with `fill_holes_size`.** It spreads each point over a small neighborhood, but only across depth differences smaller than `fill_holes_error`, so it fills a surface without bridging the gap between a foreground object and the wall behind it. Start at `2` and raise it only if the image is still speckled; too large and thin structures get fattened. + +`upscale` is for the specific case where the consumer needs the depth image back at the camera's full resolution — pairing it pixel-for-pixel with the full-size color image, for instance. It interpolates each decimated block bilinearly from its corners, and only where all four have depth and agree to within `upscale_depth_error_ratio`, so it stops at depth discontinuities rather than stretching a foreground object onto the wall behind it. Leave it off otherwise: it restores resolution the lidar never measured, at full-resolution cost. + +## Notes + +The output is dense in *layout* but sparse in *content*: pixels with no return are zero, the ROS convention for no reading. Consumers that assume every pixel is valid will need to handle that. diff --git a/rtabmap_util/doc/rgbd_relay.md b/rtabmap_util/doc/rgbd_relay.md new file mode 100644 index 00000000..1b2b8cb7 --- /dev/null +++ b/rtabmap_util/doc/rgbd_relay.md @@ -0,0 +1,75 @@ +# rgbd_relay + +Republishes an [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html), optionally compressing or decompressing it on the way through. + +An `RGBDImage` can carry its images raw or compressed. This node converts between the two so that the expensive form crosses the network only where it has to: compress before a wifi link, decompress on the other side. + +With both `compress` and `uncompress` left false the message is forwarded untouched, which makes the node a plain relay — useful to give a topic a second name, or to bridge two incompatible QoS profiles with `qos_sub` and `qos_pub`. See [Bridging QoS profiles](#bridging-qos-profiles). + +## Usage + +Compress before sending over a slow link: + +```bash +ros2 run rtabmap_util rgbd_relay --ros-args \ + -r rgbd_image:=/camera/rgbd_image \ + -p compress:=true +``` + +```python +ComposableNode( + package='rtabmap_util', + plugin='rtabmap_util::RGBDRelay', + name='rgbd_relay', + parameters=[{'compress': True}], + remappings=[('rgbd_image', '/camera/rgbd_image')]) +``` + +## Subscribed Topics + +| Topic | Type | Description | +|---|---|---| +| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | Queue depth `queue_sub`, 5 by default. | + +## Published Topics + +| Topic | Type | Description | +|---|---|---| +| `rgbd_image_relay` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | Queue depth `queue_pub`, 1 by default. Published only when someone is subscribed. | + +## Parameters + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `compress` | `bool` | `false` | Fill the compressed fields of the output. Color becomes JPEG; depth becomes PNG, or JPEG when the message carries a stereo pair rather than depth. Fields already compressed on input are passed through as-is. | +| `uncompress` | `bool` | `false` | Fill the raw fields of the output by decoding the compressed ones. Fields already raw on input are passed through as-is. | +| `qos` | `int` | `0` | Reliability of both sides: `0` system default, `1` reliable, `2` best effort. | +| `qos_sub` | `int` | value of `qos` | Reliability of the `rgbd_image` subscription alone. | +| `qos_pub` | `int` | value of `qos` | Reliability of the `rgbd_image_relay` publisher alone. | +| `queue_sub` | `int` | `5` | Queue depth of the `rgbd_image` subscription. Must be at least 1. | +| `queue_pub` | `int` | `1` | Queue depth of the `rgbd_image_relay` publisher. Must be at least 1. | + +## Bridging QoS profiles + +A subscriber that asks for **reliable** will not connect to a publisher offering **best effort** — the request cannot be satisfied, so the two silently never match. A best-effort subscriber, on the other hand, connects to either. + +That is a real problem when a camera driver publishes best effort and the consumer insists on reliable. Set the two sides of the relay separately and it forwards across the gap: + +```bash +ros2 run rtabmap_util rgbd_relay --ros-args \ + -r rgbd_image:=/camera/rgbd_image \ + -p qos_sub:=2 \ + -p qos_pub:=1 +``` + +Both parameters default to `qos`, so setting `qos` alone configures both sides at once. + +Reliability is all that is bridged — durability is left at the default, so a transient-local publisher is not converted. The queue depths are separate too, through `queue_sub` and `queue_pub`. + +## Notes + +Setting neither `compress` nor `uncompress` forwards the message unchanged and skips all image handling — the cheapest path by a wide margin. + +Setting both is allowed and produces a message carrying each image twice, raw and compressed. That is rarely what you want. + +Depth is compressed as **PNG**, a stereo right image as **JPEG**. diff --git a/rtabmap_util/doc/rgbd_split.md b/rtabmap_util/doc/rgbd_split.md new file mode 100644 index 00000000..f968a31a --- /dev/null +++ b/rtabmap_util/doc/rgbd_split.md @@ -0,0 +1,76 @@ +# rgbd_split + +Splits an [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) back into the standard ROS image topics. + +`RGBDImage` bundles color, depth and both camera infos into one message so they arrive together, which is what RTAB-Map wants. Everything else in the ROS ecosystem — RViz, `image_view`, [`depth_image_proc`](https://docs.ros.org/en/jazzy/p/depth_image_proc/) — expects separate `Image` and `CameraInfo` topics. This node unpacks the bundle for them. + +It is the inverse of [rtabmap_sync](https://docs.ros.org/en/jazzy/p/rtabmap_sync/)'s `rgbd_sync`, and of its `stereo_sync` when `stereo` is set — those two are what produce an `RGBDImage` in the first place. + +## Usage + +```bash +ros2 run rtabmap_util rgbd_split --ros-args -r rgbd_image:=/camera/rgbd_image +``` + +```python +ComposableNode( + package='rtabmap_util', + plugin='rtabmap_util::RGBDSplit', + name='rgbd_split', + remappings=[('rgbd_image', '/camera/rgbd_image')]) +``` + +## Subscribed Topics + +| Topic | Type | Description | +|---|---|---| +| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | Queue depth `queue_sub`, 5 by default. Raw or compressed images are both accepted. | + +## Published Topics + +The output topics are named after the **resolved** input topic, so remapping `rgbd_image` moves the outputs with it. With `rgbd_image` remapped to `/camera/rgbd_image` they are `/camera/rgbd_image/rgb/image` and so on. Setting `stereo: true` renames the two halves `left` and `right`, see [Stereo messages](#stereo-messages). + +| Topic | Type | Description | +|---|---|---| +| `/rgb/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | The color image, decompressed if needed. | +| `/rgb/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | | +| `/depth/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | The depth image, or the right image of a stereo pair. | +| `/depth/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | For a stereo pair this is the right camera, and its `P(0,3)` carries the baseline. | + +Each half is only unpacked if something is subscribed to it, so subscribing to color alone does not pay for depth decompression. + +## Parameters + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `qos` | `int` | `0` | Reliability of both sides: `0` system default, `1` reliable, `2` best effort. | +| `qos_sub` | `int` | value of `qos` | Reliability of the `rgbd_image` subscription alone. | +| `qos_pub` | `int` | value of `qos` | Reliability of the four output publishers alone. | +| `queue_sub` | `int` | `5` | Queue depth of the `rgbd_image` subscription. Must be at least 1. | +| `queue_pub` | `int` | `1` | Queue depth of every publisher. Must be at least 1. | +| `stereo` | `bool` | `false` | Name the outputs `left`/`right` instead of `rgb`/`depth`. See [Stereo messages](#stereo-messages). | + +## Stereo messages + +The node handles **stereo** `RGBDImage` messages as well as RGB-D ones. In a stereo message the "depth" slot holds the right image, and the second camera info carries the baseline; the depth topics then carry the right camera, correctly typed as `mono8` or `bgr8` rather than mislabeled as depth. + +That works, but the topic names lie. Set `stereo: true` and the outputs are named for what they hold: + +| `stereo` | Output topics | +|---|---| +| `false` (default) | `/rgb/image`, `/rgb/camera_info`, `/depth/image`, `/depth/camera_info` | +| `true` | `/left/image`, `/left/camera_info`, `/right/image`, `/right/camera_info` | + +```bash +ros2 run rtabmap_util rgbd_split --ros-args \ + -r rgbd_image:=/camera/rgbd_image \ + -p stereo:=true +``` + +Only the names change — the message contents and the order of the two halves are the same either way, so the `rgb` slot always becomes the left image. The two namings are exclusive: with `stereo: true` nothing is published on `rgb`/`depth`. + +The node checks the setting against what actually arrives, going by the encoding of the second half: `16UC1`, `32FC1` and `mono16` are depth, anything else is an image. If the two disagree it logs a warning **once** and keeps forwarding — a mismatch makes the topic name misleading, not the data wrong, so it is never worth dropping a frame over. + +## Notes + +If a message has no `frame_id` on one of its sub-messages, the node fills it in from the other one so the output is always usable by TF. diff --git a/rtabmap_util/include/rtabmap_util/MapsManager.h b/rtabmap_util/include/rtabmap_util/MapsManager.h index f6dd3c61..b144fab5 100644 --- a/rtabmap_util/include/rtabmap_util/MapsManager.h +++ b/rtabmap_util/include/rtabmap_util/MapsManager.h @@ -57,22 +57,131 @@ class GridMap; namespace rtabmap_util { +/** + * @brief Turns a pose graph into the map topics, and publishes them. + * + * Given a set of node poses and the sensor data behind them, MapsManager assembles the + * ground and obstacle point clouds, the 2D occupancy grid, the octomap and the elevation + * map, and publishes whichever of them somebody is subscribed to. + * + * It is shared by rtabmap_slam's `rtabmap` node and rtabmap_util's `map_assembler`, which + * is why those two produce identical maps from identical parameters. + * + * @par Lifecycle + * Callers follow a fixed order: + * 1. init() to declare the ROS parameters and advertise the topics, + * 2. backwardCompatibilityParameters() to pick up parameters that have since moved into + * the RTAB-Map library, then setParameters() to apply the whole set, + * 3. updateMapCaches() whenever the graph changes, then publishMaps(). + * + * @par Laziness + * Nothing is assembled or published without a subscriber, and with `map_cleanup` set the + * caches are released once the last one goes away. A node can therefore call + * updateMapCaches() and publishMaps() unconditionally on every graph update and pay + * nothing while nobody is listening. + * + */ class MapsManager { public: MapsManager(); virtual ~MapsManager(); + + /** + * @brief Declares the ROS parameters and advertises the map topics on @p node. + * + * Must be called before anything else, and exactly once per node: the parameters are + * declared here, and declaring them twice throws. + * + * @param node node to advertise on and read parameters from + * @param name prefix used in the log lines, normally the node's name + * @param usePublicNamespace unused, kept for source compatibility + */ void init(rclcpp::Node & node, const std::string & name, bool usePublicNamespace); + + /// Drops every cached local grid, assembled cloud and global map. void clear(); + + /// @return True if any map topic has at least one subscriber. bool hasSubscribers() const; + + /// @return True if the map topics are latched, i.e. delivered to late subscribers. bool isLatching() const {return latching_;} + + /** + * @brief Whether the map changed on the last updateMapCaches(). + * + * @note Reports true when nothing is subscribed to the grid topics. The answer comes + * from OccupancyGrid::update(), which only runs when a grid is wanted, so with + * nobody listening the safe assumption is that the graph moved. + */ bool isMapUpdated() const; + + /** + * @brief Copies parameters that moved from rtabmap_ros into the RTAB-Map library. + * + * Reads the old ROS parameter names off @p node and, for each one that is set, writes + * its value into @p parameters under the RTAB-Map name that replaced it, with a + * warning. Call it before setParameters(). + * + * @param[in] node node to read the legacy parameters from + * @param[in,out] parameters parameter set to fill in + */ void backwardCompatibilityParameters(rclcpp::Node & node, rtabmap::ParametersMap & parameters) const; + + /** + * @brief Applies the RTAB-Map parameters, rebuilding the map objects. + * + * The occupancy grid, octomap and elevation map are recreated, so anything already + * assembled is lost; the cached local grids are kept. + */ void setParameters(const rtabmap::ParametersMap & parameters); + + /** + * @brief Installs an already assembled 2D map, e.g. one loaded from a database. + * + * @param map the grid, `CV_8SC1` with -1 unknown, 0 free, 100 occupied + * @param xMin world x of the map's origin, in meters + * @param yMin world y of the map's origin, in meters + * @param cellSize resolution, in meters + * @param poses poses of the nodes @p map was assembled from + * @param memory optional memory to load the missing local grids from, so the map can + * keep growing from where it left off + * + * @warning @p poses must not be empty. The grid is kept only together with the nodes + * it came from, so that the manager knows which are already in it; a call + * with no poses is ignored with a warning. + */ void set2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map & poses, const rtabmap::Memory * memory = 0); + /** + * @brief Applies the `map_filter_radius`/`map_filter_angle` thinning to @p poses. + * @return The poses that survive, or all of them when filtering is disabled. + */ std::map getFilteredPoses( const std::map & poses); + /** + * @brief Brings the local grid cache and the global maps up to date with the graph. + * + * For every pose not already mapped, the local occupancy grid is taken from the + * signature or the memory, or regenerated from the sensor data when the node carries + * none, and added to the cache. The global maps are then reassembled. + * + * @param poses node poses; landmarks (negative ids) are ignored, and id 0 is + * the not-yet-committed node, kept only if `map_always_update` + * @param memory memory to load node data from, may be null if @p signatures + * carries everything + * @param updateGrid force the occupancy grid to be updated + * @param updateOctomap force the octomap to be updated + * @param signatures node data, keyed by id, for nodes not in @p memory + * @return The poses actually mapped, after filtering. + * + * @note With @p updateGrid and @p updateOctomap both false, what gets updated is + * decided by which topics have subscribers. That is also the only way the + * elevation map is ever built, as it has no flag of its own. + * @note At least one of @p memory and @p signatures must be non-empty, and @p poses + * must not be empty; otherwise an error is logged and nothing is returned. + */ std::map updateMapCaches( const std::map & poses, const rtabmap::Memory * memory, @@ -80,25 +189,49 @@ public: bool updateOctomap, const std::map & signatures = std::map()); + /** + * @brief Publishes every map topic that has a subscriber. + * + * @param poses the same poses updateMapCaches() returned + * @param stamp stamp for all published messages + * @param mapFrameId frame id for all published messages + */ void publishMaps( const std::map & poses, const rclcpp::Time & stamp, const std::string & mapFrameId); + /** + * @brief The 2D occupancy grid as a ternary map. + * @param[out] xMin world x of the map's origin, in meters + * @param[out] yMin world y of the map's origin, in meters + * @param[out] gridCellSize resolution, in meters + * @return `CV_8SC1`, -1 unknown, 0 free, 100 occupied. Empty if nothing is assembled. + */ cv::Mat getGridMap( float & xMin, float & yMin, float & gridCellSize); + /** + * @brief The 2D occupancy grid as probabilities. + * @param[out] xMin world x of the map's origin, in meters + * @param[out] yMin world y of the map's origin, in meters + * @param[out] gridCellSize resolution, in meters + * @return `CV_8SC1`, -1 unknown, otherwise 0-100. Empty if nothing is assembled. + */ cv::Mat getGridProbMap( float & xMin, float & yMin, float & gridCellSize); #ifdef RTABMAP_OCTOMAP + /// @return The octomap, owned by this object. Never null. const rtabmap::OctoMap * getOctomap() const {return octomap_;} #endif + /// @return The global occupancy grid, owned by this object. Never null. const rtabmap::OccupancyGrid * getOccupancyGrid() const {return occupancyGrid_;} + /// @return The local grid segmenter, owned by this object. Never null. const rtabmap::LocalGridMaker * getLocalMapMaker() const {return localMapMaker_;} private: diff --git a/rtabmap_util/include/rtabmap_util/map_assembler.hpp b/rtabmap_util/include/rtabmap_util/map_assembler.hpp index 3c874ee0..7972d57f 100644 --- a/rtabmap_util/include/rtabmap_util/map_assembler.hpp +++ b/rtabmap_util/include/rtabmap_util/map_assembler.hpp @@ -62,6 +62,9 @@ private: void timerCallback(); + /// Subscribes to "mapData"; the node is live from here on. + void subscribeToMapData(); + #ifdef WITH_OCTOMAP_MSGS #ifdef RTABMAP_OCTOMAP void octomapBinaryCallback( @@ -100,6 +103,7 @@ private: #endif #endif bool localGridsRegenerated_; + double initializeFromRtabmapTimeout_; }; } \ No newline at end of file diff --git a/rtabmap_util/include/rtabmap_util/point_cloud_aggregator.hpp b/rtabmap_util/include/rtabmap_util/point_cloud_aggregator.hpp index ff72354a..6ae2820d 100644 --- a/rtabmap_util/include/rtabmap_util/point_cloud_aggregator.hpp +++ b/rtabmap_util/include/rtabmap_util/point_cloud_aggregator.hpp @@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include namespace rtabmap_util { @@ -66,8 +67,7 @@ private: const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_2); void combineClouds(const std::vector & cloudMsgs); - std::thread * warningThread_; - bool callbackCalled_; + std::unique_ptr syncDiagnostic_; typedef message_filters::sync_policies::ExactTime ExactSync4Policy; typedef message_filters::sync_policies::ApproximateTime ApproxSync4Policy; diff --git a/rtabmap_util/include/rtabmap_util/point_cloud_assembler.hpp b/rtabmap_util/include/rtabmap_util/point_cloud_assembler.hpp index cc756894..359cccbe 100644 --- a/rtabmap_util/include/rtabmap_util/point_cloud_assembler.hpp +++ b/rtabmap_util/include/rtabmap_util/point_cloud_assembler.hpp @@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include namespace rtabmap_util { @@ -73,8 +74,7 @@ private: void callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg); private: - std::thread * warningThread_; - bool callbackCalled_; + std::unique_ptr syncDiagnostic_; rclcpp::Subscription::SharedPtr cloudSub_; rclcpp::Publisher::SharedPtr cloudPub_; diff --git a/rtabmap_util/include/rtabmap_util/rgbd_split.hpp b/rtabmap_util/include/rtabmap_util/rgbd_split.hpp index d31a736a..6478dc5e 100644 --- a/rtabmap_util/include/rtabmap_util/rgbd_split.hpp +++ b/rtabmap_util/include/rtabmap_util/rgbd_split.hpp @@ -48,6 +48,9 @@ public: void callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) const; private: + /// True when the outputs are named left/right rather than rgb/depth. + bool stereo_; + rclcpp::Subscription::SharedPtr rgbdImageSub_; image_transport::Publisher rgbPub_; diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index 35bc2f09..a349e6d1 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -36,7 +36,10 @@ rtabmap_sync grid_map_ros + ament_cmake_gtest + ament_cmake + rosdoc2.yaml diff --git a/rtabmap_util/rosdoc2.yaml b/rtabmap_util/rosdoc2.yaml new file mode 100644 index 00000000..6b7fbdda --- /dev/null +++ b/rtabmap_util/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_util +## Build the docs locally with: +## rosdoc2 build --package-path rtabmap_util --output-directory doc_output + +## This 'attic section' self-documents this file's type and version. +type: 'rosdoc2 config' +version: 1 + +--- + +settings: + ## Generate the standard index page from package.xml (description, maintainer, + ## license, links) and a table of contents for the builders below. + generate_package_index: true + + ## This is an ament_cmake package, so doxygen runs on the public headers by + ## default and there are no Python modules to document. + always_run_doxygen: false + always_run_sphinx_apidoc: false + +builders: + ## Doxygen parses the public C++ API out of include/. + - doxygen: { + name: 'rtabmap_util Public C/C++ API', + output_dir: 'generated/doxygen' + } + ## Sphinx renders the landing page and pulls the Doxygen XML in through + ## breathe/exhale so the API is browsable alongside the narrative docs. + - sphinx: { + name: 'rtabmap_util', + doxygen_xml_directory: 'generated/doxygen/xml', + output_dir: '' + } diff --git a/rtabmap_util/src/DbPlayerNode.cpp b/rtabmap_util/src/DbPlayerNode.cpp index cd892cea..9b178149 100644 --- a/rtabmap_util/src/DbPlayerNode.cpp +++ b/rtabmap_util/src/DbPlayerNode.cpp @@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/ULogger.h" #include "rclcpp/rclcpp.hpp" +#include + #ifndef _WIN32 #include #include @@ -83,7 +85,19 @@ int main(int argc, char **argv) rclcpp::NodeOptions options; options.arguments(arguments); - auto node = std::make_shared(options); + std::shared_ptr node; + try + { + node = std::make_shared(options); + } + catch(const std::exception & e) + { + // The node reports what went wrong before throwing; keep the process exit clean + // rather than letting an uncaught exception abort. + UERROR("%s", e.what()); + rclcpp::shutdown(); + return -1; + } rclcpp::Rate pauseRate(10); diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 19c1a9a2..b142d14f 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -273,6 +273,12 @@ void MapsManager::set2DMap( const std::map & poses, const rtabmap::Memory * memory) { + if(!map.empty() && poses.empty()) + { + UWARN("Ignoring the 2D map (%dx%d): no poses were given. Pass the poses of the " + "nodes the map was assembled from.", map.cols, map.rows); + return; + } occupancyGrid_->setMap(map, xMin, yMin, cellSize, poses); //update cache in case the map should be updated if(memory && @@ -1420,6 +1426,7 @@ void MapsManager::publishMaps( msg->header.frame_id = mapFrameId; msg->header.stamp = stamp; elevationMapPub_->publish(std::move(msg)); + latched_.at(&elevationMapPub_) = true; } if(elevationMapPub_->get_subscription_count() == 0) { diff --git a/rtabmap_util/src/nodelets/db_player.cpp b/rtabmap_util/src/nodelets/db_player.cpp index 167af087..cacd6796 100644 --- a/rtabmap_util/src/nodelets/db_player.cpp +++ b/rtabmap_util/src/nodelets/db_player.cpp @@ -28,6 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + #include #include @@ -106,13 +108,14 @@ DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) : qosGlobalPose_ = this->declare_parameter("qos_global_pose", qos_); qosGps_ = this->declare_parameter("qos_gps", qos_); qosImu_ = this->declare_parameter("qos_imu", qos_); + qosEnvSensor_ = this->declare_parameter("qos_env_sensor", qos_); // A general 360 lidar with 0.5 deg increment scanAngleMin_ = this->declare_parameter("scan_angle_min", -M_PI); scanAngleMax_ = this->declare_parameter("scan_angle_max", M_PI); scanAngleIncrement_ = this->declare_parameter("scan_angle_increment", M_PI / 720.0); scanRangeMin_ = this->declare_parameter("scan_range_min", 0.0); - scanRangeMax_ = this->declare_parameter("scan_range_max", 60); + scanRangeMax_ = this->declare_parameter("scan_range_max", 60.0); RCLCPP_INFO(get_logger(), "frame_id = %s", frameId_.c_str()); RCLCPP_INFO(get_logger(), "odom_frame_id = %s", odomFrameId_.c_str()); @@ -136,8 +139,11 @@ DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) : if(databasePath.empty()) { + // Throwing rather than exiting: this node can be loaded in a component container + // next to others, and taking the whole process down with it would be rude. RCLCPP_ERROR(get_logger(), "Parameter \"database\" must be set (path to a RTAB-Map database)."); - exit(-1); + throw std::invalid_argument( + "db_player: parameter \"database\" must be set (path to a RTAB-Map database)."); } databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir()); @@ -151,7 +157,8 @@ DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) : if(!reader_->init()) { RCLCPP_ERROR(get_logger(), "Cannot open database \"%s\".", databasePath.c_str()); - exit(-1); + throw std::runtime_error( + uFormat("db_player: cannot open database \"%s\".", databasePath.c_str())); } const std::string servicePrefix = get_name() + std::string("/"); @@ -300,8 +307,12 @@ void DbPlayer::initializePublishers(const rtabmap::OdometryEvent & odom) if(!odom.data().laserScanRaw().isEmpty()) { - if(!scanPub_.get() && odom.data().laserScanRaw().is2d()) + // The publisher has to match the scan being replayed, not just whichever one has + // not been created yet: a 2D database must never advertise "scan_cloud". + if(odom.data().laserScanRaw().is2d()) { + if(!scanPub_.get()) + { scanPub_ = this->create_publisher("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosScan_)); if(odom.data().laserScanRaw().angleIncrement() > 0.0f) { @@ -316,6 +327,7 @@ void DbPlayer::initializePublishers(const rtabmap::OdometryEvent & odom) RCLCPP_INFO(get_logger(), " scan_range_min=%f", scanRangeMin_); RCLCPP_INFO(get_logger(), " scan_range_max=%f", scanRangeMax_); } + } } else if(!scanCloudPub_.get()) { @@ -569,7 +581,6 @@ bool DbPlayer::publishNextFrame() envSensorPub_->get_subscription_count() > 0 && !odom.data().envSensors().empty()) { - rtabmap_msgs::msg::EnvSensor msg; for(rtabmap::EnvSensors::const_iterator iter=odom.data().envSensors().begin(); iter!=odom.data().envSensors().end(); ++iter) { rtabmap_msgs::msg::EnvSensor msg; diff --git a/rtabmap_util/src/nodelets/disparity_to_depth.cpp b/rtabmap_util/src/nodelets/disparity_to_depth.cpp index e537b313..3c582c7f 100644 --- a/rtabmap_util/src/nodelets/disparity_to_depth.cpp +++ b/rtabmap_util/src/nodelets/disparity_to_depth.cpp @@ -28,6 +28,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include +#include + #include #ifdef PRE_ROS_IRON @@ -44,16 +47,27 @@ DisparityToDepth::DisparityToDepth(const rclcpp::NodeOptions & options) : { int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; qos = this->declare_parameter("qos", qos); + // Each side can be set independently so the node can bridge a producer and a + // consumer that don't agree on reliability. Both default to qos. + int qosSub = this->declare_parameter("qos_sub", qos); + int qosPub = this->declare_parameter("qos_pub", qos); + int queueSub = this->declare_parameter("queue_sub", 1); + int queuePub = this->declare_parameter("queue_pub", 1); + + UASSERT_MSG(queueSub >= 1 && queuePub >= 1, + uFormat("queue_sub (%d) and queue_pub (%d) must be at least 1", queueSub, queuePub).c_str()); + + const rclcpp::QoS pubQos = rclcpp::QoS(queuePub).reliability((rmw_qos_reliability_policy_t)qosPub); #ifdef PRE_ROS_LYRICAL - pub32f_ = image_transport::create_publisher(this, "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - pub16u_ = image_transport::create_publisher(this, "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + pub32f_ = image_transport::create_publisher(this, "depth", pubQos.get_rmw_qos_profile()); + pub16u_ = image_transport::create_publisher(this, "depth_raw", pubQos.get_rmw_qos_profile()); #else - pub32f_ = image_transport::create_publisher(*this, "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); - pub16u_ = image_transport::create_publisher(*this, "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); + pub32f_ = image_transport::create_publisher(*this, "depth", pubQos); + pub16u_ = image_transport::create_publisher(*this, "depth_raw", pubQos); #endif - sub_ = create_subscription("disparity", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&DisparityToDepth::callback, this, std::placeholders::_1)); + sub_ = create_subscription("disparity", rclcpp::QoS(queueSub).reliability((rmw_qos_reliability_policy_t)qosSub), std::bind(&DisparityToDepth::callback, this, std::placeholders::_1)); } DisparityToDepth::~DisparityToDepth(){} diff --git a/rtabmap_util/src/nodelets/lidar_deskewing.cpp b/rtabmap_util/src/nodelets/lidar_deskewing.cpp index d15e189b..518b6f8e 100644 --- a/rtabmap_util/src/nodelets/lidar_deskewing.cpp +++ b/rtabmap_util/src/nodelets/lidar_deskewing.cpp @@ -61,7 +61,7 @@ void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstShared msg->header.frame_id, fixedFrameId_, msg->header.stamp, - rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec) + rclcpp::Duration::from_seconds(msg->ranges.size()*msg->time_increment), + rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec) + rclcpp::Duration::from_seconds((msg->ranges.empty()?0:msg->ranges.size()-1)*msg->time_increment), *tfBuffer_, waitForTransformDuration_); if(tmpT.isNull()) diff --git a/rtabmap_util/src/nodelets/map_assembler.cpp b/rtabmap_util/src/nodelets/map_assembler.cpp index 07e91ad7..40a01570 100644 --- a/rtabmap_util/src/nodelets/map_assembler.cpp +++ b/rtabmap_util/src/nodelets/map_assembler.cpp @@ -54,12 +54,18 @@ MapAssembler::MapAssembler(const rclcpp::NodeOptions & options) : Node("map_assembler", options), lastNodeAdded_(-1), rtabmapNodeName_("rtabmap"), - localGridsRegenerated_(false) + localGridsRegenerated_(false), + initializeFromRtabmapTimeout_(5.0) { std::string configPath; configPath = this->declare_parameter("config_path", configPath); localGridsRegenerated_ = this->declare_parameter("regenerate_local_grids", localGridsRegenerated_); rtabmapNodeName_ = this->declare_parameter("rtabmap", rtabmapNodeName_); + // Seconds to wait for rtabmap's get_map_data service on start-up, which is how + // map_assembler catches up on a map that already exists. Set it to 0 to skip the call + // entirely: the subscription to "mapData" is then created right away instead of after + // the wait, which is what you want when map_assembler starts before rtabmap. + initializeFromRtabmapTimeout_ = this->declare_parameter("initialize_from_rtabmap_timeout", initializeFromRtabmapTimeout_); //parameters rtabmap::ParametersMap parameters; @@ -175,6 +181,7 @@ MapAssembler::MapAssembler(const rclcpp::NodeOptions & options) : } RCLCPP_INFO(this->get_logger(), "%s: regenerate_local_grids = %s", this->get_name(), localGridsRegenerated_?"true":"false"); + RCLCPP_INFO(this->get_logger(), "%s: initialize_from_rtabmap_timeout = %fs (0=don't ask rtabmap for the map)", this->get_name(), initializeFromRtabmapTimeout_); mapsManager_.init(*this, this->get_name(), true); mapsManager_.backwardCompatibilityParameters(*this, parameters); mapsManager_.setParameters(parameters); @@ -189,13 +196,29 @@ MapAssembler::MapAssembler(const rclcpp::NodeOptions & options) : #endif #endif - std::string getMapSrv = rtabmapNodeName_+"/get_map_data"; + if(initializeFromRtabmapTimeout_ > 0.0) + { + std::string getMapSrv = rtabmapNodeName_+"/get_map_data"; - // We cannot call the service and wait in the constructor, lets call it later and subscribe afterwards - serviceCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - timerCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - client_ = this->create_client(getMapSrv, rclcpp::ServicesQoS(), serviceCbGroup_); // Put it in a different group than the timer - timer_ = this->create_wall_timer(1s, std::bind(&MapAssembler::timerCallback, this), timerCbGroup_); + // We cannot call the service and wait in the constructor, lets call it later and subscribe afterwards + serviceCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + timerCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + client_ = this->create_client(getMapSrv, rclcpp::ServicesQoS(), serviceCbGroup_); // Put it in a different group than the timer + timer_ = this->create_wall_timer(1s, std::bind(&MapAssembler::timerCallback, this), timerCbGroup_); + } + else + { + subscribeToMapData(); + } +} + +void MapAssembler::subscribeToMapData() +{ + rclcpp::SubscriptionOptions options; + // Null unless we came through the timer, in which case the node's default group is used. + options.callback_group = timerCbGroup_; + mapDataSub_ = create_subscription("mapData", rclcpp::QoS(1), + std::bind(&MapAssembler::mapDataReceivedCallback, this, std::placeholders::_1), options); } MapAssembler::~MapAssembler() {} @@ -213,7 +236,8 @@ void MapAssembler::timerCallback() std::string getMapSrv = rtabmapNodeName_+"/get_map_data"; RCLCPP_INFO(this->get_logger(), "Calling service \"%s\"...", getMapSrv.c_str()); - if(client_->wait_for_service(5s)) + if(client_->wait_for_service( + std::chrono::duration(initializeFromRtabmapTimeout_))) { auto request = std::make_shared(); request->global_map = false; @@ -237,18 +261,16 @@ void MapAssembler::timerCallback() } else { - RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available after waiting for 5 seconds, " + RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available after waiting for %f seconds, " "may not be a problem if rtabmap is started afterwards. If rtabmap " "is started after in localization mode, call %s/publish_maps " "service with graph_only=false to make sure map_assembler has all the data.", getMapSrv.c_str(), + initializeFromRtabmapTimeout_, rtabmapNodeName_.c_str()); } - rclcpp::SubscriptionOptions options; - options.callback_group = timerCbGroup_; - mapDataSub_ = create_subscription("mapData", rclcpp::QoS(1), - std::bind(&MapAssembler::mapDataReceivedCallback, this, std::placeholders::_1), options); + subscribeToMapData(); } void MapAssembler::mapDataReceivedCallback(const rtabmap_msgs::msg::MapData::ConstSharedPtr msg) diff --git a/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp b/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp index a8cc5333..3b95c71b 100644 --- a/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp @@ -41,8 +41,6 @@ namespace rtabmap_util PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options) : Node("point_cloud_aggregator", options), - warningThread_(0), - callbackCalled_(false), exactSync4_(0), approxSync4_(0), exactSync3_(0), @@ -162,23 +160,16 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options) } - warningThread_ = new std::thread([&](){ - rclcpp::Rate r(1.0/5.0); - while(!callbackCalled_) - { - r.sleep(); - if(!callbackCalled_) - { - RCLCPP_WARN(this->get_logger(), "%s: Did not receive data since 5 seconds! Make sure the input topics are " - "published (\"$ ros2 topic hz my_topic\") and the timestamps in their " - "header are set. %s%s", - this->get_name(), - approx?"":"Parameter \"approx_sync\" is false, which means that input " - "topics should have all the exact timestamp for the callback to be called.", - subscribedTopicsMsg.c_str()); - } - } - }); + syncDiagnostic_.reset(new rtabmap_sync::SyncDiagnostic(this, 0.5)); + syncDiagnostic_->init(cloudSub_1_.getSubscriber()->get_topic_name(), + uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are " + "published (\"$ ros2 topic hz my_topic\") and the timestamps in their " + "header are set. %s%s", + this->get_name(), + approx?"":"Parameter \"approx_sync\" is false, which means that input " + "topics should have all the exact timestamp for the callback to be called.", + subscribedTopicsMsg.c_str())); + RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str()); } @@ -190,13 +181,6 @@ PointCloudAggregator::~PointCloudAggregator() delete approxSync3_; delete exactSync2_; delete approxSync2_; - - if(warningThread_) - { - callbackCalled_=true; - warningThread_->join(); - delete warningThread_; - } } void PointCloudAggregator::clouds4_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_1, @@ -234,8 +218,8 @@ void PointCloudAggregator::clouds2_callback(const sensor_msgs::msg::PointCloud2: } void PointCloudAggregator::combineClouds(const std::vector & cloudMsgs) { - callbackCalled_ = true; UASSERT(cloudMsgs.size() > 1); + syncDiagnostic_->tickInput(cloudMsgs[0]->header.stamp); if(cloudPub_->get_subscription_count()) { pcl::PCLPointCloud2::Ptr output(new pcl::PCLPointCloud2); @@ -420,6 +404,7 @@ void PointCloudAggregator::combineClouds(const std::vectorheader.frame_id = frameId; cloudPub_->publish(std::move(rosCloud)); } + syncDiagnostic_->tickOutput(cloudMsgs[0]->header.stamp); } } diff --git a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp index 81302ac1..975dec03 100644 --- a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp @@ -45,8 +45,6 @@ namespace rtabmap_util PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) : Node("point_cloud_assembler", options), - warningThread_(0), - callbackCalled_(false), exactSync_(0), exactInfoSync_(0), maxClouds_(0), @@ -170,22 +168,14 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) : syncOdomSub_.getSubscriber()->get_topic_name()); } - warningThread_ = new std::thread([&](){ - rclcpp::Rate r(1.0/5.0); - while(!callbackCalled_) - { - r.sleep(); - if(!callbackCalled_) - { - RCLCPP_WARN(this->get_logger(), - "%s: Did not receive data since 5 seconds! Make sure the input topics are " - "published (\"$ ros2 topic hz my_topic\") and the timestamps in their " - "header are set. %s", - get_name(), - subscribedTopicsMsg_.c_str()); - } - } - }); + syncDiagnostic_.reset(new rtabmap_sync::SyncDiagnostic(this, 0.5)); + syncDiagnostic_->init( + cloudSub_?cloudSub_->get_topic_name():syncCloudSub_.getSubscriber()->get_topic_name(), + uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are " + "published (\"$ ros2 topic hz my_topic\") and the timestamps in their " + "header are set. %s", + get_name(), + subscribedTopicsMsg_.c_str())); RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg_.c_str()); } @@ -194,20 +184,12 @@ PointCloudAssembler::~PointCloudAssembler() { delete exactSync_; delete exactInfoSync_; - - if(warningThread_) - { - callbackCalled_=true; - warningThread_->join(); - delete warningThread_; - } } void PointCloudAssembler::callbackCloudOdom( const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg, const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg) { - callbackCalled_ = true; rtabmap::Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose); if(!odom.isNull()) { @@ -270,7 +252,6 @@ void PointCloudAssembler::callbackCloudOdomInfo( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg) { - callbackCalled_ = true; rtabmap::Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose); if(!odom.isNull()) { @@ -293,7 +274,7 @@ void PointCloudAssembler::callbackCloudOdomInfo( void PointCloudAssembler::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg) { - callbackCalled_ = true; + syncDiagnostic_->tickInput(cloudMsg->header.stamp); if(cloudPub_->get_subscription_count()) { UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height, @@ -487,6 +468,7 @@ void PointCloudAssembler::callbackCloud(const sensor_msgs::msg::PointCloud2::Con rosCloud.header.frame_id = frameId_; } cloudPub_->publish(rosCloud); + syncDiagnostic_->tickOutput(cloudMsg->header.stamp); if(circularBuffer_) { if(!isMoving) diff --git a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp index 30c0cab1..44edcc35 100644 --- a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp +++ b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp @@ -164,9 +164,10 @@ void PointCloudToDepthImage::callback( if(cloudDisplacement.isNull()) { - RCLCPP_ERROR(this->get_logger(), "Could not find transform between %s and %s, accordingly to %s, aborting!", - pointCloud2Msg->header.frame_id.c_str(), - cameraInfoMsg->header.frame_id.c_str(), + RCLCPP_ERROR(this->get_logger(), "Could not find how %s moved between the cloud (%f) and the camera info (%f) stamps, accordingly to %s, aborting!", + pointCloud2Msg->header.frame_id.c_str(), + cloudStamp, + infoStamp, fixedFrameId_.c_str()); return; } diff --git a/rtabmap_util/src/nodelets/rgbd_relay.cpp b/rtabmap_util/src/nodelets/rgbd_relay.cpp index 45749ccd..d3bcb301 100644 --- a/rtabmap_util/src/nodelets/rgbd_relay.cpp +++ b/rtabmap_util/src/nodelets/rgbd_relay.cpp @@ -43,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/Compression.h" #include "rtabmap/utilite/UConversion.h" +#include "rtabmap/utilite/ULogger.h" namespace rtabmap_util { @@ -54,11 +55,20 @@ RGBDRelay::RGBDRelay(const rclcpp::NodeOptions & options) : { int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; qos = this->declare_parameter("qos", qos); + // The two sides can be set independently so the relay can bridge a publisher + // and a subscriber that don't agree on reliability. Both default to qos. + int qosSub = this->declare_parameter("qos_sub", qos); + int qosPub = this->declare_parameter("qos_pub", qos); + int queueSub = this->declare_parameter("queue_sub", 5); + int queuePub = this->declare_parameter("queue_pub", 1); compress_ = this->declare_parameter("compress", compress_); uncompress_ = this->declare_parameter("uncompress", uncompress_); - rgbdImageSub_ = create_subscription("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDRelay::callback, this, std::placeholders::_1)); - rgbdImagePub_ = create_publisher("rgbd_image_relay", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); + UASSERT_MSG(queueSub >= 1 && queuePub >= 1, + uFormat("queue_sub (%d) and queue_pub (%d) must be at least 1", queueSub, queuePub).c_str()); + + rgbdImageSub_ = create_subscription("rgbd_image", rclcpp::QoS(queueSub).reliability((rmw_qos_reliability_policy_t)qosSub), std::bind(&RGBDRelay::callback, this, std::placeholders::_1)); + rgbdImagePub_ = create_publisher("rgbd_image_relay", rclcpp::QoS(queuePub).reliability((rmw_qos_reliability_policy_t)qosPub)); } void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) const @@ -125,7 +135,7 @@ void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co // already raw, just copy pointer output->rgb = input->rgb; } - if(!input->rgb_compressed.data.empty()) + else if(!input->rgb_compressed.data.empty()) { cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(output->rgb); } @@ -135,20 +145,37 @@ void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co // already raw, just copy pointer output->depth = input->depth; } - else if(input->depth_compressed.format.compare("jpg")==0) + else if(!input->depth_compressed.data.empty()) { - // right stereo image - cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(output->depth); - } - else - { - // dpeth image + // Decode first, then pick the encoding from what actually came out. + // Branching on the "jpg"/"png" format string instead would abort on a + // right image compressed as PNG, which nothing forbids. auto cvImg = std::make_unique(); cvImg->header = input->depth_compressed.header; cvImg->image = rtabmap::uncompressImage(input->depth_compressed.data); - UASSERT(cvImg->image.empty() || cvImg->image.type() == CV_32FC1 || cvImg->image.type() == CV_16UC1); - cvImg->encoding = cvImg->image.empty()?"":cvImg->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1; - cvImg->toImageMsg(output->depth); + if(cvImg->image.empty()) + { + RCLCPP_ERROR(this->get_logger(), "Could not decompress the depth/right image of \"%s\" (format=\"%s\").", + rgbdImageSub_->get_topic_name(), input->depth_compressed.format.c_str()); + } + else + { + switch(cvImg->image.type()) + { + case CV_32FC1: cvImg->encoding = sensor_msgs::image_encodings::TYPE_32FC1; break; + case CV_16UC1: cvImg->encoding = sensor_msgs::image_encodings::TYPE_16UC1; break; + case CV_8UC1: cvImg->encoding = sensor_msgs::image_encodings::MONO8; break; + case CV_8UC3: cvImg->encoding = sensor_msgs::image_encodings::BGR8; break; + default: + RCLCPP_ERROR(this->get_logger(), "Unsupported decompressed depth/right image type %d.", cvImg->image.type()); + cvImg->image = cv::Mat(); + break; + } + if(!cvImg->image.empty()) + { + cvImg->toImageMsg(output->depth); + } + } } } diff --git a/rtabmap_util/src/nodelets/rgbd_split.cpp b/rtabmap_util/src/nodelets/rgbd_split.cpp index e8c401e7..7d3ad63e 100644 --- a/rtabmap_util/src/nodelets/rgbd_split.cpp +++ b/rtabmap_util/src/nodelets/rgbd_split.cpp @@ -26,6 +26,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include +#include +#include +#include +#include #ifdef PRE_ROS_IRON #include @@ -37,24 +41,55 @@ namespace rtabmap_util { RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) : - Node("rgbd_split", options) + Node("rgbd_split", options), + stereo_(false) { int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; qos = this->declare_parameter("qos", qos); + // Each side can be set independently so the node can bridge a producer and a + // consumer that don't agree on reliability. Both default to qos. + int qosSub = this->declare_parameter("qos_sub", qos); + int qosPub = this->declare_parameter("qos_pub", qos); + int queueSub = this->declare_parameter("queue_sub", 5); + int queuePub = this->declare_parameter("queue_pub", 1); + // A stereo RGBDImage carries the right image in the depth slot, so name the outputs + // left/right instead of rgb/depth to say what they really are. + stereo_ = this->declare_parameter("stereo", false); RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos); + RCLCPP_INFO(this->get_logger(), "%s: queue_sub = %d", get_name(), queueSub); + RCLCPP_INFO(this->get_logger(), "%s: queue_pub = %d", get_name(), queuePub); + RCLCPP_INFO(this->get_logger(), "%s: stereo = %s", get_name(), stereo_?"true":"false"); - rgbdImageSub_ = create_subscription("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDSplit::callback, this, std::placeholders::_1)); + UASSERT_MSG(queueSub >= 1 && queuePub >= 1, + uFormat("queue_sub (%d) and queue_pub (%d) must be at least 1", queueSub, queuePub).c_str()); + + rgbdImageSub_ = create_subscription("rgbd_image", rclcpp::QoS(queueSub).reliability((rmw_qos_reliability_policy_t)qosSub), std::bind(&RGBDSplit::callback, this, std::placeholders::_1)); + + const std::string base = rgbdImageSub_->get_topic_name(); + const std::string firstName = stereo_?"/left":"/rgb"; + const std::string secondName = stereo_?"/right":"/depth"; + const rclcpp::QoS pubQos = rclcpp::QoS(queuePub).reliability((rmw_qos_reliability_policy_t)qosPub); #ifdef PRE_ROS_LYRICAL - rgbPub_ = image_transport::create_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - depthPub_ = image_transport::create_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + rgbPub_ = image_transport::create_publisher(this, base + firstName + "/image", pubQos.get_rmw_qos_profile()); + depthPub_ = image_transport::create_publisher(this, base + secondName + "/image", pubQos.get_rmw_qos_profile()); #else - rgbPub_ = image_transport::create_publisher(*this, std::string(rgbdImageSub_->get_topic_name()) + "/rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); - depthPub_ = image_transport::create_publisher(*this, std::string(rgbdImageSub_->get_topic_name()) + "/depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); + rgbPub_ = image_transport::create_publisher(*this, base + firstName + "/image", pubQos); + depthPub_ = image_transport::create_publisher(*this, base + secondName + "/image", pubQos); #endif - rgbInfoPub_ = this->create_publisher(std::string(rgbdImageSub_->get_topic_name()) + "/rgb/camera_info", 1); - depthInfoPub_ = this->create_publisher(std::string(rgbdImageSub_->get_topic_name()) + "/depth/camera_info", 1); + rgbInfoPub_ = this->create_publisher(base + firstName + "/camera_info", pubQos); + depthInfoPub_ = this->create_publisher(base + secondName + "/camera_info", pubQos); + + // Resolved names: the outputs are derived from the input topic, so a remapping of + // "rgbd_image" moves all four with it. Print them so it is clear what to subscribe to. + RCLCPP_INFO(this->get_logger(), "%s: subscribed to:\n %s", get_name(), rgbdImageSub_->get_topic_name()); + RCLCPP_INFO(this->get_logger(), "%s: publishing:\n %s,\n %s,\n %s,\n %s", + get_name(), + rgbPub_.getTopic().c_str(), + rgbInfoPub_->get_topic_name(), + depthPub_.getTopic().c_str(), + depthInfoPub_->get_topic_name()); } @@ -100,7 +135,36 @@ void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co #ifdef CV_BRIDGE_HYDRO ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this."); #else - cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(outputImage); + // Decode first, then pick the encoding from what actually came out. Going + // by the "jpg"/"png" format string instead would mislabel a depth PNG as + // mono8 (cv_bridge cannot infer 16-bit from it), and would abort outright on + // a right image compressed as PNG, which nothing forbids. + cv_bridge::CvImage cvImg; + cvImg.header = input->depth_compressed.header; + cvImg.image = rtabmap::uncompressImage(input->depth_compressed.data); + if(cvImg.image.empty()) + { + RCLCPP_ERROR(this->get_logger(), "Could not decompress the depth/right image of \"%s\" (format=\"%s\").", + rgbdImageSub_->get_topic_name(), input->depth_compressed.format.c_str()); + } + else + { + switch(cvImg.image.type()) + { + case CV_32FC1: cvImg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; break; + case CV_16UC1: cvImg.encoding = sensor_msgs::image_encodings::TYPE_16UC1; break; + case CV_8UC1: cvImg.encoding = sensor_msgs::image_encodings::MONO8; break; + case CV_8UC3: cvImg.encoding = sensor_msgs::image_encodings::BGR8; break; + default: + RCLCPP_ERROR(this->get_logger(), "Unsupported decompressed depth/right image type %d.", cvImg.image.type()); + cvImg.image = cv::Mat(); + break; + } + } + if(!cvImg.image.empty()) + { + cvImg.toImageMsg(outputImage); + } #endif } if(outputCameraInfo.header.frame_id.empty()) { @@ -119,6 +183,37 @@ void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co outputImage.header = outputCameraInfo.header; } } + // The "depth" slot of an RGBDImage holds either a depth image or the right image + // of a stereo pair, and "stereo" decides which name it goes out under. Warn when + // the two disagree: the topic name would be lying to every consumer downstream. + // Both directions only warn and keep forwarding -- publishing a right image on + // the depth topic is what this node has always done, and setups rely on it. + if(!outputImage.data.empty()) + { + const bool isDepth = + outputImage.encoding == sensor_msgs::image_encodings::TYPE_16UC1 || + outputImage.encoding == sensor_msgs::image_encodings::TYPE_32FC1 || + outputImage.encoding == sensor_msgs::image_encodings::MONO16; + if(stereo_ && isDepth) + { + RCLCPP_WARN_ONCE(this->get_logger(), + "Parameter \"stereo\" is true, so the second half is published as \"%s\", " + "but the received image is a depth image (encoding=\"%s\"), not the right " + "image of a stereo pair. Set \"stereo\" to false to publish it as depth. " + "(This warning is printed only once)", + depthPub_.getTopic().c_str(), outputImage.encoding.c_str()); + } + else if(!stereo_ && !isDepth) + { + RCLCPP_WARN_ONCE(this->get_logger(), + "Parameter \"stereo\" is false, so the second half is published as \"%s\", " + "but the received image is not a depth image (encoding=\"%s\"): it looks " + "like the right image of a stereo pair. Set \"stereo\" to true to publish " + "it under a name that says so. (This warning is printed only once)", + depthPub_.getTopic().c_str(), outputImage.encoding.c_str()); + } + } + depthPub_.publish(outputImage); depthInfoPub_->publish(outputCameraInfo); } diff --git a/rtabmap_util/test/db_builders.hpp b/rtabmap_util/test/db_builders.hpp new file mode 100644 index 00000000..bc6340d1 --- /dev/null +++ b/rtabmap_util/test/db_builders.hpp @@ -0,0 +1,366 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#ifndef RTABMAP_UTIL_DB_BUILDERS_HPP_ +#define RTABMAP_UTIL_DB_BUILDERS_HPP_ + +/** + * @file + * @brief Synthetic RTAB-Map databases for the db_player tests. + * + * db_player replays whatever a database happens to contain, and which topics it even + * creates depends on the payloads it finds. Rather than ship a recorded database, each + * scenario is written here with DBDriver so the expected values are visible right next + * to the assertions. + * + * @note Only the *compressed* buffers of a SensorData are persisted, so every payload is + * compressed before being handed to the driver. Saving raw-only data silently + * writes empty blobs. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include + +namespace rtabmap_util_test { + +//============================================================================ +// What every synthetic database contains, and what the tests assert against +//============================================================================ + +constexpr int kDbFrames = 3; ///< nodes in each database +constexpr double kFirstStamp = 1000.0; ///< stamp of node 1, seconds +constexpr double kStampStep = 0.05; ///< seconds between consecutive nodes +constexpr float kPoseStep = 0.5f; ///< meters along x between odometry poses +constexpr double kOdomVariance = 0.25; ///< diagonal of the odometry covariance + +constexpr int kImageWidth = 80; +constexpr int kImageHeight = 60; +constexpr double kFx = 100.0; +constexpr double kFy = 100.0; +constexpr double kCx = 40.0; +constexpr double kCy = 30.0; +constexpr double kBaseline = 0.12; +constexpr uint16_t kDepthMillimeters = 1500; + +constexpr double kGpsLongitude = -71.9; +constexpr double kGpsLatitude = 45.4; +constexpr double kGpsAltitude = 123.0; +constexpr double kGpsError = 2.5; +constexpr double kEnvSensorValue = 21.5; + +/// The odometry pose of node @p id, one step further along x than the previous one. +inline rtabmap::Transform poseOf(int id) +{ + return rtabmap::Transform(kPoseStep * float(id - 1), 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); +} + +/// The stamp of node @p id. +inline double stampOfNode(int id) +{ + return kFirstStamp + kStampStep * double(id - 1); +} + +/// Where the camera sits on the robot: 10 cm forward, 20 cm up, looking forward. +inline rtabmap::Transform cameraLocalTransform() +{ + return rtabmap::Transform(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f) * + rtabmap::CameraModel::opticalRotation(); +} + +/// Where the lidar sits on the robot. +inline rtabmap::Transform scanLocalTransform() +{ + return rtabmap::Transform(0.05f, 0.0f, 0.3f, 0.0f, 0.0f, 0.0f); +} + +/// The ground truth pose of node @p id, offset from the odometry pose so they differ. +inline rtabmap::Transform groundTruthOf(int id) +{ + return rtabmap::Transform(kPoseStep * float(id - 1), 1.0f, 0.0f, 0.0f, 0.0f, 0.0f); +} + +/// The prior (global) pose of node @p id. +inline rtabmap::Transform globalPoseOf(int id) +{ + return rtabmap::Transform(kPoseStep * float(id - 1), 2.0f, 0.0f, 0.0f, 0.0f, 0.0f); +} + +//============================================================================ +// A database file that cleans itself up +//============================================================================ + +/// A uniquely named database path under the test temp directory, erased on destruction. +class TempDatabase +{ +public: + explicit TempDatabase(const std::string & tag) + { + static int counter = 0; + path_ = std::string(::testing::TempDir()) + + uFormat("rtabmap_util_db_player_%s_%d_%d.db", tag.c_str(), (int)getpid(), ++counter); + UFile::erase(path_.c_str()); + } + + ~TempDatabase() { UFile::erase(path_.c_str()); } + + TempDatabase(const TempDatabase &) = delete; + TempDatabase & operator=(const TempDatabase &) = delete; + + const std::string & path() const { return path_; } + +private: + std::string path_; +}; + +//============================================================================ +// Writing the databases +//============================================================================ + +/// Builds the sensor payload of node @p id; see the writeXxxDatabase() functions. +typedef std::function DataBuilder; + +/// Adds anything beyond the payload: links, ground truth, and so on. +typedef std::function NodeDecorator; + +/** + * @brief Writes @p frames consecutive nodes sharing the plumbing every replay needs. + * + * Nodes are numbered from 1, stamped kStampStep apart (db_player replays at the database + * stamps, so a node without one aborts the read), posed kPoseStep apart along x, and + * joined by the neighbor links that carry the odometry covariance. + */ +inline void writeDatabase( + const std::string & path, int frames, + const DataBuilder & makeData, + const NodeDecorator & decorate = NodeDecorator()) +{ + rtabmap::DBDriver * driver = rtabmap::DBDriver::create(); + ASSERT_NE(driver, nullptr); + ASSERT_TRUE(driver->openConnection(path, /*overwritten=*/true)) << "cannot create " << path; + + for(int id=1; id<=frames; ++id) + { + const double stamp = stampOfNode(id); + rtabmap::Signature * s = new rtabmap::Signature( + id, /*mapId=*/0, /*weight=*/1, stamp, /*label=*/"", + poseOf(id), rtabmap::Transform(), makeData(id, stamp)); + + if(id > 1) + { + // The backward neighbor link is where DBReader reads the odometry + // covariance from: it publishes the inverse of this information matrix. + const rtabmap::Transform motion(kPoseStep, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); + s->addLink(rtabmap::Link(id, id-1, rtabmap::Link::kNeighbor, motion.inverse(), + cv::Mat::eye(6, 6, CV_64FC1) / kOdomVariance)); + } + if(decorate) + { + decorate(id, *s); + } + + driver->asyncSave(s); // the driver takes ownership + driver->emptyTrashes(false); + } + + driver->closeConnection(true); + delete driver; +} + +/// A color image whose pixels identify the node, so a test can tell frames apart. +inline cv::Mat makeRgb(int id) +{ + return cv::Mat(kImageHeight, kImageWidth, CV_8UC3, cv::Scalar(id, 2*id, 3*id)); +} + +inline cv::Mat makeDepth() +{ + return cv::Mat(kImageHeight, kImageWidth, CV_16UC1, cv::Scalar(kDepthMillimeters)); +} + +inline rtabmap::CameraModel rgbdCameraModel() +{ + return rtabmap::CameraModel(kFx, kFy, kCx, kCy, cameraLocalTransform(), 0.0, + cv::Size(kImageWidth, kImageHeight)); +} + +inline rtabmap::StereoCameraModel stereoCameraModel() +{ + return rtabmap::StereoCameraModel(kFx, kFy, kCx, kCy, kBaseline, cameraLocalTransform(), + cv::Size(kImageWidth, kImageHeight)); +} + +/// RGB + registered depth from a single camera. +inline void writeRgbdDatabase(const std::string & path, int frames = kDbFrames) +{ + writeDatabase(path, frames, [](int id, double stamp) { + rtabmap::SensorData data; + data.setId(id); + data.setStamp(stamp); + data.setRGBDImage(rtabmap::compressImage2(makeRgb(id), ".png"), + rtabmap::compressImage2(makeDepth(), ".png"), rgbdCameraModel()); + return data; + }); +} + +/// A rectified mono stereo pair. +inline void writeStereoDatabase(const std::string & path, int frames = kDbFrames) +{ + writeDatabase(path, frames, [](int id, double stamp) { + const cv::Mat left(kImageHeight, kImageWidth, CV_8UC1, cv::Scalar(id)); + const cv::Mat right(kImageHeight, kImageWidth, CV_8UC1, cv::Scalar(2*id)); + rtabmap::SensorData data; + data.setId(id); + data.setStamp(stamp); + data.setStereoImage(rtabmap::compressImage2(left, ".png"), + rtabmap::compressImage2(right, ".png"), stereoCameraModel()); + return data; + }); +} + +/// A color image with no calibration at all, which db_player replays on "image". +inline void writeImageOnlyDatabase(const std::string & path, int frames = kDbFrames) +{ + writeDatabase(path, frames, [](int id, double stamp) { + rtabmap::SensorData data; + data.setId(id); + data.setStamp(stamp); + data.setRGBDImage(rtabmap::compressImage2(makeRgb(id), ".png"), cv::Mat(), + std::vector()); + return data; + }); +} + +//============================================================================ +// Laser scans +//============================================================================ + +constexpr int kScanBins = 20; +constexpr float kScanAngleMin = -1.0f; +constexpr float kScanAngleMax = 1.0f; +constexpr float kScanAngleIncrement = 0.1f; // (max-min)/kScanBins +constexpr float kScanRangeMin = 0.1f; +constexpr float kScanRangeMax = 10.0f; + +/// The range measured in bin @p bin of the 2D scan. +inline float scanRangeOf(int bin) { return 1.0f + 0.1f * float(bin); } + +/** + * @brief A 2D scan with one point at the center of every bin. + * + * db_player re-bins the cartesian points back into a LaserScan message, so putting each + * point at a bin center makes the expected index exact rather than a rounding coin flip. + */ +inline rtabmap::LaserScan makeScan2d() +{ + cv::Mat points(1, kScanBins, CV_32FC2); + for(int bin=0; bin(0, bin) = cv::Vec2f(range * std::cos(angle), range * std::sin(angle)); + } + return rtabmap::LaserScan(rtabmap::compressData2(points), rtabmap::LaserScan::kXY, + kScanRangeMin, kScanRangeMax, kScanAngleMin, kScanAngleMax, kScanAngleIncrement, + scanLocalTransform()); +} + +constexpr int kScanCloudPoints = 50; + +inline rtabmap::LaserScan makeScan3d() +{ + cv::Mat points(1, kScanCloudPoints, CV_32FC3); + for(int i=0; i(0, i) = cv::Vec3f(1.0f + 0.01f*float(i), 0.02f*float(i), 0.5f); + } + return rtabmap::LaserScan(rtabmap::compressData2(points), /*maxPoints=*/0, /*maxRange=*/0.0f, + rtabmap::LaserScan::kXYZ, scanLocalTransform()); +} + +/// A 2D lidar only, no camera. +inline void writeScan2dDatabase(const std::string & path, int frames = kDbFrames) +{ + writeDatabase(path, frames, [](int id, double stamp) { + rtabmap::SensorData data; + data.setId(id); + data.setStamp(stamp); + data.setLaserScan(makeScan2d()); + return data; + }); +} + +/// A 3D lidar only, no camera. +inline void writeScan3dDatabase(const std::string & path, int frames = kDbFrames) +{ + writeDatabase(path, frames, [](int id, double stamp) { + rtabmap::SensorData data; + data.setId(id); + data.setStamp(stamp); + data.setLaserScan(makeScan3d()); + return data; + }); +} + +//============================================================================ +// Everything else db_player can replay +//============================================================================ + +/// The gravity orientation stored as a link, which is how DBReader rebuilds an IMU. +inline rtabmap::Transform gravityTransform() +{ + return rtabmap::Transform(0.0f, 0.0f, 0.0f, 0.1f, 0.2f, 0.0f); +} + +/** + * @brief RGB-D plus the optional channels: ground truth, prior pose, GPS, gravity and an + * environmental sensor. + * + * @note The prior's information matrix must not leave a huge rotational variance, or + * DBReader drops the global pose on the assumption GPS already provided the prior. + */ +inline void writeRichDatabase(const std::string & path, int frames = kDbFrames) +{ + writeDatabase(path, frames, + [](int id, double stamp) { + rtabmap::SensorData data; + data.setId(id); + data.setStamp(stamp); + data.setRGBDImage(rtabmap::compressImage2(makeRgb(id), ".png"), + rtabmap::compressImage2(makeDepth(), ".png"), rgbdCameraModel()); + data.setGPS(rtabmap::GPS(stamp, kGpsLongitude, kGpsLatitude, kGpsAltitude, + kGpsError, /*bearing=*/0.0)); + rtabmap::EnvSensors sensors; + sensors.insert(std::make_pair(rtabmap::EnvSensor::kAmbientTemperature, + rtabmap::EnvSensor(rtabmap::EnvSensor::kAmbientTemperature, + kEnvSensorValue, stamp))); + data.setEnvSensors(sensors); + return data; + }, + [](int id, rtabmap::Signature & s) { + s.setGroundTruthPose(groundTruthOf(id)); + s.addLink(rtabmap::Link(id, id, rtabmap::Link::kPosePrior, globalPoseOf(id), + cv::Mat::eye(6, 6, CV_64FC1) * 100.0)); + s.addLink(rtabmap::Link(id, id, rtabmap::Link::kGravity, gravityTransform(), + cv::Mat::eye(6, 6, CV_64FC1))); + }); +} + +} // namespace rtabmap_util_test + +#endif /* RTABMAP_UTIL_DB_BUILDERS_HPP_ */ diff --git a/rtabmap_util/test/msg_builders.hpp b/rtabmap_util/test/msg_builders.hpp new file mode 100644 index 00000000..5a23bab7 --- /dev/null +++ b/rtabmap_util/test/msg_builders.hpp @@ -0,0 +1,217 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#ifndef RTABMAP_UTIL_MSG_BUILDERS_HPP_ +#define RTABMAP_UTIL_MSG_BUILDERS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#ifdef PRE_ROS_IRON +#include +#else +#include +#endif + +#include +#include +#include + +namespace rtabmap_util_test { + +inline rclcpp::Time stampOf(double seconds) +{ + return rclcpp::Time( + int32_t(seconds), uint32_t((seconds - int32_t(seconds)) * 1e9), RCL_ROS_TIME); +} + +/// A rectified pinhole CameraInfo; @p tx is P(0,3), non-zero for a stereo right camera. +inline sensor_msgs::msg::CameraInfo makeCameraInfo( + const std::string & frameId, double stamp, int width, int height, + double tx = 0.0, double fx = 100.0) +{ + sensor_msgs::msg::CameraInfo info; + info.header.frame_id = frameId; + info.header.stamp = stampOf(stamp); + info.width = width; + info.height = height; + info.distortion_model = "plumb_bob"; + info.d = {0.0, 0.0, 0.0, 0.0, 0.0}; + info.k = {fx, 0.0, width/2.0, 0.0, fx, height/2.0, 0.0, 0.0, 1.0}; + info.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + info.p = {fx, 0.0, width/2.0, tx, 0.0, fx, height/2.0, 0.0, 0.0, 0.0, 1.0, 0.0}; + return info; +} + +inline sensor_msgs::msg::Image makeImage( + const std::string & frameId, double stamp, + const cv::Mat & image, const std::string & encoding) +{ + std_msgs::msg::Header header; + header.frame_id = frameId; + header.stamp = stampOf(stamp); + sensor_msgs::msg::Image msg; + cv_bridge::CvImage(header, encoding, image).toImageMsg(msg); + return msg; +} + +/// An RGB-D message with raw bgr8 color and 16UC1 depth. +inline rtabmap_msgs::msg::RGBDImage makeRGBDImage( + const std::string & frameId, double stamp, int width = 8, int height = 8, + const cv::Scalar & rgbColor = cv::Scalar(10, 20, 30), uint16_t depthValue = 1500) +{ + rtabmap_msgs::msg::RGBDImage msg; + msg.header.frame_id = frameId; + msg.header.stamp = stampOf(stamp); + msg.rgb = makeImage(frameId, stamp, cv::Mat(height, width, CV_8UC3, rgbColor), "bgr8"); + msg.depth = makeImage(frameId, stamp, + cv::Mat(height, width, CV_16UC1, cv::Scalar(depthValue)), "16UC1"); + msg.rgb_camera_info = makeCameraInfo(frameId, stamp, width, height); + msg.depth_camera_info = makeCameraInfo(frameId, stamp, width, height); + return msg; +} + +/** + * @brief An RGB-D message carrying a stereo pair instead of depth. + * + * The "depth" slot holds the mono8 right image and the second camera info carries the + * baseline in P(0,3), which is what makes consumers treat the pair as stereo rather + * than as color plus depth. + */ +inline rtabmap_msgs::msg::RGBDImage makeStereoRGBDImage( + const std::string & frameId, double stamp, int width = 8, int height = 8, + double baseline = 0.12, double fx = 100.0) +{ + rtabmap_msgs::msg::RGBDImage msg; + msg.header.frame_id = frameId; + msg.header.stamp = stampOf(stamp); + msg.rgb = makeImage(frameId, stamp, + cv::Mat(height, width, CV_8UC3, cv::Scalar(10, 20, 30)), "bgr8"); + msg.depth = makeImage(frameId, stamp, + cv::Mat(height, width, CV_8UC1, cv::Scalar(60)), "mono8"); // right image + msg.rgb_camera_info = makeCameraInfo(frameId, stamp, width, height, 0.0, fx); + msg.depth_camera_info = + makeCameraInfo(frameId, stamp, width, height, -fx*baseline, fx); + return msg; +} + +/** + * @brief A dense unorganized XYZ float cloud. + * + * @note This writes the points exactly as given: it does not model sensor motion. To + * build a cloud that deskewing can actually correct, use makeSkewedWallScan(), + * which derives the distortion from the same trajectory the TF describes. + * + * @param withTimeChannel add a FLOAT32 "t" channel of per-point offsets, as a spinning + * lidar publishes, so the cloud can be deskewed. + */ +inline sensor_msgs::msg::PointCloud2 makeXYZCloud( + const std::string & frameId, double stamp, + const std::vector & points, + bool withTimeChannel = false, double sweepDuration = 0.099) +{ + sensor_msgs::msg::PointCloud2 cloud; + cloud.header.frame_id = frameId; + cloud.header.stamp = stampOf(stamp); + cloud.height = 1; + cloud.width = points.size(); + cloud.is_bigendian = false; + cloud.is_dense = true; + + const int fieldCount = withTimeChannel ? 4 : 3; + cloud.fields.resize(fieldCount); + const char * names[4] = {"x", "y", "z", "t"}; + for(int i=0; i(&cloud.data[i * cloud.point_step]); + p[0] = points[i].x; + p[1] = points[i].y; + p[2] = points[i].z; + if(withTimeChannel) + { + p[3] = points.size() > 1 + ? float(sweepDuration * double(i) / double(points.size() - 1)) + : 0.0f; + } + } + return cloud; +} + +/** + * @brief The raw scan of a flat wall captured while the sensor moves straight at it. + * + * Sample @p i is taken at `i * step` seconds into the sweep, by which time the sensor + * has closed in by `displacement(elapsed)`. Expressed in the sensor frame at capture + * time the wall therefore appears to slide closer: a straight wall is recorded bent. + * Deskewing with the same motion must flatten it back to @p wallDistance. + * + * @param frameId sensor frame + * @param stamp stamp of the first sample, which is also the message stamp + * @param sampleCount number of samples along the wall + * @param sweepDuration seconds from the first sample to the last + * @param wallDistance distance to the wall at the first sample, in meters + * @param displacement distance travelled as a function of seconds since the first + * sample; must match the motion published to TF + */ +inline sensor_msgs::msg::PointCloud2 makeSkewedWallScan( + const std::string & frameId, double stamp, + size_t sampleCount, double sweepDuration, float wallDistance, + const std::function & displacement) +{ + std::vector points; + points.reserve(sampleCount); + for(size_t i=0; i 1 ? sweepDuration * double(i) / double(sampleCount - 1) : 0.0; + points.push_back(cv::Point3f( + wallDistance - float(displacement(elapsed)), // the skew + -1.0f + 2.0f * float(i) / float(sampleCount > 1 ? sampleCount - 1 : 1), + 0.0f)); + } + return makeXYZCloud(frameId, stamp, points, /*withTimeChannel=*/true, sweepDuration); +} + +/** + * @brief Reads the x/y/z of a point from any FLOAT32 xyz cloud. + * + * Looks the offsets up in the field list rather than assuming they are 0/4/8, so it also + * works on clouds produced by laser_geometry, which lay their fields out differently. + */ +inline cv::Point3f readXYZ(const sensor_msgs::msg::PointCloud2 & cloud, size_t index) +{ + uint32_t xOffset = 0, yOffset = 4, zOffset = 8; + for(size_t i=0; i(base + xOffset), + *reinterpret_cast(base + yOffset), + *reinterpret_cast(base + zOffset)); +} + +} // namespace rtabmap_util_test + +#endif /* RTABMAP_UTIL_MSG_BUILDERS_HPP_ */ diff --git a/rtabmap_util/test/node_test_utils.hpp b/rtabmap_util/test/node_test_utils.hpp new file mode 100644 index 00000000..1031c1c1 --- /dev/null +++ b/rtabmap_util/test/node_test_utils.hpp @@ -0,0 +1,320 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef RTABMAP_UTIL_NODE_TEST_UTILS_HPP_ +#define RTABMAP_UTIL_NODE_TEST_UTILS_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace rtabmap_util_test { + +/** + * @brief Brings rclcpp up once for the whole test binary. + * + * Registered as a gtest global environment so it runs before the first test and shuts + * down after the last one, which keeps gtest_main usable. + */ +class RclcppEnvironment : public ::testing::Environment +{ +public: + void SetUp() override + { + if(!rclcpp::ok()) + { + rclcpp::init(0, nullptr); + } + } + void TearDown() override + { + if(rclcpp::ok()) + { + rclcpp::shutdown(); + } + } +}; + +/// Registers RclcppEnvironment. Call once at file scope in each test binary. +inline ::testing::Environment * registerRclcppEnvironment() +{ + static ::testing::Environment * const env = + ::testing::AddGlobalTestEnvironment(new RclcppEnvironment); + return env; +} + +/** + * @brief Base fixture for driving a node under test over real ROS topics. + * + * The node under test and a helper node share one single-threaded executor, so + * publishing, the node's callback and the assertion all happen on the same thread and + * the tests stay deterministic. No launch files and no separate processes are involved: + * everything runs in the gtest binary. + */ +class NodeTest : public ::testing::Test +{ +protected: + void SetUp() override + { + executor_ = std::make_shared(); + helper_ = std::make_shared("rtabmap_util_test_helper"); + executor_->add_node(helper_); + } + + void TearDown() override + { + for(const rclcpp::Node::SharedPtr & node : nodes_) + { + executor_->remove_node(node); + } + nodes_.clear(); + executor_->remove_node(helper_); + helper_.reset(); + executor_.reset(); + } + + /// Adds a node under test to the shared executor and keeps it alive for the test. + template + std::shared_ptr addNode(const std::shared_ptr & node) + { + executor_->add_node(node); + nodes_.push_back(node); + return node; + } + + /// The helper node, used to publish inputs and subscribe to outputs. + rclcpp::Node::SharedPtr helper() { return helper_; } + + /** + * @brief Spins until @p done returns true, or the timeout elapses. + * @return true if @p done became true + */ + bool spinUntil( + const std::function & done, + std::chrono::milliseconds timeout = std::chrono::milliseconds(5000)) + { + const std::chrono::steady_clock::time_point deadline = + std::chrono::steady_clock::now() + timeout; + while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline) + { + if(done()) + { + return true; + } + executor_->spin_once(std::chrono::milliseconds(10)); + } + return done(); + } + + /** + * @brief Runs every node of the fixture on a multi-threaded executor until @p done. + * + * A node whose callback waits on another of its own callbacks -- a service call made + * from a timer, say -- makes no progress under spinUntil(), because the second + * callback cannot run while the first is still on the stack. Such nodes put the two + * callbacks in different callback groups precisely so a multi-threaded executor can + * overlap them; this hands them the threads to do it, then puts the nodes back on the + * usual single-threaded executor. + * + * @warning Callbacks run on executor threads for the duration, so do not have any + * Collector subscribed while this runs: the test thread would read its + * messages while another thread appends to them. Use it to get a node + * through its start-up handshake, before subscribing to anything. + */ + bool spinMultiThreadedUntil( + const std::function & done, + std::chrono::milliseconds timeout = std::chrono::milliseconds(15000)) + { + rclcpp::executors::MultiThreadedExecutor booting(rclcpp::ExecutorOptions(), 4); + for(const rclcpp::Node::SharedPtr & node : nodes_) + { + executor_->remove_node(node); + booting.add_node(node); + } + executor_->remove_node(helper_); + booting.add_node(helper_); + + std::thread spinner([&booting]() { booting.spin(); }); + const std::chrono::steady_clock::time_point deadline = + std::chrono::steady_clock::now() + timeout; + while(rclcpp::ok() && !done() && std::chrono::steady_clock::now() < deadline) + { + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + const bool result = done(); + booting.cancel(); + spinner.join(); + + booting.remove_node(helper_); + executor_->add_node(helper_); + for(const rclcpp::Node::SharedPtr & node : nodes_) + { + booting.remove_node(node); + executor_->add_node(node); + } + return result; + } + + /// Spins for a fixed duration, for the "nothing should happen" assertions. + void spinFor(std::chrono::milliseconds duration) + { + const std::chrono::steady_clock::time_point deadline = + std::chrono::steady_clock::now() + duration; + while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline) + { + executor_->spin_once(std::chrono::milliseconds(10)); + } + } + + /** + * @brief Waits until @p publisher has at least @p count matched subscriptions. + * + * Publishing before the node under test has discovered the topic silently drops the + * message, which is the most common cause of a flaky in-process node test. + */ + template + bool waitForSubscriber(const PublisherT & publisher, size_t count = 1) + { + return spinUntil([&]() { return publisher->get_subscription_count() >= count; }); + } + + /** + * @brief Waits until @p subscription sees at least one publisher. + * + * Several nodes only publish when they have subscribers, so the test's subscription + * has to be discovered before the input is sent. + */ + template + bool waitForPublisher(const SubscriptionT & subscription, size_t count = 1) + { + return spinUntil([&]() { return subscription->get_publisher_count() >= count; }); + } + + /** + * @brief Publishes a static transform on /tf_static. + * + * /tf_static is transient-local, so a listener that subscribes later still receives + * it. That makes static frames far less timing-sensitive in tests than /tf. + */ + void publishStaticTf( + const std::string & parent, const std::string & child, + double x = 0.0, double y = 0.0, double z = 0.0) + { + if(!staticTfPublisher_) + { + staticTfPublisher_ = helper_->create_publisher( + "/tf_static", rclcpp::QoS(100).transient_local()); + } + geometry_msgs::msg::TransformStamped t; + t.header.stamp = helper_->now(); + t.header.frame_id = parent; + t.child_frame_id = child; + t.transform.translation.x = x; + t.transform.translation.y = y; + t.transform.translation.z = z; + t.transform.rotation.w = 1.0; + tf2_msgs::msg::TFMessage msg; + msg.transforms.push_back(t); + staticTfPublisher_->publish(msg); + spinFor(std::chrono::milliseconds(100)); + } + + /// Publishes a static transform with a rotation, given as roll/pitch/yaw. + void publishStaticTfRPY( + const std::string & parent, const std::string & child, + double roll, double pitch, double yaw, + double x = 0.0, double y = 0.0, double z = 0.0) + { + if(!staticTfPublisher_) + { + staticTfPublisher_ = helper_->create_publisher( + "/tf_static", rclcpp::QoS(100).transient_local()); + } + tf2::Quaternion q; + q.setRPY(roll, pitch, yaw); + geometry_msgs::msg::TransformStamped t; + t.header.stamp = helper_->now(); + t.header.frame_id = parent; + t.child_frame_id = child; + t.transform.translation.x = x; + t.transform.translation.y = y; + t.transform.translation.z = z; + t.transform.rotation = tf2::toMsg(q); + tf2_msgs::msg::TFMessage msg; + msg.transforms.push_back(t); + staticTfPublisher_->publish(msg); + spinFor(std::chrono::milliseconds(100)); + } + + /// Collects every message received on @p topic, for later assertions. + template + struct Collector + { + typename rclcpp::Subscription::SharedPtr subscription; + std::vector messages; + size_t size() const { return messages.size(); } + bool empty() const { return messages.empty(); } + const MsgT & back() const { return *messages.back(); } + const MsgT & front() const { return *messages.front(); } + }; + + /// Subscribes the helper node to @p topic and records everything it receives. + template + std::shared_ptr> collect( + const std::string & topic, const rclcpp::QoS & qos = rclcpp::QoS(10)) + { + std::shared_ptr> collector = std::make_shared>(); + collector->subscription = helper_->create_subscription( + topic, qos, + [collector](const typename MsgT::ConstSharedPtr msg) { + collector->messages.push_back(msg); + }); + return collector; + } + +private: + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + rclcpp::Node::SharedPtr helper_; + std::vector nodes_; + rclcpp::Publisher::SharedPtr staticTfPublisher_; +}; + +} // namespace rtabmap_util_test + +#endif /* RTABMAP_UTIL_NODE_TEST_UTILS_HPP_ */ diff --git a/rtabmap_util/test/test_db_player.cpp b/rtabmap_util/test/test_db_player.cpp new file mode 100644 index 00000000..f045de04 --- /dev/null +++ b/rtabmap_util/test/test_db_player.cpp @@ -0,0 +1,809 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "node_test_utils.hpp" +#include "db_builders.hpp" + +#include + +#include + +#include +#include + +#include + +using namespace rtabmap_util_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); + +/// Replay at 1000x the recorded stamps: db_player sleeps between frames otherwise. +constexpr double kReplayRate = 1000.0; + +bool hasParameter(const std::vector & overrides, const std::string & name) +{ + for(size_t i=0; i overrides = {}) + { + if(!hasParameter(overrides, "database")) + { + overrides.push_back(rclcpp::Parameter("database", databasePath)); + } + if(!hasParameter(overrides, "rate")) + { + overrides.push_back(rclcpp::Parameter("rate", kReplayRate)); + } + player_ = addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(overrides))); + } + + /// Reads one frame, which is what creates the publishers. + void primePublishers() + { + ASSERT_TRUE(player_->publishNextFrame()) << "the database has no readable frame"; + } + + /// Replays frames until @p done, or the database runs out. + bool replayUntil(const std::function & done) + { + for(int i=0; ipublishNextFrame()) { break; } + spinFor(std::chrono::milliseconds(30)); + } + return done(); + } + + /** + * @brief The database node a replayed message came from, recovered from its stamp. + * + * How many frames a test ends up replaying depends on discovery, so the expected + * pose is derived from the stamp the message itself carries rather than assumed. + * That also checks the stamp really comes from the database. + */ + static int nodeIdOf(const builtin_interfaces::msg::Time & stamp) + { + const double seconds = rtabmap_conversions::timestampFromROS(stamp); + return int(std::round((seconds - kFirstStamp) / kStampStep)) + 1; + } + + /// The most recent transform published for @p parent -> @p child. + static bool findTransform( + const Collector & tf, + const std::string & parent, const std::string & child, + geometry_msgs::msg::TransformStamped & out) + { + bool found = false; + for(size_t i=0; itransforms.size(); ++j) + { + const geometry_msgs::msg::TransformStamped & t = tf.messages[i]->transforms[j]; + if(t.header.frame_id == parent && t.child_frame_id == child) + { + out = t; + found = true; + } + } + } + return found; + } + + static rtabmap::Transform toRtabmap(const geometry_msgs::msg::TransformStamped & t) + { + return rtabmap_conversions::transformFromGeometryMsg(t.transform); + } + + /// Subscribes to /tf and waits for db_player's broadcaster to be discovered. + std::shared_ptr> collectTf() + { + std::shared_ptr> tf = + collect("/tf", rclcpp::QoS(100)); + EXPECT_TRUE(waitForPublisher(tf->subscription)); + return tf; + } + + std::shared_ptr player_; +}; + +//============================================================================ +// RGB-D +//============================================================================ + +TEST_F(DbPlayerTest, ReplaysRgbAndDepthImages) +{ + TempDatabase db("rgbd"); + writeRgbdDatabase(db.path()); + start(db.path()); + primePublishers(); + + std::shared_ptr> rgb = + collect("rgb/image"); + std::shared_ptr> depth = + collect("depth/image"); + ASSERT_TRUE(waitForPublisher(rgb->subscription)); + ASSERT_TRUE(waitForPublisher(depth->subscription)); + + ASSERT_TRUE(replayUntil([&]() { return !rgb->empty() && !depth->empty(); })) + << "no image replayed"; + + EXPECT_EQ(rgb->back().encoding, sensor_msgs::image_encodings::BGR8); + EXPECT_EQ(rgb->back().width, uint32_t(kImageWidth)); + EXPECT_EQ(rgb->back().height, uint32_t(kImageHeight)); + EXPECT_EQ(rgb->back().header.frame_id, "camera_optical_link"); + + EXPECT_EQ(depth->back().encoding, sensor_msgs::image_encodings::TYPE_16UC1); + EXPECT_EQ(depth->back().header.frame_id, "camera_optical_link") + << "depth is registered with the color camera, so it shares its frame"; + EXPECT_EQ(*reinterpret_cast(depth->back().data.data()), kDepthMillimeters); +} + +TEST_F(DbPlayerTest, StampsImagesWithTheDatabaseStamps) +{ + TempDatabase db("stamps"); + writeRgbdDatabase(db.path()); + start(db.path()); + primePublishers(); + + std::shared_ptr> rgb = + collect("rgb/image"); + ASSERT_TRUE(waitForPublisher(rgb->subscription)); + ASSERT_TRUE(replayUntil([&]() { return !rgb->empty(); })); + + const int id = nodeIdOf(rgb->front().header.stamp); + EXPECT_GE(id, 2) << "the first frame only creates the publishers"; + EXPECT_LE(id, kDbFrames); + EXPECT_NEAR(rtabmap_conversions::timestampFromROS(rgb->front().header.stamp), + stampOfNode(id), 1e-6); +} + +TEST_F(DbPlayerTest, ReplaysCameraCalibration) +{ + TempDatabase db("caminfo"); + writeRgbdDatabase(db.path()); + start(db.path()); + primePublishers(); + + std::shared_ptr> rgb = + collect("rgb/image"); + std::shared_ptr> rgbInfo = + collect("rgb/camera_info"); + std::shared_ptr> depthInfo = + collect("depth/camera_info"); + ASSERT_TRUE(waitForPublisher(rgb->subscription)); + ASSERT_TRUE(waitForPublisher(rgbInfo->subscription)); + ASSERT_TRUE(replayUntil([&]() { return !rgbInfo->empty() && !depthInfo->empty(); })); + + EXPECT_EQ(rgbInfo->back().width, uint32_t(kImageWidth)); + EXPECT_EQ(rgbInfo->back().height, uint32_t(kImageHeight)); + EXPECT_NEAR(rgbInfo->back().k[0], kFx, 1e-6); + EXPECT_NEAR(rgbInfo->back().k[2], kCx, 1e-6); + EXPECT_NEAR(rgbInfo->back().k[4], kFy, 1e-6); + EXPECT_NEAR(rgbInfo->back().k[5], kCy, 1e-6); + EXPECT_EQ(rgbInfo->back().header.frame_id, "camera_optical_link"); + + EXPECT_NEAR(depthInfo->back().k[0], kFx, 1e-6) + << "the depth camera info repeats the color calibration"; +} + +TEST_F(DbPlayerTest, ReplaysImageWithoutCalibrationOnImageTopic) +{ + // A database with no calibration at all is still replayable, on "image". + TempDatabase db("imageonly"); + writeImageOnlyDatabase(db.path()); + start(db.path()); + primePublishers(); + + std::shared_ptr> image = + collect("image"); + ASSERT_TRUE(waitForPublisher(image->subscription)); + ASSERT_TRUE(replayUntil([&]() { return !image->empty(); })); + + EXPECT_EQ(image->back().encoding, sensor_msgs::image_encodings::BGR8); + EXPECT_EQ(image->back().width, uint32_t(kImageWidth)); +} + +//============================================================================ +// Stereo +//============================================================================ + +TEST_F(DbPlayerTest, ReplaysStereoPairAndCalibration) +{ + TempDatabase db("stereo"); + writeStereoDatabase(db.path()); + start(db.path()); + primePublishers(); + + std::shared_ptr> left = + collect("left/image"); + std::shared_ptr> right = + collect("right/image"); + std::shared_ptr> leftInfo = + collect("left/camera_info"); + std::shared_ptr> rightInfo = + collect("right/camera_info"); + ASSERT_TRUE(waitForPublisher(left->subscription)); + ASSERT_TRUE(waitForPublisher(right->subscription)); + ASSERT_TRUE(replayUntil([&]() { + return !left->empty() && !right->empty() && + !leftInfo->empty() && !rightInfo->empty(); })); + + EXPECT_EQ(left->back().encoding, sensor_msgs::image_encodings::MONO8); + EXPECT_EQ(left->back().header.frame_id, "left_camera_optical_link"); + EXPECT_EQ(right->back().encoding, sensor_msgs::image_encodings::MONO8); + EXPECT_EQ(right->back().header.frame_id, "right_camera_optical_link"); + + // Both cameras share the intrinsics of a rectified pair and are stamped with the + // frame of the image they belong to. + EXPECT_EQ(leftInfo->back().width, uint32_t(kImageWidth)); + EXPECT_EQ(leftInfo->back().height, uint32_t(kImageHeight)); + EXPECT_NEAR(leftInfo->back().k[0], kFx, 1e-6); + EXPECT_NEAR(leftInfo->back().k[2], kCx, 1e-6); + EXPECT_NEAR(leftInfo->back().k[4], kFy, 1e-6); + EXPECT_NEAR(leftInfo->back().k[5], kCy, 1e-6); + EXPECT_EQ(leftInfo->back().header.frame_id, "left_camera_optical_link"); + EXPECT_NEAR(rightInfo->back().k[0], kFx, 1e-6); + EXPECT_EQ(rightInfo->back().header.frame_id, "right_camera_optical_link"); + + // Only the right camera carries the baseline: it is P(0,3) = -fx*baseline, and the + // left camera of a rectified pair sits at the origin of the stereo frame. + EXPECT_NEAR(leftInfo->back().p[3], 0.0, 1e-6); + EXPECT_NEAR(rightInfo->back().p[3], -kFx*kBaseline, 1e-6); +} + +//============================================================================ +// Laser scans +//============================================================================ + +TEST_F(DbPlayerTest, Replays2dLaserScan) +{ + TempDatabase db("scan2d"); + writeScan2dDatabase(db.path()); + start(db.path()); + primePublishers(); + + std::shared_ptr> scan = + collect("scan"); + ASSERT_TRUE(waitForPublisher(scan->subscription)); + ASSERT_TRUE(replayUntil([&]() { return !scan->empty(); })) << "no scan replayed"; + + const sensor_msgs::msg::LaserScan & msg = scan->back(); + EXPECT_EQ(msg.header.frame_id, "base_laser_link"); + + // The scan carries its own angles, so the scan_angle_* parameters are not used. + EXPECT_NEAR(msg.angle_min, kScanAngleMin, 1e-6); + EXPECT_NEAR(msg.angle_max, kScanAngleMax, 1e-6); + EXPECT_NEAR(msg.angle_increment, kScanAngleIncrement, 1e-6); + EXPECT_NEAR(msg.range_min, kScanRangeMin, 1e-6); + EXPECT_NEAR(msg.range_max, kScanRangeMax, 1e-6); + + // db_player re-bins the cartesian points, so every bin must come back at its range. + ASSERT_EQ(msg.ranges.size(), size_t(kScanBins)); + for(int bin=0; bin> scan = + collect("scan"); + std::shared_ptr> cloud = + collect("scan_cloud"); + + while(player_->publishNextFrame()) { spinFor(std::chrono::milliseconds(30)); } + + EXPECT_FALSE(scan->empty()) << "the 2D scan must still be replayed"; + EXPECT_EQ(cloud->subscription->get_publisher_count(), 0u) + << "a 2D database must not advertise scan_cloud"; + EXPECT_TRUE(cloud->empty()); +} + +TEST_F(DbPlayerTest, A3dScanNeverAdvertisesScan) +{ + TempDatabase db("scan3donly"); + writeScan3dDatabase(db.path()); + start(db.path()); + + std::shared_ptr> scan = + collect("scan"); + std::shared_ptr> cloud = + collect("scan_cloud"); + + while(player_->publishNextFrame()) { spinFor(std::chrono::milliseconds(30)); } + + EXPECT_FALSE(cloud->empty()) << "the 3D scan must still be replayed"; + EXPECT_EQ(scan->subscription->get_publisher_count(), 0u) + << "a 3D database must not advertise scan"; +} + +TEST_F(DbPlayerTest, UsesScanParametersWhenTheScanHasNoAngles) +{ + // A scan saved without angle metadata falls back to the scan_angle_*/scan_range_* + // parameters, which is how a database recorded from a 3D lidar can be replayed as 2D. + const double angleMin = -0.5; + const double angleIncrement = 0.05; + const int targetBin = 10; + // The center of the target bin: db_player truncates (angle-angle_min)/increment, so a + // bearing on a bin boundary would land on either side depending on the rounding. + const float bearing = float(angleMin + (double(targetBin) + 0.5) * angleIncrement); + const float nearest = 1.0f; + + TempDatabase db("scan2dnoangles"); + writeDatabase(db.path(), kDbFrames, [bearing, nearest](int id, double stamp) { + cv::Mat points(1, kScanBins, CV_32FC2); + for(int bin=0; bin(0, bin) = + cv::Vec2f(range * std::cos(bearing), range * std::sin(bearing)); + } + rtabmap::SensorData data; + data.setId(id); + data.setStamp(stamp); + data.setLaserScan(rtabmap::LaserScan(rtabmap::compressData2(points), + /*maxPoints=*/0, /*maxRange=*/0.0f, rtabmap::LaserScan::kXY, + scanLocalTransform())); + return data; + }); + start(db.path(), {rclcpp::Parameter("scan_angle_min", angleMin), + rclcpp::Parameter("scan_angle_max", 0.5), + rclcpp::Parameter("scan_angle_increment", angleIncrement), + rclcpp::Parameter("scan_range_min", 0.2), + rclcpp::Parameter("scan_range_max", 20.0)}); + primePublishers(); + + std::shared_ptr> scan = + collect("scan"); + ASSERT_TRUE(waitForPublisher(scan->subscription)); + ASSERT_TRUE(replayUntil([&]() { return !scan->empty(); })); + + const sensor_msgs::msg::LaserScan & msg = scan->back(); + EXPECT_NEAR(msg.angle_min, angleMin, 1e-6); + EXPECT_NEAR(msg.angle_max, 0.5, 1e-6); + EXPECT_NEAR(msg.angle_increment, angleIncrement, 1e-6); + EXPECT_NEAR(msg.range_min, 0.2, 1e-6); + EXPECT_NEAR(msg.range_max, 20.0, 1e-6); + ASSERT_EQ(msg.ranges.size(), 20u) << "ceil((0.5 - -0.5)/0.05)"; + + EXPECT_NEAR(msg.ranges[targetBin], nearest, 1e-3) + << "every point shares a bearing, so only its bin is filled, at the nearest range"; + for(size_t bin=0; bin> cloud = + collect("scan_cloud"); + ASSERT_TRUE(waitForPublisher(cloud->subscription)); + ASSERT_TRUE(replayUntil([&]() { return !cloud->empty(); })) << "no cloud replayed"; + + EXPECT_EQ(cloud->back().header.frame_id, "base_laser_link"); + EXPECT_EQ(cloud->back().width * cloud->back().height, uint32_t(kScanCloudPoints)); +} + +//============================================================================ +// Odometry +//============================================================================ + +TEST_F(DbPlayerTest, ReplaysOdometryWithItsCovariance) +{ + TempDatabase db("odom"); + writeRgbdDatabase(db.path()); + start(db.path()); + primePublishers(); + + std::shared_ptr> odom = + collect("odom"); + ASSERT_TRUE(waitForPublisher(odom->subscription)); + ASSERT_TRUE(replayUntil([&]() { return !odom->empty(); })) << "no odometry replayed"; + + const nav_msgs::msg::Odometry & msg = odom->back(); + EXPECT_EQ(msg.header.frame_id, "odom"); + EXPECT_EQ(msg.child_frame_id, "base_link"); + + const int id = nodeIdOf(msg.header.stamp); + ASSERT_GE(id, 1); + ASSERT_LE(id, kDbFrames); + EXPECT_NEAR(msg.pose.pose.position.x, poseOf(id).x(), 1e-5) + << "the pose must be the one recorded for node " << id; + EXPECT_NEAR(msg.pose.pose.position.y, 0.0, 1e-5); + + // The covariance is the inverse of the neighbor link's information matrix. + EXPECT_NEAR(msg.pose.covariance[0], kOdomVariance, 1e-6); + EXPECT_NEAR(msg.pose.covariance[35], kOdomVariance, 1e-6); +} + +TEST_F(DbPlayerTest, IgnoreOdomDropsTheOdometry) +{ + TempDatabase db("ignoreodom"); + writeRgbdDatabase(db.path()); + start(db.path(), {rclcpp::Parameter("ignore_odom", true)}); + primePublishers(); + + std::shared_ptr> odom = + collect("odom"); + std::shared_ptr> rgb = + collect("rgb/image"); + ASSERT_TRUE(waitForPublisher(rgb->subscription)); + ASSERT_TRUE(replayUntil([&]() { return !rgb->empty(); })) + << "the images must still be replayed"; + + EXPECT_EQ(odom->subscription->get_publisher_count(), 0u) + << "with no odometry in the stream the topic is never even created"; + EXPECT_TRUE(odom->empty()); +} + +//============================================================================ +// Transforms +//============================================================================ + +TEST_F(DbPlayerTest, BroadcastsOdometryAndCameraTransforms) +{ + TempDatabase db("tf"); + writeRgbdDatabase(db.path()); + start(db.path()); + std::shared_ptr> tf = collectTf(); + + // TF is not gated on subscribers, so the very first frame already broadcasts. + ASSERT_TRUE(replayUntil([&]() { return !tf->empty(); })) << "nothing broadcast on /tf"; + + geometry_msgs::msg::TransformStamped odomToBase; + ASSERT_TRUE(findTransform(*tf, "odom", "base_link", odomToBase)); + const int id = nodeIdOf(odomToBase.header.stamp); + ASSERT_GE(id, 1); + ASSERT_LE(id, kDbFrames); + EXPECT_NEAR(odomToBase.transform.translation.x, poseOf(id).x(), 1e-5); + + geometry_msgs::msg::TransformStamped baseToCamera; + ASSERT_TRUE(findTransform(*tf, "base_link", "camera_optical_link", baseToCamera)); + EXPECT_LT(toRtabmap(baseToCamera).getDistance(cameraLocalTransform()), 1e-4f) + << "the camera transform is the model's local transform: " + << toRtabmap(baseToCamera).prettyPrint(); +} + +TEST_F(DbPlayerTest, BroadcastsStereoTransformsShiftedByTheBaseline) +{ + TempDatabase db("stereotf"); + writeStereoDatabase(db.path()); + start(db.path()); + std::shared_ptr> tf = collectTf(); + ASSERT_TRUE(replayUntil([&]() { return !tf->empty(); })); + + geometry_msgs::msg::TransformStamped baseToLeft, baseToRight; + ASSERT_TRUE(findTransform(*tf, "base_link", "left_camera_optical_link", baseToLeft)); + ASSERT_TRUE(findTransform(*tf, "base_link", "right_camera_optical_link", baseToRight)); + + EXPECT_LT(toRtabmap(baseToLeft).getDistance(cameraLocalTransform()), 1e-4f); + + // The right camera carries the baseline in Tx, which db_player turns back into a + // translation along the optical x axis so the frame sits next to the left one. + const rtabmap::Transform expectedRight = + cameraLocalTransform() * rtabmap::Transform(kBaseline, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); + EXPECT_LT(toRtabmap(baseToRight).getDistance(expectedRight), 1e-4f) + << toRtabmap(baseToRight).prettyPrint(); +} + +TEST_F(DbPlayerTest, BroadcastsTheLaserTransform) +{ + TempDatabase db("scantf"); + writeScan3dDatabase(db.path()); + start(db.path()); + std::shared_ptr> tf = collectTf(); + ASSERT_TRUE(replayUntil([&]() { return !tf->empty(); })); + + geometry_msgs::msg::TransformStamped baseToLaser; + ASSERT_TRUE(findTransform(*tf, "base_link", "base_laser_link", baseToLaser)); + EXPECT_LT(toRtabmap(baseToLaser).getDistance(scanLocalTransform()), 1e-4f) + << toRtabmap(baseToLaser).prettyPrint(); +} + +TEST_F(DbPlayerTest, BroadcastsGroundTruthAndImuTransforms) +{ + TempDatabase db("richtf"); + writeRichDatabase(db.path()); + start(db.path()); + std::shared_ptr> tf = collectTf(); + ASSERT_TRUE(replayUntil([&]() { return !tf->empty(); })); + + geometry_msgs::msg::TransformStamped worldToGt; + ASSERT_TRUE(findTransform(*tf, "world", "base_link_gt", worldToGt)); + const int id = nodeIdOf(worldToGt.header.stamp); + ASSERT_GE(id, 1); + ASSERT_LE(id, kDbFrames); + EXPECT_LT(toRtabmap(worldToGt).getDistance(groundTruthOf(id)), 1e-4f) + << "the ground truth is published apart from the odometry"; + + geometry_msgs::msg::TransformStamped baseToImu; + ASSERT_TRUE(findTransform(*tf, "base_link", "imu_link", baseToImu)); + EXPECT_TRUE(toRtabmap(baseToImu).isIdentity()) + << "a gravity link is already expressed in the base frame"; +} + +TEST_F(DbPlayerTest, RenamesFramesFromParameters) +{ + TempDatabase db("frames"); + writeRgbdDatabase(db.path()); + start(db.path(), {rclcpp::Parameter("frame_id", std::string("robot")), + rclcpp::Parameter("odom_frame_id", std::string("world_odom")), + rclcpp::Parameter("camera_frame_id", std::string("optical"))}); + std::shared_ptr> tf = collectTf(); + ASSERT_TRUE(replayUntil([&]() { return !tf->empty(); })); + + geometry_msgs::msg::TransformStamped t; + EXPECT_TRUE(findTransform(*tf, "world_odom", "robot", t)); + EXPECT_TRUE(findTransform(*tf, "robot", "optical", t)); + EXPECT_FALSE(findTransform(*tf, "odom", "base_link", t)) << "the defaults must be gone"; +} + +TEST_F(DbPlayerTest, PublishTfFalseBroadcastsNothing) +{ + TempDatabase db("notf"); + writeRgbdDatabase(db.path()); + start(db.path(), {rclcpp::Parameter("publish_tf", false)}); + std::shared_ptr> tf = + collect("/tf", rclcpp::QoS(100)); + + ASSERT_TRUE(player_->publishNextFrame()); + ASSERT_TRUE(player_->publishNextFrame()); + spinFor(std::chrono::milliseconds(300)); + + EXPECT_TRUE(tf->empty()) << "publish_tf:=false must not create the broadcaster"; +} + +//============================================================================ +// The optional channels +//============================================================================ + +TEST_F(DbPlayerTest, ReplaysGlobalPose) +{ + TempDatabase db("globalpose"); + writeRichDatabase(db.path()); + start(db.path()); + primePublishers(); + + std::shared_ptr> pose = + collect("global_pose"); + ASSERT_TRUE(waitForPublisher(pose->subscription)); + ASSERT_TRUE(replayUntil([&]() { return !pose->empty(); })) << "no global pose replayed"; + + const int id = nodeIdOf(pose->back().header.stamp); + ASSERT_GE(id, 1); + ASSERT_LE(id, kDbFrames); + EXPECT_EQ(pose->back().header.frame_id, "base_link"); + EXPECT_NEAR(pose->back().pose.pose.position.y, globalPoseOf(id).y(), 1e-5) + << "the prior pose is offset in y, unlike the odometry"; + // The prior was saved with an information matrix of 100*I. + EXPECT_NEAR(pose->back().pose.covariance[0], 0.01, 1e-6); +} + +TEST_F(DbPlayerTest, ReplaysGpsFix) +{ + TempDatabase db("gps"); + writeRichDatabase(db.path()); + start(db.path()); + primePublishers(); + + std::shared_ptr> gps = + collect("gps/fix"); + ASSERT_TRUE(waitForPublisher(gps->subscription)); + ASSERT_TRUE(replayUntil([&]() { return !gps->empty(); })) << "no GPS replayed"; + + const sensor_msgs::msg::NavSatFix & msg = gps->back(); + EXPECT_NEAR(msg.longitude, kGpsLongitude, 1e-9); + EXPECT_NEAR(msg.latitude, kGpsLatitude, 1e-9); + EXPECT_NEAR(msg.altitude, kGpsAltitude, 1e-9); + EXPECT_EQ(msg.position_covariance_type, + uint8_t(sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN)); + EXPECT_NEAR(msg.position_covariance[0], kGpsError*kGpsError, 1e-9) + << "the reported error is squared into a variance"; + EXPECT_NEAR(msg.position_covariance[4], kGpsError*kGpsError, 1e-9); + EXPECT_NEAR(msg.position_covariance[8], kGpsError*kGpsError, 1e-9); +} + +TEST_F(DbPlayerTest, ReplaysImu) +{ + TempDatabase db("imu"); + writeRichDatabase(db.path()); + start(db.path()); + primePublishers(); + + std::shared_ptr> imu = + collect("imu"); + ASSERT_TRUE(waitForPublisher(imu->subscription)); + ASSERT_TRUE(replayUntil([&]() { return !imu->empty(); })) << "no IMU replayed"; + + EXPECT_EQ(imu->back().header.frame_id, "imu_link"); + + // DBReader rebuilds the IMU from the gravity link, so only the orientation survives. + const Eigen::Quaterniond expected = gravityTransform().getQuaterniond(); + EXPECT_NEAR(std::abs(imu->back().orientation.w), std::abs(expected.w()), 1e-5); + EXPECT_NEAR(std::abs(imu->back().orientation.x), std::abs(expected.x()), 1e-5); + EXPECT_NEAR(std::abs(imu->back().orientation.y), std::abs(expected.y()), 1e-5); + EXPECT_NEAR(std::abs(imu->back().orientation.z), std::abs(expected.z()), 1e-5); +} + +TEST_F(DbPlayerTest, ReplaysEnvSensor) +{ + TempDatabase db("envsensor"); + writeRichDatabase(db.path()); + start(db.path()); + primePublishers(); + + std::shared_ptr> env = + collect("env_sensor"); + ASSERT_TRUE(waitForPublisher(env->subscription)); + ASSERT_TRUE(replayUntil([&]() { return !env->empty(); })) << "no env sensor replayed"; + + EXPECT_EQ(env->back().type, int(rtabmap::EnvSensor::kAmbientTemperature)); + EXPECT_NEAR(env->back().value, kEnvSensorValue, 1e-9); + EXPECT_EQ(env->back().header.frame_id, "base_link"); +} + +TEST_F(DbPlayerTest, PublishesClockWhenAsked) +{ + TempDatabase db("clock"); + writeRgbdDatabase(db.path()); + start(db.path(), {rclcpp::Parameter("publish_clock", true)}); + std::shared_ptr> clock = + collect("/clock"); + ASSERT_TRUE(waitForPublisher(clock->subscription)); + + // The clock is not gated on subscribers either. + ASSERT_TRUE(replayUntil([&]() { return !clock->empty(); })) << "no clock published"; + + const int id = nodeIdOf(clock->back().clock); + ASSERT_GE(id, 1); + ASSERT_LE(id, kDbFrames); + EXPECT_NEAR(rtabmap_conversions::timestampFromROS(clock->back().clock), + stampOfNode(id), 1e-6) << "the clock follows the database stamps"; +} + +TEST_F(DbPlayerTest, NoClockByDefault) +{ + TempDatabase db("noclock"); + writeRgbdDatabase(db.path()); + start(db.path()); + std::shared_ptr> clock = + collect("/clock"); + + ASSERT_TRUE(player_->publishNextFrame()); + ASSERT_TRUE(player_->publishNextFrame()); + spinFor(std::chrono::milliseconds(300)); + + EXPECT_TRUE(clock->empty()); +} + +//============================================================================ +// Reading the database +//============================================================================ + +TEST_F(DbPlayerTest, StopsAtTheEndOfTheDatabase) +{ + TempDatabase db("end"); + writeRgbdDatabase(db.path(), 4); + start(db.path()); + + int frames = 0; + while(player_->publishNextFrame()) + { + ++frames; + ASSERT_LE(frames, 10) << "publishNextFrame() never reported the end"; + } + EXPECT_EQ(frames, 4) << "every node must be replayed exactly once"; +} + +TEST_F(DbPlayerTest, StartIdSkipsTheEarlierNodes) +{ + TempDatabase db("startid"); + writeRgbdDatabase(db.path(), 4); + start(db.path(), {rclcpp::Parameter("start_id", 3)}); + std::shared_ptr> tf = collectTf(); + + int frames = 0; + while(player_->publishNextFrame()) { ++frames; } + spinFor(std::chrono::milliseconds(200)); + EXPECT_EQ(frames, 2) << "nodes 3 and 4 only"; + + geometry_msgs::msg::TransformStamped t; + ASSERT_TRUE(findTransform(*tf, "odom", "base_link", t)); + EXPECT_EQ(nodeIdOf(tf->front().transforms[0].header.stamp), 3) + << "the replay must start at node 3"; +} + +//============================================================================ +// Pause / resume +//============================================================================ + +TEST_F(DbPlayerTest, StartsRunning) +{ + TempDatabase db("pause"); + writeRgbdDatabase(db.path()); + start(db.path()); + EXPECT_FALSE(player_->isPaused()); +} + +TEST_F(DbPlayerTest, PauseAndResumeServicesTogglePlayback) +{ + TempDatabase db("pausesrv"); + writeRgbdDatabase(db.path()); + start(db.path()); + + rclcpp::Client::SharedPtr pause = + helper()->create_client("db_player/pause"); + rclcpp::Client::SharedPtr resume = + helper()->create_client("db_player/resume"); + ASSERT_TRUE(spinUntil([&]() { return pause->service_is_ready() && resume->service_is_ready(); })) + << "the pause/resume services were never advertised"; + + pause->async_send_request(std::make_shared()); + ASSERT_TRUE(spinUntil([&]() { return player_->isPaused(); })) << "pause had no effect"; + + resume->async_send_request(std::make_shared()); + ASSERT_TRUE(spinUntil([&]() { return !player_->isPaused(); })) << "resume had no effect"; +} + +//============================================================================ +// Opening the database +//============================================================================ + +TEST_F(DbPlayerTest, ThrowsWithoutADatabaseParameter) +{ + // The node used to exit(-1) here, which took down every other node sharing its + // component container. Throwing lets the caller decide. + EXPECT_THROW( + std::make_shared(rclcpp::NodeOptions()), + std::invalid_argument); +} + +TEST_F(DbPlayerTest, ThrowsWhenTheDatabaseCannotBeOpened) +{ + TempDatabase db("missing"); // the path is never written + EXPECT_THROW( + std::make_shared(rclcpp::NodeOptions().parameter_overrides( + {rclcpp::Parameter("database", db.path())})), + std::runtime_error); +} diff --git a/rtabmap_util/test/test_disparity_to_depth.cpp b/rtabmap_util/test/test_disparity_to_depth.cpp new file mode 100644 index 00000000..ae622e59 --- /dev/null +++ b/rtabmap_util/test/test_disparity_to_depth.cpp @@ -0,0 +1,248 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "node_test_utils.hpp" + +#include + +#include +#include +#include +#include + +using namespace rtabmap_util_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); + +constexpr float kBaseline = 0.1f; // t, meters +constexpr float kFocal = 500.0f; // f, pixels +constexpr int kWidth = 4; +constexpr int kHeight = 4; + +/// A 4x4 32FC1 disparity image, every pixel set to @p disparity. +stereo_msgs::msg::DisparityImage makeDisparity( + float disparity, + const std::string & encoding = sensor_msgs::image_encodings::TYPE_32FC1) +{ + stereo_msgs::msg::DisparityImage msg; + msg.header.frame_id = "camera_link"; + msg.header.stamp = rclcpp::Time(1000, 0, RCL_ROS_TIME); + msg.t = kBaseline; + msg.f = kFocal; + msg.min_disparity = 1.0f; + msg.max_disparity = 100.0f; + + msg.image.header = msg.header; + msg.image.encoding = encoding; + msg.image.height = kHeight; + msg.image.width = kWidth; + msg.image.step = kWidth * sizeof(float); + msg.image.data.resize(msg.image.step * kHeight); + float * p = reinterpret_cast(msg.image.data.data()); + for(int i=0; i(&img.data[row * img.step + col * sizeof(float)]); +} + +uint16_t pixel16u(const sensor_msgs::msg::Image & img, int row, int col) +{ + return *reinterpret_cast(&img.data[row * img.step + col * sizeof(uint16_t)]); +} +} // namespace + +class DisparityToDepthTest : public NodeTest {}; + +TEST_F(DisparityToDepthTest, ConvertsDisparityToMetricDepth) +{ + addNode(std::make_shared(rclcpp::NodeOptions())); + + std::shared_ptr> depth = + collect("depth"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("disparity", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(depth->subscription)) << "the node never advertised depth"; + + // depth = baseline * focal / disparity = 0.1 * 500 / 10 = 5 m + pub->publish(makeDisparity(10.0f)); + ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); })); + + const sensor_msgs::msg::Image & img = depth->back(); + EXPECT_EQ(img.encoding, sensor_msgs::image_encodings::TYPE_32FC1); + EXPECT_EQ(img.width, uint32_t(kWidth)); + EXPECT_EQ(img.height, uint32_t(kHeight)); + EXPECT_EQ(img.header.frame_id, "camera_link") << "the input header must be preserved"; + EXPECT_NEAR(pixel32f(img, 0, 0), 5.0f, 1e-4); + EXPECT_NEAR(pixel32f(img, kHeight-1, kWidth-1), 5.0f, 1e-4); +} + +TEST_F(DisparityToDepthTest, PublishesMillimetersOnDepthRaw) +{ + addNode(std::make_shared(rclcpp::NodeOptions())); + + std::shared_ptr> raw = + collect("depth_raw"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("disparity", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(raw->subscription)); + + pub->publish(makeDisparity(10.0f)); + ASSERT_TRUE(spinUntil([&]() { return !raw->empty(); })); + + const sensor_msgs::msg::Image & img = raw->back(); + EXPECT_EQ(img.encoding, sensor_msgs::image_encodings::TYPE_16UC1); + EXPECT_EQ(pixel16u(img, 0, 0), 5000) << "5 m expressed in millimeters"; +} + +TEST_F(DisparityToDepthTest, PublishesBothUnitsConsistentlyFromOneInput) +{ + // With both topics subscribed the node fills the 32FC1 and 16UC1 images in the same + // pass. The two must describe the same depth, one in meters and one in millimeters. + addNode(std::make_shared(rclcpp::NodeOptions())); + + std::shared_ptr> meters = + collect("depth"); + std::shared_ptr> millimeters = + collect("depth_raw"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("disparity", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(meters->subscription)); + ASSERT_TRUE(waitForPublisher(millimeters->subscription)); + + // A disparity of 25 gives 0.1 * 500 / 25 = 2 m. + pub->publish(makeDisparity(25.0f)); + ASSERT_TRUE(spinUntil([&]() { return !meters->empty() && !millimeters->empty(); })) + << "both outputs must be produced from a single input"; + + EXPECT_EQ(meters->back().encoding, sensor_msgs::image_encodings::TYPE_32FC1); + EXPECT_EQ(millimeters->back().encoding, sensor_msgs::image_encodings::TYPE_16UC1); + + for(int row=0; rowback(), row, col); + const uint16_t mm = pixel16u(millimeters->back(), row, col); + EXPECT_NEAR(m, 2.0f, 1e-4) << "at " << row << "," << col; + EXPECT_EQ(mm, 2000) << "at " << row << "," << col; + EXPECT_EQ(mm, uint16_t(m * 1000.0f)) << "the two units must agree at " << row << "," << col; + } + } +} + +TEST_F(DisparityToDepthTest, LeavesOutOfRangeDisparityAtZero) +{ + addNode(std::make_shared(rclcpp::NodeOptions())); + + std::shared_ptr> depth = + collect("depth"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("disparity", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(depth->subscription)); + + // Above max_disparity (100), so no depth can be computed. + pub->publish(makeDisparity(500.0f)); + ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); })); + + EXPECT_FLOAT_EQ(pixel32f(depth->back(), 0, 0), 0.0f); +} + +TEST_F(DisparityToDepthTest, RejectsNon32FC1Input) +{ + addNode(std::make_shared(rclcpp::NodeOptions())); + + std::shared_ptr> depth = + collect("depth"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("disparity", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(depth->subscription)); + + pub->publish(makeDisparity(10.0f, sensor_msgs::image_encodings::TYPE_16UC1)); + spinFor(std::chrono::milliseconds(400)); + + EXPECT_TRUE(depth->empty()) << "only 32FC1 disparity is supported"; +} + +TEST_F(DisparityToDepthTest, HonorsTheConfiguredQueueDepths) +{ + // Queue depth is not observable from outside, so this pins down that the parameters + // are accepted and the node still converts with them set. + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({rclcpp::Parameter("queue_sub", 20), + rclcpp::Parameter("queue_pub", 10)}))); + + std::shared_ptr> depth = + collect("depth"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("disparity", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(depth->subscription)); + + pub->publish(makeDisparity(1.0f)); + ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); })); + EXPECT_EQ(depth->back().encoding, sensor_msgs::image_encodings::TYPE_32FC1); +} + +TEST_F(DisparityToDepthTest, RejectsAZeroQueueDepth) +{ + EXPECT_THROW( + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({rclcpp::Parameter("queue_pub", 0)}))), + UException); +} + +TEST_F(DisparityToDepthTest, BridgesABestEffortSourceToAReliableConsumer) +{ + // A reliable subscription refuses to match a best-effort publisher, so setting the + // two sides apart is what lets the conversion cross that gap. + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({rclcpp::Parameter("qos_sub", 2), + rclcpp::Parameter("qos_pub", 1)}))); + + std::shared_ptr> depth = + collect("depth", rclcpp::QoS(10).reliable()); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher( + "disparity", rclcpp::QoS(10).best_effort()); + ASSERT_TRUE(waitForSubscriber(pub)) << "a best-effort source must reach the node"; + ASSERT_TRUE(waitForPublisher(depth->subscription)) + << "a reliable consumer must be able to subscribe to the depth output"; + + pub->publish(makeDisparity(1.0f)); + ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); })); + EXPECT_EQ(depth->back().encoding, sensor_msgs::image_encodings::TYPE_32FC1); +} + +TEST_F(DisparityToDepthTest, TheTwoQosSidesFallBackToQos) +{ + // Only qos is given, so both sides must be best effort: a reliable consumer matches + // neither the publishers nor, from the other end, the subscription. + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({rclcpp::Parameter("qos", 2)}))); + + std::shared_ptr> depth = + collect("depth", rclcpp::QoS(10).reliable()); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher( + "disparity", rclcpp::QoS(10).best_effort()); + EXPECT_TRUE(waitForSubscriber(pub)) << "the subscription must have followed qos"; + + spinFor(std::chrono::milliseconds(500)); + EXPECT_EQ(depth->subscription->get_publisher_count(), 0u) + << "the publishers must have followed qos too: best effort, so a reliable " + "consumer cannot match them"; +} diff --git a/rtabmap_util/test/test_imu_to_tf.cpp b/rtabmap_util/test/test_imu_to_tf.cpp new file mode 100644 index 00000000..f35ca7a7 --- /dev/null +++ b/rtabmap_util/test/test_imu_to_tf.cpp @@ -0,0 +1,205 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "node_test_utils.hpp" + +#include + +#include +#include +#include +#include +#include + +using namespace rtabmap_util_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); + +/// An Imu message whose orientation is a pure rotation of @p yaw about z. +sensor_msgs::msg::Imu makeImu(const std::string & frameId, double stamp, double yaw = 0.0) +{ + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw); + + sensor_msgs::msg::Imu msg; + msg.header.frame_id = frameId; + msg.header.stamp = rclcpp::Time(int32_t(stamp), uint32_t((stamp - int32_t(stamp)) * 1e9), RCL_ROS_TIME); + msg.orientation = tf2::toMsg(q); + return msg; +} +} // namespace + +class ImuToTFTest : public NodeTest +{ +protected: + rclcpp::Publisher::SharedPtr staticTfKeepAlive_; +}; + +TEST_F(ImuToTFTest, BroadcastsOrientationAsTf) +{ + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({rclcpp::Parameter("fixed_frame_id", "odom")}))); + + std::shared_ptr> tf = + collect("/tf"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("imu/data", 10); + ASSERT_TRUE(waitForSubscriber(pub)) << "the node never subscribed to imu/data"; + + pub->publish(makeImu("imu_link", 1000.0, /*yaw=*/M_PI/2.0)); + ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); })) << "no transform was broadcast"; + + ASSERT_EQ(tf->back().transforms.size(), 1u); + const geometry_msgs::msg::TransformStamped & t = tf->back().transforms[0]; + EXPECT_EQ(t.header.frame_id, "odom"); + EXPECT_EQ(t.child_frame_id, "imu_link") << "with no base_frame_id the imu frame is used"; + + // The broadcast rotation must be the IMU's orientation. + tf2::Quaternion q; + tf2::fromMsg(t.transform.rotation, q); + EXPECT_NEAR(tf2::getYaw(q), M_PI/2.0, 1e-6); + + // It is an orientation only: no translation. + EXPECT_NEAR(t.transform.translation.x, 0.0, 1e-9); + EXPECT_NEAR(t.transform.translation.y, 0.0, 1e-9); + EXPECT_NEAR(t.transform.translation.z, 0.0, 1e-9); +} + +TEST_F(ImuToTFTest, UsesTheConfiguredFixedFrame) +{ + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({rclcpp::Parameter("fixed_frame_id", "my_odom")}))); + + std::shared_ptr> tf = + collect("/tf"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("imu/data", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + pub->publish(makeImu("imu_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); })); + + EXPECT_EQ(tf->back().transforms[0].header.frame_id, "my_odom"); +} + +TEST_F(ImuToTFTest, PreservesTheImuStamp) +{ + addNode(std::make_shared(rclcpp::NodeOptions())); + + std::shared_ptr> tf = + collect("/tf"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("imu/data", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + const sensor_msgs::msg::Imu imu = makeImu("imu_link", 1234.5); + pub->publish(imu); + ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); })); + + EXPECT_EQ(tf->back().transforms[0].header.stamp.sec, imu.header.stamp.sec); + EXPECT_EQ(tf->back().transforms[0].header.stamp.nanosec, imu.header.stamp.nanosec); +} + +TEST_F(ImuToTFTest, ReportsTheOrientationInTheBaseFrame) +{ + // With base_frame_id set and the mounting transform available, the node re-expresses + // the IMU orientation in the base frame and broadcasts that frame instead. + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("fixed_frame_id", "odom"), + rclcpp::Parameter("base_frame_id", "base_link"), + rclcpp::Parameter("wait_for_transform_duration", 0.5)}))); + publishStaticTf("base_link", "imu_link", 0.1, 0.0, 0.2); // translation only + + std::shared_ptr> tf = + collect("/tf"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("imu/data", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + pub->publish(makeImu("imu_link", 1000.0, /*yaw=*/M_PI/2.0)); + ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); })) + << "with the mounting transform available a transform must be broadcast"; + + const geometry_msgs::msg::TransformStamped & t = tf->back().transforms[0]; + EXPECT_EQ(t.header.frame_id, "odom"); + EXPECT_EQ(t.child_frame_id, "base_link") + << "the base frame is broadcast, not the imu frame"; + + // The mounting has no rotation, so the orientation is unchanged. + tf2::Quaternion q; + tf2::fromMsg(t.transform.rotation, q); + EXPECT_NEAR(tf2::getYaw(q), M_PI/2.0, 1e-6); +} + +TEST_F(ImuToTFTest, IgnoresAYawOnlyMountingOffset) +{ + // The node strips the yaw of the mounting transform (it uses only getYaw to build + // the correction), so a purely yaw-rotated mount leaves the reported orientation + // alone: the IMU's absolute yaw is what matters, not how it is bolted on. + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("fixed_frame_id", "odom"), + rclcpp::Parameter("base_frame_id", "base_link"), + rclcpp::Parameter("wait_for_transform_duration", 0.5)}))); + + // base_link -> imu_link rotated 90 degrees about z. + { + tf2::Quaternion mount; + mount.setRPY(0.0, 0.0, M_PI/2.0); + geometry_msgs::msg::TransformStamped m; + m.header.stamp = helper()->now(); + m.header.frame_id = "base_link"; + m.child_frame_id = "imu_link"; + m.transform.rotation = tf2::toMsg(mount); + tf2_msgs::msg::TFMessage msg; + msg.transforms.push_back(m); + rclcpp::Publisher::SharedPtr staticPub = + helper()->create_publisher( + "/tf_static", rclcpp::QoS(100).transient_local()); + staticPub->publish(msg); + spinFor(std::chrono::milliseconds(150)); + staticTfKeepAlive_ = staticPub; + } + + std::shared_ptr> tf = + collect("/tf"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("imu/data", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + pub->publish(makeImu("imu_link", 1000.0, /*yaw=*/M_PI/4.0)); + ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); })); + + const geometry_msgs::msg::TransformStamped & t = tf->back().transforms[0]; + EXPECT_EQ(t.child_frame_id, "base_link"); + + tf2::Quaternion q; + tf2::fromMsg(t.transform.rotation, q); + EXPECT_NEAR(tf2::getYaw(q), M_PI/4.0, 1e-5) + << "the mounting yaw must cancel out, leaving the imu's own yaw"; +} + +TEST_F(ImuToTFTest, DropsTheMessageWhenTheBaseTransformIsMissing) +{ + // base_frame_id differs from the imu frame, so the node needs imu_link -> base_link + // from TF. Nothing publishes it, so nothing may be broadcast. + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("base_frame_id", "base_link"), + rclcpp::Parameter("wait_for_transform_duration", 0.0)}))); + + std::shared_ptr> tf = + collect("/tf"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("imu/data", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + pub->publish(makeImu("imu_link", 1000.0, M_PI/2.0)); + spinFor(std::chrono::milliseconds(500)); + + EXPECT_TRUE(tf->empty()) << "without the base transform the node must not broadcast"; +} diff --git a/rtabmap_util/test/test_lidar_deskewing.cpp b/rtabmap_util/test/test_lidar_deskewing.cpp new file mode 100644 index 00000000..e28db65c --- /dev/null +++ b/rtabmap_util/test/test_lidar_deskewing.cpp @@ -0,0 +1,204 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "node_test_utils.hpp" +#include "msg_builders.hpp" + +#include + +#include + +#include +#include + +using namespace rtabmap_util_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); +} + +class LidarDeskewingTest : public NodeTest +{ +protected: + static constexpr double kSweep = 0.099; ///< first sample to last, seconds + static constexpr double kSpeed = 1.0; ///< m/s, straight at the wall + static constexpr float kWall = 5.0f; ///< distance to the wall, meters + + /// Distance travelled since the first sample. Drives both the TF and the skew. + static double travelled(double elapsed) { return kSpeed * elapsed; } + + /// Publishes odom -> lidar following exactly that trajectory. + void publishOdomMotion(double startStamp) + { + rclcpp::Publisher::SharedPtr tfPub = + helper()->create_publisher("/tf", rclcpp::QoS(100)); + spinFor(std::chrono::milliseconds(100)); // let the node's listener subscribe + + // Covers exactly the sweep, from the first sample to the last. Nothing beyond: + // asking for more than laser_geometry needs would be a regression. + for(int i=0; i<=2; ++i) + { + const double elapsed = kSweep * double(i) / 2.0; + geometry_msgs::msg::TransformStamped t; + t.header.stamp = stampOf(startStamp + elapsed); + t.header.frame_id = "odom"; + t.child_frame_id = "lidar"; + t.transform.translation.x = travelled(elapsed); + t.transform.rotation.w = 1.0; + tf2_msgs::msg::TFMessage msg; + msg.transforms.push_back(t); + tfPub->publish(msg); + } + spinFor(std::chrono::milliseconds(200)); // let the buffer fill + tfPub_ = tfPub; // keep the publisher alive + } + + rclcpp::Publisher::SharedPtr tfPub_; +}; + +TEST_F(LidarDeskewingTest, DeskewsACloudUsingTf) +{ + // The wall is recorded bent because the sensor closes in during the sweep, and TF + // carries that same motion. A correct deskew must flatten it back to kWall. + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("fixed_frame_id", "odom"), + rclcpp::Parameter("wait_for_transform", 0.2)}))); + + publishOdomMotion(1000.0); + + std::shared_ptr> out = + collect("input_cloud/deskewed"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("input_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + const size_t sampleCount = 20; + const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan( + "lidar", 1000.0, sampleCount, kSweep, kWall, &travelled); + + // The input really is bent: the last sample is a full sweep of travel closer. + ASSERT_NEAR(readXYZ(in, 0).x, kWall, 1e-4); + ASSERT_NEAR(readXYZ(in, sampleCount-1).x, kWall - float(kSpeed*kSweep), 1e-4); + + pub->publish(in); + ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })) << "no deskewed cloud published"; + + const sensor_msgs::msg::PointCloud2 & cloud = out->back(); + EXPECT_EQ(cloud.header.frame_id, "lidar") << "output stays in the sensor frame"; + ASSERT_EQ(cloud.width, sampleCount); + + // Every sample must land back on the wall. + for(size_t i=0; i(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("fixed_frame_id", "odom"), + rclcpp::Parameter("wait_for_transform", 0.2)}))); + + publishOdomMotion(1000.0); + + std::shared_ptr> out = + collect("input_scan/deskewed"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("input_scan", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + sensor_msgs::msg::LaserScan scan; + scan.header.frame_id = "lidar"; + scan.header.stamp = stampOf(1000.0); + scan.angle_min = -0.4f; + scan.angle_max = 0.4f; + scan.angle_increment = 0.05f; + scan.range_min = 0.1f; + scan.range_max = 30.0f; + const size_t rayCount = size_t((scan.angle_max - scan.angle_min) / scan.angle_increment) + 1; + scan.time_increment = float(kSweep / double(rayCount - 1)); + scan.ranges.resize(rayCount); + for(size_t i=0; ipublish(scan); + ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })) << "no deskewed scan published"; + + const sensor_msgs::msg::PointCloud2 & cloud = out->back(); + EXPECT_EQ(cloud.header.frame_id, "lidar") << "output stays in the sensor frame"; + ASSERT_EQ(cloud.width, rayCount); + + // Without deskewing the last ray would sit a full sweep of travel short of the wall. + for(size_t i=0; i(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("fixed_frame_id", "odom"), + rclcpp::Parameter("wait_for_transform", 0.0)}))); + + std::shared_ptr> out = + collect("input_cloud/deskewed"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("input_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + const sensor_msgs::msg::PointCloud2 in = + makeXYZCloud("lidar", 1000.0, {{5.0f, 0.0f, 0.0f}, {5.0f, 1.0f, 0.0f}}, true); + pub->publish(in); + ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })) + << "the cloud must still be forwarded"; + + EXPECT_EQ(out->back().data, in.data) << "and forwarded byte for byte, still skewed"; +} + +TEST_F(LidarDeskewingTest, DropsAScanWhenTfIsMissing) +{ + // The 2D scan path does the opposite of the cloud path: it returns early and + // publishes nothing when the transform is unavailable. + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("fixed_frame_id", "odom"), + rclcpp::Parameter("wait_for_transform", 0.0)}))); + + std::shared_ptr> out = + collect("input_scan/deskewed"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("input_scan", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + sensor_msgs::msg::LaserScan scan; + scan.header.frame_id = "lidar"; + scan.header.stamp = stampOf(1000.0); + scan.angle_min = -1.0f; + scan.angle_max = 1.0f; + scan.angle_increment = 0.1f; + scan.time_increment = 0.001f; + scan.range_min = 0.1f; + scan.range_max = 30.0f; + scan.ranges.assign(21, 5.0f); + pub->publish(scan); + spinFor(std::chrono::milliseconds(400)); + + EXPECT_TRUE(out->empty()) << "the scan path drops the message instead of forwarding it"; +} diff --git a/rtabmap_util/test/test_map_assembler.cpp b/rtabmap_util/test/test_map_assembler.cpp new file mode 100644 index 00000000..e2b22312 --- /dev/null +++ b/rtabmap_util/test/test_map_assembler.cpp @@ -0,0 +1,549 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "node_test_utils.hpp" +#include "msg_builders.hpp" + +#include + +#include + +#include +#include + +#include +#include +#include + +#include + +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) +#include +#endif + +using namespace rtabmap_util_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); + +constexpr float kCellSize = 0.05f; +/// Anything above this is an obstacle once a grid is regenerated from a scan. +constexpr float kGroundHeight = 0.1f; +constexpr float kObstacleHeight = 0.5f; + +cv::Mat toCellMat(const std::vector & points) +{ + if(points.empty()) { return cv::Mat(); } + cv::Mat mat(1, int(points.size()), CV_32FC3); + for(size_t i=0; i(0, int(i)) = cv::Vec3f(points[i].x, points[i].y, points[i].z); + } + return mat; +} + +/** + * @brief A graph node as it arrives on "mapData". + * + * map_assembler only caches a node that carries compressed images or a compressed scan, + * so the scan is what makes the node acceptable at all. The occupancy grid is what + * MapsManager normally uses; the two are deliberately given different geometry so a test + * can tell which one ended up in the map. + * + * @param scan points of the raw scan, in the node's frame + * @param ground ground cells of the ready-made grid + * @param obstacles obstacle cells of the ready-made grid + */ +rtabmap::Signature makeNode( + int id, const rtabmap::Transform & pose, + const std::vector & scan, + const std::vector & ground, + const std::vector & obstacles) +{ + rtabmap::SensorData data; + data.setId(id); + data.setStamp(1000.0 + id); + data.setLaserScan(rtabmap::LaserScan(rtabmap::compressData2(toCellMat(scan)), + /*maxPoints=*/0, /*maxRange=*/0.0f, rtabmap::LaserScan::kXYZ)); + if(!ground.empty() || !obstacles.empty()) + { + data.setOccupancyGrid(toCellMat(ground), toCellMat(obstacles), cv::Mat(), kCellSize, + cv::Point3f(0, 0, 0)); + } + return rtabmap::Signature(id, /*mapId=*/0, /*weight=*/1, data.stamp(), /*label=*/"", + pose, rtabmap::Transform(), data); +} + +cv::Point3f pointAt(const sensor_msgs::msg::PointCloud2 & cloud, size_t index) +{ + uint32_t xo = 0, yo = 4, zo = 8; + for(size_t i=0; i(base + xo), + *reinterpret_cast(base + yo), + *reinterpret_cast(base + zo)); +} + +bool containsPoint(const sensor_msgs::msg::PointCloud2 & cloud, const cv::Point3f & expected, + float tolerance = 1e-3f) +{ + for(size_t i=0; icreate_service( + std::string(kRtabmapName) + "/get_map_data", + [this](const std::shared_ptr, + std::shared_ptr response) { + ++getMapCalls_; + response->data = initialMap_; + }); + } + + /// Creates the node with the start-up call skipped, so it subscribes right away. + void start(std::vector overrides = {}) + { + overrides.push_back(rclcpp::Parameter("initialize_from_rtabmap_timeout", 0.0)); + createNode(overrides); + ASSERT_TRUE(waitForSubscriber(mapDataPub_)) + << "map_assembler never subscribed to mapData"; + } + + /** + * @brief Creates the node with the start-up call enabled, as it is by default. + * + * It only subscribes to "mapData" once that call has returned, and the call blocks a + * callback on another callback of the same node, so it needs a multi-threaded + * executor to get through. + */ + void startInitializingFromRtabmap(double timeout = 5.0, + std::vector overrides = {}) + { + overrides.push_back( + rclcpp::Parameter("initialize_from_rtabmap_timeout", timeout)); + createNode(overrides); + ASSERT_TRUE(spinMultiThreadedUntil( + [&]() { return mapDataPub_->get_subscription_count() > 0; })) + << "map_assembler never subscribed to mapData"; + } + + void createNode(std::vector overrides) + { + overrides.push_back(rclcpp::Parameter("rtabmap", std::string(kRtabmapName))); + assembler_ = addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(overrides))); + mapDataPub_ = helper()->create_publisher("mapData", + rclcpp::QoS(1)); + } + + /// A MapData carrying @p signatures and a graph over their poses. + static rtabmap_msgs::msg::MapData makeMapData( + const std::vector & signatures, + const std::vector & graphIds = {}) + { + std::map poses; + rtabmap_msgs::msg::MapData msg; + msg.header.frame_id = "map"; + msg.header.stamp = stampOf(2000.0); + + for(size_t i=0; i(), + rtabmap::Transform::getIdentity(), msg.graph); + return msg; + } + + static rtabmap::Transform poseOf(int id) + { + return rtabmap::Transform(2.0f * float(id - 1), 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); + } + + /// Two nodes whose ready-made grids hold one ground and one obstacle cell each. + static std::vector twoNodes() + { + return { + makeNode(1, poseOf(1), + /*scan=*/{cv::Point3f(0.5f, -0.1f, 0.0f), + cv::Point3f(0.5f, 0.1f, kObstacleHeight)}, + /*ground=*/{cv::Point3f(0.5f, -0.1f, 0.0f)}, + /*obstacles=*/{cv::Point3f(1.0f, 0.1f, 0.0f)}), + makeNode(2, poseOf(2), + /*scan=*/{cv::Point3f(0.5f, -0.1f, 0.0f), + cv::Point3f(0.5f, 0.1f, kObstacleHeight)}, + /*ground=*/{cv::Point3f(0.5f, -0.1f, 0.0f)}, + /*obstacles=*/{cv::Point3f(1.0f, 0.1f, 0.0f)})}; + } + + void publishMapData(const rtabmap_msgs::msg::MapData & msg) + { + mapDataPub_->publish(msg); + } + + std::shared_ptr assembler_; + rclcpp::Publisher::SharedPtr mapDataPub_; + rclcpp::Service::SharedPtr getMapService_; + rtabmap_msgs::msg::MapData initialMap_; + std::atomic_int getMapCalls_{0}; +}; + +constexpr const char * MapAssemblerTest::kRtabmapName; + +//============================================================================ +// Start-up +//============================================================================ + +TEST_F(MapAssemblerTest, AsksRtabmapForTheMapByDefault) +{ + advertiseGetMapData(makeMapData(twoNodes())); + createNode({}); // no overrides at all, so the default timeout applies + ASSERT_TRUE(spinMultiThreadedUntil( + [&]() { return mapDataPub_->get_subscription_count() > 0; })); + + EXPECT_EQ(getMapCalls_.load(), 1) << "the start-up service call is made exactly once"; +} + +TEST_F(MapAssemblerTest, SkipsTheStartUpCallWhenTheTimeoutIsZero) +{ + // Nothing to catch up on, so the node should not spend its start-up waiting on a + // service: it subscribes immediately instead. + advertiseGetMapData(makeMapData(twoNodes())); + start(); + EXPECT_EQ(getMapCalls_.load(), 0) << "rtabmap must not be called with a zero timeout"; +} + +TEST_F(MapAssemblerTest, SubscribesAnywayWhenRtabmapNeverAnswers) +{ + // Nothing advertises get_map_data, so the start-up call times out. The node must + // still come up and subscribe, since rtabmap may be started afterwards. + // Short, because unlike every other test here this one waits the timeout out. + startInitializingFromRtabmap(/*timeout=*/0.5); + EXPECT_EQ(getMapCalls_.load(), 0); +} + +TEST_F(MapAssemblerTest, WaitsForRtabmapToShowUpDuringTheTimeout) +{ + // get_map_data is not advertised when the node starts asking for it: it appears part + // way through the wait. The call must still go through, which is what makes the + // timeout a real wait rather than a check of what happens to be up already. + // + // The node's timer fires one second after construction and then waits 750 ms, so + // advertising at 1250 ms lands inside that window with room on both sides. + std::thread rtabmapStartsLate([this]() { + std::this_thread::sleep_for(std::chrono::milliseconds(1250)); + advertiseGetMapData(makeMapData(twoNodes())); + }); + + startInitializingFromRtabmap(/*timeout=*/0.75); + rtabmapStartsLate.join(); + + EXPECT_EQ(getMapCalls_.load(), 1) << "rtabmap showed up before the wait expired"; +} + +TEST_F(MapAssemblerTest, StartsFromTheMapRtabmapHandsBack) +{ + // The nodes come from the start-up call, and the graph that arrives later names them + // without resending their data. The map must still be assembled from the cache. + advertiseGetMapData(makeMapData(twoNodes())); + startInitializingFromRtabmap(); + + std::shared_ptr> cloud = + collect("cloud_map"); + ASSERT_TRUE(waitForPublisher(cloud->subscription)); + + publishMapData(makeMapData({}, /*graphIds=*/{1, 2})); + ASSERT_TRUE(spinUntil([&]() { return !cloud->empty(); })) << "no cloud assembled"; + + EXPECT_EQ(pointCount(cloud->back()), 4u) << "one ground and one obstacle cell per node"; +} + +//============================================================================ +// Assembling +//============================================================================ + +TEST_F(MapAssemblerTest, AssemblesTheCloudFromMapData) +{ + start(); + + std::shared_ptr> cloud = + collect("cloud_map"); + std::shared_ptr> obstacles = + collect("cloud_obstacles"); + ASSERT_TRUE(waitForPublisher(cloud->subscription)); + + publishMapData(makeMapData(twoNodes())); + ASSERT_TRUE(spinUntil([&]() { return !cloud->empty() && !obstacles->empty(); })) + << "no cloud assembled"; + + EXPECT_EQ(cloud->back().header.frame_id, "map") << "the frame comes from the message"; + EXPECT_EQ(pointCount(cloud->back()), 4u); + EXPECT_EQ(pointCount(obstacles->back()), 2u); + // Node 2 sits 2 m along x, so its obstacle cell lands at 3 m. + EXPECT_TRUE(containsPoint(obstacles->back(), cv::Point3f(1.0f, 0.1f, 0.0f))); + EXPECT_TRUE(containsPoint(obstacles->back(), cv::Point3f(3.0f, 0.1f, 0.0f))); +} + +TEST_F(MapAssemblerTest, PublishesTheOccupancyGrid) +{ + start(); + + std::shared_ptr> grid = + collect("map"); + ASSERT_TRUE(waitForPublisher(grid->subscription)); + + publishMapData(makeMapData(twoNodes())); + ASSERT_TRUE(spinUntil([&]() { return !grid->empty(); })) << "no grid assembled"; + + EXPECT_EQ(grid->back().header.frame_id, "map"); + EXPECT_NEAR(grid->back().info.resolution, kCellSize, 1e-6); + EXPECT_GT(grid->back().info.width, 0u); +} + +TEST_F(MapAssemblerTest, IgnoresAnEmptyMapData) +{ + start(); + + std::shared_ptr> cloud = + collect("cloud_map"); + ASSERT_TRUE(waitForPublisher(cloud->subscription)); + + rtabmap_msgs::msg::MapData empty; + empty.header.frame_id = "map"; + empty.header.stamp = stampOf(2000.0); + publishMapData(empty); + spinFor(std::chrono::milliseconds(400)); + + EXPECT_TRUE(cloud->empty()) << "a message with no graph and no nodes is nothing to do"; +} + +TEST_F(MapAssemblerTest, PublishesAnEmptyMapForAGraphWithNoCachedNodes) +{ + // A graph can name nodes whose data map_assembler has never seen -- it has no cache + // at all here. It still publishes, using the poses as they are. + start(); + + std::shared_ptr> cloud = + collect("cloud_map"); + ASSERT_TRUE(waitForPublisher(cloud->subscription)); + + publishMapData(makeMapData({}, /*graphIds=*/{1, 2})); + ASSERT_TRUE(spinUntil([&]() { return !cloud->empty(); })) << "nothing published"; + + EXPECT_EQ(pointCount(cloud->back()), 0u) << "no data cached, so nothing to assemble"; + EXPECT_EQ(cloud->back().header.frame_id, "map"); +} + +//============================================================================ +// regenerate_local_grids +//============================================================================ + +TEST_F(MapAssemblerTest, UsesTheGridsThatCameWithTheNodes) +{ + // By default the ready-made grid wins: its obstacle is at y=+0.1, the scan's is at + // y=-0.1 with the ground point, so the two are told apart by where the cells land. + start({rclcpp::Parameter(rtabmap::Parameters::kGridSensor(), std::string("0")), + rclcpp::Parameter(rtabmap::Parameters::kGridNormalsSegmentation(), + std::string("false")), + rclcpp::Parameter(rtabmap::Parameters::kGridMaxGroundHeight(), + std::string("0.1"))}); + + std::shared_ptr> obstacles = + collect("cloud_obstacles"); + ASSERT_TRUE(waitForPublisher(obstacles->subscription)); + + publishMapData(makeMapData({twoNodes()[0]})); + ASSERT_TRUE(spinUntil([&]() { return !obstacles->empty(); })); + + EXPECT_EQ(pointCount(obstacles->back()), 1u); + EXPECT_TRUE(containsPoint(obstacles->back(), cv::Point3f(1.0f, 0.1f, 0.0f))) + << "the obstacle cell of the grid that came with the node"; +} + +TEST_F(MapAssemblerTest, RegenerateLocalGridsRebuildsThemFromTheScan) +{ + // With regenerate_local_grids the grid that came with the node is thrown away, so + // MapsManager segments the scan instead: the raised scan point becomes the obstacle. + start({rclcpp::Parameter("regenerate_local_grids", true), + rclcpp::Parameter(rtabmap::Parameters::kGridSensor(), std::string("0")), + rclcpp::Parameter(rtabmap::Parameters::kGridNormalsSegmentation(), + std::string("false")), + rclcpp::Parameter(rtabmap::Parameters::kGridMaxGroundHeight(), + std::string("0.1"))}); + + std::shared_ptr> obstacles = + collect("cloud_obstacles"); + ASSERT_TRUE(waitForPublisher(obstacles->subscription)); + + publishMapData(makeMapData({twoNodes()[0]})); + ASSERT_TRUE(spinUntil([&]() { return !obstacles->empty(); })); + + EXPECT_EQ(pointCount(obstacles->back()), 1u); + EXPECT_TRUE(containsPoint(obstacles->back(), + cv::Point3f(0.5f, 0.1f, kObstacleHeight), kCellSize)) + << "the raised scan point, not the cell the node arrived with"; + EXPECT_FALSE(containsPoint(obstacles->back(), cv::Point3f(1.0f, 0.1f, 0.0f))) + << "the grid that came with the node must have been discarded"; +} + +//============================================================================ +// Services +//============================================================================ + +TEST_F(MapAssemblerTest, ResetEmptiesTheMap) +{ + start(); + + std::shared_ptr> cloud = + collect("cloud_map"); + ASSERT_TRUE(waitForPublisher(cloud->subscription)); + + publishMapData(makeMapData(twoNodes())); + ASSERT_TRUE(spinUntil([&]() { return !cloud->empty(); })); + ASSERT_EQ(pointCount(cloud->back()), 4u); + + rclcpp::Client::SharedPtr reset = + helper()->create_client("map_assembler/reset"); + ASSERT_TRUE(spinUntil([&]() { return reset->service_is_ready(); })) + << "the reset service was never advertised"; + reset->async_send_request(std::make_shared()); + ASSERT_TRUE(spinUntil([&]() { return cloud->size() >= 1u; })); + spinFor(std::chrono::milliseconds(200)); + + // The cache is gone, so the same graph now assembles nothing. + const size_t before = cloud->size(); + publishMapData(makeMapData({}, /*graphIds=*/{1, 2})); + ASSERT_TRUE(spinUntil([&]() { return cloud->size() > before; })); + EXPECT_EQ(pointCount(cloud->back()), 0u) << "reset must drop the cached nodes"; +} + +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) +TEST_F(MapAssemblerTest, ServesTheBinaryOctomap) +{ + start(); + + std::shared_ptr> cloud = + collect("cloud_map"); + ASSERT_TRUE(waitForPublisher(cloud->subscription)); + publishMapData(makeMapData(twoNodes())); + ASSERT_TRUE(spinUntil([&]() { return !cloud->empty(); })); + + rclcpp::Client::SharedPtr client = + helper()->create_client( + "map_assembler/octomap_binary"); + ASSERT_TRUE(spinUntil([&]() { return client->service_is_ready(); })); + + auto future = client->async_send_request( + std::make_shared()); + ASSERT_TRUE(spinUntil([&]() { + return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; })) + << "octomap_binary never answered"; + + // Keep the response alive: future.get() hands back a temporary shared_ptr, so binding + // a reference into it would dangle. + const std::shared_ptr response = future.get(); + EXPECT_EQ(response->map.header.frame_id, "map") + << "the frame of the last map data received"; + EXPECT_TRUE(response->map.binary); + EXPECT_FALSE(response->map.data.empty()) + << "the octomap is built on demand from the cache"; +} + +TEST_F(MapAssemblerTest, ServesTheFullOctomap) +{ + start(); + + std::shared_ptr> cloud = + collect("cloud_map"); + ASSERT_TRUE(waitForPublisher(cloud->subscription)); + publishMapData(makeMapData(twoNodes())); + ASSERT_TRUE(spinUntil([&]() { return !cloud->empty(); })); + + rclcpp::Client::SharedPtr client = + helper()->create_client( + "map_assembler/octomap_full"); + ASSERT_TRUE(spinUntil([&]() { return client->service_is_ready(); })); + + auto future = client->async_send_request( + std::make_shared()); + ASSERT_TRUE(spinUntil([&]() { + return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; })); + + EXPECT_FALSE(future.get()->map.binary); +} + +TEST_F(MapAssemblerTest, ServesAnEmptyOctomapWithoutData) +{ + start(); + + rclcpp::Client::SharedPtr client = + helper()->create_client( + "map_assembler/octomap_binary"); + ASSERT_TRUE(spinUntil([&]() { return client->service_is_ready(); })); + + auto future = client->async_send_request( + std::make_shared()); + ASSERT_TRUE(spinUntil([&]() { + return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; })); + + EXPECT_TRUE(future.get()->map.data.empty()) << "nothing cached, nothing to serve"; +} +#endif diff --git a/rtabmap_util/test/test_maps_manager.cpp b/rtabmap_util/test/test_maps_manager.cpp new file mode 100644 index 00000000..aa49d7f1 --- /dev/null +++ b/rtabmap_util/test/test_maps_manager.cpp @@ -0,0 +1,1060 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "node_test_utils.hpp" + +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include + +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) +#include +#endif +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) +#include +#endif + +using namespace rtabmap_util_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); + +constexpr float kCellSize = 0.05f; + +/// Anything at or below this height is ground, anything above it an obstacle. +constexpr float kGroundHeight = 0.1f; +/// The height of node 2's obstacle, which is what puts it on the obstacle side. +constexpr float kObstacleHeight = 0.5f; + +/** + * @brief A node carrying a ready-made local occupancy grid. + * + * MapsManager only regenerates a local grid when the sensor data has none + * (`gridCellSize() == 0`). Handing it the cells directly keeps the assembled map exactly + * predictable, instead of depending on how a depth image or scan would be segmented. + * + * @param cells coordinates in the node's own frame; the pose is applied when assembling. + */ +rtabmap::Signature makeGridSignature( + int id, const rtabmap::Transform & pose, + const std::vector & ground, + const std::vector & obstacles, + const std::vector & empty = {}) +{ + auto toMat = [](const std::vector & points) { + if(points.empty()) { return cv::Mat(); } + cv::Mat mat(1, int(points.size()), CV_32FC3); + for(size_t i=0; i(0, int(i)) = cv::Vec3f(points[i].x, points[i].y, points[i].z); + } + return mat; + }; + + rtabmap::SensorData data; + data.setId(id); + data.setStamp(1000.0 + id); + data.setOccupancyGrid(toMat(ground), toMat(obstacles), toMat(empty), kCellSize, + cv::Point3f(0, 0, 0)); + + rtabmap::Signature s(id, /*mapId=*/0, /*weight=*/1, data.stamp(), /*label=*/"", pose, + rtabmap::Transform(), data); + return s; +} + +/** + * @brief A node carrying a raw laser scan, which MapsManager has to segment itself. + * + * This is the other half of updateMapCaches(): when the sensor data has no local grid it + * builds one with LocalGridMaker instead of just caching the cells. With the parameters + * in MapsManagerTest::sceneParameters() the segmentation is a plain height passthrough, + * so which points come back as ground and which as obstacles is decided by their z alone. + */ +rtabmap::Signature makeScanSignature( + int id, const rtabmap::Transform & pose, const std::vector & points) +{ + cv::Mat scan(1, int(points.size()), CV_32FC3); + for(size_t i=0; i(0, int(i)) = cv::Vec3f(points[i].x, points[i].y, points[i].z); + } + + rtabmap::SensorData data; + data.setId(id); + data.setStamp(1000.0 + id); + data.setLaserScan(rtabmap::LaserScan(scan, /*maxPoints=*/0, /*maxRange=*/0.0f, + rtabmap::LaserScan::kXYZ)); + + return rtabmap::Signature(id, /*mapId=*/0, /*weight=*/1, data.stamp(), /*label=*/"", + pose, rtabmap::Transform(), data); +} + +/// Reads point @p index of an XYZRGB cloud. +cv::Point3f pointAt(const sensor_msgs::msg::PointCloud2 & cloud, size_t index) +{ + uint32_t xo = 0, yo = 4, zo = 8; + for(size_t i=0; i(base + xo), + *reinterpret_cast(base + yo), + *reinterpret_cast(base + zo)); +} + +/// Reads the packed rgb field of point @p index as (r,g,b). +cv::Vec3b colorAt(const sensor_msgs::msg::PointCloud2 & cloud, size_t index) +{ + uint32_t offset = 16; + for(size_t i=0; i> 16), uint8_t(packed >> 8), uint8_t(packed)); +} + +/** + * @brief The center of octomap voxel (@p i, @p j, @p k) at kCellSize resolution. + * + * Cells handed to the octomap have to sit on voxel centers when their neighbors matter: + * a coordinate on a voxel boundary (a multiple of the cell size) falls on either side + * depending on rounding, so a cell meant to touch its neighbor may not. + */ +cv::Point3f voxelCenter(int i, int j, int k) +{ + return cv::Point3f((float(i)+0.5f)*kCellSize, (float(j)+0.5f)*kCellSize, + (float(k)+0.5f)*kCellSize); +} + +/// Where OctoMap::createCloud() reports the voxel centerd at @p center: x and y at the +/// cell corner, z at the center. +cv::Point3f asReported(const cv::Point3f & center) +{ + return cv::Point3f(center.x - 0.5f*kCellSize, center.y - 0.5f*kCellSize, center.z); +} + +/// True if @p cloud holds a point within @p tolerance of @p expected. +bool containsPoint(const sensor_msgs::msg::PointCloud2 & cloud, const cv::Point3f & expected, + float tolerance = 1e-3f) +{ + for(size_t i=0; i= int(map.info.width) || row >= int(map.info.height)) + { + return -2; + } + return map.data[size_t(row) * map.info.width + col]; +} +/** + * @brief True if any cell within @p radius cells of (@p x, @p y) holds @p value. + * + * Used for the octomap grid, which is discretized on OctoMap's own voxel lattice: the + * cell containing a given point can sit a column away from where the same point lands in + * the occupancy grid, and pinning that offset would be testing octomap's internals. + */ +bool hasValueNear(const nav_msgs::msg::OccupancyGrid & map, double x, double y, + int8_t value, int radius = 1) +{ + const int col = int((x - map.info.origin.position.x) / map.info.resolution); + const int row = int((y - map.info.origin.position.y) / map.info.resolution); + for(int r=row-radius; r<=row+radius; ++r) + { + for(int c=col-radius; c<=col+radius; ++c) + { + if(r >= 0 && c >= 0 && r < int(map.info.height) && c < int(map.info.width) && + map.data[size_t(r) * map.info.width + c] == value) + { + return true; + } + } + } + return false; +} + +/// How many cells of @p map hold @p value. +int countCells(const nav_msgs::msg::OccupancyGrid & map, int8_t value) +{ + int count = 0; + for(size_t i=0; i & overrides = {}, + const rtabmap::ParametersMap & rtabmapParameters = rtabmap::ParametersMap()) + { + // Each test gets its own namespace: MapsManager reports whether anyone is + // listening, and a subscription from a previous test in this process can still + // be winding down on the shared topic names. + static int counter = 0; + namespace_ = uFormat("/maps_manager_test_%d", ++counter); + node_ = addNode(std::make_shared("maps_manager_test", namespace_, + rclcpp::NodeOptions().parameter_overrides(overrides))); + maps_ = std::make_shared(); + maps_->init(*node_, "test", true); + + rtabmap::ParametersMap parameters = sceneParameters(); + for(rtabmap::ParametersMap::const_iterator iter=rtabmapParameters.begin(); + iter!=rtabmapParameters.end(); ++iter) + { + parameters[iter->first] = iter->second; + } + maps_->setParameters(parameters); + } + + /// The fully qualified name of one of MapsManager's topics. + std::string topic(const std::string & name) const { return namespace_ + "/" + name; } + + /** + * @brief The two-node scene every geometric assertion below is written against. + * + * @note The cells span both axes on purpose. An occupancy grid is a 2D map, and + * OccupancyGrid::assemble() deliberately builds nothing from a scene that is + * only a line of cells, so a fixture laid out along a single axis would give + * an empty grid with working clouds. + * @note Node 1 also carries empty cells, which is what the octomap reports as free + * space; they do not reach the ground/obstacle clouds. + * @note The two nodes deliberately arrive differently: node 1 with a ready-made local + * grid, node 2 with a raw scan MapsManager has to segment itself. Both branches + * of updateMapCaches() are therefore exercised by every test below. + */ + std::map scene() + { + std::map signatures; + signatures.insert(std::make_pair(1, makeGridSignature(1, poseOf(1), + {cv::Point3f(0.5f, -0.1f, 0.0f), cv::Point3f(0.5f, 0.1f, 0.0f)}, + {cv::Point3f(1.0f, 0.0f, 0.0f)}, + {cv::Point3f(0.2f, -0.1f, 0.0f), cv::Point3f(0.2f, 0.1f, 0.0f)}))); + // Node 2 hands over the raw scan instead, so MapsManager has to segment it: the + // point at ground height becomes a ground cell, the raised one an obstacle. + signatures.insert(std::make_pair(2, makeScanSignature(2, poseOf(2), + {cv::Point3f(0.5f, 0.1f, 0.0f), + cv::Point3f(1.0f, -0.1f, kObstacleHeight)}))); + return signatures; + } + + static rtabmap::Transform poseOf(int id) + { + return rtabmap::Transform(2.0f * float(id - 1), 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); + } + + static std::map posesOfScene() + { + std::map poses; + poses.insert(std::make_pair(1, poseOf(1))); + poses.insert(std::make_pair(2, poseOf(2))); + return poses; + } + + /// Feeds the scene in and publishes it, then spins so the messages arrive. + void updateAndPublish(bool updateGrid = true, bool updateOctomap = false) + { + const std::map signatures = scene(); + const std::map poses = posesOfScene(); + maps_->updateMapCaches(poses, /*memory=*/0, updateGrid, updateOctomap, signatures); + maps_->publishMaps(poses, node_->now(), "map"); + spinFor(std::chrono::milliseconds(100)); + } + + /// Feeds the scene in with the octomap updated, then publishes. + void updateAndPublishOctomap() + { + const std::map poses = posesOfScene(); + maps_->updateMapCaches(poses, /*memory=*/0, /*updateGrid=*/false, + /*updateOctomap=*/true, scene()); + maps_->publishMaps(poses, node_->now(), "map"); + spinFor(std::chrono::milliseconds(150)); + } + + /// Subscribes and waits until MapsManager has seen the subscription. + template + std::shared_ptr> collectFromMaps(const std::string & name) + { + std::shared_ptr> collector = collect(topic(name)); + EXPECT_TRUE(waitForPublisher(collector->subscription)) + << "no publisher on " << topic(name); + EXPECT_TRUE(spinUntil([&]() { return maps_->hasSubscribers(); })) + << "MapsManager never saw the subscription on " << topic(name); + return collector; + } + + std::string namespace_; + rclcpp::Node::SharedPtr node_; + std::shared_ptr maps_; +}; + +//============================================================================ +// Assembled clouds +//============================================================================ + +TEST_F(MapsManagerTest, AssemblesGroundAndObstacleClouds) +{ + start(); + std::shared_ptr> ground = + collectFromMaps("cloud_ground"); + std::shared_ptr> obstacles = + collectFromMaps("cloud_obstacles"); + + updateAndPublish(); + + ASSERT_FALSE(ground->empty()) << "no ground cloud published"; + ASSERT_FALSE(obstacles->empty()) << "no obstacle cloud published"; + + EXPECT_EQ(ground->back().header.frame_id, "map"); + EXPECT_EQ(ground->back().width * ground->back().height, 3u) + << "two ground cells from node 1 and one from node 2"; + EXPECT_EQ(obstacles->back().width * obstacles->back().height, 2u); + + // The cells are stored in each node's own frame and placed by its pose. + EXPECT_TRUE(containsPoint(obstacles->back(), cv::Point3f(1.0f, 0.0f, 0.0f))) + << "node 1 sits at the origin"; + EXPECT_TRUE(containsPoint(obstacles->back(), cv::Point3f(3.0f, -0.1f, kObstacleHeight))) + << "node 2 sits 2 m along x, so its obstacle lands at 3 m"; +} + +TEST_F(MapsManagerTest, ColorsGroundGreenAndObstaclesRed) +{ + start(); + std::shared_ptr> ground = + collectFromMaps("cloud_ground"); + std::shared_ptr> obstacles = + collectFromMaps("cloud_obstacles"); + + updateAndPublish(); + ASSERT_FALSE(ground->empty()); + ASSERT_FALSE(obstacles->empty()); + + EXPECT_EQ(colorAt(ground->back(), 0), cv::Vec3b(0, 255, 0)); + EXPECT_EQ(colorAt(obstacles->back(), 0), cv::Vec3b(255, 0, 0)); +} + +TEST_F(MapsManagerTest, CloudMapCombinesGroundAndObstacles) +{ + start(); + std::shared_ptr> cloudMap = + collectFromMaps("cloud_map"); + + updateAndPublish(); + + ASSERT_FALSE(cloudMap->empty()) << "no cloud map published"; + EXPECT_EQ(cloudMap->back().width * cloudMap->back().height, 5u) + << "three ground cells plus two obstacles"; + EXPECT_TRUE(containsPoint(cloudMap->back(), cv::Point3f(1.0f, 0.0f, 0.0f))); + EXPECT_TRUE(containsPoint(cloudMap->back(), cv::Point3f(2.5f, 0.1f, 0.0f))) + << "node 2's ground cell"; +} + +TEST_F(MapsManagerTest, RegeneratesLocalGridsFromARawScan) +{ + // The branch of updateMapCaches() where the sensor data has no local grid, so + // LocalGridMaker builds one. Two scan points, split by height alone. + start(); + std::shared_ptr> ground = + collectFromMaps("cloud_ground"); + std::shared_ptr> obstacles = + collectFromMaps("cloud_obstacles"); + + std::map signatures; + signatures.insert(std::make_pair(1, makeScanSignature(1, rtabmap::Transform::getIdentity(), + {cv::Point3f(0.5f, -0.1f, 0.0f), + cv::Point3f(0.5f, 0.1f, kObstacleHeight)}))); + std::map poses; + poses.insert(std::make_pair(1, rtabmap::Transform::getIdentity())); + + maps_->updateMapCaches(poses, /*memory=*/0, true, false, signatures); + maps_->publishMaps(poses, node_->now(), "map"); + spinFor(std::chrono::milliseconds(150)); + + ASSERT_FALSE(ground->empty()) << "no ground cloud published"; + ASSERT_FALSE(obstacles->empty()) << "no obstacle cloud published"; + EXPECT_EQ(ground->back().width * ground->back().height, 1u) + << "the point at ground height"; + EXPECT_EQ(obstacles->back().width * obstacles->back().height, 1u) + << "the raised point"; + // The cells are snapped to the grid, so they land within a cell of the scan points. + EXPECT_TRUE(containsPoint(ground->back(), cv::Point3f(0.5f, -0.1f, 0.0f), kCellSize)); + EXPECT_TRUE(containsPoint(obstacles->back(), + cv::Point3f(0.5f, 0.1f, kObstacleHeight), kCellSize)); +} + +TEST_F(MapsManagerTest, TheGroundHeightDecidesWhatIsAnObstacle) +{ + // Same scan, but with the threshold lifted above the raised point: it is ground now, + // which is what shows the height passthrough is doing the segmenting. + rtabmap::ParametersMap parameters; + parameters.insert(rtabmap::ParametersPair( + rtabmap::Parameters::kGridMaxGroundHeight(), "1.0")); + start({}, parameters); + + std::shared_ptr> ground = + collectFromMaps("cloud_ground"); + std::shared_ptr> obstacles = + collectFromMaps("cloud_obstacles"); + + std::map signatures; + signatures.insert(std::make_pair(1, makeScanSignature(1, rtabmap::Transform::getIdentity(), + {cv::Point3f(0.5f, -0.1f, 0.0f), + cv::Point3f(0.5f, 0.1f, kObstacleHeight)}))); + std::map poses; + poses.insert(std::make_pair(1, rtabmap::Transform::getIdentity())); + + maps_->updateMapCaches(poses, /*memory=*/0, true, false, signatures); + maps_->publishMaps(poses, node_->now(), "map"); + spinFor(std::chrono::milliseconds(150)); + + ASSERT_FALSE(ground->empty()); + ASSERT_FALSE(obstacles->empty()); + EXPECT_EQ(ground->back().width * ground->back().height, 2u) + << "both points are below the raised threshold"; + EXPECT_EQ(obstacles->back().width * obstacles->back().height, 0u); +} + +//============================================================================ +// Occupancy grid +//============================================================================ + +TEST_F(MapsManagerTest, PublishesTheOccupancyGrid) +{ + start(); + std::shared_ptr> grid = + collectFromMaps("map"); + + updateAndPublish(); + + ASSERT_FALSE(grid->empty()) << "no occupancy grid published"; + const nav_msgs::msg::OccupancyGrid & map = grid->back(); + EXPECT_EQ(map.header.frame_id, "map"); + EXPECT_NEAR(map.info.resolution, kCellSize, 1e-6); + EXPECT_GT(map.info.width, 0u); + + // The map must agree with what getGridMap() hands out. + float xMin = 0.0f, yMin = 0.0f, cellSize = 0.0f; + const cv::Mat pixels = maps_->getGridMap(xMin, yMin, cellSize); + EXPECT_NEAR(map.info.origin.position.x, xMin, 1e-6); + EXPECT_NEAR(map.info.origin.position.y, yMin, 1e-6); + EXPECT_NEAR(cellSize, kCellSize, 1e-6); + EXPECT_EQ(map.info.width, uint32_t(pixels.cols)); + EXPECT_EQ(map.info.height, uint32_t(pixels.rows)); + + // Obstacles are occupied, ground is free. + EXPECT_EQ(cellAt(map, 1.0, 0.0), 100) << "node 1's obstacle"; + EXPECT_EQ(cellAt(map, 3.0, -0.1), 100) << "node 2's obstacle"; + EXPECT_EQ(cellAt(map, 0.5, -0.1), 0) << "node 1's ground"; +} + +TEST_F(MapsManagerTest, CellSizeParameterChangesTheResolution) +{ + rtabmap::ParametersMap parameters; + parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGridCellSize(), "0.1")); + start({}, parameters); + + float xMin = 0.0f, yMin = 0.0f, cellSize = 0.0f; + maps_->getGridMap(xMin, yMin, cellSize); + EXPECT_NEAR(cellSize, 0.1f, 1e-6) << "setParameters must reach the occupancy grid"; +} + +TEST_F(MapsManagerTest, GridProbMapUsesProbabilities) +{ + start(); + std::shared_ptr> grid = + collectFromMaps("grid_prob_map"); + + updateAndPublish(); + + ASSERT_FALSE(grid->empty()) << "no probability grid published"; + EXPECT_NEAR(grid->back().info.resolution, kCellSize, 1e-6); + // The probability map reports 0..100 instead of the ternary free/occupied/unknown. + EXPECT_GT(cellAt(grid->back(), 1.0, 0.0), 50) << "the obstacle cell is likely occupied"; +} + +//============================================================================ +// Poses +//============================================================================ + +TEST_F(MapsManagerTest, KeepsEveryPoseWithoutAFilterRadius) +{ + start(); + std::map poses; + for(int id=1; id<=4; ++id) + { + poses.insert(std::make_pair(id, rtabmap::Transform(0.1f*float(id), 0, 0, 0, 0, 0))); + } + EXPECT_EQ(maps_->getFilteredPoses(poses).size(), poses.size()) + << "map_filter_radius defaults to 0, which disables the filter"; +} + +TEST_F(MapsManagerTest, FilterRadiusThinsNearbyPoses) +{ + start({rclcpp::Parameter("map_filter_radius", 1.0)}); + std::map poses; + for(int id=1; id<=4; ++id) + { + // All within a meter of each other, and all facing the same way. + poses.insert(std::make_pair(id, rtabmap::Transform(0.1f*float(id), 0, 0, 0, 0, 0))); + } + EXPECT_LT(maps_->getFilteredPoses(poses).size(), poses.size()) + << "poses closer than the radius must be dropped"; + EXPECT_GE(maps_->getFilteredPoses(poses).size(), 1u); +} + +TEST_F(MapsManagerTest, DropsTheLatestPoseUnlessAlwaysUpdating) +{ + // Pose 0 is the "current" node, not yet in the graph. It is only mapped when + // map_always_update is set, otherwise the map only shows committed nodes. + start({rclcpp::Parameter("map_empty_ray_tracing", false)}); + + std::map signatures = scene(); + signatures.insert(std::make_pair(0, makeGridSignature(0, rtabmap::Transform(), + {}, {cv::Point3f(9.0f, 0.0f, 0.0f)}))); + std::map poses = posesOfScene(); + poses.insert(std::make_pair(0, rtabmap::Transform::getIdentity())); + + const std::map filtered = + maps_->updateMapCaches(poses, 0, true, false, signatures); + EXPECT_EQ(filtered.find(0), filtered.end()) << "node 0 must be dropped by default"; + EXPECT_EQ(filtered.size(), 2u); +} + +TEST_F(MapsManagerTest, KeepsTheLatestPoseWhenAlwaysUpdating) +{ + start({rclcpp::Parameter("map_always_update", true), + rclcpp::Parameter("map_empty_ray_tracing", false)}); + + std::map signatures = scene(); + signatures.insert(std::make_pair(0, makeGridSignature(0, rtabmap::Transform(), + {}, {cv::Point3f(9.0f, 0.0f, 0.0f)}))); + std::map poses = posesOfScene(); + poses.insert(std::make_pair(0, rtabmap::Transform::getIdentity())); + + const std::map filtered = + maps_->updateMapCaches(poses, 0, true, false, signatures); + EXPECT_NE(filtered.find(0), filtered.end()) << "node 0 must be kept"; + EXPECT_EQ(filtered.size(), 3u); +} + +TEST_F(MapsManagerTest, IgnoresLandmarkPoses) +{ + // Landmarks use negative ids and have no grid to contribute. + start(); + std::map poses; + poses.insert(std::make_pair(-5, rtabmap::Transform::getIdentity())); + poses.insert(std::make_pair(1, poseOf(1))); + poses.insert(std::make_pair(2, poseOf(2))); + + const std::map filtered = + maps_->updateMapCaches(poses, 0, true, false, scene()); + EXPECT_EQ(filtered.find(-5), filtered.end()); + EXPECT_EQ(filtered.size(), 2u); +} + +TEST_F(MapsManagerTest, RefusesEmptyPoses) +{ + start(); + EXPECT_TRUE(maps_->updateMapCaches(std::map(), 0, true, false, + scene()).empty()); +} + +TEST_F(MapsManagerTest, RefusesWithoutMemoryOrSignatures) +{ + start(); + EXPECT_TRUE(maps_->updateMapCaches(posesOfScene(), 0, true, false, + std::map()).empty()); +} + +//============================================================================ +// Subscriber bookkeeping +//============================================================================ + +TEST_F(MapsManagerTest, HasNoSubscribersOnItsOwn) +{ + start(); + spinFor(std::chrono::milliseconds(100)); + EXPECT_FALSE(maps_->hasSubscribers()); +} + +TEST_F(MapsManagerTest, HasSubscribersOnceSomeoneListens) +{ + start(); + collectFromMaps("cloud_map"); + EXPECT_TRUE(maps_->hasSubscribers()); +} + +TEST_F(MapsManagerTest, AssumesTheMapChangedWithoutGridSubscribers) +{ + // Whether the map changed is only known from OccupancyGrid::update(), which is only + // run when someone wants a grid. With nobody listening the answer is assumed true. + start(); + spinFor(std::chrono::milliseconds(100)); + EXPECT_TRUE(maps_->isMapUpdated()); +} + +TEST_F(MapsManagerTest, ReportsTheMapUnchangedOnASecondIdenticalUpdate) +{ + start(); + collectFromMaps("map"); + + maps_->updateMapCaches(posesOfScene(), 0, true, false, scene()); + EXPECT_TRUE(maps_->isMapUpdated()) << "the first update adds both nodes"; + + maps_->updateMapCaches(posesOfScene(), 0, true, false, scene()); + EXPECT_FALSE(maps_->isMapUpdated()) << "nothing moved and nothing was added"; +} + +TEST_F(MapsManagerTest, PublishesNothingWithoutSubscribers) +{ + start(); + maps_->updateMapCaches(posesOfScene(), 0, true, false, scene()); + maps_->publishMaps(posesOfScene(), node_->now(), "map"); + + // Subscribing afterwards with a volatile subscription sees nothing. + std::shared_ptr> cloud = + collect(topic("cloud_map")); + spinFor(std::chrono::milliseconds(200)); + EXPECT_TRUE(cloud->empty()); +} + +//============================================================================ +// Latching +//============================================================================ + +TEST_F(MapsManagerTest, LatchesTheMapForLateSubscribers) +{ + start(); // latch defaults to true + EXPECT_TRUE(maps_->isLatching()); + collectFromMaps("map"); + updateAndPublish(); + + // A subscriber joining after the fact still gets the last map, because the publisher + // is transient local. + std::shared_ptr> late = + collect(topic("map"), + rclcpp::QoS(1).reliable().transient_local()); + EXPECT_TRUE(spinUntil([&]() { return !late->empty(); })) + << "the latched map was not delivered"; +} + +TEST_F(MapsManagerTest, DoesNotLatchWhenLatchIsFalse) +{ + start({rclcpp::Parameter("latch", false)}); + EXPECT_FALSE(maps_->isLatching()); + collectFromMaps("map"); + updateAndPublish(); + + // With a volatile publisher there is no history to hand out (and a transient local + // subscription is not even compatible), so a late subscriber gets nothing. + std::shared_ptr> late = + collect(topic("map"), + rclcpp::QoS(1).reliable().transient_local()); + spinFor(std::chrono::milliseconds(300)); + EXPECT_TRUE(late->empty()); +} + +//============================================================================ +// Caches +//============================================================================ + +TEST_F(MapsManagerTest, ClearEmptiesTheAssembledClouds) +{ + start(); + std::shared_ptr> cloud = + collectFromMaps("cloud_map"); + updateAndPublish(); + ASSERT_FALSE(cloud->empty()); + ASSERT_GT(cloud->back().width * cloud->back().height, 0u); + + maps_->clear(); + maps_->publishMaps(posesOfScene(), node_->now(), "map"); + spinFor(std::chrono::milliseconds(150)); + + EXPECT_EQ(cloud->back().width * cloud->back().height, 0u) + << "clear() must drop the cached grids, leaving nothing to assemble"; +} + +TEST_F(MapsManagerTest, Set2DMapInstallsAGridDirectly) +{ + // Used when a map comes back from the database rather than from local grids. + start(); + cv::Mat map(4, 6, CV_8SC1, cv::Scalar(-1)); + map.at(2, 3) = 100; + map.at(1, 1) = 0; + + // The poses are not optional: set2DMap() keeps the map only when it is told which + // nodes it was assembled from. + maps_->set2DMap(map, /*xMin=*/-1.0f, /*yMin=*/-0.5f, kCellSize, posesOfScene()); + + float xMin = 0.0f, yMin = 0.0f, cellSize = 0.0f; + const cv::Mat out = maps_->getGridMap(xMin, yMin, cellSize); + ASSERT_FALSE(out.empty()); + EXPECT_EQ(out.cols, 6); + EXPECT_EQ(out.rows, 4); + EXPECT_NEAR(xMin, -1.0f, 1e-6); + EXPECT_NEAR(yMin, -0.5f, 1e-6); + EXPECT_NEAR(cellSize, kCellSize, 1e-6); + EXPECT_EQ(out.at(2, 3), 100); + EXPECT_EQ(out.at(1, 1), 0); +} + +//============================================================================ +// Parameters that moved to the rtabmap library +//============================================================================ + +TEST_F(MapsManagerTest, Set2DMapNeedsThePosesTheMapCameFrom) +{ + // The grid is kept only together with the poses it was assembled from, so that it + // knows which nodes are already in it. Without them the map is dropped, and + // MapsManager warns rather than leaving the caller to wonder. + start(); + cv::Mat map(4, 6, CV_8SC1, cv::Scalar(-1)); + map.at(2, 3) = 100; + + maps_->set2DMap(map, -1.0f, -0.5f, kCellSize, std::map()); + + float xMin = 0.0f, yMin = 0.0f, cellSize = 0.0f; + EXPECT_TRUE(maps_->getGridMap(xMin, yMin, cellSize).empty()); +} + +TEST_F(MapsManagerTest, CopiesMovedParametersToTheirNewNames) +{ + start(); + node_->declare_parameter("grid_cell_size", 0.1); + node_->declare_parameter("proj_max_ground_height", 0.3); + + rtabmap::ParametersMap parameters; + maps_->backwardCompatibilityParameters(*node_, parameters); + + ASSERT_TRUE(parameters.find(rtabmap::Parameters::kGridCellSize()) != parameters.end()) + << "grid_cell_size must be copied to " << rtabmap::Parameters::kGridCellSize(); + EXPECT_NEAR(uStr2Float(parameters.at(rtabmap::Parameters::kGridCellSize())), 0.1f, 1e-6); + + ASSERT_TRUE(parameters.find(rtabmap::Parameters::kGridMaxGroundHeight()) != parameters.end()); + EXPECT_NEAR(uStr2Float(parameters.at(rtabmap::Parameters::kGridMaxGroundHeight())), 0.3f, 1e-6); +} + +TEST_F(MapsManagerTest, LeavesUnsetLegacyParametersAlone) +{ + start(); + rtabmap::ParametersMap parameters; + maps_->backwardCompatibilityParameters(*node_, parameters); + EXPECT_TRUE(parameters.empty()) << "nothing was declared, so nothing should be copied"; +} + +//============================================================================ +// Octomap +//============================================================================ + +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) + +TEST_F(MapsManagerTest, PublishesTheBinaryOctomap) +{ + start(); + std::shared_ptr> binary = + collectFromMaps("octomap_binary"); + + updateAndPublishOctomap(); + + ASSERT_FALSE(binary->empty()) << "no binary octomap published"; + EXPECT_EQ(binary->back().header.frame_id, "map"); + EXPECT_TRUE(binary->back().binary); + EXPECT_EQ(binary->back().id, "ColorOcTree") + << "rtabmap keeps a color per voxel, so the tree type is not a plain OcTree"; + EXPECT_NEAR(binary->back().resolution, kCellSize, 1e-6); + EXPECT_FALSE(binary->back().data.empty()) << "the serialized tree must not be empty"; +} + +TEST_F(MapsManagerTest, PublishesTheFullOctomap) +{ + start(); + std::shared_ptr> full = + collectFromMaps("octomap_full"); + + updateAndPublishOctomap(); + + ASSERT_FALSE(full->empty()) << "no full octomap published"; + EXPECT_FALSE(full->back().binary) << "the full tree carries occupancy probabilities"; + EXPECT_EQ(full->back().id, "ColorOcTree") + << "consumers deserialize on this id, so both messages must report the same type"; + EXPECT_NEAR(full->back().resolution, kCellSize, 1e-6); + EXPECT_FALSE(full->back().data.empty()); +} + +TEST_F(MapsManagerTest, PublishesTheOctomapOccupiedSpace) +{ + start(); + std::shared_ptr> occupied = + collectFromMaps("octomap_occupied_space"); + + updateAndPublishOctomap(); + + ASSERT_FALSE(occupied->empty()) << "no octomap cloud published"; + EXPECT_EQ(occupied->back().header.frame_id, "map"); + EXPECT_EQ(occupied->back().width * occupied->back().height, 5u) + << "occupied space is the obstacles plus the ground: 2 + 3 cells"; +} + +TEST_F(MapsManagerTest, PublishesTheOctomapObstacles) +{ + start(); + std::shared_ptr> obstacles = + collectFromMaps("octomap_obstacles"); + + updateAndPublishOctomap(); + + ASSERT_FALSE(obstacles->empty()) << "no octomap obstacles published"; + EXPECT_EQ(obstacles->back().width * obstacles->back().height, 2u); + // Points come back at voxel centers, up to half a cell from where they went in. + EXPECT_TRUE(containsPoint(obstacles->back(), cv::Point3f(1.0f, 0.0f, 0.0f), kCellSize)); + EXPECT_TRUE(containsPoint(obstacles->back(), + cv::Point3f(3.0f, -0.1f, kObstacleHeight), kCellSize)); +} + +TEST_F(MapsManagerTest, PublishesTheOctomapGround) +{ + start(); + std::shared_ptr> ground = + collectFromMaps("octomap_ground"); + + updateAndPublishOctomap(); + + ASSERT_FALSE(ground->empty()) << "no octomap ground published"; + EXPECT_EQ(ground->back().width * ground->back().height, 3u) + << "the ground cells only, not the empty ones"; + EXPECT_TRUE(containsPoint(ground->back(), cv::Point3f(0.5f, -0.1f, 0.0f), kCellSize)); +} + +TEST_F(MapsManagerTest, PublishesTheOctomapEmptySpace) +{ + start(); + std::shared_ptr> empty = + collectFromMaps("octomap_empty_space"); + + updateAndPublishOctomap(); + + ASSERT_FALSE(empty->empty()) << "no octomap empty space published"; + EXPECT_EQ(empty->back().header.frame_id, "map"); + EXPECT_EQ(empty->back().width * empty->back().height, 2u) + << "node 1's two empty cells, and nothing else: ground cells are stored as " + "occupied nodes flagged as ground, so they are not free space"; + // createCloud() reports x and y at the cell corner but z at the cell center. + EXPECT_TRUE(containsPoint(empty->back(), + cv::Point3f(0.2f, -0.1f, 0.5f*kCellSize), 1e-3f)); + EXPECT_TRUE(containsPoint(empty->back(), + cv::Point3f(0.2f, 0.1f, 0.5f*kCellSize), 1e-3f)); +} + +TEST_F(MapsManagerTest, PublishesTheOctomapFrontier) +{ + // A frontier cell is a free cell with at least one unknown face neighbor. Nothing + // encloses this scene, so the frontier is exactly the free space: node 1's two empty + // cells. The ground and obstacle cells are occupied nodes and never qualify. + start(); + std::shared_ptr> frontier = + collectFromMaps("octomap_global_frontier_space"); + + updateAndPublishOctomap(); + + ASSERT_FALSE(frontier->empty()) << "no octomap frontier published"; + EXPECT_EQ(frontier->back().header.frame_id, "map"); + EXPECT_EQ(frontier->back().width * frontier->back().height, 2u); + EXPECT_TRUE(containsPoint(frontier->back(), + cv::Point3f(0.2f, -0.1f, 0.5f*kCellSize), 1e-3f)); + EXPECT_TRUE(containsPoint(frontier->back(), + cv::Point3f(0.2f, 0.1f, 0.5f*kCellSize), 1e-3f)); +} + +TEST_F(MapsManagerTest, AnEnclosedEmptyCellIsNotAFrontier) +{ + // The frontier rule in one scene: two identical empty cells, one walled in on all six + // faces by obstacles and one out in the open. Both are free space, but only the open + // one has an unknown neighbor, so only it is a frontier. + start(); + std::shared_ptr> frontier = + collectFromMaps("octomap_global_frontier_space"); + std::shared_ptr> empty = + collect(topic("octomap_empty_space")); + ASSERT_TRUE(waitForPublisher(empty->subscription)); + + const cv::Point3f enclosed = voxelCenter(19, 0, 9); + const cv::Point3f open = voxelCenter(39, 0, 9); + + std::map signatures; + signatures.insert(std::make_pair(1, makeGridSignature(1, rtabmap::Transform::getIdentity(), + /*ground=*/{}, + /*obstacles=*/{voxelCenter(18, 0, 9), voxelCenter(20, 0, 9), // -x, +x + voxelCenter(19, -1, 9), voxelCenter(19, 1, 9), // -y, +y + voxelCenter(19, 0, 8), voxelCenter(19, 0, 10)}, // -z, +z + /*empty=*/{enclosed, open}))); + std::map poses; + poses.insert(std::make_pair(1, rtabmap::Transform::getIdentity())); + + maps_->updateMapCaches(poses, /*memory=*/0, /*updateGrid=*/false, /*updateOctomap=*/true, + signatures); + maps_->publishMaps(poses, node_->now(), "map"); + spinFor(std::chrono::milliseconds(200)); + + ASSERT_FALSE(empty->empty()) << "no octomap empty space published"; + ASSERT_FALSE(frontier->empty()) << "no octomap frontier published"; + + // Both cells are free space... + EXPECT_EQ(empty->back().width * empty->back().height, 2u); + EXPECT_TRUE(containsPoint(empty->back(), asReported(enclosed), 1e-3f)); + EXPECT_TRUE(containsPoint(empty->back(), asReported(open), 1e-3f)); + + // ...but the walled-in one is not on the frontier. + EXPECT_EQ(frontier->back().width * frontier->back().height, 1u); + EXPECT_TRUE(containsPoint(frontier->back(), asReported(open), 1e-3f)) + << "the open cell borders unknown space"; + EXPECT_FALSE(containsPoint(frontier->back(), asReported(enclosed), 1e-3f)) + << "all six face neighbors of the enclosed cell are known, so it is not a frontier"; +} + +TEST_F(MapsManagerTest, PublishesTheOctomapGrid) +{ + start(); + std::shared_ptr> grid = + collectFromMaps("octomap_grid"); + + updateAndPublishOctomap(); + + ASSERT_FALSE(grid->empty()) << "no octomap grid published"; + const nav_msgs::msg::OccupancyGrid & map = grid->back(); + EXPECT_EQ(map.header.frame_id, "map"); + EXPECT_EQ(countCells(map, 100), 2) << "one occupied cell per obstacle"; + EXPECT_TRUE(hasValueNear(map, 1.0, 0.0, 100)) << "node 1's obstacle"; + EXPECT_TRUE(hasValueNear(map, 3.0, -0.1, 100)) << "node 2's obstacle"; + EXPECT_TRUE(hasValueNear(map, 0.5, -0.1, 0)) << "node 1's ground is free space"; +} + +TEST_F(MapsManagerTest, ExposesTheOctomap) +{ + start(); + ASSERT_NE(maps_->getOctomap(), nullptr); +} +#endif + +//============================================================================ +// Elevation map (grid_map) +//============================================================================ + +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) +TEST_F(MapsManagerTest, PublishesTheElevationMap) +{ + // updateMapCaches() has no explicit flag for the elevation map: it is only built + // through the "nothing requested, so follow the subscribers" path. + start(); + std::shared_ptr> elevation = + collectFromMaps("elevation_map"); + + const std::map poses = posesOfScene(); + maps_->updateMapCaches(poses, /*memory=*/0, /*updateGrid=*/false, /*updateOctomap=*/false, + scene()); + maps_->publishMaps(poses, node_->now(), "map"); + spinFor(std::chrono::milliseconds(150)); + + ASSERT_FALSE(elevation->empty()) << "no elevation map published"; + const grid_map_msgs::msg::GridMap & msg = elevation->back(); + EXPECT_EQ(msg.header.frame_id, "map"); + EXPECT_NEAR(msg.info.resolution, kCellSize, 1e-6); + EXPECT_GT(msg.info.length_x, 0.0); + EXPECT_GT(msg.info.length_y, 0.0); + + ASSERT_FALSE(msg.layers.empty()) << "the grid map must carry its layers"; + EXPECT_NE(std::find(msg.layers.begin(), msg.layers.end(), "elevation"), msg.layers.end()) + << "the elevation layer is what makes this an elevation map"; + EXPECT_EQ(msg.data.size(), msg.layers.size()) << "one data matrix per layer"; +} + +TEST_F(MapsManagerTest, DoesNotRepublishAnUnchangedElevationMap) +{ + // Like every other map, once latched it should stay put until something changes. + start(); + std::shared_ptr> elevation = + collectFromMaps("elevation_map"); + + const std::map poses = posesOfScene(); + maps_->updateMapCaches(poses, 0, false, false, scene()); + maps_->publishMaps(poses, node_->now(), "map"); + spinFor(std::chrono::milliseconds(150)); + ASSERT_FALSE(elevation->empty()); + const size_t afterFirst = elevation->size(); + + // Nothing new to assemble, so nothing to send. + maps_->updateMapCaches(poses, 0, false, false, scene()); + maps_->publishMaps(poses, node_->now(), "map"); + spinFor(std::chrono::milliseconds(150)); + + EXPECT_EQ(elevation->size(), afterFirst) + << "the latched elevation map was republished unchanged"; +} +#endif + +TEST_F(MapsManagerTest, ExposesTheOccupancyGridAndLocalMapMaker) +{ + start(); + ASSERT_NE(maps_->getOccupancyGrid(), nullptr); + ASSERT_NE(maps_->getLocalMapMaker(), nullptr); + EXPECT_NEAR(maps_->getOccupancyGrid()->getCellSize(), kCellSize, 1e-6); +} + diff --git a/rtabmap_util/test/test_obstacles_detection.cpp b/rtabmap_util/test/test_obstacles_detection.cpp new file mode 100644 index 00000000..50c9e499 --- /dev/null +++ b/rtabmap_util/test/test_obstacles_detection.cpp @@ -0,0 +1,423 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "node_test_utils.hpp" +#include "msg_builders.hpp" + +#include + +#include +#include + +using namespace rtabmap_util_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); + +/** + * @brief A dense ground plane at z=0 plus a vertical wall in front of it. + * + * The 0.05 m spacing matters: the segmentation clusters points with + * Grid/ClusterRadius (0.1 m by default) and drops clusters below + * Grid/MinClusterSize (10), so a sparser cloud is discarded entirely. + */ +std::vector groundAndWall() +{ + std::vector points; + for(int i=0; i<=20; ++i) // ground: 1 m x 1 m at z=0 + { + for(int j=0; j<=20; ++j) + { + points.push_back(cv::Point3f(0.3f + 0.05f*i, -0.5f + 0.05f*j, 0.0f)); + } + } + for(int j=0; j<=20; ++j) // wall: vertical, 1 m wide, 0.75 m tall + { + for(int k=1; k<=15; ++k) + { + points.push_back(cv::Point3f(1.4f, -0.5f + 0.05f*j, 0.05f*k)); + } + } + return points; +} +/** + * @brief A plane that is horizontal in the map frame, given a base frame pitched by + * @p pitch. In the base frame it therefore rises with x: z = x * tan(pitch). + */ +std::vector planeLevelInMapFrame(double pitch) +{ + std::vector points; + for(int i=0; i<=20; ++i) + { + const float x = 0.8f + 0.05f*i; + for(int j=0; j<=20; ++j) + { + points.push_back(cv::Point3f(x, -0.5f + 0.05f*j, x * float(std::tan(pitch)))); + } + } + return points; +} +/// Two flat 25-point patches: one about 0.5 m from the sensor, one about 3 m away. +std::vector nearAndFarPatches() +{ + std::vector points; + for(int i=0; i<5; ++i) + { + for(int j=0; j<5; ++j) + { + points.push_back(cv::Point3f(0.4f + 0.05f*i, -0.1f + 0.05f*j, 0.0f)); + points.push_back(cv::Point3f(2.9f + 0.05f*i, -0.1f + 0.05f*j, 0.0f)); + } + } + return points; +} + +/// Smallest and largest x in a cloud, to tell the near patch from the far one. +std::pair xExtent(const sensor_msgs::msg::PointCloud2 & cloud) +{ + float lo = std::numeric_limits::max(); + float hi = -std::numeric_limits::max(); + for(size_t i=0; i(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("frame_id", "base_link"), + rclcpp::Parameter("wait_for_transform", 0.1)}))); + publishStaticTf("base_link", "lidar"); + + std::shared_ptr> ground = + collect("ground"); + std::shared_ptr> obstacles = + collect("obstacles"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(ground->subscription)); + + pub->publish(makeXYZCloud("lidar", 1000.0, groundAndWall())); + ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); })) + << "both ground and obstacles must be published"; + + EXPECT_GT(ground->back().width, 0u) << "the flat points must be classified as ground"; + EXPECT_GT(obstacles->back().width, 0u) << "the wall must be classified as obstacles"; + // The clouds are transformed back into the frame of the input topic, not frame_id. + EXPECT_EQ(ground->back().header.frame_id, "lidar"); + EXPECT_EQ(obstacles->back().header.frame_id, "lidar"); +} + +TEST_F(ObstaclesDetectionTest, ProjectsObstaclesOntoTheGroundPlane) +{ + // proj_obstacles is the obstacles cloud flattened to z=0, with flat surfaces removed. + // Note it is published in frame_id, unlike ground/obstacles which keep the input frame. + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("frame_id", "base_link"), + rclcpp::Parameter("wait_for_transform", 0.1)}))); + publishStaticTf("base_link", "lidar"); + + std::shared_ptr> proj = + collect("proj_obstacles"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(proj->subscription)); + + pub->publish(makeXYZCloud("lidar", 1000.0, groundAndWall())); + ASSERT_TRUE(spinUntil([&]() { return !proj->empty(); })); + + const sensor_msgs::msg::PointCloud2 & cloud = proj->back(); + ASSERT_GT(cloud.width, 0u) << "the wall must survive as a projected obstacle"; + EXPECT_EQ(cloud.header.frame_id, "base_link") + << "proj_obstacles uses frame_id, not the input frame"; + + for(size_t i=0; i(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("frame_id", "base_link"), + rclcpp::Parameter("wait_for_transform", 0.1), + rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")), + rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.2"))}))); + publishStaticTf("base_link", "lidar"); + + std::shared_ptr> ground = + collect("ground"); + std::shared_ptr> obstacles = + collect("obstacles"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(ground->subscription)); + + pub->publish(makeXYZCloud("lidar", 1000.0, groundAndWall())); + ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); })); + + EXPECT_GT(ground->back().width, 0u) << "the z=0 plane is below the 0.2 m threshold"; + EXPECT_GT(obstacles->back().width, 0u) << "the wall rises above it"; +} + +TEST_F(ObstaclesDetectionTest, MapFrameIdAloneDoesNotMoveTheHeightReference) +{ + // The robot sits 1 m above the map origin, but Grid/MapFrameProjection is false by + // default, so pose.z() is ignored and the heights stay relative to the base frame. + // Setting map_frame_id on its own therefore changes nothing here. + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("frame_id", "base_link"), + rclcpp::Parameter("map_frame_id", "map"), + rclcpp::Parameter("wait_for_transform", 0.1), + rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")), + rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.2"))}))); + publishStaticTf("base_link", "lidar"); + publishStaticTf("map", "base_link", 0.0, 0.0, 1.0); + + std::shared_ptr> ground = + collect("ground"); + std::shared_ptr> obstacles = + collect("obstacles"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(ground->subscription)); + + pub->publish(makeXYZCloud("lidar", 1000.0, groundAndWall())); + ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); })); + + EXPECT_GT(ground->back().width, 0u) + << "without Grid/MapFrameProjection the map height is not applied"; +} + +TEST_F(ObstaclesDetectionTest, MapFrameIdLevelsTheGroundUsingRollAndPitch) +{ + // Only pose.z() is gated by Grid/MapFrameProjection: roll and pitch are always + // applied. So map_frame_id on its own still levels the segmentation to the map's + // horizontal, which is what matters when the robot is on a slope. + const double pitch = 10.0 * M_PI / 180.0; + + // A plane that is level in the map frame, seen from a base frame pitched by 10 deg: + // in the base frame it rises to well above the 0.1 m ground threshold. + const std::vector plane = planeLevelInMapFrame(pitch); + ASSERT_GT(plane.back().z, 0.1f) << "precondition: tilted beyond the threshold"; + + // Without a map frame the tilt is taken at face value: not ground. + { + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("frame_id", "base_link"), + rclcpp::Parameter("wait_for_transform", 0.1), + rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")), + rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.1"))}))); + publishStaticTf("base_link", "lidar"); + + std::shared_ptr> ground = + collect("ground"); + std::shared_ptr> obstacles = + collect("obstacles"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(ground->subscription)); + + pub->publish(makeXYZCloud("lidar", 1000.0, plane)); + ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); })); + EXPECT_EQ(ground->back().width, 0u) + << "a slope read in the base frame is not ground"; + } +} + +TEST_F(ObstaclesDetectionTest, MapFrameIdRecoversTheGroundOnASlope) +{ + // Same tilted plane, but now the node knows the robot is pitched in the map frame, + // so it levels the cloud and the slope becomes ground again. + const double pitch = 10.0 * M_PI / 180.0; + + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("frame_id", "base_link"), + rclcpp::Parameter("map_frame_id", "map"), + rclcpp::Parameter("wait_for_transform", 0.1), + rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")), + rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.1"))}))); + publishStaticTf("base_link", "lidar"); + publishStaticTfRPY("map", "base_link", 0.0, pitch, 0.0); + + std::shared_ptr> ground = + collect("ground"); + std::shared_ptr> obstacles = + collect("obstacles"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(ground->subscription)); + + pub->publish(makeXYZCloud("lidar", 1000.0, planeLevelInMapFrame(pitch))); + ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); })); + + EXPECT_GT(ground->back().width, 0u) + << "leveled by the map pitch, the slope is ground -- roll/pitch apply even " + "though Grid/MapFrameProjection is false"; +} + +TEST_F(ObstaclesDetectionTest, MapFrameProjectionSegmentsRelativeToTheMap) +{ + // Same setup plus Grid/MapFrameProjection=true. Now pose.z() participates, the whole + // cloud sits 1 m up in the map frame, and nothing is below the 0.2 m ground + // threshold any more. + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("frame_id", "base_link"), + rclcpp::Parameter("map_frame_id", "map"), + rclcpp::Parameter("wait_for_transform", 0.1), + rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")), + rclcpp::Parameter("Grid/MapFrameProjection", std::string("true")), + rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.2"))}))); + publishStaticTf("base_link", "lidar"); + publishStaticTf("map", "base_link", 0.0, 0.0, 1.0); + + std::shared_ptr> ground = + collect("ground"); + std::shared_ptr> obstacles = + collect("obstacles"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(ground->subscription)); + + pub->publish(makeXYZCloud("lidar", 1000.0, groundAndWall())); + ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); })); + + EXPECT_EQ(ground->back().width, 0u) + << "lifted 1 m in the map frame, nothing is below the ground threshold"; + EXPECT_GT(obstacles->back().width, 0u) << "everything becomes an obstacle instead"; +} + +/// Runs the node with the given Grid range settings and returns the ground cloud. +class ObstaclesDetectionRangeTest : public NodeTest +{ +protected: + sensor_msgs::msg::PointCloud2 groundWithRange( + const std::string & rangeMin, const std::string & rangeMax, + const std::vector & points) + { + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("frame_id", "base_link"), + rclcpp::Parameter("wait_for_transform", 0.1), + rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")), + rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.2")), + rclcpp::Parameter("Grid/RangeMin", rangeMin), + rclcpp::Parameter("Grid/RangeMax", rangeMax)}))); + publishStaticTf("base_link", "lidar"); + + std::shared_ptr> ground = + collect("ground"); + std::shared_ptr> obstacles = + collect("obstacles"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("cloud", 10); + EXPECT_TRUE(waitForSubscriber(pub)); + EXPECT_TRUE(waitForPublisher(ground->subscription)); + + pub->publish(makeXYZCloud("lidar", 1000.0, points)); + EXPECT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); })); + return ground->empty() ? sensor_msgs::msg::PointCloud2() : ground->back(); + } +}; + +TEST_F(ObstaclesDetectionRangeTest, RangeFilteringDisabledKeepsEverything) +{ + // Grid/RangeMax=0 means no upper limit, so both patches survive. + const sensor_msgs::msg::PointCloud2 ground = + groundWithRange("0.0", "0.0", nearAndFarPatches()); + EXPECT_EQ(ground.width, 50u); +} + +TEST_F(ObstaclesDetectionRangeTest, GridRangeMaxDropsDistantPoints) +{ + // Only the patch inside 1 m survives. + const sensor_msgs::msg::PointCloud2 ground = + groundWithRange("0.0", "1.0", nearAndFarPatches()); + ASSERT_EQ(ground.width, 25u); + + const std::pair extent = xExtent(ground); + EXPECT_NEAR(extent.first, 0.4f, 1e-3); + EXPECT_LT(extent.second, 1.0f) << "nothing beyond the 1 m limit may remain"; +} + +TEST_F(ObstaclesDetectionRangeTest, GridRangeMinDropsNearbyPoints) +{ + // The mirror image: everything closer than 1 m is discarded instead. + const sensor_msgs::msg::PointCloud2 ground = + groundWithRange("1.0", "0.0", nearAndFarPatches()); + ASSERT_EQ(ground.width, 25u); + + const std::pair extent = xExtent(ground); + EXPECT_GT(extent.first, 1.0f) << "nothing closer than the 1 m limit may remain"; + EXPECT_NEAR(extent.second, 3.1f, 1e-3); +} + +TEST_F(ObstaclesDetectionRangeTest, DefaultRangeMaxIsFiveMeters) +{ + // Grid/RangeMax defaults to 5.0, not infinity: a patch at 6 m is silently dropped + // even though no range parameter was set. + std::vector points = nearAndFarPatches(); + for(int i=0; i<5; ++i) + { + for(int j=0; j<5; ++j) + { + points.push_back(cv::Point3f(5.9f + 0.05f*i, -0.1f + 0.05f*j, 0.0f)); + } + } + + const sensor_msgs::msg::PointCloud2 ground = + groundWithRange("0.0", "5.0", points); // the defaults, stated explicitly + EXPECT_EQ(ground.width, 50u) << "the 6 m patch is beyond the default range"; + EXPECT_LT(xExtent(ground).second, 5.0f); +} + +TEST_F(ObstaclesDetectionTest, PublishesEmptyCloudsForAnEmptyInput) +{ + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({rclcpp::Parameter("frame_id", "base_link")}))); + publishStaticTf("base_link", "lidar"); + + std::shared_ptr> ground = + collect("ground"); + std::shared_ptr> obstacles = + collect("obstacles"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(ground->subscription)); + + pub->publish(makeXYZCloud("lidar", 1000.0, {})); + ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); })) + << "an empty input must still produce output, not a dropped message"; + + EXPECT_EQ(ground->back().width, 0u); + EXPECT_EQ(obstacles->back().width, 0u); +} diff --git a/rtabmap_util/test/test_point_cloud_aggregator.cpp b/rtabmap_util/test/test_point_cloud_aggregator.cpp new file mode 100644 index 00000000..45469223 --- /dev/null +++ b/rtabmap_util/test/test_point_cloud_aggregator.cpp @@ -0,0 +1,194 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "node_test_utils.hpp" +#include "msg_builders.hpp" + +#include + +#include + +using namespace rtabmap_util_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); +} + +class PointCloudAggregatorTest : public NodeTest +{ +protected: + /// odom -> base_link advancing along x at 1 m/s across the two cloud stamps. + void publishOdomMotion(double startStamp, double duration) + { + rclcpp::Publisher::SharedPtr tfPub = + helper()->create_publisher("/tf", rclcpp::QoS(100)); + spinFor(std::chrono::milliseconds(100)); + for(int i=0; i<=6; ++i) + { + const double elapsed = duration * double(i) / 6.0; + geometry_msgs::msg::TransformStamped t; + t.header.stamp = stampOf(startStamp + elapsed); + t.header.frame_id = "odom"; + t.child_frame_id = "base_link"; + t.transform.translation.x = elapsed; // 1 m/s + t.transform.rotation.w = 1.0; + tf2_msgs::msg::TFMessage msg; + msg.transforms.push_back(t); + tfPub->publish(msg); + } + spinFor(std::chrono::milliseconds(200)); + tfPub_ = tfPub; + } + + /** + * @brief Publishes three pairs of clouds observing one landmark 5 m ahead in odom. + * + * Pair k is stamped at 1000.0+0.2k and 0.1 s later. The robot drives at 1 m/s, so + * each sensor measures the landmark at 5 m minus the distance travelled by then. + */ + void publishPairs( + const rclcpp::Publisher::SharedPtr & pub1, + const rclcpp::Publisher::SharedPtr & pub2) + { + for(int k=0; k<3; ++k) + { + const double t1 = 1000.0 + 0.2*double(k); + const double t2 = t1 + 0.1; + pub1->publish(makeXYZCloud("lidar_a", t1, {{float(5.0-(t1-1000.0)), 0.0f, 0.0f}})); + pub2->publish(makeXYZCloud("lidar_b", t2, {{float(5.0-(t2-1000.0)), 0.0f, 0.0f}})); + spinFor(std::chrono::milliseconds(50)); + } + } + + rclcpp::Publisher::SharedPtr tfPub_; +}; + +TEST_F(PointCloudAggregatorTest, CombinesTwoSynchronizedClouds) +{ + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("count", 2), + rclcpp::Parameter("frame_id", "base_link"), + rclcpp::Parameter("approx_sync", true), + rclcpp::Parameter("wait_for_transform", 0.2)}))); + publishStaticTf("base_link", "lidar_a", 0.0, 0.2, 0.0); + publishStaticTf("base_link", "lidar_b", 0.0, -0.2, 0.0); + + std::shared_ptr> out = + collect("combined_cloud"); + rclcpp::Publisher::SharedPtr pub1 = + helper()->create_publisher("cloud1", 10); + rclcpp::Publisher::SharedPtr pub2 = + helper()->create_publisher("cloud2", 10); + ASSERT_TRUE(waitForSubscriber(pub1)); + ASSERT_TRUE(waitForSubscriber(pub2)); + + const std::vector a = {{1.0f, 0.0f, 0.0f}, {2.0f, 0.0f, 0.0f}}; + const std::vector b = {{3.0f, 0.0f, 0.0f}}; + pub1->publish(makeXYZCloud("lidar_a", 1000.0, a)); + pub2->publish(makeXYZCloud("lidar_b", 1000.0, b)); + + ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })) << "no combined cloud published"; + EXPECT_EQ(out->back().width, a.size() + b.size()) << "every input point must survive"; + EXPECT_EQ(out->back().header.frame_id, "base_link") + << "the combined cloud is expressed in frame_id"; +} + +TEST_F(PointCloudAggregatorTest, AlignsCloudsCapturedAtDifferentTimesWhileMoving) +{ + // The two sensors fire 0.1 s apart while the robot drives forward at 1 m/s, so they + // see the same world point at different ranges. With fixed_frame_id set, the second + // cloud is motion-compensated back to the first one's stamp and the two coincide. + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("count", 2), + rclcpp::Parameter("frame_id", "base_link"), + rclcpp::Parameter("fixed_frame_id", "odom"), + rclcpp::Parameter("approx_sync", true), + rclcpp::Parameter("wait_for_transform", 0.2)}))); + publishStaticTf("base_link", "lidar_a"); + publishStaticTf("base_link", "lidar_b"); + publishOdomMotion(1000.0, 0.6); + + std::shared_ptr> out = + collect("combined_cloud"); + rclcpp::Publisher::SharedPtr pub1 = + helper()->create_publisher("cloud1", 10); + rclcpp::Publisher::SharedPtr pub2 = + helper()->create_publisher("cloud2", 10); + ASSERT_TRUE(waitForSubscriber(pub1)); + ASSERT_TRUE(waitForSubscriber(pub2)); + + // A landmark 5 m ahead in odom, the robot driving at 1 m/s. Several pairs are sent + // because the ApproximateTime policy needs a following message before it can commit + // to a match when the stamps differ; the first emitted pair is the one asserted on. + publishPairs(pub1, pub2); + + ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })) << "no combined cloud"; + const sensor_msgs::msg::PointCloud2 & cloud = out->front(); + ASSERT_EQ(cloud.width, 2u); + + // Both observations of the same landmark must land on the same point. + EXPECT_NEAR(readXYZ(cloud, 0).x, 5.0f, 5e-3); + EXPECT_NEAR(readXYZ(cloud, 1).x, 5.0f, 5e-3) + << "the later cloud must be compensated for the 0.1 m of motion"; +} + +TEST_F(PointCloudAggregatorTest, WithoutAFixedFrameCloudsAreNotMotionCompensated) +{ + // Same inputs, no fixed_frame_id: the second cloud is taken at face value and the + // two observations stay 0.1 m apart. This is what fixed_frame_id exists to fix. + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("count", 2), + rclcpp::Parameter("frame_id", "base_link"), + rclcpp::Parameter("approx_sync", true), + rclcpp::Parameter("wait_for_transform", 0.2)}))); + publishStaticTf("base_link", "lidar_a"); + publishStaticTf("base_link", "lidar_b"); + publishOdomMotion(1000.0, 0.6); + + std::shared_ptr> out = + collect("combined_cloud"); + rclcpp::Publisher::SharedPtr pub1 = + helper()->create_publisher("cloud1", 10); + rclcpp::Publisher::SharedPtr pub2 = + helper()->create_publisher("cloud2", 10); + ASSERT_TRUE(waitForSubscriber(pub1)); + ASSERT_TRUE(waitForSubscriber(pub2)); + + publishPairs(pub1, pub2); + + ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })); + const sensor_msgs::msg::PointCloud2 & cloud = out->front(); + ASSERT_EQ(cloud.width, 2u); + + EXPECT_NEAR(readXYZ(cloud, 0).x, 5.0f, 5e-3); + EXPECT_NEAR(readXYZ(cloud, 1).x, 4.9f, 5e-3) + << "uncompensated, the second observation stays where it was measured"; +} + +TEST_F(PointCloudAggregatorTest, WaitsForEveryInput) +{ + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("count", 2), + rclcpp::Parameter("frame_id", "base_link"), + rclcpp::Parameter("approx_sync", true)}))); + publishStaticTf("base_link", "lidar_a"); + publishStaticTf("base_link", "lidar_b"); + + std::shared_ptr> out = + collect("combined_cloud"); + rclcpp::Publisher::SharedPtr pub1 = + helper()->create_publisher("cloud1", 10); + ASSERT_TRUE(waitForSubscriber(pub1)); + + // Only one of the two inputs arrives: the synchronizer must not fire. + pub1->publish(makeXYZCloud("lidar_a", 1000.0, {{1.0f, 0.0f, 0.0f}})); + spinFor(std::chrono::milliseconds(500)); + + EXPECT_TRUE(out->empty()) << "a single input must not produce a combined cloud"; +} diff --git a/rtabmap_util/test/test_point_cloud_assembler.cpp b/rtabmap_util/test/test_point_cloud_assembler.cpp new file mode 100644 index 00000000..9e7cc7f7 --- /dev/null +++ b/rtabmap_util/test/test_point_cloud_assembler.cpp @@ -0,0 +1,399 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "node_test_utils.hpp" +#include "msg_builders.hpp" + +#include + +#include +#include + +using namespace rtabmap_util_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); +} + +class PointCloudAssemblerTest : public NodeTest +{ +protected: + /// Starts the assembler with @p overrides, plus a static odom -> lidar transform. + void start(const std::vector & overrides) + { + addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(overrides))); + publishStaticTf("odom", "lidar"); + publishStaticTf("lidar", "base_link"); + out_ = collect("assembled_cloud"); + pub_ = helper()->create_publisher("cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub_)); + } + + static bool hasField(const sensor_msgs::msg::PointCloud2 & cloud, const std::string & name) + { + for(size_t i=0; i> out_; + rclcpp::Publisher::SharedPtr pub_; +}; + +TEST_F(PointCloudAssemblerTest, PublishesAfterMaxCloudsAreAccumulated) +{ + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("max_clouds", 3), + rclcpp::Parameter("fixed_frame_id", "odom"), + rclcpp::Parameter("wait_for_transform", 0.2)}))); + publishStaticTf("odom", "lidar"); + + std::shared_ptr> out = + collect("assembled_cloud"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + const std::vector points = {{1.0f, 0.0f, 0.0f}, {2.0f, 0.0f, 0.0f}}; + + // The first two clouds are only accumulated. + pub->publish(makeXYZCloud("lidar", 1000.0, points)); + pub->publish(makeXYZCloud("lidar", 1000.1, points)); + spinFor(std::chrono::milliseconds(300)); + EXPECT_TRUE(out->empty()) << "nothing is published before max_clouds is reached"; + + // The third completes the batch. + pub->publish(makeXYZCloud("lidar", 1000.2, points)); + ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })) + << "the assembled cloud must be published on the third input"; + + EXPECT_EQ(out->back().width, 3 * points.size()) << "all three clouds must be included"; + EXPECT_EQ(out->back().header.frame_id, "lidar") + << "the assembled cloud comes back in the sensor frame"; +} + +TEST_F(PointCloudAssemblerTest, AssemblingTimePublishesAfterTheConfiguredSpan) +{ + // An alternative trigger to max_clouds: publish once the newest cloud is at least + // assembling_time newer than the oldest one held. + start({rclcpp::Parameter("max_clouds", 0), + rclcpp::Parameter("assembling_time", 0.25), + rclcpp::Parameter("fixed_frame_id", "odom"), + rclcpp::Parameter("wait_for_transform", 0.2)}); + + const std::vector points = {{1.0f, 0.0f, 0.0f}}; + for(int i=0; i<3; ++i) // 1000.0, 1000.1, 1000.2 -- span 0.2 s, below 0.25 + { + pub_->publish(makeXYZCloud("lidar", 1000.0 + 0.1*i, points)); + } + spinFor(std::chrono::milliseconds(300)); + EXPECT_TRUE(out_->empty()) << "0.2 s of clouds is short of assembling_time"; + + pub_->publish(makeXYZCloud("lidar", 1000.3, points)); // span now 0.3 s + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) + << "crossing assembling_time must publish"; + EXPECT_EQ(out_->back().width, 4u) << "all four clouds are included"; +} + +TEST_F(PointCloudAssemblerTest, CircularBufferPublishesOnEveryCloud) +{ + // With a circular buffer the node emits a sliding window instead of filling up, + // clearing and starting again: every input produces an output. + start({rclcpp::Parameter("max_clouds", 3), + rclcpp::Parameter("circular_buffer", true), + rclcpp::Parameter("fixed_frame_id", "odom"), + rclcpp::Parameter("wait_for_transform", 0.2)}); + + const std::vector points = {{1.0f, 0.0f, 0.0f}}; + for(int i=0; i<4; ++i) + { + pub_->publish(makeXYZCloud("lidar", 1000.0 + 0.1*i, points)); + ASSERT_TRUE(spinUntil([&]() { return out_->size() >= size_t(i+1); })) + << "cloud " << i << " did not produce an output"; + } + EXPECT_EQ(out_->size(), 4u) << "one output per input, not one per full batch"; + // The window is capped at max_clouds. + EXPECT_LE(out_->back().width, 3u); +} + +TEST_F(PointCloudAssemblerTest, RangeMaxDropsDistantPoints) +{ + start({rclcpp::Parameter("max_clouds", 1), + rclcpp::Parameter("range_max", 3.0), + rclcpp::Parameter("fixed_frame_id", "odom"), + rclcpp::Parameter("wait_for_transform", 0.2)}); + + // Two points inside 3 m, one well beyond it. + pub_->publish(makeXYZCloud("lidar", 1000.0, + {{1.0f, 0.0f, 0.0f}, {2.0f, 0.0f, 0.0f}, {9.0f, 0.0f, 0.0f}})); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_EQ(out_->back().width, 2u) << "the 9 m point must be filtered out"; +} + +TEST_F(PointCloudAssemblerTest, RemoveZDropsTheZField) +{ + start({rclcpp::Parameter("max_clouds", 1), + rclcpp::Parameter("remove_z", true), + rclcpp::Parameter("fixed_frame_id", "odom"), + rclcpp::Parameter("wait_for_transform", 0.2)}); + + pub_->publish(makeXYZCloud("lidar", 1000.0, {{1.0f, 0.0f, 0.5f}})); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + // The field is removed entirely, not zeroed: the output is a 2D cloud. + EXPECT_TRUE(hasField(out_->back(), "x")); + EXPECT_TRUE(hasField(out_->back(), "y")); + EXPECT_FALSE(hasField(out_->back(), "z")) << "remove_z drops the field itself"; +} + +TEST_F(PointCloudAssemblerTest, FrameIdSetsTheOutputFrame) +{ + start({rclcpp::Parameter("max_clouds", 1), + rclcpp::Parameter("frame_id", "base_link"), + rclcpp::Parameter("fixed_frame_id", "odom"), + rclcpp::Parameter("wait_for_transform", 0.2)}); + + pub_->publish(makeXYZCloud("lidar", 1000.0, {{1.0f, 0.0f, 0.0f}})); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_EQ(out_->back().header.frame_id, "base_link") + << "the assembled cloud is returned in frame_id when it is set"; +} + +TEST_F(PointCloudAssemblerTest, SkipCloudsIgnoresIntermediateClouds) +{ + // skip_clouds=1 keeps every other cloud, so reaching max_clouds=2 takes four inputs. + start({rclcpp::Parameter("max_clouds", 2), + rclcpp::Parameter("skip_clouds", 1), + rclcpp::Parameter("fixed_frame_id", "odom"), + rclcpp::Parameter("wait_for_transform", 0.2)}); + + const std::vector points = {{1.0f, 0.0f, 0.0f}}; + pub_->publish(makeXYZCloud("lidar", 1000.0, points)); + pub_->publish(makeXYZCloud("lidar", 1000.1, points)); + spinFor(std::chrono::milliseconds(300)); + EXPECT_TRUE(out_->empty()) << "one of those two was skipped, so the batch is short"; + + pub_->publish(makeXYZCloud("lidar", 1000.2, points)); + pub_->publish(makeXYZCloud("lidar", 1000.3, points)); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + EXPECT_EQ(out_->back().width, 2u) << "two kept clouds, two skipped"; +} + +TEST_F(PointCloudAssemblerTest, LinearUpdateSkipsCloudsWhileStationary) +{ + // With linear_update set, a cloud captured without the robot having moved far enough + // is discarded rather than accumulated, so a parked robot never fills a batch. + start({rclcpp::Parameter("max_clouds", 3), + rclcpp::Parameter("linear_update", 0.5), + rclcpp::Parameter("fixed_frame_id", "odom"), + rclcpp::Parameter("wait_for_transform", 0.2)}); + + const std::vector points = {{1.0f, 0.0f, 0.0f}}; + for(int i=0; i<5; ++i) // the TF is static, so the robot never moves + { + pub_->publish(makeXYZCloud("lidar", 1000.0 + 0.1*i, points)); + } + spinFor(std::chrono::milliseconds(500)); + + EXPECT_TRUE(out_->empty()) + << "a stationary robot must not accumulate a batch when linear_update is set"; +} + +TEST_F(PointCloudAssemblerTest, WithoutLinearUpdateEveryCloudCounts) +{ + // The same stationary robot, with the motion filter disabled: the batch fills. + start({rclcpp::Parameter("max_clouds", 3), + rclcpp::Parameter("linear_update", 0.0), + rclcpp::Parameter("fixed_frame_id", "odom"), + rclcpp::Parameter("wait_for_transform", 0.2)}); + + const std::vector points = {{1.0f, 0.0f, 0.0f}}; + for(int i=0; i<3; ++i) + { + pub_->publish(makeXYZCloud("lidar", 1000.0 + 0.1*i, points)); + } + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + EXPECT_EQ(out_->back().width, 3u); +} + +TEST_F(PointCloudAssemblerTest, VoxelSizeDownsamplesTheCloud) +{ + start({rclcpp::Parameter("max_clouds", 1), + rclcpp::Parameter("voxel_size", 0.5), + rclcpp::Parameter("fixed_frame_id", "odom"), + rclcpp::Parameter("wait_for_transform", 0.2)}); + + // 100 points packed into a 0.2 m cube: a 0.5 m voxel grid collapses them. + std::vector dense; + for(int i=0; i<10; ++i) + { + for(int j=0; j<10; ++j) + { + dense.push_back(cv::Point3f(1.0f + 0.02f*i, 0.02f*j, 0.0f)); + } + } + pub_->publish(makeXYZCloud("lidar", 1000.0, dense)); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_LT(out_->back().width, dense.size()) + << "voxel_size must reduce the point count"; + EXPECT_GT(out_->back().width, 0u); +} + +/// Fixture for the odometry-synchronized modes, which need fixed_frame_id to be empty. +class PointCloudAssemblerOdomTest : public NodeTest +{ +protected: + void start(std::vector overrides) + { + // fixed_frame_id defaults to "odom"; it has to be cleared for the node to + // subscribe to the odometry topic instead of reading TF directly. + overrides.push_back(rclcpp::Parameter("fixed_frame_id", "")); + addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(overrides))); + publishStaticTf("odom", "lidar"); + out_ = collect("assembled_cloud"); + cloudPub_ = helper()->create_publisher("cloud", 10); + odomPub_ = helper()->create_publisher("odom", 10); + odomInfoPub_ = helper()->create_publisher("odom_info", 10); + ASSERT_TRUE(waitForSubscriber(cloudPub_)); + ASSERT_TRUE(waitForSubscriber(odomPub_)); + } + + /// An odometry message at the origin; a null one has an all-zero orientation. + nav_msgs::msg::Odometry makeOdom(double stamp, bool null = false) + { + nav_msgs::msg::Odometry odom; + odom.header.stamp = stampOf(stamp); + odom.header.frame_id = "odom"; + odom.child_frame_id = "lidar"; + odom.pose.pose.orientation.w = null ? 0.0 : 1.0; + return odom; + } + + std::shared_ptr> out_; + rclcpp::Publisher::SharedPtr cloudPub_; + rclcpp::Publisher::SharedPtr odomPub_; + rclcpp::Publisher::SharedPtr odomInfoPub_; +}; + +TEST_F(PointCloudAssemblerOdomTest, TakesTheFixedFrameFromTheOdometryMessage) +{ + // With fixed_frame_id empty the node syncs cloud with odom and uses the odometry + // header's frame as the fixed frame. + start({rclcpp::Parameter("max_clouds", 2), + rclcpp::Parameter("wait_for_transform", 0.2)}); + + const std::vector points = {{1.0f, 0.0f, 0.0f}}; + for(int i=0; i<2; ++i) + { + const double t = 1000.0 + 0.1*i; + cloudPub_->publish(makeXYZCloud("lidar", t, points)); + odomPub_->publish(makeOdom(t)); // exact sync: identical stamps + spinFor(std::chrono::milliseconds(50)); + } + + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) + << "cloud+odom synchronization must drive the assembly"; + EXPECT_EQ(out_->back().width, 2u); +} + +TEST_F(PointCloudAssemblerOdomTest, NullOdometryResetsTheBuffer) +{ + // A null odometry means tracking was lost, so the accumulated clouds are dropped + // rather than being stitched across the discontinuity. + start({rclcpp::Parameter("max_clouds", 3), + rclcpp::Parameter("wait_for_transform", 0.2)}); + + const std::vector points = {{1.0f, 0.0f, 0.0f}}; + + // Two good clouds, then a lost-tracking frame, then two more. + for(int i=0; i<2; ++i) + { + const double t = 1000.0 + 0.1*i; + cloudPub_->publish(makeXYZCloud("lidar", t, points)); + odomPub_->publish(makeOdom(t)); + spinFor(std::chrono::milliseconds(50)); + } + cloudPub_->publish(makeXYZCloud("lidar", 1000.2, points)); + odomPub_->publish(makeOdom(1000.2, /*null=*/true)); + spinFor(std::chrono::milliseconds(150)); + EXPECT_TRUE(out_->empty()) << "the null odometry must not complete the batch"; + + // After the reset it takes three fresh clouds again, not one. + for(int i=0; i<2; ++i) + { + const double t = 1000.3 + 0.1*i; + cloudPub_->publish(makeXYZCloud("lidar", t, points)); + odomPub_->publish(makeOdom(t)); + spinFor(std::chrono::milliseconds(50)); + } + EXPECT_TRUE(out_->empty()) << "the buffer restarted, so two clouds are not enough"; + + cloudPub_->publish(makeXYZCloud("lidar", 1000.5, points)); + odomPub_->publish(makeOdom(1000.5)); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + EXPECT_EQ(out_->back().width, 3u) << "only the post-reset clouds are assembled"; +} + +TEST_F(PointCloudAssemblerOdomTest, SubscribeOdomInfoKeepsOnlyKeyFrames) +{ + // With subscribe_odom_info the node also takes OdomInfo and accumulates a cloud only + // when that frame became a key frame. + start({rclcpp::Parameter("max_clouds", 2), + rclcpp::Parameter("subscribe_odom_info", true), + rclcpp::Parameter("wait_for_transform", 0.2)}); + ASSERT_TRUE(waitForSubscriber(odomInfoPub_)); + + const std::vector points = {{1.0f, 0.0f, 0.0f}}; + auto publishFrame = [&](double t, bool keyFrame) { + rtabmap_msgs::msg::OdomInfo info; + info.header.stamp = stampOf(t); + info.header.frame_id = "odom"; + info.key_frame_added = keyFrame; + cloudPub_->publish(makeXYZCloud("lidar", t, points)); + odomPub_->publish(makeOdom(t)); + odomInfoPub_->publish(info); + spinFor(std::chrono::milliseconds(60)); + }; + + publishFrame(1000.0, false); + publishFrame(1000.1, false); + publishFrame(1000.2, false); + EXPECT_TRUE(out_->empty()) << "non key frames must be ignored"; + + publishFrame(1000.3, true); + publishFrame(1000.4, true); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + EXPECT_EQ(out_->back().width, 2u) << "only the two key frames are assembled"; +} + +TEST_F(PointCloudAssemblerTest, DropsCloudsWithoutTheFixedFrame) +{ + // No TF at all, so the assembler cannot place the clouds relative to each other. + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("max_clouds", 2), + rclcpp::Parameter("fixed_frame_id", "odom"), + rclcpp::Parameter("wait_for_transform", 0.0)}))); + + std::shared_ptr> out = + collect("assembled_cloud"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + pub->publish(makeXYZCloud("lidar", 1000.0, {{1.0f, 0.0f, 0.0f}})); + pub->publish(makeXYZCloud("lidar", 1000.1, {{1.0f, 0.0f, 0.0f}})); + spinFor(std::chrono::milliseconds(500)); + + EXPECT_TRUE(out->empty()); +} diff --git a/rtabmap_util/test/test_point_cloud_xyz.cpp b/rtabmap_util/test/test_point_cloud_xyz.cpp new file mode 100644 index 00000000..700ea6cf --- /dev/null +++ b/rtabmap_util/test/test_point_cloud_xyz.cpp @@ -0,0 +1,344 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "node_test_utils.hpp" +#include "msg_builders.hpp" + +#include + +#include + +#include + +using namespace rtabmap_util_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); + +constexpr int kWidth = 16; +constexpr int kHeight = 16; +constexpr double kFx = 100.0; + +/// A depth image where every pixel is at @p meters. +sensor_msgs::msg::Image makeDepth( + double stamp, float meters, + const std::string & encoding = sensor_msgs::image_encodings::TYPE_32FC1) +{ + cv::Mat image; + if(encoding == sensor_msgs::image_encodings::TYPE_32FC1) + { + image = cv::Mat(kHeight, kWidth, CV_32FC1, cv::Scalar(meters)); + } + else + { + image = cv::Mat(kHeight, kWidth, CV_16UC1, cv::Scalar(uint16_t(meters*1000.0f))); + } + return makeImage("camera_link", stamp, image, encoding); +} + +/// A disparity image where every pixel carries @p disparity, so depth = f*t/disparity. +stereo_msgs::msg::DisparityImage makeDisparity( + double stamp, float disparity, float focal = float(kFx), float baseline = 0.1f) +{ + stereo_msgs::msg::DisparityImage msg; + msg.header.frame_id = "camera_link"; + msg.header.stamp = stampOf(stamp); + msg.f = focal; + msg.t = baseline; + msg.min_disparity = 1.0f; + msg.max_disparity = 100.0f; + msg.image = makeImage("camera_link", stamp, + cv::Mat(kHeight, kWidth, CV_32FC1, cv::Scalar(disparity)), + sensor_msgs::image_encodings::TYPE_32FC1); + return msg; +} + +/// The same, in the 16SC1 fixed-point form where the stored value is 16*disparity. +stereo_msgs::msg::DisparityImage makeDisparity16SC1(double stamp, float disparity) +{ + stereo_msgs::msg::DisparityImage msg = makeDisparity(stamp, disparity); + msg.image = makeImage("camera_link", stamp, + cv::Mat(kHeight, kWidth, CV_16SC1, cv::Scalar(short(disparity*16.0f))), + sensor_msgs::image_encodings::TYPE_16SC1); + return msg; +} + +bool hasField(const sensor_msgs::msg::PointCloud2 & cloud, const std::string & name) +{ + for(size_t i=0; i & overrides = {}) + { + addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(overrides))); + out_ = collect("cloud"); + depthPub_ = helper()->create_publisher("depth/image", 10); + infoPub_ = helper()->create_publisher("depth/camera_info", 10); + ASSERT_TRUE(waitForSubscriber(depthPub_)); + ASSERT_TRUE(waitForSubscriber(infoPub_)); + ASSERT_TRUE(waitForPublisher(out_->subscription)); + } + + /// Publishes a synchronized depth + camera_info pair. + void publishFrame(double stamp, float meters, + const std::string & encoding = sensor_msgs::image_encodings::TYPE_32FC1) + { + depthPub_->publish(makeDepth(stamp, meters, encoding)); + infoPub_->publish(makeCameraInfo("camera_link", stamp, kWidth, kHeight, 0.0, kFx)); + } + + std::shared_ptr> out_; + rclcpp::Publisher::SharedPtr depthPub_; + rclcpp::Publisher::SharedPtr infoPub_; +}; + +TEST_F(PointCloudXYZTest, ProjectsDepthIntoACloud) +{ + start(); + publishFrame(1000.0, 2.0f); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published"; + + const sensor_msgs::msg::PointCloud2 & cloud = out_->back(); + EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight)) + << "one point per pixel at decimation 1"; + EXPECT_EQ(cloud.header.frame_id, "camera_link") + << "the cloud takes the depth image's frame"; + + // The principal-point pixel projects straight ahead at the measured depth. + const size_t center = size_t(kHeight/2) * kWidth + kWidth/2; + EXPECT_NEAR(readXYZ(cloud, center).z, 2.0f, 1e-3); +} + +TEST_F(PointCloudXYZTest, Accepts16UC1Millimeters) +{ + start(); + publishFrame(1000.0, 2.0f, sensor_msgs::image_encodings::TYPE_16UC1); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const size_t center = size_t(kHeight/2) * kWidth + kWidth/2; + EXPECT_NEAR(readXYZ(out_->back(), center).z, 2.0f, 1e-3) + << "millimeter depth must be converted to meters"; +} + +TEST_F(PointCloudXYZTest, RejectsUnsupportedEncoding) +{ + start(); + depthPub_->publish(makeImage("camera_link", 1000.0, + cv::Mat(kHeight, kWidth, CV_8UC3, cv::Scalar(1,2,3)), "bgr8")); + infoPub_->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx)); + spinFor(std::chrono::milliseconds(400)); + + EXPECT_TRUE(out_->empty()) << "only 32FC1, 16UC1 and mono16 depth are supported"; +} + +TEST_F(PointCloudXYZTest, DecimationReducesThePointCount) +{ + start({rclcpp::Parameter("decimation", 2)}); + publishFrame(1000.0, 2.0f); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_EQ(out_->back().width * out_->back().height, uint32_t(kWidth*kHeight)/4) + << "decimation 2 keeps one pixel in four"; +} + +TEST_F(PointCloudXYZTest, MaxDepthMarksFarPointsInvalid) +{ + // cloudFromDepth keeps the cloud organized: points outside the depth range become + // NaN rather than disappearing, so the point count is unchanged. + start({rclcpp::Parameter("max_depth", 1.0)}); + publishFrame(1000.0, 5.0f); // beyond the limit + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const sensor_msgs::msg::PointCloud2 & cloud = out_->back(); + EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight)) + << "the cloud stays organized"; + EXPECT_TRUE(std::isnan(readXYZ(cloud, 0).z)) << "every point is past max_depth"; + EXPECT_TRUE(std::isnan(readXYZ(cloud, kWidth*kHeight-1).z)); +} + +TEST_F(PointCloudXYZTest, WithinMaxDepthPointsStayValid) +{ + start({rclcpp::Parameter("max_depth", 10.0)}); + publishFrame(1000.0, 5.0f); // inside the limit + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const size_t center = size_t(kHeight/2) * kWidth + kWidth/2; + EXPECT_FALSE(std::isnan(readXYZ(out_->back(), center).z)); + EXPECT_NEAR(readXYZ(out_->back(), center).z, 5.0f, 1e-3); +} + +TEST_F(PointCloudXYZTest, MinDepthMarksNearPointsInvalid) +{ + start({rclcpp::Parameter("min_depth", 3.0)}); + publishFrame(1000.0, 1.0f); // closer than the limit + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_TRUE(std::isnan(readXYZ(out_->back(), 0).z)) + << "every point is nearer than min_depth"; +} + +TEST_F(PointCloudXYZTest, FilterNaNsRemovesInvalidPoints) +{ + // With filter_nans the invalid points are dropped instead, giving an unorganized + // cloud that is empty when nothing is in range. + start({rclcpp::Parameter("max_depth", 1.0), + rclcpp::Parameter("filter_nans", true)}); + publishFrame(1000.0, 5.0f); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_EQ(out_->back().width * out_->back().height, 0u) + << "filter_nans must remove the out-of-range points"; +} + +TEST_F(PointCloudXYZTest, NormalKAddsNormalFields) +{ + start({rclcpp::Parameter("normal_k", 10)}); + publishFrame(1000.0, 2.0f); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_TRUE(hasField(out_->back(), "normal_x")) + << "asking for normals must change the point type"; + EXPECT_TRUE(hasField(out_->back(), "normal_z")); +} + +TEST_F(PointCloudXYZTest, NoNormalFieldsByDefault) +{ + start(); + publishFrame(1000.0, 2.0f); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_FALSE(hasField(out_->back(), "normal_x")); +} + +TEST_F(PointCloudXYZTest, StaysSilentWithoutASubscriber) +{ + // The projection is skipped entirely when nobody wants the cloud. + addNode(std::make_shared(rclcpp::NodeOptions())); + rclcpp::Publisher::SharedPtr depthPub = + helper()->create_publisher("depth/image", 10); + rclcpp::Publisher::SharedPtr infoPub = + helper()->create_publisher("depth/camera_info", 10); + ASSERT_TRUE(waitForSubscriber(depthPub)); + + depthPub->publish(makeDepth(1000.0, 2.0f)); + infoPub->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx)); + spinFor(std::chrono::milliseconds(300)); + + std::shared_ptr> late = + collect("cloud"); + spinFor(std::chrono::milliseconds(200)); + EXPECT_TRUE(late->empty()); +} + +//============================================================================ +// disparity/image + disparity/camera_info +//============================================================================ + +class PointCloudXYZDisparityTest : public NodeTest +{ +protected: + void start(const std::vector & overrides = {}) + { + addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(overrides))); + out_ = collect("cloud"); + dispPub_ = helper()->create_publisher( + "disparity/image", 10); + infoPub_ = helper()->create_publisher( + "disparity/camera_info", 10); + ASSERT_TRUE(waitForSubscriber(dispPub_)); + ASSERT_TRUE(waitForSubscriber(infoPub_)); + ASSERT_TRUE(waitForPublisher(out_->subscription)); + } + + void publishFrame(double stamp, const stereo_msgs::msg::DisparityImage & disparity) + { + dispPub_->publish(disparity); + infoPub_->publish(makeCameraInfo("camera_link", stamp, kWidth, kHeight, 0.0, kFx)); + } + + std::shared_ptr> out_; + rclcpp::Publisher::SharedPtr dispPub_; + rclcpp::Publisher::SharedPtr infoPub_; +}; + +TEST_F(PointCloudXYZDisparityTest, ProjectsDisparityIntoACloud) +{ + start(); + publishFrame(1000.0, makeDisparity(1000.0, 5.0f)); // depth = f*t/d = 100*0.1/5 = 2 m + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published"; + + const sensor_msgs::msg::PointCloud2 & cloud = out_->back(); + EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight)); + EXPECT_EQ(cloud.header.frame_id, "camera_link") + << "the cloud takes the disparity image's frame"; + + const size_t center = size_t(kHeight/2) * kWidth + kWidth/2; + EXPECT_NEAR(readXYZ(cloud, center).z, 2.0f, 1e-3); +} + +TEST_F(PointCloudXYZDisparityTest, Accepts16SC1FixedPointDisparity) +{ + // The 16-bit form stores 16*disparity, so the same 5 px must still give 2 m. + start(); + publishFrame(1000.0, makeDisparity16SC1(1000.0, 5.0f)); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const size_t center = size_t(kHeight/2) * kWidth + kWidth/2; + EXPECT_NEAR(readXYZ(out_->back(), center).z, 2.0f, 1e-3); +} + +TEST_F(PointCloudXYZDisparityTest, RejectsUnsupportedDisparityEncoding) +{ + start(); + stereo_msgs::msg::DisparityImage msg = makeDisparity(1000.0, 5.0f); + msg.image = makeImage("camera_link", 1000.0, + cv::Mat(kHeight, kWidth, CV_8UC1, cv::Scalar(5)), "mono8"); + publishFrame(1000.0, msg); + spinFor(std::chrono::milliseconds(400)); + + EXPECT_TRUE(out_->empty()) << "only 32FC1 and 16SC1 disparity are supported"; +} + +TEST_F(PointCloudXYZDisparityTest, MaxDepthMarksFarPointsInvalid) +{ + // Like the depth path, cloudFromDisparity keeps the cloud organized and turns the + // out-of-range points into NaN instead of removing them. + start({rclcpp::Parameter("max_depth", 1.0)}); + publishFrame(1000.0, makeDisparity(1000.0, 5.0f)); // 2 m, beyond the limit + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const sensor_msgs::msg::PointCloud2 & cloud = out_->back(); + EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight)); + EXPECT_TRUE(std::isnan(readXYZ(cloud, size_t(kHeight/2)*kWidth + kWidth/2).z)); +} + +TEST_F(PointCloudXYZDisparityTest, FilterNaNsRemovesInvalidPoints) +{ + start({rclcpp::Parameter("max_depth", 1.0), + rclcpp::Parameter("filter_nans", true)}); + publishFrame(1000.0, makeDisparity(1000.0, 5.0f)); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_EQ(out_->back().width * out_->back().height, 0u); +} + +TEST_F(PointCloudXYZDisparityTest, DecimationReducesThePointCount) +{ + start({rclcpp::Parameter("decimation", 2)}); + publishFrame(1000.0, makeDisparity(1000.0, 5.0f)); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_EQ(out_->back().width * out_->back().height, uint32_t(kWidth*kHeight)/4); +} diff --git a/rtabmap_util/test/test_point_cloud_xyzrgb.cpp b/rtabmap_util/test/test_point_cloud_xyzrgb.cpp new file mode 100644 index 00000000..a17221a0 --- /dev/null +++ b/rtabmap_util/test/test_point_cloud_xyzrgb.cpp @@ -0,0 +1,531 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "node_test_utils.hpp" +#include "msg_builders.hpp" + +#include + +#include + +#include + +using namespace rtabmap_util_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); + +constexpr int kWidth = 16; +constexpr int kHeight = 16; +constexpr double kFx = 100.0; +constexpr int kCenter = (kHeight/2) * kWidth + kWidth/2; + +/// The color every synthetic RGB image is painted with, in OpenCV's BGR order. +const cv::Scalar kColor(10, 20, 30); + +sensor_msgs::msg::Image makeRgb(double stamp, const std::string & encoding = "bgr8") +{ + if(encoding == "mono8") + { + return makeImage("camera_link", stamp, + cv::Mat(kHeight, kWidth, CV_8UC1, cv::Scalar(128)), encoding); + } + return makeImage("camera_link", stamp, + cv::Mat(kHeight, kWidth, CV_8UC3, kColor), encoding); +} + +sensor_msgs::msg::Image makeDepth(double stamp, float meters, + const std::string & encoding = sensor_msgs::image_encodings::TYPE_32FC1) +{ + cv::Mat image = encoding == sensor_msgs::image_encodings::TYPE_32FC1 + ? cv::Mat(kHeight, kWidth, CV_32FC1, cv::Scalar(meters)) + : cv::Mat(kHeight, kWidth, CV_16UC1, cv::Scalar(uint16_t(meters*1000.0f))); + return makeImage("camera_link", stamp, image, encoding); +} + +/// A disparity image where every pixel carries @p disparity, so depth = f*t/disparity. +stereo_msgs::msg::DisparityImage makeDisparity(double stamp, float disparity, + float focal = float(kFx), float baseline = 0.1f) +{ + stereo_msgs::msg::DisparityImage msg; + msg.header.frame_id = "camera_link"; + msg.header.stamp = stampOf(stamp); + msg.f = focal; + msg.t = baseline; + msg.min_disparity = 1.0f; + msg.max_disparity = 100.0f; + msg.image = makeImage("camera_link", stamp, + cv::Mat(kHeight, kWidth, CV_32FC1, cv::Scalar(disparity)), + sensor_msgs::image_encodings::TYPE_32FC1); + return msg; +} + +bool hasField(const sensor_msgs::msg::PointCloud2 & cloud, const std::string & name) +{ + for(size_t i=0; i> 16) & 0xFF), + uint8_t((packed >> 8) & 0xFF), + uint8_t(packed & 0xFF)); +} +} // namespace + +class PointCloudXYZRGBTest : public NodeTest +{ +protected: + void start(const std::vector & overrides = {}) + { + addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(overrides))); + out_ = collect("cloud"); + rgbPub_ = helper()->create_publisher("rgb/image", 10); + depthPub_ = helper()->create_publisher("depth/image", 10); + infoPub_ = helper()->create_publisher("rgb/camera_info", 10); + ASSERT_TRUE(waitForSubscriber(rgbPub_)); + ASSERT_TRUE(waitForSubscriber(depthPub_)); + ASSERT_TRUE(waitForSubscriber(infoPub_)); + ASSERT_TRUE(waitForPublisher(out_->subscription)); + } + + /// Publishes a synchronized rgb + depth + camera_info triple. + void publishFrame(double stamp, float meters, + const std::string & depthEncoding = sensor_msgs::image_encodings::TYPE_32FC1, + const std::string & rgbEncoding = "bgr8") + { + rgbPub_->publish(makeRgb(stamp, rgbEncoding)); + depthPub_->publish(makeDepth(stamp, meters, depthEncoding)); + infoPub_->publish(makeCameraInfo("camera_link", stamp, kWidth, kHeight, 0.0, kFx)); + } + + std::shared_ptr> out_; + rclcpp::Publisher::SharedPtr rgbPub_; + rclcpp::Publisher::SharedPtr depthPub_; + rclcpp::Publisher::SharedPtr infoPub_; +}; + +//============================================================================ +// rgb + depth + camera_info +//============================================================================ + +TEST_F(PointCloudXYZRGBTest, ProjectsRgbAndDepthIntoAColoredCloud) +{ + start(); + publishFrame(1000.0, 2.0f); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published"; + + const sensor_msgs::msg::PointCloud2 & cloud = out_->back(); + EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight)) + << "one point per pixel at decimation 1"; + EXPECT_EQ(cloud.header.frame_id, "camera_link") + << "the cloud takes the RGB image's frame"; + EXPECT_TRUE(hasField(cloud, "rgb")) << "the whole point of this node"; + EXPECT_NEAR(readXYZ(cloud, kCenter).z, 2.0f, 1e-3); + + // The RGB image is uniform, so every point carries the same color. cv_bridge hands + // the node a bgr8 image, which reaches the cloud as r=30, g=20, b=10. + const cv::Vec3b rgb = readRGB(cloud, kCenter); + EXPECT_EQ(int(rgb[0]), 30); + EXPECT_EQ(int(rgb[1]), 20); + EXPECT_EQ(int(rgb[2]), 10); +} + +TEST_F(PointCloudXYZRGBTest, Accepts16UC1Millimeters) +{ + start(); + publishFrame(1000.0, 2.0f, sensor_msgs::image_encodings::TYPE_16UC1); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_NEAR(readXYZ(out_->back(), kCenter).z, 2.0f, 1e-3) + << "millimeter depth must be converted to meters"; +} + +TEST_F(PointCloudXYZRGBTest, AcceptsMono8Color) +{ + start(); + publishFrame(1000.0, 2.0f, sensor_msgs::image_encodings::TYPE_32FC1, "mono8"); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const cv::Vec3b rgb = readRGB(out_->back(), kCenter); + EXPECT_EQ(int(rgb[0]), 128) << "a grey image gives grey points"; + EXPECT_EQ(int(rgb[1]), 128); + EXPECT_EQ(int(rgb[2]), 128); +} + +TEST_F(PointCloudXYZRGBTest, RejectsUnsupportedDepthEncoding) +{ + start(); + rgbPub_->publish(makeRgb(1000.0)); + depthPub_->publish(makeImage("camera_link", 1000.0, + cv::Mat(kHeight, kWidth, CV_8UC3, kColor), "bgr8")); + infoPub_->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx)); + spinFor(std::chrono::milliseconds(400)); + + EXPECT_TRUE(out_->empty()) << "only 32FC1, 16UC1 and mono16 depth are supported"; +} + +TEST_F(PointCloudXYZRGBTest, DecimationReducesThePointCount) +{ + start({rclcpp::Parameter("decimation", 2)}); + publishFrame(1000.0, 2.0f); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_EQ(out_->back().width * out_->back().height, uint32_t(kWidth*kHeight)/4) + << "decimation 2 keeps one pixel in four"; +} + +TEST_F(PointCloudXYZRGBTest, RoiRatiosCropTheCloud) +{ + // A quarter off each side of a 16x16 image leaves an 8x8 window. + start({rclcpp::Parameter("roi_ratios", std::string("0.25 0.25 0.25 0.25"))}); + publishFrame(1000.0, 2.0f); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_EQ(out_->back().width * out_->back().height, 64u); +} + +TEST_F(PointCloudXYZRGBTest, MaxDepthMarksFarPointsInvalid) +{ + // cloudFromDepthRGB keeps the cloud organized: out-of-range points become NaN + // rather than disappearing, so the point count is unchanged. + start({rclcpp::Parameter("max_depth", 1.0)}); + publishFrame(1000.0, 5.0f); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_EQ(out_->back().width * out_->back().height, uint32_t(kWidth*kHeight)); + EXPECT_TRUE(std::isnan(readXYZ(out_->back(), kCenter).z)); +} + +TEST_F(PointCloudXYZRGBTest, MinDepthMarksNearPointsInvalid) +{ + start({rclcpp::Parameter("min_depth", 3.0)}); + publishFrame(1000.0, 1.0f); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_TRUE(std::isnan(readXYZ(out_->back(), kCenter).z)); +} + +TEST_F(PointCloudXYZRGBTest, FilterNaNsRemovesInvalidPoints) +{ + start({rclcpp::Parameter("max_depth", 1.0), + rclcpp::Parameter("filter_nans", true)}); + publishFrame(1000.0, 5.0f); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_EQ(out_->back().width * out_->back().height, 0u) + << "filter_nans must remove the out-of-range points"; +} + +TEST_F(PointCloudXYZRGBTest, NormalKAddsNormalFields) +{ + start({rclcpp::Parameter("normal_k", 10)}); + publishFrame(1000.0, 2.0f); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_TRUE(hasField(out_->back(), "normal_x")) + << "asking for normals must change the point type"; + EXPECT_TRUE(hasField(out_->back(), "rgb")) << "and must keep the color"; +} + +TEST_F(PointCloudXYZRGBTest, NoNormalFieldsByDefault) +{ + start(); + publishFrame(1000.0, 2.0f); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_FALSE(hasField(out_->back(), "normal_x")); +} + +TEST_F(PointCloudXYZRGBTest, VoxelSizeThinsTheCloud) +{ + // A frontal plane at 2 m spans about 0.32 m across a 16-pixel image at fx=100, so a + // 0.1 m voxel grid collapses the 256 points into far fewer. + start({rclcpp::Parameter("voxel_size", 0.1)}); + publishFrame(1000.0, 2.0f); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const uint32_t points = out_->back().width * out_->back().height; + EXPECT_GT(points, 0u); + EXPECT_LT(points, uint32_t(kWidth*kHeight)); +} + +TEST_F(PointCloudXYZRGBTest, StaysSilentWithoutASubscriber) +{ + // The projection is skipped entirely when nobody wants the cloud. + addNode(std::make_shared(rclcpp::NodeOptions())); + rclcpp::Publisher::SharedPtr rgbPub = + helper()->create_publisher("rgb/image", 10); + rclcpp::Publisher::SharedPtr depthPub = + helper()->create_publisher("depth/image", 10); + rclcpp::Publisher::SharedPtr infoPub = + helper()->create_publisher("rgb/camera_info", 10); + ASSERT_TRUE(waitForSubscriber(rgbPub)); + ASSERT_TRUE(waitForSubscriber(depthPub)); + + rgbPub->publish(makeRgb(1000.0)); + depthPub->publish(makeDepth(1000.0, 2.0f)); + infoPub->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx)); + spinFor(std::chrono::milliseconds(300)); + + std::shared_ptr> late = + collect("cloud"); + spinFor(std::chrono::milliseconds(200)); + EXPECT_TRUE(late->empty()); +} + +//============================================================================ +// rgbd_image +//============================================================================ + +class PointCloudXYZRGBRgbdTest : public NodeTest +{ +protected: + void start(const std::vector & overrides = {}) + { + addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(overrides))); + out_ = collect("cloud"); + rgbdPub_ = helper()->create_publisher("rgbd_image", 10); + ASSERT_TRUE(waitForSubscriber(rgbdPub_)); + ASSERT_TRUE(waitForPublisher(out_->subscription)); + } + + std::shared_ptr> out_; + rclcpp::Publisher::SharedPtr rgbdPub_; +}; + +TEST_F(PointCloudXYZRGBRgbdTest, ProjectsAnRgbdImage) +{ + start(); + rgbdPub_->publish(makeRGBDImage("camera_link", 1000.0, kWidth, kHeight)); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published"; + + const sensor_msgs::msg::PointCloud2 & cloud = out_->back(); + EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight)); + EXPECT_EQ(cloud.header.frame_id, "camera_link"); + EXPECT_TRUE(hasField(cloud, "rgb")); + EXPECT_NEAR(readXYZ(cloud, kCenter).z, 1.5f, 1e-3) + << "makeRGBDImage() fills the depth image with 1500 mm"; +} + +TEST_F(PointCloudXYZRGBRgbdTest, IgnoresAnInvalidRgbdImage) +{ + // isValid() is false without any image data, and nothing must be published. + start(); + rtabmap_msgs::msg::RGBDImage msg; + msg.header.frame_id = "camera_link"; + msg.header.stamp = stampOf(1000.0); + rgbdPub_->publish(msg); + spinFor(std::chrono::milliseconds(400)); + + EXPECT_TRUE(out_->empty()); +} + +TEST_F(PointCloudXYZRGBRgbdTest, PublishesAnEmptyCloudForAColorOnlyRgbdImage) +{ + // Depth is optional in an RGBDImage, so color alone must not be treated as a broken + // message: there is simply nothing to project. + start(); + rtabmap_msgs::msg::RGBDImage msg = makeRGBDImage("camera_link", 1000.0, kWidth, kHeight); + msg.depth = sensor_msgs::msg::Image(); + rgbdPub_->publish(msg); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published"; + + EXPECT_EQ(out_->back().width * out_->back().height, 0u); +} + +TEST_F(PointCloudXYZRGBRgbdTest, ProjectsAStereoRgbdImage) +{ + // A stereo pair in an RGBDImage is dense-matched instead of read as depth. + start(); + rgbdPub_->publish(makeStereoRGBDImage("camera_link", 1000.0, 160, 120)); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published"; + + EXPECT_EQ(out_->back().width * out_->back().height, uint32_t(160*120)); + EXPECT_TRUE(hasField(out_->back(), "rgb")); +} + +//============================================================================ +// left/image + disparity + left/camera_info +//============================================================================ + +class PointCloudXYZRGBDisparityTest : public NodeTest +{ +protected: + void start(const std::vector & overrides = {}) + { + addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(overrides))); + out_ = collect("cloud"); + leftPub_ = helper()->create_publisher("left/image", 10); + dispPub_ = helper()->create_publisher("disparity", 10); + infoPub_ = helper()->create_publisher("left/camera_info", 10); + ASSERT_TRUE(waitForSubscriber(leftPub_)); + ASSERT_TRUE(waitForSubscriber(dispPub_)); + ASSERT_TRUE(waitForSubscriber(infoPub_)); + ASSERT_TRUE(waitForPublisher(out_->subscription)); + } + + void publishFrame(double stamp, float disparity) + { + leftPub_->publish(makeRgb(stamp)); + dispPub_->publish(makeDisparity(stamp, disparity)); + infoPub_->publish(makeCameraInfo("camera_link", stamp, kWidth, kHeight, 0.0, kFx)); + } + + std::shared_ptr> out_; + rclcpp::Publisher::SharedPtr leftPub_; + rclcpp::Publisher::SharedPtr dispPub_; + rclcpp::Publisher::SharedPtr infoPub_; +}; + +TEST_F(PointCloudXYZRGBDisparityTest, ProjectsDisparityIntoAColoredCloud) +{ + start(); + publishFrame(1000.0, 5.0f); // depth = f*t/d = 100*0.1/5 = 2 m + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published"; + + const sensor_msgs::msg::PointCloud2 & cloud = out_->back(); + EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight)); + EXPECT_EQ(cloud.header.frame_id, "camera_link") + << "the cloud takes the disparity image's frame"; + EXPECT_TRUE(hasField(cloud, "rgb")); + EXPECT_NEAR(readXYZ(cloud, kCenter).z, 2.0f, 1e-3); + + const cv::Vec3b rgb = readRGB(cloud, kCenter); + EXPECT_EQ(int(rgb[0]), 30); + EXPECT_EQ(int(rgb[2]), 10); +} + +TEST_F(PointCloudXYZRGBDisparityTest, RejectsUnsupportedDisparityEncoding) +{ + start(); + stereo_msgs::msg::DisparityImage msg = makeDisparity(1000.0, 5.0f); + msg.image = makeImage("camera_link", 1000.0, + cv::Mat(kHeight, kWidth, CV_8UC1, cv::Scalar(5)), "mono8"); + leftPub_->publish(makeRgb(1000.0)); + dispPub_->publish(msg); + infoPub_->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx)); + spinFor(std::chrono::milliseconds(400)); + + EXPECT_TRUE(out_->empty()) << "only 32FC1 and 16SC1 disparity are supported"; +} + +TEST_F(PointCloudXYZRGBDisparityTest, MaxDepthMarksFarPointsInvalid) +{ + start({rclcpp::Parameter("max_depth", 1.0)}); + publishFrame(1000.0, 5.0f); // 2 m, beyond the limit + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_TRUE(std::isnan(readXYZ(out_->back(), kCenter).z)); +} + +//============================================================================ +// left/image + right/image + both camera_infos +//============================================================================ + +class PointCloudXYZRGBStereoTest : public NodeTest +{ +protected: + static constexpr int kStereoWidth = 160; + static constexpr int kStereoHeight = 120; + static constexpr float kBaseline = 0.12f; + static constexpr int kDisparity = 6; // depth = fx*baseline/d = 100*0.12/6 = 2 m + + void start(const std::vector & overrides = {}) + { + addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(overrides))); + out_ = collect("cloud"); + leftPub_ = helper()->create_publisher("left/image", 10); + rightPub_ = helper()->create_publisher("right/image", 10); + leftInfoPub_ = helper()->create_publisher("left/camera_info", 10); + rightInfoPub_ = helper()->create_publisher("right/camera_info", 10); + ASSERT_TRUE(waitForSubscriber(leftPub_)); + ASSERT_TRUE(waitForSubscriber(rightPub_)); + ASSERT_TRUE(waitForSubscriber(leftInfoPub_)); + ASSERT_TRUE(waitForSubscriber(rightInfoPub_)); + ASSERT_TRUE(waitForPublisher(out_->subscription)); + } + + /** + * Publishes a textured pair whose true disparity is kDisparity everywhere: the right + * image is the left one shifted, which is what a plane at a constant depth looks like. + */ + void publishFrame(double stamp) + { + cv::Mat left(kStereoHeight, kStereoWidth + kDisparity, CV_8UC1); + cv::RNG rng(42); + rng.fill(left, cv::RNG::UNIFORM, 0, 256); + + cv::Mat leftBgr; + cv::cvtColor(cv::Mat(left, cv::Rect(0, 0, kStereoWidth, kStereoHeight)), + leftBgr, cv::COLOR_GRAY2BGR); + cv::Mat right(left, cv::Rect(kDisparity, 0, kStereoWidth, kStereoHeight)); + + leftPub_->publish(makeImage("camera_link", stamp, leftBgr, "bgr8")); + rightPub_->publish(makeImage("camera_link", stamp, right.clone(), "mono8")); + leftInfoPub_->publish(makeCameraInfo( + "camera_link", stamp, kStereoWidth, kStereoHeight, 0.0, kFx)); + rightInfoPub_->publish(makeCameraInfo( + "camera_link", stamp, kStereoWidth, kStereoHeight, -kFx*kBaseline, kFx)); + } + + std::shared_ptr> out_; + rclcpp::Publisher::SharedPtr leftPub_; + rclcpp::Publisher::SharedPtr rightPub_; + rclcpp::Publisher::SharedPtr leftInfoPub_; + rclcpp::Publisher::SharedPtr rightInfoPub_; +}; + +TEST_F(PointCloudXYZRGBStereoTest, MatchesAStereoPairIntoAColoredCloud) +{ + start({rclcpp::Parameter("StereoBM/NumDisparities", std::string("16")), + rclcpp::Parameter("StereoBM/BlockSize", std::string("9"))}); + publishFrame(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published"; + + const sensor_msgs::msg::PointCloud2 & cloud = out_->back(); + EXPECT_EQ(cloud.width * cloud.height, uint32_t(kStereoWidth*kStereoHeight)) + << "the cloud stays organized, one point per pixel"; + EXPECT_EQ(cloud.header.frame_id, "camera_link") + << "the cloud takes the left image's frame"; + EXPECT_TRUE(hasField(cloud, "rgb")); + + // The pair is a shifted copy of itself, so the whole matched area sits at one depth. + const size_t center = size_t(kStereoHeight/2) * kStereoWidth + kStereoWidth/2; + EXPECT_NEAR(readXYZ(cloud, center).z, 2.0f, 0.2f); +} + +TEST_F(PointCloudXYZRGBStereoTest, RejectsUnsupportedStereoEncoding) +{ + start(); + leftPub_->publish(makeImage("camera_link", 1000.0, + cv::Mat(kStereoHeight, kStereoWidth, CV_32FC1, cv::Scalar(1.0f)), "32FC1")); + rightPub_->publish(makeImage("camera_link", 1000.0, + cv::Mat(kStereoHeight, kStereoWidth, CV_8UC1, cv::Scalar(0)), "mono8")); + leftInfoPub_->publish(makeCameraInfo( + "camera_link", 1000.0, kStereoWidth, kStereoHeight, 0.0, kFx)); + rightInfoPub_->publish(makeCameraInfo( + "camera_link", 1000.0, kStereoWidth, kStereoHeight, -kFx*kBaseline, kFx)); + spinFor(std::chrono::milliseconds(400)); + + EXPECT_TRUE(out_->empty()) << "only 8-bit and mono16 stereo images are supported"; +} diff --git a/rtabmap_util/test/test_pointcloud_to_depthimage.cpp b/rtabmap_util/test/test_pointcloud_to_depthimage.cpp new file mode 100644 index 00000000..252797ed --- /dev/null +++ b/rtabmap_util/test/test_pointcloud_to_depthimage.cpp @@ -0,0 +1,370 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "node_test_utils.hpp" +#include "msg_builders.hpp" + +#include + +#include + +using namespace rtabmap_util_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); + +constexpr int kWidth = 16; +constexpr int kHeight = 16; +constexpr double kFx = 100.0; + +float pixel32f(const sensor_msgs::msg::Image & img, int row, int col) +{ + return *reinterpret_cast(&img.data[row * img.step + col * sizeof(float)]); +} + +uint16_t pixel16u(const sensor_msgs::msg::Image & img, int row, int col) +{ + return *reinterpret_cast(&img.data[row * img.step + col * sizeof(uint16_t)]); +} +} // namespace + +class PointCloudToDepthImageTest : public NodeTest +{ +protected: + void start(const std::vector & overrides = {}, bool withTf = true) + { + addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(overrides))); + if(withTf) + { + publishStaticTf("camera_link", "lidar"); + } + image32_ = collect("image"); + image16_ = collect("image_raw"); + cloudPub_ = helper()->create_publisher("cloud", 10); + infoPub_ = helper()->create_publisher("camera_info", 10); + ASSERT_TRUE(waitForSubscriber(cloudPub_)); + ASSERT_TRUE(waitForSubscriber(infoPub_)); + ASSERT_TRUE(waitForPublisher(image32_->subscription)); + } + + /// Publishes a cloud and its camera info with identical stamps. + void publishFrame(double stamp, const std::vector & points) + { + cloudPub_->publish(makeXYZCloud("lidar", stamp, points)); + infoPub_->publish(makeCameraInfo("camera_link", stamp, kWidth, kHeight, 0.0, kFx)); + } + + /// A block of points straight ahead of the optical axis at @p depth meters. + static std::vector blockAt(float depth) + { + std::vector points; + for(int i=-2; i<=2; ++i) + { + for(int j=-2; j<=2; ++j) + { + points.push_back(cv::Point3f(0.01f*i, 0.01f*j, depth)); + } + } + return points; + } + + std::shared_ptr> image32_; + std::shared_ptr> image16_; + rclcpp::Publisher::SharedPtr cloudPub_; + rclcpp::Publisher::SharedPtr infoPub_; +}; + +TEST_F(PointCloudToDepthImageTest, ProjectsACloudIntoADepthImage) +{ + start(); + publishFrame(1000.0, blockAt(2.0f)); + ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) << "no depth image"; + + const sensor_msgs::msg::Image & img = image32_->back(); + EXPECT_EQ(img.encoding, sensor_msgs::image_encodings::TYPE_32FC1); + EXPECT_EQ(img.width, uint32_t(kWidth)); + EXPECT_EQ(img.height, uint32_t(kHeight)); + EXPECT_EQ(img.header.frame_id, "camera_link") + << "the depth image belongs to the camera, not the cloud"; + + // The points sit on the optical axis, so they land on the principal point. + EXPECT_NEAR(pixel32f(img, kHeight/2, kWidth/2), 2.0f, 1e-3); + // A corner sees nothing. + EXPECT_FLOAT_EQ(pixel32f(img, 0, 0), 0.0f); +} + +TEST_F(PointCloudToDepthImageTest, PublishesMillimetersOnImageRaw) +{ + start(); + publishFrame(1000.0, blockAt(2.0f)); + ASSERT_TRUE(spinUntil([&]() { return !image16_->empty(); })); + + const sensor_msgs::msg::Image & img = image16_->back(); + EXPECT_EQ(img.encoding, sensor_msgs::image_encodings::TYPE_16UC1); + EXPECT_EQ(pixel16u(img, kHeight/2, kWidth/2), 2000) << "2 m expressed in millimeters"; +} + +TEST_F(PointCloudToDepthImageTest, EmptyCloudGivesAnAllZeroImage) +{ + start(); + publishFrame(1000.0, {}); + ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) + << "an empty cloud must still produce an image, not a dropped frame"; + + const sensor_msgs::msg::Image & img = image32_->back(); + EXPECT_EQ(img.width, uint32_t(kWidth)); + for(int row=0; row> infoOut = + collect("image/camera_info"); + ASSERT_TRUE(waitForPublisher(infoOut->subscription)); + + publishFrame(1000.0, blockAt(2.0f)); + ASSERT_TRUE(spinUntil([&]() { return !image32_->empty() && !infoOut->empty(); })); + + EXPECT_EQ(image32_->back().width, uint32_t(kWidth)/2); + EXPECT_EQ(image32_->back().height, uint32_t(kHeight)/2); + EXPECT_NEAR(infoOut->back().p[0], kFx/2.0, 1e-6) + << "the published camera info must match the decimated image"; + EXPECT_EQ(infoOut->back().width, uint32_t(kWidth)/2); +} + +TEST_F(PointCloudToDepthImageTest, FailsWithoutTheCloudToCameraTransform) +{ + start({rclcpp::Parameter("wait_for_transform", 0.0)}, /*withTf=*/false); + publishFrame(1000.0, blockAt(2.0f)); + spinFor(std::chrono::milliseconds(400)); + + EXPECT_TRUE(image32_->empty()) + << "without TF the cloud cannot be placed in the camera frame"; +} + +TEST_F(PointCloudToDepthImageTest, StaysSilentWithoutASubscriber) +{ + // The projection is skipped entirely when neither image topic is subscribed. + addNode(std::make_shared(rclcpp::NodeOptions())); + publishStaticTf("camera_link", "lidar"); + rclcpp::Publisher::SharedPtr cloudPub = + helper()->create_publisher("cloud", 10); + rclcpp::Publisher::SharedPtr infoPub = + helper()->create_publisher("camera_info", 10); + ASSERT_TRUE(waitForSubscriber(cloudPub)); + + cloudPub->publish(makeXYZCloud("lidar", 1000.0, blockAt(2.0f))); + infoPub->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx)); + spinFor(std::chrono::milliseconds(300)); + + std::shared_ptr> late = + collect("image"); + spinFor(std::chrono::milliseconds(200)); + EXPECT_TRUE(late->empty()); +} + +//============================================================================ +// Motion compensation between the cloud stamp and the camera_info stamp +//============================================================================ + +/** + * With approximate synchronization the cloud and the camera info rarely share a stamp. + * When @c fixed_frame_id is set, the node asks TF how the lidar moved over that interval + * and folds the displacement into the camera's local transform, so the cloud is projected + * from where the camera was at its own stamp instead of where the lidar was. + * + * The frames follow the usual convention, as in rtabmap's own projectCloudToCamera tests: + * the cloud is expressed in a lidar frame with x forward, and the camera is attached to it + * through the optical rotation, so a point straight ahead lands on the principal point. + */ +class PointCloudToDepthImageMotionTest : public NodeTest +{ +protected: + static constexpr double kSpeed = 1.0; ///< m/s + static constexpr double kCloudStamp = 1000.0; + static constexpr double kInfoDelay = 0.04; ///< the camera info lags the cloud by this + static constexpr float kRange = 2.0f; ///< distance to the point, meters + static constexpr int kFrames = 3; ///< see publishFrames() + static constexpr double kPeriod = 0.2; ///< seconds between frames + + /// Distance travelled between the two stamps: what the node has to compensate for. + static double travelled() { return kSpeed * kInfoDelay; } + + /// The same distance seen sideways by the camera, in pixels. + static int shiftInPixels() { return int(kFx * travelled() / double(kRange)); } + + /** + * @param axis 'x' to drive straight at the point, 'y' to drive sideways past it, + * '0' to stand still + */ + void start(const std::vector & overrides, char axis = 'x') + { + addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(overrides))); + // The camera is bolted to the lidar, looking the same way: x right, y down, + // z forward against the lidar's x forward, y left, z up. + publishStaticTfRPY("lidar", "camera_link", -M_PI/2.0, 0.0, -M_PI/2.0); + if(axis != '0') + { + publishOdomMotion(axis); + } + image32_ = collect("image"); + cloudPub_ = helper()->create_publisher("cloud", 10); + infoPub_ = helper()->create_publisher("camera_info", 10); + ASSERT_TRUE(waitForSubscriber(cloudPub_)); + ASSERT_TRUE(waitForSubscriber(infoPub_)); + ASSERT_TRUE(waitForPublisher(image32_->subscription)); + } + + /// Publishes odom -> lidar moving at kSpeed, covering every stamp used below. + void publishOdomMotion(char axis) + { + rclcpp::Publisher::SharedPtr tfPub = + helper()->create_publisher("/tf", rclcpp::QoS(100)); + spinFor(std::chrono::milliseconds(100)); // let the node's listener subscribe + + // One sample per cloud stamp and per camera info stamp, plus a margin on each side + // so that nothing has to be extrapolated. + std::vector elapsedSamples; + elapsedSamples.push_back(-kPeriod); + for(int k=0; kpublish(msg); + } + spinFor(std::chrono::milliseconds(200)); // let the buffer fill + tfPub_ = tfPub; // keep the publisher alive + } + + /** + * @brief Publishes kFrames cloud/camera_info pairs, the info @p delay seconds late. + * + * Each cloud holds a single point straight ahead of the lidar. The speed is constant, + * so every pair needs the same correction and the first output is enough to assert on. + * A burst is needed because ApproximateTime emits nothing for a lone pair whose stamps + * differ: it cannot rule out a better match still to come. + */ + void publishFrames(double delay) + { + for(int k=0; kpublish(makeXYZCloud("lidar", kCloudStamp + elapsed, + {cv::Point3f(kRange, 0.0f, 0.0f)})); + infoPub_->publish(makeCameraInfo( + "camera_link", kCloudStamp + elapsed + delay, kWidth, kHeight, 0.0, kFx)); + } + } + + /// The column the single point landed in on @p row, or -1 if that row is empty. + static int hitColumn(const sensor_msgs::msg::Image & img, int row) + { + for(int col=0; col> image32_; + rclcpp::Publisher::SharedPtr cloudPub_; + rclcpp::Publisher::SharedPtr infoPub_; + rclcpp::Publisher::SharedPtr tfPub_; +}; + +TEST_F(PointCloudToDepthImageMotionTest, NoShiftWhenTheStampsMatch) +{ + // The control case: same frames, same scene, nothing to compensate. It also proves the + // optical rotation is right, since a wrongly oriented camera sees nothing at all. + start({rclcpp::Parameter("fixed_frame_id", std::string("odom"))}); + publishFrames(0.0); + ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) << "no depth image"; + + const sensor_msgs::msg::Image & img = image32_->front(); + EXPECT_EQ(hitColumn(img, kHeight/2), kWidth/2) + << "a point straight ahead belongs at the principal point"; + EXPECT_NEAR(pixel32f(img, kHeight/2, kWidth/2), kRange, 1e-3); +} + +TEST_F(PointCloudToDepthImageMotionTest, ClosesTheGapWhenDrivingAtThePoint) +{ + // The camera info is 40 ms younger than the cloud and the robot closes in at 1 m/s, so + // by the time of the exposure the point is 4 cm nearer than the lidar measured it. + start({rclcpp::Parameter("fixed_frame_id", std::string("odom"))}, 'x'); + publishFrames(kInfoDelay); + ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) << "no depth image"; + + const sensor_msgs::msg::Image & img = image32_->front(); + EXPECT_EQ(hitColumn(img, kHeight/2), kWidth/2) + << "driving straight at the point does not move it across the image"; + EXPECT_NEAR(pixel32f(img, kHeight/2, kWidth/2), kRange - float(travelled()), 1e-3) + << "the depth must be corrected for the distance travelled"; +} + +TEST_F(PointCloudToDepthImageMotionTest, ShiftsThePointWhenDrivingPastIt) +{ + // Moving sideways instead: the point slides across the image by fx*d/Z pixels, and + // stays at the same range. + start({rclcpp::Parameter("fixed_frame_id", std::string("odom"))}, 'y'); + publishFrames(kInfoDelay); + ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) << "no depth image"; + + const sensor_msgs::msg::Image & img = image32_->front(); + EXPECT_EQ(hitColumn(img, kHeight/2), kWidth/2 + shiftInPixels()) + << "the robot moved left, so the point must appear further right"; + EXPECT_NEAR(pixel32f(img, kHeight/2, kWidth/2 + shiftInPixels()), kRange, 1e-3) + << "only the bearing changed, not the range"; + EXPECT_FLOAT_EQ(pixel32f(img, kHeight/2, kWidth/2), 0.0f) + << "and it is no longer at the principal point"; +} + +TEST_F(PointCloudToDepthImageMotionTest, IgnoresTheStampDifferenceWithoutAFixedFrame) +{ + // Without fixed_frame_id there is nothing to measure the motion against, so the cloud + // is projected as if both messages were captured at the same instant. That is why the + // node logs a fatal error when approximate sync is used without one. + start({rclcpp::Parameter("fixed_frame_id", std::string(""))}, 'x'); + publishFrames(kInfoDelay); + ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) << "no depth image"; + + EXPECT_NEAR(pixel32f(image32_->front(), kHeight/2, kWidth/2), kRange, 1e-3) + << "no fixed frame, no compensation"; +} + +TEST_F(PointCloudToDepthImageMotionTest, FailsWhenTheFixedFrameIsUnknown) +{ + // fixed_frame_id is set but odom -> lidar was never published: the displacement cannot + // be measured, and projecting anyway would silently misplace the points. + start({rclcpp::Parameter("fixed_frame_id", std::string("odom")), + rclcpp::Parameter("wait_for_transform", 0.0)}, '0'); + publishFrames(kInfoDelay); + spinFor(std::chrono::milliseconds(400)); + + EXPECT_TRUE(image32_->empty()); +} diff --git a/rtabmap_util/test/test_rgbd_relay.cpp b/rtabmap_util/test/test_rgbd_relay.cpp new file mode 100644 index 00000000..6489bef3 --- /dev/null +++ b/rtabmap_util/test/test_rgbd_relay.cpp @@ -0,0 +1,350 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "node_test_utils.hpp" +#include "msg_builders.hpp" + +#include + +#include +#include + +using namespace rtabmap_util_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); +} + +class RGBDRelayTest : public NodeTest +{ +protected: + /// Starts the node, wires up the input publisher and the output collector. + void start(bool compress, bool uncompress) + { + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("compress", compress), + rclcpp::Parameter("uncompress", uncompress)}))); + + out_ = collect("rgbd_image_relay"); + pub_ = helper()->create_publisher("rgbd_image", 10); + ASSERT_TRUE(waitForSubscriber(pub_)); + ASSERT_TRUE(waitForPublisher(out_->subscription)); + } + + std::shared_ptr> out_; + rclcpp::Publisher::SharedPtr pub_; +}; + +TEST_F(RGBDRelayTest, RepublishesUnchangedByDefault) +{ + start(/*compress=*/false, /*uncompress=*/false); + + const rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + pub_->publish(in); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = out_->back(); + EXPECT_EQ(got.header.frame_id, in.header.frame_id); + EXPECT_EQ(got.rgb.data, in.rgb.data) << "the payload must be passed through untouched"; + EXPECT_EQ(got.depth.data, in.depth.data); + EXPECT_TRUE(got.rgb_compressed.data.empty()); + EXPECT_TRUE(got.depth_compressed.data.empty()); + EXPECT_NEAR(got.rgb_camera_info.p[0], in.rgb_camera_info.p[0], 1e-9); +} + +TEST_F(RGBDRelayTest, CompressesRawImagesWhenAsked) +{ + start(/*compress=*/true, /*uncompress=*/false); + + pub_->publish(makeRGBDImage("camera_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = out_->back(); + EXPECT_FALSE(got.rgb_compressed.data.empty()) << "rgb must be compressed"; + EXPECT_FALSE(got.depth_compressed.data.empty()) << "depth must be compressed"; + // Depth is lossless png; color is jpg. + EXPECT_EQ(got.depth_compressed.format, "png"); + EXPECT_TRUE(got.rgb.data.empty()) << "the raw image is not carried as well"; +} + +TEST_F(RGBDRelayTest, CompressesAStereoPairAsJpeg) +{ + // When the camera infos describe a stereo pair, the "depth" slot holds the right + // image and is compressed as JPEG, not as a lossless depth PNG. + start(/*compress=*/true, /*uncompress=*/false); + + pub_->publish(makeStereoRGBDImage("camera_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = out_->back(); + ASSERT_FALSE(got.depth_compressed.data.empty()); + EXPECT_NE(got.depth_compressed.format, "png") + << "a stereo right image must not take the depth PNG path"; + EXPECT_NE(got.depth_compressed.format.find("jp"), std::string::npos) + << "expected a jpeg format, got \"" << got.depth_compressed.format << "\""; + EXPECT_LT(got.depth_camera_info.p[3], 0.0) << "the baseline must survive the relay"; +} + +TEST_F(RGBDRelayTest, CompressesDepthAsLosslessPng) +{ + // The same call with no baseline is treated as color + depth instead. + start(/*compress=*/true, /*uncompress=*/false); + + pub_->publish(makeRGBDImage("camera_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = out_->back(); + ASSERT_FALSE(got.depth_compressed.data.empty()); + EXPECT_EQ(got.depth_compressed.format, "png") << "depth must stay lossless"; + EXPECT_DOUBLE_EQ(got.depth_camera_info.p[3], 0.0) << "no baseline: not stereo"; +} + +TEST_F(RGBDRelayTest, UncompressRestoresAStereoRightImage) +{ + // The uncompress path branches on the format: "jpg" means a stereo right image and + // goes through cv_bridge, anything else is a depth image and goes through rtabmap. + start(/*compress=*/false, /*uncompress=*/true); + + rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0); + const cv::Mat right(8, 8, CV_8UC1, cv::Scalar(60)); + cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", right) + .toCompressedImageMsg(in.depth_compressed, cv_bridge::JPG); + in.depth = sensor_msgs::msg::Image(); + + pub_->publish(in); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = out_->back(); + ASSERT_FALSE(got.depth.data.empty()) << "the right image must be decompressed"; + EXPECT_EQ(got.depth.encoding, "mono8") << "restored as an 8-bit image, not depth"; + EXPECT_EQ(got.depth.width, 8u); + EXPECT_EQ(got.depth.height, 8u); +} + +TEST_F(RGBDRelayTest, UncompressRestoresRawImages) +{ + start(/*compress=*/false, /*uncompress=*/true); + + // Feed it a message that carries only compressed depth. + rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + const cv::Mat depth(8, 8, CV_16UC1, cv::Scalar(1500)); + in.depth = sensor_msgs::msg::Image(); + in.depth_compressed.header = in.header; + in.depth_compressed.format = "png"; + in.depth_compressed.data = rtabmap::compressImage(depth, ".png"); + + pub_->publish(in); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = out_->back(); + ASSERT_FALSE(got.depth.data.empty()) << "depth must be decompressed"; + EXPECT_EQ(got.depth.encoding, sensor_msgs::image_encodings::TYPE_16UC1); + EXPECT_EQ(got.depth.width, 8u); + EXPECT_EQ(got.depth.height, 8u); +} + +TEST_F(RGBDRelayTest, UncompressPrefersTheRawImageOverTheCompressedOne) +{ + // A message may carry both. The raw image is already usable, so decompressing the + // other copy would be wasted work -- and the two paths must agree, as the depth + // branch below does. + start(/*compress=*/false, /*uncompress=*/true); + + rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + // A compressed copy whose content differs, so it is obvious which one was used. + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", + cv::Mat(8, 8, CV_8UC3, cv::Scalar(200, 200, 200))) + .toCompressedImageMsg(in.rgb_compressed, cv_bridge::PNG); + + pub_->publish(in); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + ASSERT_FALSE(out_->back().rgb.data.empty()); + EXPECT_EQ(out_->back().rgb.data, in.rgb.data) + << "the raw image must be forwarded, not the decompressed copy"; +} + +TEST_F(RGBDRelayTest, StaysSilentWithoutASubscriber) +{ + // No collector, so the relay's output has no subscriber and it must not do the work. + addNode(std::make_shared(rclcpp::NodeOptions())); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_image", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + pub->publish(makeRGBDImage("camera_link", 1000.0)); + spinFor(std::chrono::milliseconds(300)); + + // Subscribing only now must not retroactively receive anything. + std::shared_ptr> late = + collect("rgbd_image_relay"); + spinFor(std::chrono::milliseconds(200)); + EXPECT_TRUE(late->empty()); +} + +/// Feeds a compressed right image in @p format through the uncompress path. +class RGBDRelayRightImageTest : public NodeTest +{ +protected: + rtabmap_msgs::msg::RGBDImage relay(cv_bridge::Format format) + { + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({rclcpp::Parameter("uncompress", true)}))); + + std::shared_ptr> out = + collect("rgbd_image_relay"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_image", 10); + EXPECT_TRUE(waitForSubscriber(pub)); + EXPECT_TRUE(waitForPublisher(out->subscription)); + + rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0); + cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", + cv::Mat(8, 8, CV_8UC1, cv::Scalar(60))) + .toCompressedImageMsg(in.depth_compressed, format); + in.depth = sensor_msgs::msg::Image(); + + pub->publish(in); + EXPECT_TRUE(spinUntil([&]() { return !out->empty(); })); + return out->empty() ? rtabmap_msgs::msg::RGBDImage() : out->back(); + } +}; + +TEST_F(RGBDRelayRightImageTest, UncompressesAJpegRightImage) +{ + const rtabmap_msgs::msg::RGBDImage got = relay(cv_bridge::JPG); + ASSERT_FALSE(got.depth.data.empty()); + EXPECT_EQ(got.depth.encoding, sensor_msgs::image_encodings::MONO8); + EXPECT_EQ(got.depth.step, 8u); +} + +TEST_F(RGBDRelayRightImageTest, UncompressesAPngRightImage) +{ + // A losslessly compressed right image must not be mistaken for depth and abort. + const rtabmap_msgs::msg::RGBDImage got = relay(cv_bridge::PNG); + ASSERT_FALSE(got.depth.data.empty()); + EXPECT_EQ(got.depth.encoding, sensor_msgs::image_encodings::MONO8); + EXPECT_EQ(got.depth.step, 8u); +} + +/// QoS of the two sides, set independently through qos_sub and qos_pub. +/// +/// A reliable subscription refuses to match a best-effort publisher, while a best-effort +/// subscription matches either. Every assertion below rests on that asymmetry: whether a +/// connection is established at all is what tells us which reliability the node picked. +class RGBDRelayQosTest : public NodeTest +{ +protected: + enum Reliability { kSystemDefault = 0, kReliable = 1, kBestEffort = 2 }; + + void startRelay(const std::vector & params) + { + addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(params))); + } + + rclcpp::Publisher::SharedPtr input(Reliability reliability) + { + rclcpp::QoS qos(10); + reliability == kBestEffort ? qos.best_effort() : qos.reliable(); + return helper()->create_publisher("rgbd_image", qos); + } + + std::shared_ptr> output(Reliability reliability) + { + rclcpp::QoS qos(10); + reliability == kBestEffort ? qos.best_effort() : qos.reliable(); + return collect("rgbd_image_relay", qos); + } +}; + +TEST_F(RGBDRelayQosTest, BridgesABestEffortSourceToAReliableConsumer) +{ + // The point of splitting the parameter: a sensor publishing best effort feeding a + // consumer that only accepts reliable. Neither could talk to the other directly. + startRelay({rclcpp::Parameter("qos_sub", int(kBestEffort)), + rclcpp::Parameter("qos_pub", int(kReliable))}); + + std::shared_ptr> out = output(kReliable); + rclcpp::Publisher::SharedPtr pub = input(kBestEffort); + ASSERT_TRUE(waitForSubscriber(pub)) << "a best-effort source must reach the relay"; + ASSERT_TRUE(waitForPublisher(out->subscription)) + << "a reliable consumer must be able to subscribe to the relayed topic"; + + pub->publish(makeRGBDImage("camera_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })); + EXPECT_EQ(out->back().header.frame_id, "camera_link"); +} + +TEST_F(RGBDRelayQosTest, QosSubOverridesQosOnTheInputOnly) +{ + // qos says reliable, which a best-effort source could not match; qos_sub overrides it. + startRelay({rclcpp::Parameter("qos", int(kReliable)), + rclcpp::Parameter("qos_sub", int(kBestEffort))}); + + rclcpp::Publisher::SharedPtr pub = input(kBestEffort); + EXPECT_TRUE(waitForSubscriber(pub)) << "qos_sub must win over qos on the subscription"; + + // The output side kept qos, so a reliable consumer still matches it. + std::shared_ptr> out = output(kReliable); + EXPECT_TRUE(waitForPublisher(out->subscription)) + << "qos_sub must not affect the publisher"; +} + +TEST_F(RGBDRelayQosTest, QosPubOverridesQosOnTheOutputOnly) +{ + // qos says best effort, which no reliable consumer could match; qos_pub overrides it. + startRelay({rclcpp::Parameter("qos", int(kBestEffort)), + rclcpp::Parameter("qos_pub", int(kReliable))}); + + std::shared_ptr> out = output(kReliable); + EXPECT_TRUE(waitForPublisher(out->subscription)) + << "qos_pub must win over qos on the publisher"; + + // The input side kept qos, so it is still best effort and accepts a best-effort source. + rclcpp::Publisher::SharedPtr pub = input(kBestEffort); + EXPECT_TRUE(waitForSubscriber(pub)) << "qos_pub must not affect the subscription"; +} + +TEST_F(RGBDRelayQosTest, BothSidesFallBackToQos) +{ + // Only qos is given, so both sides must be best effort -- as before the split. + startRelay({rclcpp::Parameter("qos", int(kBestEffort))}); + + rclcpp::Publisher::SharedPtr pub = input(kBestEffort); + EXPECT_TRUE(waitForSubscriber(pub)) << "the subscription must have followed qos"; + + std::shared_ptr> out = output(kReliable); + spinFor(std::chrono::milliseconds(500)); + EXPECT_EQ(out->subscription->get_publisher_count(), 0u) + << "the publisher must have followed qos too: best effort, so a reliable " + "consumer cannot match it"; +} + +TEST_F(RGBDRelayQosTest, HonorsTheConfiguredQueueDepths) +{ + // Queue depth is not directly observable from outside, so this only pins down that + // the parameters are accepted and the relay still works with them set. + startRelay({rclcpp::Parameter("queue_sub", 20), rclcpp::Parameter("queue_pub", 10)}); + + std::shared_ptr> out = output(kReliable); + rclcpp::Publisher::SharedPtr pub = input(kReliable); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(out->subscription)); + + pub->publish(makeRGBDImage("camera_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })); + EXPECT_EQ(out->back().header.frame_id, "camera_link"); +} + +TEST_F(RGBDRelayQosTest, RejectsAZeroQueueDepth) +{ + // rclcpp::QoS(0) is not a meaningful depth, so say so at construction rather than + // leaving the relay silently misconfigured. + EXPECT_THROW( + startRelay({rclcpp::Parameter("queue_sub", 0)}), + UException); +} diff --git a/rtabmap_util/test/test_rgbd_split.cpp b/rtabmap_util/test/test_rgbd_split.cpp new file mode 100644 index 00000000..f032d998 --- /dev/null +++ b/rtabmap_util/test/test_rgbd_split.cpp @@ -0,0 +1,397 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "node_test_utils.hpp" +#include "msg_builders.hpp" + +#include + +#include +#include + +using namespace rtabmap_util_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); +} + +class RGBDSplitTest : public NodeTest {}; + +TEST_F(RGBDSplitTest, SplitsIntoImageAndCameraInfoTopics) +{ + addNode(std::make_shared(rclcpp::NodeOptions())); + + // The node derives its output topics from the input topic name. + std::shared_ptr> rgb = + collect("rgbd_image/rgb/image"); + std::shared_ptr> depth = + collect("rgbd_image/depth/image"); + std::shared_ptr> rgbInfo = + collect("rgbd_image/rgb/camera_info"); + std::shared_ptr> depthInfo = + collect("rgbd_image/depth/camera_info"); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_image", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(rgb->subscription)); + ASSERT_TRUE(waitForPublisher(depth->subscription)); + + const rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + pub->publish(in); + ASSERT_TRUE(spinUntil([&]() { + return !rgb->empty() && !depth->empty() && !rgbInfo->empty() && !depthInfo->empty(); + })) << "not all four outputs were published"; + + EXPECT_EQ(rgb->back().encoding, "bgr8"); + EXPECT_EQ(rgb->back().data, in.rgb.data); + EXPECT_EQ(depth->back().encoding, sensor_msgs::image_encodings::TYPE_16UC1); + EXPECT_EQ(depth->back().data, in.depth.data); + + EXPECT_NEAR(rgbInfo->back().p[0], in.rgb_camera_info.p[0], 1e-9); + EXPECT_EQ(rgbInfo->back().width, in.rgb_camera_info.width); + EXPECT_NEAR(depthInfo->back().p[0], in.depth_camera_info.p[0], 1e-9); +} + +TEST_F(RGBDSplitTest, FallsBackToTheInputHeaderForTheDepthCameraInfo) +{ + addNode(std::make_shared(rclcpp::NodeOptions())); + + std::shared_ptr> depth = + collect("rgbd_image/depth/image"); + std::shared_ptr> depthInfo = + collect("rgbd_image/depth/camera_info"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_image", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(depth->subscription)); + + // Depth camera info with no frame id: the node fills it from the message header. + rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + in.depth_camera_info.header.frame_id = ""; + pub->publish(in); + ASSERT_TRUE(spinUntil([&]() { return !depthInfo->empty(); })); + + EXPECT_EQ(depthInfo->back().header.frame_id, "camera_link"); +} + +TEST_F(RGBDSplitTest, PassesAStereoPairThroughUnchanged) +{ + // The node does not distinguish stereo from depth: it forwards whatever is in the + // "depth" slot, so a stereo right image is published on .../depth/image along with + // the right camera info carrying the baseline. + addNode(std::make_shared(rclcpp::NodeOptions())); + + std::shared_ptr> right = + collect("rgbd_image/depth/image"); + std::shared_ptr> rightInfo = + collect("rgbd_image/depth/camera_info"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_image", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(right->subscription)); + + const rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0); + pub->publish(in); + ASSERT_TRUE(spinUntil([&]() { return !right->empty() && !rightInfo->empty(); })); + + EXPECT_EQ(right->back().encoding, "mono8") << "the right image is forwarded as-is"; + EXPECT_EQ(right->back().data, in.depth.data); + EXPECT_LT(rightInfo->back().p[3], 0.0) << "the baseline must reach the consumer"; +} + +TEST_F(RGBDSplitTest, DecompressesDepthWithTheCorrectEncoding) +{ + // rtabmap compresses depth as a PNG whose format string cv_bridge cannot interpret. + // The node must decode it itself and label it 16UC1, not mono8: the buffer is two + // bytes per pixel and a wrong encoding makes every consumer misread it. + addNode(std::make_shared(rclcpp::NodeOptions())); + + std::shared_ptr> depth = + collect("rgbd_image/depth/image"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_image", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(depth->subscription)); + + rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + const cv::Mat original(8, 8, CV_16UC1, cv::Scalar(1500)); + in.depth = sensor_msgs::msg::Image(); + in.depth_compressed.header = in.header; + in.depth_compressed.format = "png"; + in.depth_compressed.data = rtabmap::compressImage(original, ".png"); + + pub->publish(in); + ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); })); + + const sensor_msgs::msg::Image & got = depth->back(); + EXPECT_EQ(got.encoding, sensor_msgs::image_encodings::TYPE_16UC1) + << "a 16-bit depth buffer must not be labeled mono8"; + EXPECT_EQ(got.width, 8u); + EXPECT_EQ(got.height, 8u); + ASSERT_EQ(got.step, 16u) << "two bytes per pixel"; + EXPECT_EQ(*reinterpret_cast(&got.data[0]), 1500) + << "and the values must survive the round trip"; +} + +/// Feeds a compressed right image in @p format and returns what lands on depth/image. +class RGBDSplitRightImageTest : public NodeTest +{ +protected: + sensor_msgs::msg::Image split(cv_bridge::Format format) + { + addNode(std::make_shared(rclcpp::NodeOptions())); + + std::shared_ptr> right = + collect("rgbd_image/depth/image"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_image", 10); + EXPECT_TRUE(waitForSubscriber(pub)); + EXPECT_TRUE(waitForPublisher(right->subscription)); + + rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0); + cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", + cv::Mat(8, 8, CV_8UC1, cv::Scalar(60))) + .toCompressedImageMsg(in.depth_compressed, format); + in.depth = sensor_msgs::msg::Image(); + + pub->publish(in); + EXPECT_TRUE(spinUntil([&]() { return !right->empty(); })) + << "the right image must be decompressed, not rejected"; + return right->empty() ? sensor_msgs::msg::Image() : right->back(); + } +}; + +TEST_F(RGBDSplitRightImageTest, DecompressesAJpegRightImage) +{ + // What stereo_sync emits. + const sensor_msgs::msg::Image got = split(cv_bridge::JPG); + EXPECT_EQ(got.encoding, sensor_msgs::image_encodings::MONO8); + EXPECT_EQ(got.step, 8u) << "one byte per pixel, not mistaken for 16-bit depth"; +} + +TEST_F(RGBDSplitRightImageTest, DecompressesAPngRightImage) +{ + // Nothing forbids a producer from compressing the right image losslessly, and a + // stereo pipeline may prefer it since JPEG artifacts hurt matching. Going by the + // format string alone would send this down the depth path and abort on the assert. + const sensor_msgs::msg::Image got = split(cv_bridge::PNG); + EXPECT_EQ(got.encoding, sensor_msgs::image_encodings::MONO8); + EXPECT_EQ(got.step, 8u); +} + +/// Queue depths and the reach of the qos parameter. +class RGBDSplitQosTest : public NodeTest +{ +protected: + void startSplit(const std::vector & params) + { + addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(params))); + } +}; + +TEST_F(RGBDSplitQosTest, HonorsTheConfiguredQueueDepths) +{ + // Queue depth is not observable from outside, so this pins down that the parameters + // are accepted and the node still splits with them set. + startSplit({rclcpp::Parameter("queue_sub", 20), rclcpp::Parameter("queue_pub", 10)}); + + std::shared_ptr> rgb = + collect("rgbd_image/rgb/image"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_image", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(rgb->subscription)); + + pub->publish(makeRGBDImage("camera_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !rgb->empty(); })); + EXPECT_EQ(rgb->back().encoding, "bgr8"); +} + +TEST_F(RGBDSplitQosTest, RejectsAZeroQueueDepth) +{ + EXPECT_THROW(startSplit({rclcpp::Parameter("queue_pub", 0)}), UException); +} + +TEST_F(RGBDSplitQosTest, AppliesQosToTheCameraInfoPublishersToo) +{ + // A best-effort node must be best effort on every output, camera infos included: + // a reliable consumer must not match any of them. + startSplit({rclcpp::Parameter("qos", 2)}); + + std::shared_ptr> rgbInfo = + collect( + "rgbd_image/rgb/camera_info", rclcpp::QoS(10).reliable()); + std::shared_ptr> depthInfo = + collect( + "rgbd_image/depth/camera_info", rclcpp::QoS(10).reliable()); + spinFor(std::chrono::milliseconds(500)); + + EXPECT_EQ(rgbInfo->subscription->get_publisher_count(), 0u) + << "the rgb camera info publisher ignored qos"; + EXPECT_EQ(depthInfo->subscription->get_publisher_count(), 0u) + << "the depth camera info publisher ignored qos"; +} + +/// Output topic naming, controlled by the stereo parameter. +class RGBDSplitStereoNamingTest : public NodeTest +{ +protected: + void startSplit(bool stereo) + { + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({rclcpp::Parameter("stereo", stereo)}))); + } +}; + +TEST_F(RGBDSplitStereoNamingTest, PublishesOnLeftAndRightWhenStereoIsSet) +{ + startSplit(/*stereo=*/true); + + std::shared_ptr> left = + collect("rgbd_image/left/image"); + std::shared_ptr> right = + collect("rgbd_image/right/image"); + std::shared_ptr> leftInfo = + collect("rgbd_image/left/camera_info"); + std::shared_ptr> rightInfo = + collect("rgbd_image/right/camera_info"); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_image", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(left->subscription)); + ASSERT_TRUE(waitForPublisher(right->subscription)); + + const rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0); + pub->publish(in); + ASSERT_TRUE(spinUntil([&]() { + return !left->empty() && !right->empty() && !leftInfo->empty() && !rightInfo->empty(); + })) << "not all four outputs were published"; + + EXPECT_EQ(left->back().data, in.rgb.data) << "the rgb slot feeds the left topic"; + EXPECT_EQ(right->back().data, in.depth.data) << "the depth slot feeds the right topic"; + EXPECT_LT(rightInfo->back().p[3], 0.0) << "the baseline must reach the right camera info"; +} + +TEST_F(RGBDSplitStereoNamingTest, DoesNotPublishOnRgbAndDepthWhenStereoIsSet) +{ + // The two namings are exclusive: nothing must be left publishing the old names. + startSplit(/*stereo=*/true); + + std::shared_ptr> rgb = + collect("rgbd_image/rgb/image"); + std::shared_ptr> depth = + collect("rgbd_image/depth/image"); + spinFor(std::chrono::milliseconds(500)); + + EXPECT_EQ(rgb->subscription->get_publisher_count(), 0u); + EXPECT_EQ(depth->subscription->get_publisher_count(), 0u); +} + +TEST_F(RGBDSplitStereoNamingTest, KeepsRgbAndDepthByDefault) +{ + startSplit(/*stereo=*/false); + + std::shared_ptr> rgb = + collect("rgbd_image/rgb/image"); + std::shared_ptr> left = + collect("rgbd_image/left/image"); + ASSERT_TRUE(waitForPublisher(rgb->subscription)); + EXPECT_EQ(left->subscription->get_publisher_count(), 0u) + << "left/right naming must be opt-in"; +} + +TEST_F(RGBDSplitStereoNamingTest, StillPublishesADepthImageOnRightWithStereoSet) +{ + // A depth image with stereo set is a misconfiguration: the node warns (once) but + // keeps forwarding, so an existing pipeline is never silently broken. + startSplit(/*stereo=*/true); + + std::shared_ptr> right = + collect("rgbd_image/right/image"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_image", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(right->subscription)); + + // makeRGBDImage carries 16UC1 depth, not a right image. + pub->publish(makeRGBDImage("camera_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !right->empty(); })) + << "the image must still be forwarded, warning or not"; + EXPECT_EQ(right->back().encoding, sensor_msgs::image_encodings::TYPE_16UC1); +} + +TEST_F(RGBDSplitStereoNamingTest, StillPublishesARightImageOnDepthWithStereoUnset) +{ + // The inverse misconfiguration, and the one this node has always allowed: a stereo + // pair with stereo left false. It warns, but the right image must still come out. + startSplit(/*stereo=*/false); + + std::shared_ptr> depth = + collect("rgbd_image/depth/image"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_image", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(depth->subscription)); + + const rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0); + pub->publish(in); + ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); })) + << "the right image must still be forwarded, warning or not"; + EXPECT_EQ(depth->back().encoding, "mono8"); + EXPECT_EQ(depth->back().data, in.depth.data); +} + +TEST_F(RGBDSplitStereoNamingTest, DoesNotWarnOnAnEmptySecondHalf) +{ + // A color-only RGBDImage leaves the depth slot empty, whose encoding is "". That + // must not be mistaken for a right image: nothing is published, nothing to warn about. + startSplit(/*stereo=*/false); + + std::shared_ptr> rgb = + collect("rgbd_image/rgb/image"); + std::shared_ptr> depth = + collect("rgbd_image/depth/image"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_image", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(rgb->subscription)); + + rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + in.depth = sensor_msgs::msg::Image(); + pub->publish(in); + ASSERT_TRUE(spinUntil([&]() { return !rgb->empty(); })); + spinFor(std::chrono::milliseconds(300)); + + EXPECT_EQ(rgb->back().encoding, "bgr8") << "the color half is unaffected"; + if(!depth->empty()) + { + EXPECT_TRUE(depth->back().data.empty()) + << "an absent depth image must not turn into a non-empty one"; + } +} + +TEST_F(RGBDSplitQosTest, QosSubAndQosPubOverrideQosPerSide) +{ + // qos says reliable, which a best-effort source could not match; qos_sub overrides + // it, while qos_pub keeps the outputs reliable for a strict consumer. + startSplit({rclcpp::Parameter("qos", 1), + rclcpp::Parameter("qos_sub", 2), + rclcpp::Parameter("qos_pub", 1)}); + + std::shared_ptr> rgb = + collect("rgbd_image/rgb/image", rclcpp::QoS(10).reliable()); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher( + "rgbd_image", rclcpp::QoS(10).best_effort()); + ASSERT_TRUE(waitForSubscriber(pub)) << "qos_sub must win over qos on the subscription"; + ASSERT_TRUE(waitForPublisher(rgb->subscription)) << "qos_pub must keep the output reliable"; + + pub->publish(makeRGBDImage("camera_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !rgb->empty(); })); + EXPECT_EQ(rgb->back().encoding, "bgr8"); +} diff --git a/tools/set_doc_distro.sh b/tools/set_doc_distro.sh new file mode 100755 index 00000000..7e9c07b9 --- /dev/null +++ b/tools/set_doc_distro.sh @@ -0,0 +1,52 @@ +#!/usr/bin/env bash +# +# Rewrites the ROS distro in every docs.ros.org link of the documentation pages. +# +# The distro cannot be resolved at documentation build time: rosdoc2 has no notion of one +# and injects nothing a Markdown page could read, and docs.ros.org has no distro-agnostic +# URL for message types. So the distro is baked into the links, and this script is how it +# gets flipped on a per-distro branch. +# +# Usage: +# tools/set_doc_distro.sh jazzy [path ...] +# +# Rewrites every *.md under the given paths, defaulting to the whole repository. Note that +# it cannot tell a link meant to track the branch from one that deliberately names a +# distro -- the root README's "Humble minimum required" note, for instance -- so pass +# explicit paths when that matters. +# +# On a distro branch, treat the documentation as *derived* rather than hand-edited, and +# the merge from the development branch never conflicts: +# +# git merge ros2 +# git checkout ros2 -- '*/doc' '*/README.md' # always take the upstream pages +# tools/set_doc_distro.sh humble # then re-stamp the distro +# git commit -a +# +set -euo pipefail + +distro=${1:-} +if [ -z "$distro" ]; then + echo "usage: $(basename "$0") e.g. $(basename "$0") jazzy" >&2 + exit 1 +fi + +# An explicit list rather than a wildcard: docs.ros.org also serves /en/api/, which is the +# legacy ROS 1 message documentation and must not be rewritten into a distro. +known='humble|iron|jazzy|kilted|lyrical|rolling' + +root=$(cd "$(dirname "$0")/.." && pwd) +shift || true +paths=("$@") +[ ${#paths[@]} -eq 0 ] && paths=("$root") + +mapfile -t files < <(grep -rlE "docs\.ros\.org/en/($known)/" --include='*.md' "${paths[@]}") + +if [ ${#files[@]} -eq 0 ]; then + echo "no documentation pages with a distro link found" >&2 + exit 0 +fi + +sed -i -E "s#docs\.ros\.org/en/($known)/#docs.ros.org/en/$distro/#g" "${files[@]}" +echo "set distro to '$distro' in ${#files[@]} file(s):" +printf ' %s\n' "${files[@]#$root/}" From 5062bf06146b936f8d91cfc7d66b4fca8895b852 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 13 Sep 2026 11:25:32 -0700 Subject: [PATCH 11/17] rtabmap_sync tests and doc (#1454) * rtabmap_sync tests and doc * Added some diagrams * cleanup some diagrams * fixing running tests in parallels --- .github/workflows/coverage.yml | 4 +- .gitignore | 8 +- README.md | 14 +- codecov.yml | 11 +- docs_build/COLCON_IGNORE | 0 rtabmap_conversions/README.md | 14 - rtabmap_sync/CMakeLists.txt | 40 ++ rtabmap_sync/README.md | 77 +++ rtabmap_sync/doc/rgb_sync.md | 90 +++ rtabmap_sync/doc/rgbd_sync.md | 164 ++++++ rtabmap_sync/doc/rgbdx_sync.md | 145 +++++ rtabmap_sync/doc/stereo_sync.md | 135 +++++ .../rtabmap_sync/CommonDataSubscriber.h | 181 ++++++ .../CommonDataSubscriberDefines.h | 35 +- .../include/rtabmap_sync/GetTopicName.h | 35 -- .../include/rtabmap_sync/SyncDiagnostic.h | 62 ++ .../include/rtabmap_sync/rgb_sync.hpp | 14 + .../include/rtabmap_sync/rgbd_sync.hpp | 15 + .../include/rtabmap_sync/rgbdx_sync.hpp | 10 + .../include/rtabmap_sync/stereo_sync.hpp | 14 + rtabmap_sync/package.xml | 3 + rtabmap_sync/rosdoc2.yaml | 35 ++ rtabmap_sync/src/nodelets/rgb_sync.cpp | 7 +- rtabmap_sync/src/nodelets/rgbd_sync.cpp | 1 + rtabmap_sync/src/nodelets/stereo_sync.cpp | 1 + .../test/common_data_subscriber_fixture.hpp | 205 +++++++ rtabmap_sync/test/msg_builders.hpp | 284 +++++++++ rtabmap_sync/test/node_test_utils.hpp | 208 +++++++ .../test/test_common_data_subscriber.cpp | 315 ++++++++++ .../test/test_common_data_subscriber_sync.cpp | 540 ++++++++++++++++++ rtabmap_sync/test/test_rgb_sync.cpp | 258 +++++++++ rtabmap_sync/test/test_rgbd_sync.cpp | 530 +++++++++++++++++ rtabmap_sync/test/test_rgbdx_sync.cpp | 263 +++++++++ rtabmap_sync/test/test_stereo_sync.cpp | 323 +++++++++++ rtabmap_sync/test/test_sync_diagnostic.cpp | 301 ++++++++++ rtabmap_util/CMakeLists.txt | 11 +- rtabmap_util/README.md | 6 - rtabmap_util/doc/imu_to_tf.md | 46 +- rtabmap_util/doc/map_assembler.md | 17 + rtabmap_util/doc/obstacles_detection.md | 14 + rtabmap_util/doc/point_cloud_aggregator.md | 21 + rtabmap_util/doc/point_cloud_assembler.md | 34 ++ rtabmap_util/doc/pointcloud_to_depthimage.md | 17 + rtabmap_util/doc/rgbd_relay.md | 37 ++ rtabmap_util/doc/rgbd_split.md | 12 + 45 files changed, 4481 insertions(+), 76 deletions(-) create mode 100644 docs_build/COLCON_IGNORE create mode 100644 rtabmap_sync/README.md create mode 100644 rtabmap_sync/doc/rgb_sync.md create mode 100644 rtabmap_sync/doc/rgbd_sync.md create mode 100644 rtabmap_sync/doc/rgbdx_sync.md create mode 100644 rtabmap_sync/doc/stereo_sync.md delete mode 100644 rtabmap_sync/include/rtabmap_sync/GetTopicName.h create mode 100644 rtabmap_sync/rosdoc2.yaml create mode 100644 rtabmap_sync/test/common_data_subscriber_fixture.hpp create mode 100644 rtabmap_sync/test/msg_builders.hpp create mode 100644 rtabmap_sync/test/node_test_utils.hpp create mode 100644 rtabmap_sync/test/test_common_data_subscriber.cpp create mode 100644 rtabmap_sync/test/test_common_data_subscriber_sync.cpp create mode 100644 rtabmap_sync/test/test_rgb_sync.cpp create mode 100644 rtabmap_sync/test/test_rgbd_sync.cpp create mode 100644 rtabmap_sync/test/test_rgbdx_sync.cpp create mode 100644 rtabmap_sync/test/test_stereo_sync.cpp create mode 100644 rtabmap_sync/test/test_sync_diagnostic.cpp diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml index dd22c6ed..2ade89c0 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 + package-name: rtabmap_conversions rtabmap_util rtabmap_sync 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. @@ -115,7 +115,7 @@ jobs: # `source` does not exist. GitHub picks sh whenever it cannot find # bash in the image's PATH, and says so in the log ("shell: sh -e"). . /opt/ros/humble/setup.sh - PKGS="rtabmap_conversions rtabmap_util" + PKGS="rtabmap_conversions rtabmap_util rtabmap_sync" # 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. diff --git a/.gitignore b/.gitignore index 5d2ec8fa..24f04474 100644 --- a/.gitignore +++ b/.gitignore @@ -2,7 +2,11 @@ .settings .vscode __pycache__ -# rosdoc2 build artifacts -docs_build +# rosdoc2 build artifacts. docs_build/ holds a copy of each package manifest, so +# colcon would otherwise see two packages of every name and fail with "Duplicate +# package names not supported" -- hence the COLCON_IGNORE, which is committed so +# nobody has to know that. rosdoc2 leaves an existing marker in place. +docs_build/* +!docs_build/COLCON_IGNORE cross_reference doc_output diff --git a/README.md b/README.md index eaee4560..0be9987b 100644 --- a/README.md +++ b/README.md @@ -37,7 +37,7 @@ The stack is split into small packages so a pipeline only pulls in what it uses. |---|---| | `rtabmap_slam` | The `rtabmap` node itself: appearance-based loop closure detection, graph optimization, memory management and map assembly. | | `rtabmap_odom` | Odometry nodes — `rgbd_odometry`, `stereo_odometry` and `icp_odometry`. Any external odometry can be used instead. | -| `rtabmap_sync` | Synchronizes camera and lidar topics into a single message so they reach the SLAM node together — `rgbd_sync`, `stereo_sync`, `rgbdx_sync`. | +| [`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`. | ### Sensor processing @@ -133,6 +133,18 @@ export CYCLONEDDS_URI="0.0.0.0" * **Papers and videos** — [introlab.github.io/rtabmap](https://introlab.github.io/rtabmap/). * **Old tutorials** — the [ROS 1 wiki](http://wiki.ros.org/rtabmap_ros/Tutorials), for anything not covered above; parameters and topic names are unchanged. +## Building the documentation + +Each package's API reference is generated with [rosdoc2](https://github.com/ros-infrastructure/rosdoc2) from the Doxygen comments in its public headers, and published to docs.ros.org. rosdoc2 documents one package per invocation, so building the whole stack is a loop over them — run it from the repository root: + +```bash +for pkg in rtabmap_*/; do + rosdoc2 build --package-path "$pkg" --output-directory doc_output || break +done +``` + +Each package lands in `doc_output//index.html`. + # License BSD-3-Clause, see [LICENSE](LICENSE). RTAB-Map itself may be built with components under other licenses; see the [rtabmap](https://github.com/introlab/rtabmap) repository. diff --git a/codecov.yml b/codecov.yml index 4fec39d4..a826350c 100644 --- a/codecov.yml +++ b/codecov.yml @@ -52,15 +52,19 @@ component_management: name: rtabmap_util paths: - rtabmap_util/** + - component_id: rtabmap_sync + name: rtabmap_sync + paths: + - rtabmap_sync/** comment: layout: "condensed_header, diff, components, files" behavior: default require_changes: true # stay quiet when coverage doesn't move -# Only rtabmap_conversions and rtabmap_util 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 +# Only rtabmap_conversions, rtabmap_util and rtabmap_sync 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. ignore: - "rtabmap_costmap_plugins/**" @@ -72,6 +76,5 @@ ignore: - "rtabmap_python/**" - "rtabmap_rviz_plugins/**" - "rtabmap_slam/**" - - "rtabmap_sync/**" - "rtabmap_viz/**" - "**/test/**" diff --git a/docs_build/COLCON_IGNORE b/docs_build/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/rtabmap_conversions/README.md b/rtabmap_conversions/README.md index 98265a7b..91e04544 100644 --- a/rtabmap_conversions/README.md +++ b/rtabmap_conversions/README.md @@ -64,20 +64,6 @@ colcon test --packages-select rtabmap_conversions colcon test-result --verbose ``` -## Documentation - -API documentation is generated with [rosdoc2](https://github.com/ros-infrastructure/rosdoc2) from the Doxygen comments in the public header, and published to [docs.ros.org](https://docs.ros.org/en/jazzy/p/rtabmap_conversions/). To build it locally: - -```bash -rosdoc2 build --package-path rtabmap_conversions --output-directory doc_output -``` - -Besides `doc_output`, rosdoc2 writes `docs_build/` and `cross_reference/` scratch directories into the current directory. `docs_build/` contains a copy of the package manifest, so colcon then sees two packages of the same name and every later build fails with `Duplicate package names not supported`. Mark it once and the problem goes away for good — rosdoc2 leaves an existing marker in place on subsequent runs: - -```bash -touch docs_build/COLCON_IGNORE -``` - ## License BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license). diff --git a/rtabmap_sync/CMakeLists.txt b/rtabmap_sync/CMakeLists.txt index ceda4040..5e1b4da8 100644 --- a/rtabmap_sync/CMakeLists.txt +++ b/rtabmap_sync/CMakeLists.txt @@ -197,4 +197,44 @@ install(DIRECTORY include/ FILES_MATCHING PATTERN "*.h" ) +############# +## Testing ## +############# +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(RTABMap REQUIRED) + + # Each node gets its own test binary: a crash or a stuck executor in one node cannot + # take the others down, and every binary starts with a clean DDS graph. + # + # 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 -- rgb/image, + # rgbd_image, odom -- with rtabmap_util's. On a shared domain they discover each other's + # publishers, and assertions then see traffic the test never sent. rtabmap_util numbers + # its own from 30; keep the two ranges apart. + set(rtabmap_sync_test_domain_id 50) + macro(rtabmap_sync_add_node_test test_name) + ament_add_gtest(${test_name} test/${test_name}.cpp + ENV ROS_DOMAIN_ID=${rtabmap_sync_test_domain_id}) + math(EXPR rtabmap_sync_test_domain_id "${rtabmap_sync_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_sync_plugins rtabmap_sync) + if("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(${test_name} ${AmentLibraries}) + else() + target_link_libraries(${test_name} ${Libraries} ${PublicLibraries}) + endif() + endif() + endmacro() + + rtabmap_sync_add_node_test(test_rgbd_sync) + rtabmap_sync_add_node_test(test_rgb_sync) + rtabmap_sync_add_node_test(test_stereo_sync) + rtabmap_sync_add_node_test(test_rgbdx_sync) + rtabmap_sync_add_node_test(test_common_data_subscriber) + rtabmap_sync_add_node_test(test_common_data_subscriber_sync) + rtabmap_sync_add_node_test(test_sync_diagnostic) +endif() + ament_package(CONFIG_EXTRAS ${CMAKE_CURRENT_BINARY_DIR}/cmake/extra_configs.cmake) diff --git a/rtabmap_sync/README.md b/rtabmap_sync/README.md new file mode 100644 index 00000000..04ddb644 --- /dev/null +++ b/rtabmap_sync/README.md @@ -0,0 +1,77 @@ +# rtabmap_sync + +Synchronization of the sensor topics [RTAB-Map](https://github.com/introlab/rtabmap) consumes. + +A SLAM node needs a camera's color image, its depth image and its calibration as one measurement, not as three topics that happen to be arriving. This package does that matching — once, in one place — and offers it in two forms: standalone nodes that pack a camera into a single [`RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html), and a base class that the consuming nodes subscribe through. + +Every node is a [composable node](https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Composition.html) as well as a standalone executable. **Compose these into the camera driver's process where you can**: they copy every pixel of every frame, and across a process boundary that copy is a serialization plus a memcpy per image. + +## Nodes + +One page per node. + +| Node | Description | +|---|---| +| [rgbd_sync](doc/rgbd_sync.md) | Color + depth + calibration → one `RGBDImage`. | +| [stereo_sync](doc/stereo_sync.md) | Left + right + two calibrations → one `RGBDImage`. | +| [rgb_sync](doc/rgb_sync.md) | Color + calibration → one `RGBDImage`, with no depth. | +| [rgbdx_sync](doc/rgbdx_sync.md) | 2 to 8 `RGBDImage` topics → one `RGBDImages`. | + +## Library + +The package also installs a C++ library, whose API is documented in the [C++ API reference](https://docs.ros.org/en/jazzy/p/rtabmap_sync/generated/index.html) generated from the headers. + +**`CommonDataSubscriber`** is the piece worth knowing about. RTAB-Map can be fed in a dozen shapes — RGB-D, stereo, RGB-only, one or several `RGBDImage`s, a 2D or 3D scan, a whole `SensorData` — each optionally alongside odometry, an `OdomInfo` and user data. Every combination needs its own `message_filters` synchronizer, so a node that wired them by hand would be mostly synchronizer boilerplate. This class owns all of them: it reads the `subscribe_*` parameters, builds the one synchronizer that matches, and calls back with a uniform set of arguments whichever inputs were used. + +[`rtabmap_slam`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_slam)'s `rtabmap` node and [`rtabmap_viz`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_viz) both derive from it, which is why they take identical input topics and parameters. If you are looking for where `subscribe_depth` or `rgbd_cameras` is implemented, it is here rather than in those packages. + +**`SyncDiagnostic`** is what every node here reports through. It watches two rates — messages going into a synchronizer, and messages coming out — because a node can be receiving everything it asked for and still publish nothing. One camera lagging is enough to stop a synchronizer emitting, and only the pair of rates tells that apart from a camera that went silent. + +## Conventions + +A few things recur across these nodes, and across the `subscribe_*` interface of `rtabmap` and `rtabmap_viz`. + +**`approx_sync`.** Inputs are either matched by nearest stamp or required to carry identical ones. **Prefer the exact policy wherever the sensor allows it**: it is cheaper and cannot mismatch. Its failure mode is unforgiving, though — stamps a nanosecond apart mean **nothing is ever published and nothing says why**, which is the most common reason a pipeline built on this package is silent. + +Which one is the default follows the sensor. `stereo_sync` and `rgb_sync` default to exact, because a stereo pair is hardware-triggered and a camera publisher sends the image and its `camera_info` together. `rgbd_sync` and `rgbdx_sync` default to approximate, for backward compatibility with the many RGB-D cameras that do not stamp color and depth identically. + +**`approx_sync_max_interval`.** Approximate matching pairs *whatever it has* if that is the best available, so a camera that stalls and resumes produces one pairing of a fresh frame with a stale one, silently. This rejects a set spanning more than a given number of seconds. It defaults to `0` (disabled), and it is worth setting — roughly a tenth of the frame period. + +**`qos`.** An integer selecting the reliability of the subscriptions: `0` system default, `1` reliable, `2` best effort. It has to be compatible with the publisher or **no messages arrive at all**, with nothing said. Sensor drivers commonly publish images best effort and `camera_info` reliable, which is why `qos_camera_info` can be set apart from `qos`. + +**`topic_queue_size` and `sync_queue_size`.** The first is the depth of each individual subscription, the second the depth of the synchronizer's own buffer. Raise `sync_queue_size` when inputs arrive at different rates or with different delays; raise `topic_queue_size` when one input arrives in bursts. The older `queue_size` parameter is deprecated and copied into `sync_queue_size`. + +**Compressed output.** `rgbd_sync`, `stereo_sync` and `rgb_sync` each publish a second topic carrying the same frame with compressed images, for sending over a slow link. Color is JPEG; depth is PNG, because JPEG artifacts in a depth image are not blur, they are invented geometry. Neither output is produced unless it has a subscriber, and `compressed_rate` caps the compressed one without touching the raw one. + +## Build options + +Two synchronizer families are behind CMake options, off by default, because each multiplies the number of templates the package instantiates — and so its build time and its binary size. + +| Option | Default | Effect | +|---|---|---| +| `RTABMAP_SYNC_MULTI_RGBD` | `OFF` | Lets a `CommonDataSubscriber` consumer synchronize 2 to 6 `RGBDImage` topics itself (`rgbd_cameras` > 1). | +| `RTABMAP_SYNC_USER_DATA` | `OFF` | Lets `subscribe_user_data` add a `UserData` topic to any of the combinations. | + +```bash +colcon build --packages-select rtabmap_sync --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON +``` + +For several cameras, **prefer turning `RTABMAP_SYNC_MULTI_RGBD` on**. The consumer then subscribes to each camera's `RGBDImage` directly and synchronizes them itself, which is one node and one full-frame copy per camera per frame less than routing everything through [rgbdx_sync](doc/rgbdx_sync.md) on the way in. + +`rgbdx_sync` is the route that needs no rebuild — against binary packages, say — and the only one that goes past 6 cameras. See [Feeding it to rtabmap](doc/rgbdx_sync.md#feeding-it-to-rtabmap). + +Without the options, asking for either is refused rather than ignored quietly — `subscribe_user_data` is reset to false with an error, and `rgbd_cameras` > 1 leaves nothing subscribed and says so. + +## Building and testing + +```bash +colcon build --packages-select rtabmap_sync +colcon test --packages-select rtabmap_sync +colcon test-result --verbose +``` + +The tests drive each node over real ROS topics inside the gtest binary — no launch files and no separate processes — so they also serve as worked examples of each node's topics and parameters. + +## License + +BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license). diff --git a/rtabmap_sync/doc/rgb_sync.md b/rtabmap_sync/doc/rgb_sync.md new file mode 100644 index 00000000..da5c157f --- /dev/null +++ b/rtabmap_sync/doc/rgb_sync.md @@ -0,0 +1,90 @@ +# rgb_sync + +Groups a camera's color image and calibration into a single [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html), with no depth. + +The monocular counterpart of [rgbd_sync](rgbd_sync.md). It exists for pipelines that have no depth to offer: a single camera doing appearance-based loop closure detection and relocalization against a map built earlier, where images are used to recognize places rather than to reconstruct them. + +Without depth, RTAB-Map cannot build a metric map from these frames alone. It can still detect that a place has been seen before, which is enough for relocalization in an existing map and for adding loop closure constraints to a graph whose geometry comes from odometry or a lidar. + +The camera cannot supply a pose here — visual odometry needs depth or a stereo baseline — so the pose has to come from somewhere else: + +```mermaid +flowchart LR + CAM["camera driver"] + SYNC["rgb_sync"] + ODOM["odometry source
wheel, lidar or external"] + MAP["rtabmap"] + CAM -->|rgb/image| SYNC + CAM -->|rgb/camera_info| SYNC + SYNC -->|rgbd_image| MAP + ODOM -->|odometry| MAP +``` + +## Usage + +```bash +ros2 run rtabmap_sync rgb_sync --ros-args \ + -r rgb/image:=/camera/image_raw \ + -r rgb/camera_info:=/camera/camera_info +``` + +```python +ComposableNode( + package='rtabmap_sync', + plugin='rtabmap_sync::RGBSync', + name='rgb_sync', + remappings=[('rgb/image', '/camera/image_raw'), + ('rgb/camera_info', '/camera/camera_info')]) +``` + +## Subscribed Topics + +| Topic | Type | Description | +|---|---|---| +| `rgb/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Color image. Goes through `image_transport`. | +| `rgb/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Calibration of the camera. | + +## Published Topics + +| Topic | Type | Description | +|---|---|---| +| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The image and its calibration. The depth slot is left empty unless `fill_empty_depth`. Published only when someone is subscribed. | +| `rgbd_image/compressed` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The same frame with the image as JPEG. Published only when someone is subscribed. | + +The output's `header.frame_id` comes from the camera_info; its `header.stamp` is the image's. + +## Parameters + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `approx_sync` | `bool` | `false` | Match the image and its calibration by nearest stamp. Defaults to **exact**; see [Synchronization](#synchronization). | +| `approx_sync_max_interval` | `double` | `0.0` | Reject pairs spanning more than this many seconds. `0` disables. | +| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. | +| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. | +| `queue_size` | `int` | — | **Deprecated**, renamed to `sync_queue_size`. | +| `qos` | `int` | `0` | Reliability of the subscription and the publishers: `0` system default, `1` reliable, `2` best effort. | +| `qos_camera_info` | `int` | value of `qos` | Reliability of the `rgb/camera_info` subscription alone. | +| `fill_empty_depth` | `bool` | `false` | Add an all-zero depth image the size of the color one. See below. | +| `compressed_rate` | `double` | `0.0` | Maximum rate, in Hz, of `rgbd_image/compressed`. `0` means every frame. | +| `image_transport` | `string` | `"raw"` | Transport for `rgb/image`, e.g. `compressed`. | + +## fill_empty_depth + +By default the output carries no depth image and no depth calibration, which is how a consumer tells "this camera has no depth" from "this frame's depth happens to be all zeros". + +Some consumers refuse a message without one. `fill_empty_depth` gives them a depth image of the right size and encoding (`16UC1`) filled with zeros, registered to the color camera and sharing its calibration. Zero in a depth image means *no reading*, so the frame still carries no geometry — the flag changes the shape of the message, not its content. Leave it off unless something downstream requires it. + +## Synchronization + +`approx_sync` defaults to **false** here. A driver built on `image_transport`'s camera publisher sends the image and its `camera_info` as a pair carrying the same stamp, so there is nothing to approximate: the exact policy is cheaper and cannot mismatch. + +Set `approx_sync:=true` when the two do not share a stamp — a `camera_info` republished on its own timer, or read from a YAML file and stamped with the current time. That is the case to watch for if the node is silent: the calibration values are constant and look fine, but their stamps never match an image. + +```bash +ros2 topic echo --once /camera/image_raw --field header.stamp +ros2 topic echo --once /camera/camera_info --field header.stamp +``` + +## Diagnostics + +The node publishes to `/diagnostics` — input rate, output rate, and a warning in the log every 5 seconds while nothing is arriving. diff --git a/rtabmap_sync/doc/rgbd_sync.md b/rtabmap_sync/doc/rgbd_sync.md new file mode 100644 index 00000000..cf1f3260 --- /dev/null +++ b/rtabmap_sync/doc/rgbd_sync.md @@ -0,0 +1,164 @@ +# rgbd_sync + +Groups an RGB-D camera's color image, depth image and calibration into a single [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html). + +A camera driver publishes three topics that only mean anything together. Keeping them together as one message is worth doing for its own sake — one topic to remap, one topic to record, and no chance of a bag holding a depth frame whose color frame was dropped — but the reason this node exists is that the synchronization has to happen *somewhere*, and doing it once here is cheaper than doing it again in every consumer. + +Doing it once also keeps the consumers *consistent*. A pipeline usually runs [`rgbd_odometry`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_odom) and `rtabmap` — often `rtabmap_viz` too — over the same camera. Given the three raw topics, each of those nodes synchronizes them independently, and with approximate matching they can settle on different pairings. `rtabmap` then maps a color/depth pair that odometry never saw, at a pose computed from a different one. + +**Without `rgbd_sync`** — each consumer matches the three topics for itself, with its own synchronizer: + +```mermaid +flowchart LR + CAM["camera driver"] + ODOM["rgbd_odometry
sync A"] + MAP["rtabmap
sync B"] + CAM -->|rgb/image| ODOM & MAP + CAM -->|depth/image| ODOM & MAP + CAM -->|rgb/camera_info| ODOM & MAP + ODOM -->|odometry| MAP +``` + +**With `rgbd_sync`** — matched once, then fanned out: + +```mermaid +flowchart LR + CAM["camera driver"] + SYNC["rgbd_sync"] + ODOM["rgbd_odometry"] + ODOMT(["odometry"]) + MAP["rtabmap"] + VIZ["rtabmap_viz"] + CAM -->|rgb/image| SYNC + CAM -->|depth/image| SYNC + CAM -->|rgb/camera_info| SYNC + SYNC -->|rgbd_image| ODOM & MAP & VIZ + ODOM --> ODOMT + ODOMT --> MAP & VIZ +``` + +The same holds when the pose comes from elsewhere — a wheel encoder, a lidar, or an external VIO. The camera then feeds only the mapping side, but every node on it still sees the identical frame: + +```mermaid +flowchart LR + CAM["camera driver"] + SYNC["rgbd_sync"] + ODOM["odometry source
wheel, lidar or external"] + ODOMT(["odometry"]) + MAP["rtabmap"] + VIZ["rtabmap_viz"] + CAM -->|rgb/image| SYNC + CAM -->|depth/image| SYNC + CAM -->|rgb/camera_info| SYNC + SYNC -->|rgbd_image| MAP & VIZ + ODOM --> ODOMT + ODOMT --> MAP & VIZ +``` + +Subscribe them all to one `RGBDImage` and the question does not arise: every node processes the identical message. + +It also gives a pipeline one place to synchronize. A consumer that has to match a camera against something on a different rate — a lidar, an IMU, odometry — matches one `RGBDImage` against them rather than three topics plus the others all at once. Synchronizing a large set in one go is the harder problem: the policy has to find a window that satisfies every input, and the more inputs with different rates and delays, the more often it settles for a poor match or none at all. Resolving the camera first, where the three topics are tightly correlated, leaves the downstream synchronizer a much easier job. + +It can also decimate the images, rescale depth into the unit RTAB-Map expects, and publish a compressed copy for a slow link. See [Compressing for a slow link](#compressing-for-a-slow-link). + +For a monocular camera use [rgb_sync](rgb_sync.md); for a stereo pair, [stereo_sync](stereo_sync.md); for several RGB-D cameras, one of these per camera feeding [rgbdx_sync](rgbdx_sync.md). + +## Usage + +```bash +ros2 run rtabmap_sync rgbd_sync --ros-args \ + -r rgb/image:=/camera/color/image_raw \ + -r depth/image:=/camera/depth/image_rect_raw \ + -r rgb/camera_info:=/camera/color/camera_info \ + -p approx_sync:=true +``` + +```python +ComposableNode( + package='rtabmap_sync', + plugin='rtabmap_sync::RGBDSync', + name='rgbd_sync', + parameters=[{'approx_sync': True}], + remappings=[('rgb/image', '/camera/color/image_raw'), + ('depth/image', '/camera/depth/image_rect_raw'), + ('rgb/camera_info', '/camera/color/camera_info')]) +``` + +**Compose it into the driver's process.** This node copies every pixel of every frame; across a process boundary that copy is a serialization and a memcpy per image, which on a 720p RGB-D stream is real CPU. In the same process with an intra-process-capable driver it is a pointer. + +## Subscribed Topics + +| Topic | Type | Description | +|---|---|---| +| `rgb/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Color image. Goes through `image_transport`, so `rgb/image/compressed` is used instead when `image_transport` is set. | +| `depth/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Depth image, registered to the color camera. `16UC1` in millimeters or `32FC1` in meters. Goes through `image_transport` under `depth_transport`. | +| `rgb/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Calibration of the color camera. Copied into both calibration slots of the output — depth is assumed registered. | + +## Published Topics + +| Topic | Type | Description | +|---|---|---| +| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The three inputs, raw. Published only when someone is subscribed. | +| `rgbd_image/compressed` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The same frame with JPEG color and PNG depth instead of raw images. Published only when someone is subscribed. | + +The output's `header.frame_id` is taken from the **camera_info**, which is the frame the calibration is expressed in — so make sure the images and the `camera_info` carry the same `frame_id`. If they disagree, the output is labelled with the calibration's frame while the pixels were measured in another, and every point projected out of them lands somewhere else. + +The output's `header.stamp` is the **later** of the color and depth stamps, so the message is never stamped before data it contains. + +## Parameters + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `approx_sync` | `bool` | `true` | Match the inputs by nearest stamp. **Set `false` if your camera allows it** — see [Synchronization](#synchronization). | +| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. Worth setting; see [Synchronization](#synchronization). | +| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. | +| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. | +| `queue_size` | `int` | — | **Deprecated**, renamed to `sync_queue_size`. Still copied to it, with a warning. | +| `qos` | `int` | `0` | Reliability of the subscriptions and the publishers: `0` system default, `1` reliable, `2` best effort. | +| `qos_camera_info` | `int` | value of `qos` | Reliability of the `rgb/camera_info` subscription alone. Drivers often publish images best effort and `camera_info` reliable. | +| `depth_scale` | `double` | `1.0` | Multiplies every depth pixel. See [Depth units](#depth-units). | +| `decimation` | `int` | `1` | Downsample both images by this factor, scaling the calibration to match. Must divide the depth image size exactly, or it is ignored. | +| `compressed_rate` | `double` | `0.0` | Maximum rate, in Hz, of `rgbd_image/compressed`. `0` means every frame. Does not affect `rgbd_image`. | +| `image_transport` | `string` | `"raw"` | Transport for `rgb/image`, e.g. `compressed`. | +| `depth_transport` | `string` | `"raw"` | Transport for `depth/image`, e.g. `compressedDepth`. | +| `rgb_image_transport` | `string` | — | **Deprecated**, renamed to `image_transport`. | +| `depth_image_transport` | `string` | — | **Deprecated**, renamed to `depth_transport`. | + +## Synchronization + +**Use `approx_sync:=false` when your camera allows it.** The exact policy is cheaper and cannot mismatch a color frame with the wrong depth frame. The catch is that it is all-or-nothing: if the stamps differ by even a nanosecond, **nothing is ever published**, with no error. That is the single most common reason a pipeline built on this node is silent, so check before switching: + +```bash +ros2 topic echo --once /camera/color/image_raw --field header.stamp +ros2 topic echo --once /camera/depth/image_rect_raw --field header.stamp +``` + +The default is nevertheless approximate, for backward compatibility: many RGB-D cameras do not stamp color and depth identically, the two sensors being read at slightly different instants. A stereo pair is normally hardware-triggered instead, which is why [stereo_sync](stereo_sync.md) defaults the other way. + +When you do stay on approximate matching, note that it pairs *whatever it has* if that is the best available. A camera that stalls for a second and resumes produces one pairing of a fresh frame with a second-old one, and nothing says so. `approx_sync_max_interval` is the guard: a set spanning more than that many seconds is dropped instead. **Set it.** A tenth of the frame period is a reasonable starting point — `0.003` for a 30 Hz camera. + +Leaving it at `0` is also what enables the warning about a large stamp difference in the log; setting it suppresses that warning. + +## Depth units + +RTAB-Map reads `16UC1` depth as millimeters and `32FC1` as meters. A driver that publishes `16UC1` in some other unit — centimeters, or a raw disparity count — produces a map at the wrong scale, and nothing about it looks broken until you measure something. + +`depth_scale` multiplies every depth pixel on the way through, so a camera publishing centimeters is fixed with `depth_scale:=10.0`. It is applied after decimation and before compression, so both outputs carry the corrected values. + +## Compressing for a slow link + +`rgbd_image/compressed` carries the same frame with the color image as **JPEG** and the depth image as **PNG**. Depth stays lossless deliberately: JPEG artifacts in a depth image are not blur, they are invented geometry. + +Neither output is produced unless it has a subscriber, so the compression costs nothing until something subscribes. + +`compressed_rate` caps the compressed topic's rate without touching the raw one — for a robot that maps locally at full rate while sending a few frames a second to an operator. + +## Decimation + +`decimation` halves (or thirds, …) both images and scales the calibration with them, which is the part that is easy to get wrong by hand: an image downsampled without its focal length being scaled produces a point cloud with the wrong field of view. + +The factor must divide the **depth** image size exactly. If it does not, the node logs a warning and stops decimating rather than resampling depth in a way that would misalign it against color. A value below 1 is treated as 1. + +## Diagnostics + +The node publishes to `/diagnostics`: the rate of the incoming color frames, the rate of the published messages, and a warning in the log every 5 seconds while nothing is arriving at all. If the input rate is healthy and the output rate is not, the inputs are arriving but not pairing — look at `approx_sync` and the stamps first. diff --git a/rtabmap_sync/doc/rgbdx_sync.md b/rtabmap_sync/doc/rgbdx_sync.md new file mode 100644 index 00000000..e6db4c04 --- /dev/null +++ b/rtabmap_sync/doc/rgbdx_sync.md @@ -0,0 +1,145 @@ +# rgbdx_sync + +Groups the [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) topics of 2 to 8 cameras into a single [`rtabmap_msgs/msg/RGBDImages`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImages.html). + +For a robot carrying several RGB-D cameras. Each camera gets its own [rgbd_sync](rgbd_sync.md) (or [stereo_sync](stereo_sync.md)), and this node synchronizes those outputs into one message so that the SLAM node sees all of them as one measurement. + +**Consider the alternative first.** Rebuilt with `RTABMAP_SYNC_MULTI_RGBD=ON`, the SLAM node subscribes to each camera's `RGBDImage` and synchronizes them itself, with no node in between — one process and one full-frame copy per camera per frame less than passing through here. This node exists for the cases that rule that out: running against binary packages, or more than the 6 cameras the build option supports. See [Feeding it to rtabmap](#feeding-it-to-rtabmap). + +The reason it is a build option at all is that each supported camera count is a separate synchronizer template, and instantiating them all costs build time and binary size. + +This node only groups: it never touches the images, the calibrations or the individual stamps. + +Each camera is packed by its own [rgbd_sync](rgbd_sync.md) first, and this node groups those into the one message the consumers subscribe to: + +```mermaid +flowchart LR + CAM0["camera 0 driver"] + CAM1["camera 1 driver"] + SYNC0["rgbd_sync"] + SYNC1["rgbd_sync"] + XSYNC["rgbdx_sync"] + ODOM["rgbd_odometry"] + ODOMT(["odometry"]) + MAP["rtabmap"] + VIZ["rtabmap_viz"] + CAM0 -->|"rgb, depth,
camera_info"| SYNC0 + CAM1 -->|"rgb, depth,
camera_info"| SYNC1 + SYNC0 -->|rgbd_image0| XSYNC + SYNC1 -->|rgbd_image1| XSYNC + XSYNC -->|rgbd_images| ODOM & MAP & VIZ + ODOM --> ODOMT + ODOMT --> MAP & VIZ +``` + +**With odometry from elsewhere** — a wheel encoder, a lidar, or an external VIO — the cameras feed only the mapping side: + +```mermaid +flowchart LR + CAM0["camera 0 driver"] + CAM1["camera 1 driver"] + SYNC0["rgbd_sync"] + SYNC1["rgbd_sync"] + XSYNC["rgbdx_sync"] + ODOM["odometry source
wheel, lidar or external"] + ODOMT(["odometry"]) + MAP["rtabmap"] + VIZ["rtabmap_viz"] + CAM0 -->|"rgb, depth,
camera_info"| SYNC0 + CAM1 -->|"rgb, depth,
camera_info"| SYNC1 + SYNC0 -->|rgbd_image0| XSYNC + SYNC1 -->|rgbd_image1| XSYNC + XSYNC -->|rgbd_images| MAP & VIZ + ODOM --> ODOMT + ODOMT --> MAP & VIZ +``` + +## Usage + +```bash +ros2 run rtabmap_sync rgbdx_sync --ros-args \ + -p rgbd_cameras:=3 \ + -r rgbd_image0:=/camera_front/rgbd_image \ + -r rgbd_image1:=/camera_left/rgbd_image \ + -r rgbd_image2:=/camera_right/rgbd_image +``` + +```python +ComposableNode( + package='rtabmap_sync', + plugin='rtabmap_sync::RGBDXSync', + name='rgbdx_sync', + parameters=[{'rgbd_cameras': 3}], + remappings=[('rgbd_image0', '/camera_front/rgbd_image'), + ('rgbd_image1', '/camera_left/rgbd_image'), + ('rgbd_image2', '/camera_right/rgbd_image')]) +``` + +The topics are numbered from **0**, and only the first `rgbd_cameras` of them are subscribed. + +## Subscribed Topics + +| Topic | Type | Description | +|---|---|---| +| `rgbd_image0` … `rgbd_image7` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | One per camera. Only the first `rgbd_cameras` are subscribed. | + +## Published Topics + +| Topic | Type | Description | +|---|---|---| +| `rgbd_images` | [`rtabmap_msgs/msg/RGBDImages`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImages.html) | The set, in topic order. Stamped and framed with `rgbd_image0`'s header; each camera keeps its own header inside the array. | + +Unlike the other nodes in this package, this one publishes whether or not anyone is subscribed — it does no per-frame work worth skipping. + +## Parameters + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `rgbd_cameras` | `int` | `2` | How many cameras to group, 2 to 8. Anything outside that range aborts at start-up. | +| `approx_sync` | `bool` | `true` | Match the cameras by nearest stamp. Set `false` only for hardware-triggered cameras. | +| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. Worth setting; see below. | +| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. | +| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. | +| `queue_size` | `int` | — | **Deprecated**, renamed to `sync_queue_size`. | +| `qos` | `int` | `0` | Reliability of the subscriptions and the publisher: `0` system default, `1` reliable, `2` best effort. | + +`rgbd_cameras` outside 2–8 is a hard error rather than a clamp: one camera needs no grouping at all, and nine cannot be synchronized by any of the templates the node holds. For one camera, subscribe to its `RGBDImage` topic directly. + +## Order matters + +The array is published in topic order — `rgbd_image0` first — and consumers index into it. RTAB-Map matches each image against the calibration and the TF frame it saw at that index, so swapping two remappings places a camera's images at another camera's extrinsics, and the map comes out with the world duplicated at an angle. + +The order is a naming convention, not something the node can check. Keep the numbering consistent with whatever else refers to those cameras. + +## Synchronization + +Separate cameras are rarely triggered together, so `approx_sync` defaults to true. Nothing is published until **every** camera has contributed: a partial set would silently drop one camera's field of view from the map, which is worse than a dropped frame. + +That also makes one silent camera stop the whole node. If `rgbd_images` goes quiet, check each input in turn: + +```bash +ros2 topic hz /camera_front/rgbd_image +``` + +Set `approx_sync_max_interval` here as well. With several free-running cameras the synchronizer has more opportunities to pair a fresh frame with a stale one, and each camera's images are placed in the map using the robot's pose at the *set's* stamp — so a camera whose frame is 200 ms old is placed wherever the robot was not. + +## Feeding it to rtabmap + +Set `rgbd_cameras` to **0** on the consumer, and remap its `rgbd_images` input to this node's output: + +```python +Node( + package='rtabmap_slam', executable='rtabmap', + parameters=[{'subscribe_rgbd': True, 'rgbd_cameras': 0}], + remappings=[('rgbd_images', '/rgbd_images')]) +``` + +`rgbd_cameras:=0` is what selects the `RGBDImages` interface: the count then comes from each message rather than from a parameter, so the same consumer handles any number of cameras without a rebuild. + +With `RTABMAP_SYNC_MULTI_RGBD=ON` instead, drop this node and point the consumer straight at the cameras — `rgbd_cameras:=3` and one remapping per `rgbd_image0`…`rgbd_image2`. Same topics, same order, one hop fewer. + +Either way, every camera needs its extrinsics in TF — a transform from the robot's base frame to each camera's frame, at each frame's stamp. + +## Diagnostics + +The node publishes to `/diagnostics`: the rate of `rgbd_image0`, the rate of published sets, and a warning in the log every 5 seconds while nothing is arriving. Since a set needs every camera, a healthy input rate with no output points at one of the *other* cameras. diff --git a/rtabmap_sync/doc/stereo_sync.md b/rtabmap_sync/doc/stereo_sync.md new file mode 100644 index 00000000..971a34d3 --- /dev/null +++ b/rtabmap_sync/doc/stereo_sync.md @@ -0,0 +1,135 @@ +# stereo_sync + +Groups a stereo pair's four topics — left image, right image and their two calibrations — into a single [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html). + +The same idea as [rgbd_sync](rgbd_sync.md), for a stereo camera: four topics that only mean anything together become one message, synchronized once instead of in every consumer. + +The default topic names say `image_rect` because rectified images are what a stereo pipeline normally carries, and what RTAB-Map assumes by default — but this node does not require it and does not rectify anything itself. Feeding it unrectified images is fine as long as you tell the consumer: set `Rtabmap/ImagesAlreadyRectified` to `false` on the `stereo_odometry` and `rtabmap` nodes, and they rectify from the calibration themselves. Otherwise run [`stereo_image_proc`](https://docs.ros.org/en/jazzy/p/stereo_image_proc/) upstream. + +In a pipeline, the one `RGBDImage` feeds everything downstream — odometry included, so every node works from the same pair: + +```mermaid +flowchart LR + CAM["stereo driver"] + SYNC["stereo_sync"] + ODOM["stereo_odometry"] + ODOMT(["odometry"]) + MAP["rtabmap"] + VIZ["rtabmap_viz"] + CAM -->|left/image_rect| SYNC + CAM -->|right/image_rect| SYNC + CAM -->|left/camera_info| SYNC + CAM -->|right/camera_info| SYNC + SYNC -->|rgbd_image| ODOM & MAP & VIZ + ODOM --> ODOMT + ODOMT --> MAP & VIZ +``` + +**With odometry from elsewhere** — a wheel encoder, a lidar, or an external VIO — the stereo pair feeds only the mapping side: + +```mermaid +flowchart LR + CAM["stereo driver"] + SYNC["stereo_sync"] + ODOM["odometry source
wheel, lidar or external"] + ODOMT(["odometry"]) + MAP["rtabmap"] + VIZ["rtabmap_viz"] + CAM -->|left/image_rect| SYNC + CAM -->|right/image_rect| SYNC + CAM -->|left/camera_info| SYNC + CAM -->|right/camera_info| SYNC + SYNC -->|rgbd_image| MAP & VIZ + ODOM --> ODOMT + ODOMT --> MAP & VIZ +``` + +## Usage + +```bash +ros2 run rtabmap_sync stereo_sync --ros-args \ + -r left/image_rect:=/stereo/left/image_rect \ + -r right/image_rect:=/stereo/right/image_rect \ + -r left/camera_info:=/stereo/left/camera_info \ + -r right/camera_info:=/stereo/right/camera_info +``` + +```python +ComposableNode( + package='rtabmap_sync', + plugin='rtabmap_sync::StereoSync', + name='stereo_sync', + remappings=[('left/image_rect', '/stereo/left/image_rect'), + ('right/image_rect', '/stereo/right/image_rect'), + ('left/camera_info', '/stereo/left/camera_info'), + ('right/camera_info', '/stereo/right/camera_info')]) +``` + +As with `rgbd_sync`, compose it into the driver's process where you can: this node copies both images of every pair. + +## Subscribed Topics + +| Topic | Type | Description | +|---|---|---| +| `left/image_rect` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Rectified left image, mono or color. Goes through `image_transport`. | +| `right/image_rect` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Rectified right image, same size and encoding. | +| `left/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Calibration of the left camera. | +| `right/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Calibration of the right camera. **Its `P[3]` must carry the baseline**; see below. | + +## Published Topics + +| Topic | Type | Description | +|---|---|---| +| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | Left image in the color slot, right image in the depth slot. Published only when someone is subscribed. | +| `rgbd_image/compressed` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The same pair, both images JPEG. Published only when someone is subscribed. | + +The output's `header.frame_id` comes from the **left** camera_info, and its `header.stamp` is the later of the two image stamps. + +## Parameters + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `approx_sync` | `bool` | `false` | Match the inputs by nearest stamp. Defaults to **exact** here; see [Synchronization](#synchronization). | +| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. Only meaningful with `approx_sync`. | +| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. | +| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. | +| `queue_size` | `int` | — | **Deprecated**, renamed to `sync_queue_size`. | +| `qos` | `int` | `0` | Reliability of the subscriptions and the publishers: `0` system default, `1` reliable, `2` best effort. | +| `qos_camera_info` | `int` | value of `qos` | Reliability of the two `camera_info` subscriptions alone. | +| `compressed_rate` | `double` | `0.0` | Maximum rate, in Hz, of `rgbd_image/compressed`. `0` means every frame. | +| `image_transport` | `string` | `"raw"` | Transport for both images, e.g. `compressed`. | + +## How a stereo pair travels in an RGBDImage + +There is no separate stereo message: the left image goes where color goes and the right image goes where depth goes. What tells a consumer to read it as a stereo pair rather than as color plus depth is the **baseline** in the second calibration — `P[3]` of the right `camera_info`, which by the ROS convention is `-fx * baseline`. + +So a right `camera_info` with `P[3] == 0` describes a camera sitting exactly on top of the left one. Nothing downstream can triangulate from that, and the failure is silent: the pair is forwarded, RTAB-Map reads a zero baseline and produces no depth. If a stereo pipeline comes out with no 3D points at all, check `P[3]` of the right camera first: + +```bash +ros2 topic echo --once /stereo/right/camera_info --field p +``` + +## Synchronization + +`approx_sync` defaults to **false** here, unlike the other nodes in this package. A stereo pair is normally hardware-triggered, so the two frames carry the same stamp, and the exact policy is both cheaper and impossible to mismatch. Mismatching a stereo pair is worse than mismatching color and depth: the disparity between two frames taken at different instants is a measurement of the camera's own motion, read as scene geometry. + +Set `approx_sync:=true` only for two free-running cameras that are not triggered together — and then set `approx_sync_max_interval` alongside it. The node warns whenever a pair's stamps differ by more than 10 ms regardless of the setting, because at that point the pair is unlikely to be worth anything. + +If the pipeline is silent with the default, the stamps are not identical. Check with: + +```bash +ros2 topic echo --once /stereo/left/image_rect --field header.stamp +ros2 topic echo --once /stereo/right/image_rect --field header.stamp +``` + +## Compressing for a slow link + +`rgbd_image/compressed` carries both images as **JPEG**. Unlike [rgbd_sync](rgbd_sync.md), there is no lossless path: both halves of a stereo pair are ordinary camera images, and neither is depth. + +JPEG artifacts do affect stereo matching, so a pipeline that computes odometry from the compressed stream will match slightly fewer features than one on the raw images. For sending frames to an operator, that does not matter; for running odometry at the far end of a link, prefer a higher JPEG quality over a lower frame rate. + +`compressed_rate` caps the compressed topic without touching `rgbd_image`. + +## Diagnostics + +The node publishes to `/diagnostics`: the rate of incoming left frames, the rate of published pairs, and a warning in the log every 5 seconds while nothing is arriving. A healthy input rate with no output means the pairs are not matching — the stamps and `approx_sync` are what to look at. diff --git a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h index 8fb31a6d..1282a4c6 100644 --- a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h +++ b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h @@ -59,34 +59,177 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +/** + * @namespace rtabmap_sync + * @brief Synchronization of the sensor topics RTAB-Map consumes. + * + * Two things live here: the standalone nodes that group a camera's topics into a single + * [RGBDImage](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) + * (`rgbd_sync`, `stereo_sync`, `rgb_sync`, `rgbdx_sync`), and CommonDataSubscriber, the + * base class through which the consuming nodes subscribe. + */ namespace rtabmap_sync { +/** + * @brief Subscribes to whichever set of sensor topics a node was configured for, and + * hands them over synchronized. + * + * RTAB-Map can be fed in a dozen shapes -- RGB-D, stereo, RGB-only, a pre-packed + * `RGBDImage` or several of them, a 2D or 3D scan, a whole `SensorData` -- each + * optionally alongside odometry, an `OdomInfo` and user data. That is far too many + * combinations for a node to wire by hand, so this class owns all of them: it reads the + * `subscribe_*` parameters, builds the one `message_filters` synchronizer that matches, + * and calls back with a uniform set of arguments no matter which inputs were used. + * + * `rtabmap_slam`'s `rtabmap` node and `rtabmap_viz` both derive from it, which is why + * they take identical topics and parameters. + * + * @par Using it + * Derive from both rclcpp::Node and this class, and call setupCallbacks() once the + * subclass is ready to receive data: + * @code + * class MyNode : public rclcpp::Node, public rtabmap_sync::CommonDataSubscriber + * { + * public: + * explicit MyNode(const rclcpp::NodeOptions & options) : + * Node("my_node", options), + * CommonDataSubscriber(*this, false) + * { + * setupCallbacks(*this); + * } + * protected: + * void commonMultiCameraCallback(...) override { ... } + * // ... and the three other callbacks + * }; + * @endcode + * The constructor declares the parameters, so they are readable from the subclass + * constructor before setupCallbacks() is called. + * + * @par Which callback fires + * Exactly one of the four, decided once at setup: + * - commonMultiCameraCallback() for anything with a camera in it, + * - commonLaserScanCallback() for a scan with no camera, + * - commonSensorDataCallback() for `subscribe_sensor_data`, + * - commonOdomCallback() when odometry is the only input. + * + * @par Conflicting parameters + * Several `subscribe_*` flags describe the same slot. Rather than refusing to start, + * setupCallbacks() drops one of the two and logs which: stereo beats depth and RGB, + * `subscribe_rgbd` beats all three, `subscribe_sensor_data` beats everything including + * `subscribe_rgbd`, `subscribe_scan` beats `subscribe_scan_cloud`, and + * `subscribe_scan_descriptor` beats both. Setting `odom_frame_id` turns off + * `subscribe_odom`, since the pose is then read from TF instead. + * + * @par Build options + * Synchronizing several `RGBDImage` topics (`rgbd_cameras` > 1) needs + * `RTABMAP_SYNC_MULTI_RGBD`, and `subscribe_user_data` needs `RTABMAP_SYNC_USER_DATA`. + * Both are off by default because each multiplies the number of synchronizer templates + * the package instantiates. Turning the first on is the better of the two ways to take + * several cameras: the node subscribes to them directly, with nothing in between. + * Without it, `rgbd_cameras=0` selects the `RGBDImages` interface -- what `rgbdx_sync` + * publishes -- which needs no rebuild and has no camera-count limit, at the cost of one + * extra node and one full-frame copy per camera. + */ class CommonDataSubscriber { public: + /** + * @brief Declares the `subscribe_*`, queue and QoS parameters on @p node. + * + * Subscribing itself happens in setupCallbacks(), so that a subclass can read the + * parameters and finish constructing before any message can arrive. + * + * @param node the node the parameters are declared on and the topics subscribed to + * @param gui true for a visualization node: `subscribe_depth` and `subscribe_rgb` + * then default to false, leaving odometry as the only default input + */ RTABMAP_SYNC_PUBLIC CommonDataSubscriber(rclcpp::Node & node, bool gui); virtual ~CommonDataSubscriber(); + /// True if subscribed to separate color, depth and camera_info topics. bool isSubscribedToDepth() const {return subscribedToDepth_;} + /// True if subscribed to a left/right image pair with their two camera_info topics. bool isSubscribedToStereo() const {return subscribedToStereo_;} + /// True if subscribed to color and camera_info with no depth. bool isSubscribedToRGB() const {return subscribedToRGB_;} + /// True if odometry comes from the `odom` topic; false when `odom_frame_id` is set. bool isSubscribedToOdom() const {return subscribedToOdom_;} + /// True if subscribed to `RGBDImage` topics, or to the `RGBDImages` container. bool isSubscribedToRGBD() const {return subscribedToRGBD_;} + /// True if subscribed to a `LaserScan`. bool isSubscribedToScan2d() const {return subscribedToScan2d_;} + /// True if subscribed to a `PointCloud2` scan. bool isSubscribedToScan3d() const {return subscribedToScan3d_;} + /// True if subscribed to a whole `SensorData`. bool isSubscribedToSensorData() const {return subscribedToSensorData_;} + /// True if an `OdomInfo` is synchronized with the data. bool isSubscribedToOdomInfo() const {return subscribedToOdomInfo_;} + /// True if any input at all is subscribed. False means no callback can ever fire. bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD() || isSubscribedToScan2d() || isSubscribedToScan3d() || isSubscribedToRGB() || isSubscribedToOdom() || isSubscribedToSensorData();} + /** + * @brief Number of `RGBDImage` topics subscribed. + * @return 0 when not subscribed to RGBD at all, and also on the `RGBDImages` + * interface (`rgbd_cameras=0`), where the count varies per message. + */ int rgbdCameras() const {return isSubscribedToRGBD()?(int)rgbdSubs_.size():0;} + /// Queue depth of each individual subscription (`topic_queue_size`). int getTopicQueueSize() const {return topicQueueSize_;} + /// Queue depth of the synchronizer (`sync_queue_size`). int getSyncQueueSize() const {return syncQueueSize_;} + /** + * @brief True if inputs are matched by nearest stamp rather than exact equality. + * + * The default depends on the inputs: false for stereo and for a scan with no camera, + * true otherwise. The `approx_sync` parameter overrides it either way. + */ bool isApproxSync() const {return approxSync_;} + /// The node name, as captured at construction. const std::string & name() const {return name_;} protected: + /** + * @brief Resolves the parameters into one synchronizer and subscribes. + * + * Call once from the subclass constructor, after the subclass is able to handle a + * callback. This is also where the conflicting-parameter rules are applied and where + * the /diagnostics reporting is set up. + * + * @param node the node to subscribe on; pass the same one given to the constructor + * @param otherTasks extra diagnostic tasks to publish alongside the input and output + * rate, so the node reports its own state in the same message + */ void setupCallbacks( rclcpp::Node & node, std::vector otherTasks = std::vector()); + /** + * @brief Called with one synchronized frame from one or more cameras. + * + * Fires for every configuration that has a camera in it, whichever way the camera was + * subscribed. The vectors hold one entry per camera and are parallel; unused inputs + * arrive empty or null rather than being signalled separately. + * + * @param odomMsg the pose, or null when odometry is not subscribed + * @param userDataMsg user data, or null + * @param imageMsgs one color image per camera + * @param depthMsgs one depth image per camera, or the right image in + * stereo; empty when there is no depth (RGB-only) + * @param cameraInfoMsgs calibration of each color camera + * @param depthCameraInfoMsgs calibration of each depth camera, or of the right + * camera in stereo, whose P(0,3) carries the baseline + * @param scanMsg a 2D scan, or a default-constructed one if none + * @param scan3dMsg a 3D scan, or a default-constructed one if none + * @param odomInfoMsg odometry details, or null + * @param globalDescriptorMsgs global descriptors, empty when none were computed + * @param localKeyPoints per-camera keypoints, in image coordinates; only ever + * set by the RGBD inputs, which can carry the features + * the odometry already extracted + * @param localPoints3d per-camera 3D points matching @p localKeyPoints, each + * expressed in **its own camera's optical frame** -- not + * in the robot's base frame. rtabmap_conversions' + * `convertRGBDMsgs()` is what moves them to the base + * frame, applying each camera's local transform. + * @param localDescriptors per-camera feature descriptors, already uncompressed + */ virtual void commonMultiCameraCallback( const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, @@ -101,6 +244,17 @@ protected: const std::vector > & localKeyPoints = std::vector >(), const std::vector > & localPoints3d = std::vector >(), const std::vector & localDescriptors = std::vector()) = 0; + /** + * @brief Called with one synchronized scan, when no camera is subscribed. + * + * @param odomMsg the pose, or null when odometry is not subscribed + * @param userDataMsg user data, or null + * @param scanMsg the 2D scan, default-constructed if the scan is 3D + * @param scan3dMsg the 3D scan, default-constructed if the scan is 2D + * @param odomInfoMsg odometry details, or null + * @param globalDescriptor the descriptor from a `ScanDescriptor` input; its `data` + * is empty when none was computed + */ virtual void commonLaserScanCallback( const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, @@ -108,15 +262,42 @@ protected: const sensor_msgs::msg::PointCloud2 & scan3dMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg, const rtabmap_msgs::msg::GlobalDescriptor & globalDescriptor = rtabmap_msgs::msg::GlobalDescriptor()) = 0; + /** + * @brief Called with odometry alone, when it is the only subscribed input. + * @param odomMsg the pose + * @param userDataMsg user data, or null + * @param odomInfoMsg odometry details, or null + */ virtual void commonOdomCallback( const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg) = 0; + /** + * @brief Called with a whole `SensorData`, for `subscribe_sensor_data`. + * + * A `SensorData` already carries the images, the scan and the calibration of one + * frame, so nothing is unpacked here: it is passed on as it arrived. + * + * @param sensorDataMsg the frame + * @param odomMsg the pose, or null when odometry is not subscribed + * @param odomInfoMsg odometry details, or null + */ virtual void commonSensorDataCallback( const rtabmap_msgs::msg::SensorData::ConstSharedPtr & sensorDataMsg, const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg) = 0; + /** + * @brief Reports that the subclass produced an output, for /diagnostics. + * + * The input side is ticked automatically as messages arrive; this is the other half, + * and it is what lets "one camera went quiet" be told apart from "the node is + * receiving everything and falling behind". Call it once per published result. + * + * @param stamp stamp of what was produced + * @param targetFrequency the rate to be judged against, or 0 to inherit the rate + * measured on the input side + */ void tick(const rclcpp::Time & stamp, double targetFrequency = 0); private: diff --git a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h index 609af84b..c5082f53 100644 --- a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h +++ b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h @@ -29,7 +29,40 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_ #include -#include + +#include + +/** + * @brief One name for the topic of a subscription, whichever kind it is. + * + * A `message_filters::Subscriber` exposes `get_topic_name()` through the subscription it + * holds, while an `image_transport::SubscriberFilter` exposes `getTopic()`. The SYNC_DECL + * macros below log what a node subscribed to and have to handle both, so this picks + * whichever the object actually has, resolved at compile time. + * + * @{ + */ +template +auto getTopicNameImpl(T const& obj, int) + -> decltype(obj->get_topic_name(), std::string()) +{ + return obj->get_topic_name(); +} + +template +auto getTopicNameImpl(T const& obj, long) + -> decltype(obj.getTopic(), std::string()) +{ + return obj.getTopic(); +} + +template +auto getTopicName(T const& obj) + -> decltype(getTopicNameImpl(obj, 0), std::string()) +{ + return getTopicNameImpl(obj, 0); +} +/** @} */ #define DATA_SYNC2(PREFIX, SYNC_NAME, MSG0, MSG1) \ typedef message_filters::sync_policies::SYNC_NAME##Time PREFIX##SYNC_NAME##SyncPolicy; \ diff --git a/rtabmap_sync/include/rtabmap_sync/GetTopicName.h b/rtabmap_sync/include/rtabmap_sync/GetTopicName.h deleted file mode 100644 index 153fbc76..00000000 --- a/rtabmap_sync/include/rtabmap_sync/GetTopicName.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - * GetTopicName.h - * - * Created on: Oct 1, 2021 - * Author: mathieu - */ - -#ifndef INCLUDE_RTABMAP_ROS_GETTOPICNAME_H_ -#define INCLUDE_RTABMAP_ROS_GETTOPICNAME_H_ - -#include - -template -auto getTopicNameImpl(T const& obj, int) - -> decltype(obj->get_topic_name(), std::string()) -{ - return obj->get_topic_name(); -} - -template -auto getTopicNameImpl(T const& obj, long) - -> decltype(obj.getTopic(), std::string()) -{ - return obj.getTopic(); -} - -template -auto getTopicName(T const& obj) - -> decltype(getTopicNameImpl(obj, 0), std::string()) -{ - return getTopicNameImpl(obj, 0); -} - - -#endif /* INCLUDE_RTABMAP_ROS_GETTOPICNAME_H_ */ diff --git a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h index f9411519..1bec4c39 100644 --- a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h +++ b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h @@ -14,8 +14,43 @@ using namespace std::chrono_literals; namespace rtabmap_sync { +/** + * @brief Reports the rate going into a synchronizer and the rate coming out of it, on + * /diagnostics. + * + * Every node in this package, and every node built on CommonDataSubscriber, publishes + * through one of these. Two statuses rather than one is the whole point: a node can be + * receiving all of its inputs and still publish nothing -- one camera lagging is enough + * to stop a synchronizer emitting -- and only the pair tells those cases apart. + * + * @par Expected rate + * With no rate given, the target is learned from the gaps between the message stamps, + * averaged over a sliding window, and only ever revised upwards to the fastest rate seen. + * A node that deliberately publishes slower than it receives -- a throttled or decimated + * output -- passes its own rate to tickOutput() instead, so it is judged against what it + * meant to do. + * + * @par Usage + * @code + * syncDiagnostic_.reset(new SyncDiagnostic(this)); + * syncDiagnostic_->init(imageSub_.getTopic(), "Did not receive data since 5 seconds!..."); + * // then, in the callback: + * syncDiagnostic_->tickInput(image->header.stamp); + * ... + * syncDiagnostic_->tickOutput(image->header.stamp); + * @endcode + * + * @note The node passed in is held as a raw pointer and must outlive this object. + */ class SyncDiagnostic { public: + /** + * @param node the node to publish /diagnostics from; must outlive this object + * @param tolerance fraction by which the measured rate may differ from the + * expected one before the status stops being OK + * @param windowSize number of stamp intervals averaged when learning the expected + * rate; must be at least 1 + */ SyncDiagnostic(rclcpp::Node * node, double tolerance = 0.2, int windowSize = 5) : node_(node), diagnosticUpdater_(node, 2.0), @@ -34,6 +69,20 @@ class SyncDiagnostic { UASSERT(windowSize_ >= 1); } + /** + * @brief Registers the tasks and starts publishing. + * + * @param topic one of the subscribed topics, used only to name the hardware the + * status belongs to: the last two segments are dropped, so + * `/back_camera/left/image` reports as `back_camera`. Pass an empty + * string when no single topic identifies the device; the hardware id is + * then `none`. + * @param topicsNotReceivedWarningMsg logged every 5 seconds while nothing is coming + * in. Worth making specific: it is what a user sees when a pipeline is + * silent, so it should name the topics and the likely causes. + * @param otherTasks extra tasks to publish in the same message, so a node's own state + * arrives alongside its rates rather than in a separate update. + */ void init( const std::string & topic, const std::string & topicsNotReceivedWarningMsg, @@ -62,6 +111,12 @@ class SyncDiagnostic { diagnosticTimer_ = node_->create_wall_timer(5s, std::bind(&SyncDiagnostic::diagnosticTimerCallback, this), nullptr); } + /** + * @brief Records that one input message arrived. + * @param stamp the message stamp; it is also checked against the clock, + * which is how an unsynchronized sender is caught + * @param expectedFrequency the rate to judge against, or 0 to learn it from the stamps + */ void tickInput(const rclcpp::Time & stamp, double expectedFrequency = 0.0) { updateFrequency( @@ -74,6 +129,13 @@ class SyncDiagnostic { lastTickInputStamp_); } + /** + * @brief Records that one output message was published. + * @param stamp the stamp of what was published + * @param expectedFrequency the rate to judge against, or 0 to inherit the rate + * measured on the input side -- the right default for a + * node that publishes one output per input + */ void tickOutput(const rclcpp::Time & stamp, double expectedFrequency = 0.0) { if(expectedFrequency == 0.0) { diff --git a/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp b/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp index 571ef3a1..1215834f 100644 --- a/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp +++ b/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp @@ -44,6 +44,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap_sync { +/** + * @brief Groups a camera's color and calibration topics into one `RGBDImage`, with no + * depth. + * + * The RGB-only counterpart of RGBDSync, for a monocular camera feeding an appearance-only + * pipeline -- loop closure detection and relocalization without 3D reconstruction. With + * `fill_empty_depth` it adds an all-zero depth image for consumers that insist on one. + * + * See the [node documentation](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_sync/doc/rgb_sync.md) + * for topics and parameters. + */ class RGBSync : public rclcpp::Node { public: @@ -60,6 +71,9 @@ private: double compressedRate_; bool fillEmptyDepth_; + /// Stamp of the last compressed message published, for compressed_rate throttling. + /// Explicitly on the ROS clock: the default is the system clock, and rclcpp refuses + /// to compare two times that do not come from the same source. rclcpp::Time lastCompressedPublished_; rclcpp::Publisher::SharedPtr rgbdImagePub_; diff --git a/rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp b/rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp index 351c55eb..8c58a0c6 100644 --- a/rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp +++ b/rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp @@ -44,6 +44,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap_sync { +/** + * @brief Groups an RGB-D camera's color, depth and calibration topics into one + * `RGBDImage`. + * + * Three topics that have to stay together are easier to keep together as one message: + * remapping is a single line, nothing downstream re-synchronizes them, and a recording + * cannot end up with a depth frame and no color. It can also decimate, rescale depth and + * publish a compressed copy for a slow link. + * + * See the [node documentation](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_sync/doc/rgbd_sync.md) + * for topics and parameters. + */ class RGBDSync : public rclcpp::Node { public: @@ -63,6 +75,9 @@ private: double compressedRate_; double approxSyncMaxInterval_; + /// Stamp of the last compressed message published, for compressed_rate throttling. + /// Explicitly on the ROS clock: the default is the system clock, and rclcpp refuses + /// to compare two times that do not come from the same source. rclcpp::Time lastCompressedPublished_; rclcpp::Publisher::SharedPtr rgbdImagePub_; diff --git a/rtabmap_sync/include/rtabmap_sync/rgbdx_sync.hpp b/rtabmap_sync/include/rtabmap_sync/rgbdx_sync.hpp index e49553dd..14454690 100644 --- a/rtabmap_sync/include/rtabmap_sync/rgbdx_sync.hpp +++ b/rtabmap_sync/include/rtabmap_sync/rgbdx_sync.hpp @@ -46,6 +46,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap_sync { +/** + * @brief Groups the `RGBDImage` topics of 2 to 8 cameras into one `RGBDImages`. + * + * For a robot carrying several RGB-D cameras. Synchronizing them here, once, means the + * consuming node subscribes to a single topic and needs no multi-camera build option -- + * `rgbd_cameras=0` on CommonDataSubscriber takes the container this publishes. + * + * See the [node documentation](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_sync/doc/rgbdx_sync.md) + * for topics and parameters. + */ class RGBDXSync : public rclcpp::Node { public: diff --git a/rtabmap_sync/include/rtabmap_sync/stereo_sync.hpp b/rtabmap_sync/include/rtabmap_sync/stereo_sync.hpp index 8f24b785..bf5dc942 100644 --- a/rtabmap_sync/include/rtabmap_sync/stereo_sync.hpp +++ b/rtabmap_sync/include/rtabmap_sync/stereo_sync.hpp @@ -44,6 +44,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap_sync { +/** + * @brief Groups a stereo pair's four topics into one `RGBDImage`. + * + * The left image goes in the color slot and the right image in the depth slot; what tells + * a consumer to read it as a stereo pair rather than as color plus depth is the baseline + * in the second calibration's P(0,3). Defaults to exact synchronization, since a stereo + * pair is normally hardware-triggered. + * + * See the [node documentation](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_sync/doc/stereo_sync.md) + * for topics and parameters. + */ class StereoSync : public rclcpp::Node { public: @@ -60,6 +71,9 @@ public: private: double compressedRate_; double approxSyncMaxInterval_; + /// Stamp of the last compressed message published, for compressed_rate throttling. + /// Explicitly on the ROS clock: the default is the system clock, and rclcpp refuses + /// to compare two times that do not come from the same source. rclcpp::Time lastCompressedPublished_; rclcpp::Publisher::SharedPtr rgbdImagePub_; diff --git a/rtabmap_sync/package.xml b/rtabmap_sync/package.xml index 39baf6b4..8346e196 100644 --- a/rtabmap_sync/package.xml +++ b/rtabmap_sync/package.xml @@ -25,7 +25,10 @@ sensor_msgs diagnostic_updater + ament_cmake_gtest + ament_cmake + rosdoc2.yaml diff --git a/rtabmap_sync/rosdoc2.yaml b/rtabmap_sync/rosdoc2.yaml new file mode 100644 index 00000000..c46ac697 --- /dev/null +++ b/rtabmap_sync/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_sync +## Build the docs locally with: +## rosdoc2 build --package-path rtabmap_sync --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_sync 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_sync', + doxygen_xml_directory: 'generated/doxygen/xml', + output_dir: '' + } diff --git a/rtabmap_sync/src/nodelets/rgb_sync.cpp b/rtabmap_sync/src/nodelets/rgb_sync.cpp index 585d85ad..adab2b2c 100644 --- a/rtabmap_sync/src/nodelets/rgb_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgb_sync.cpp @@ -46,15 +46,18 @@ namespace rtabmap_sync { RGBSync::RGBSync(const rclcpp::NodeOptions & options) : - Node("rgbd_sync", options), + Node("rgb_sync", options), compressedRate_(0), fillEmptyDepth_(false), + lastCompressedPublished_(0, 0, RCL_ROS_TIME), approxSync_(0), exactSync_(0) { int topicQueueSize = 10; int syncQueueSize = 10; - bool approxSync = true; + // A camera publisher sends the image and its camera_info together, with the same + // stamp, so the exact policy is both cheaper and impossible to mismatch. + bool approxSync = false; int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; double approxSyncMaxInterval = 0.0; approxSync = this->declare_parameter("approx_sync", approxSync); diff --git a/rtabmap_sync/src/nodelets/rgbd_sync.cpp b/rtabmap_sync/src/nodelets/rgbd_sync.cpp index 7c73a5af..988c3c2b 100644 --- a/rtabmap_sync/src/nodelets/rgbd_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgbd_sync.cpp @@ -51,6 +51,7 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : decimation_(1), compressedRate_(0), approxSyncMaxInterval_(0.0), + lastCompressedPublished_(0, 0, RCL_ROS_TIME), approxSyncDepth_(0), exactSyncDepth_(0) { diff --git a/rtabmap_sync/src/nodelets/stereo_sync.cpp b/rtabmap_sync/src/nodelets/stereo_sync.cpp index feb831a1..a688e85c 100644 --- a/rtabmap_sync/src/nodelets/stereo_sync.cpp +++ b/rtabmap_sync/src/nodelets/stereo_sync.cpp @@ -48,6 +48,7 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) : Node("stereo_sync", options), compressedRate_(0), approxSyncMaxInterval_(0.0), + lastCompressedPublished_(0, 0, RCL_ROS_TIME), approxSync_(0), exactSync_(0) { diff --git a/rtabmap_sync/test/common_data_subscriber_fixture.hpp b/rtabmap_sync/test/common_data_subscriber_fixture.hpp new file mode 100644 index 00000000..0b01f22f --- /dev/null +++ b/rtabmap_sync/test/common_data_subscriber_fixture.hpp @@ -0,0 +1,205 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#ifndef RTABMAP_SYNC_COMMON_DATA_SUBSCRIBER_FIXTURE_HPP_ +#define RTABMAP_SYNC_COMMON_DATA_SUBSCRIBER_FIXTURE_HPP_ + +#include "node_test_utils.hpp" +#include "msg_builders.hpp" + +#include + +#include + +#include +#include +#include + +namespace rtabmap_sync_test { + +/** + * @brief A concrete CommonDataSubscriber that records what reached each callback. + * + * CommonDataSubscriber is abstract and does the subscribing and synchronizing for its + * subclass -- `rtabmap_slam`'s `rtabmap` node and `rtabmap_viz` are the two real ones. + * This stands in for them: it implements the four callbacks and remembers what it was + * handed, so a test can assert on what came out of the synchronizer. + */ +class RecordingSubscriber : + public rclcpp::Node, + public rtabmap_sync::CommonDataSubscriber +{ +public: + /// One call of one of the four callbacks, flattened to what the tests assert on. + struct Record + { + enum Kind { kMultiCamera, kLaserScan, kOdom, kSensorData }; + + Kind kind = kMultiCamera; + double stamp = 0.0; ///< stamp of whichever message drove the callback + size_t images = 0; ///< number of color images + size_t depths = 0; ///< number of depth (or right) images + size_t cameraInfos = 0; + bool hasOdom = false; + bool hasOdomInfo = false; + bool hasUserData = false; + bool hasScan2d = false; ///< a non-empty LaserScan reached the callback + bool hasScan3d = false; ///< a non-empty PointCloud2 reached the callback + size_t globalDescriptors = 0; + std::string frameId; + }; + + /** + * @param options ROS options; the subscribe_* parameters go in here + * @param gui the flag the real subclasses pass: false for the SLAM node, true for + * the GUI, which defaults to subscribing to nothing but odometry + */ + RecordingSubscriber(const rclcpp::NodeOptions & options, bool gui = false) : + Node("recording_subscriber", options), + CommonDataSubscriber(*this, gui) + { + setupCallbacks(*this); + } + + const std::vector & records() const { return records_; } + bool empty() const { return records_.empty(); } + size_t size() const { return records_.size(); } + const Record & back() const { return records_.back(); } + +protected: + void commonMultiCameraCallback( + const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, + const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, + const std::vector & imageMsgs, + const std::vector & depthMsgs, + const std::vector & cameraInfoMsgs, + const std::vector & depthCameraInfoMsgs, + const sensor_msgs::msg::LaserScan & scanMsg, + const sensor_msgs::msg::PointCloud2 & scan3dMsg, + const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr & odomInfoMsg, + const std::vector & globalDescriptorMsgs, + const std::vector > &, + const std::vector > &, + const std::vector &) override + { + (void)depthCameraInfoMsgs; + Record record; + record.kind = Record::kMultiCamera; + record.images = imageMsgs.size(); + record.depths = depthMsgs.size(); + record.cameraInfos = cameraInfoMsgs.size(); + record.hasOdom = odomMsg.get() != nullptr; + record.hasOdomInfo = odomInfoMsg.get() != nullptr; + record.hasUserData = userDataMsg.get() != nullptr; + record.hasScan2d = !scanMsg.ranges.empty(); + record.hasScan3d = scan3dMsg.data.size() > 0; + record.globalDescriptors = globalDescriptorMsgs.size(); + if(!cameraInfoMsgs.empty()) + { + record.frameId = cameraInfoMsgs[0].header.frame_id; + record.stamp = rclcpp::Time(cameraInfoMsgs[0].header.stamp).seconds(); + } + add(record); + } + + void commonLaserScanCallback( + const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, + const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, + const sensor_msgs::msg::LaserScan & scanMsg, + const sensor_msgs::msg::PointCloud2 & scan3dMsg, + const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr & odomInfoMsg, + const rtabmap_msgs::msg::GlobalDescriptor & globalDescriptor) override + { + Record record; + record.kind = Record::kLaserScan; + record.hasOdom = odomMsg.get() != nullptr; + record.hasOdomInfo = odomInfoMsg.get() != nullptr; + record.hasUserData = userDataMsg.get() != nullptr; + record.hasScan2d = !scanMsg.ranges.empty(); + record.hasScan3d = scan3dMsg.data.size() > 0; + record.globalDescriptors = globalDescriptor.data.empty() ? 0 : 1; + record.frameId = record.hasScan2d ? + scanMsg.header.frame_id : scan3dMsg.header.frame_id; + record.stamp = rclcpp::Time(record.hasScan2d ? + scanMsg.header.stamp : scan3dMsg.header.stamp).seconds(); + add(record); + } + + void commonOdomCallback( + const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, + const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, + const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr & odomInfoMsg) override + { + Record record; + record.kind = Record::kOdom; + record.hasOdom = odomMsg.get() != nullptr; + record.hasOdomInfo = odomInfoMsg.get() != nullptr; + record.hasUserData = userDataMsg.get() != nullptr; + if(odomMsg.get()) + { + record.frameId = odomMsg->header.frame_id; + record.stamp = rclcpp::Time(odomMsg->header.stamp).seconds(); + } + add(record); + } + + void commonSensorDataCallback( + const rtabmap_msgs::msg::SensorData::ConstSharedPtr & sensorDataMsg, + const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, + const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr & odomInfoMsg) override + { + Record record; + record.kind = Record::kSensorData; + record.hasOdom = odomMsg.get() != nullptr; + record.hasOdomInfo = odomInfoMsg.get() != nullptr; + if(sensorDataMsg.get()) + { + record.cameraInfos = sensorDataMsg->left_camera_info.size(); + record.frameId = sensorDataMsg->header.frame_id; + record.stamp = rclcpp::Time(sensorDataMsg->header.stamp).seconds(); + } + add(record); + } + +private: + /// Also drives the output half of the diagnostics, as the real subclasses do. + void add(const Record & record) + { + records_.push_back(record); + tick(stampOf(record.stamp)); + } + + std::vector records_; +}; + +/// Fixture that starts a RecordingSubscriber and publishes its inputs. +class CommonDataSubscriberTest : public NodeTest +{ +protected: + /// Starts the subscriber under test. @p gui mirrors rtabmap_viz's constructor flag. + std::shared_ptr start( + const std::vector & params = {}, bool gui = false) + { + sub_ = addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(params), gui)); + return sub_; + } + + /// Creates a publisher on @p topic and waits for the subscriber to discover it. + template + typename rclcpp::Publisher::SharedPtr advertise(const std::string & topic) + { + typename rclcpp::Publisher::SharedPtr publisher = + helper()->create_publisher(topic, 10); + EXPECT_TRUE(waitForSubscriber(publisher)) << "nobody subscribed to " << topic; + return publisher; + } + + std::shared_ptr sub_; +}; + +} // namespace rtabmap_sync_test + +#endif /* RTABMAP_SYNC_COMMON_DATA_SUBSCRIBER_FIXTURE_HPP_ */ diff --git a/rtabmap_sync/test/msg_builders.hpp b/rtabmap_sync/test/msg_builders.hpp new file mode 100644 index 00000000..5b6b86bc --- /dev/null +++ b/rtabmap_sync/test/msg_builders.hpp @@ -0,0 +1,284 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#ifndef RTABMAP_SYNC_MSG_BUILDERS_HPP_ +#define RTABMAP_SYNC_MSG_BUILDERS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#ifdef PRE_ROS_IRON +#include +#else +#include +#endif + +#include +#include + +namespace rtabmap_sync_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 rectified pinhole CameraInfo; @p tx is P(0,3), non-zero for a stereo right camera. +inline sensor_msgs::msg::CameraInfo makeCameraInfo( + const std::string & frameId, double stamp, int width = 8, int height = 8, + double tx = 0.0, double fx = 100.0) +{ + sensor_msgs::msg::CameraInfo info; + info.header.frame_id = frameId; + info.header.stamp = stampOf(stamp); + info.width = width; + info.height = height; + info.distortion_model = "plumb_bob"; + info.d = {0.0, 0.0, 0.0, 0.0, 0.0}; + info.k = {fx, 0.0, width/2.0, 0.0, fx, height/2.0, 0.0, 0.0, 1.0}; + info.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + info.p = {fx, 0.0, width/2.0, tx, 0.0, fx, height/2.0, 0.0, 0.0, 0.0, 1.0, 0.0}; + return info; +} + +inline sensor_msgs::msg::Image makeImage( + const std::string & frameId, double stamp, + const cv::Mat & image, const std::string & encoding) +{ + std_msgs::msg::Header header; + header.frame_id = frameId; + header.stamp = stampOf(stamp); + sensor_msgs::msg::Image msg; + cv_bridge::CvImage(header, encoding, image).toImageMsg(msg); + return msg; +} + +/// A bgr8 color image of a single flat color. +inline sensor_msgs::msg::Image makeRgbImage( + const std::string & frameId, double stamp, int width = 8, int height = 8, + const cv::Scalar & color = cv::Scalar(10, 20, 30)) +{ + return makeImage(frameId, stamp, cv::Mat(height, width, CV_8UC3, color), "bgr8"); +} + +/// A 16UC1 depth image in millimeters, the encoding the RGB-D drivers publish. +inline sensor_msgs::msg::Image makeDepthImage( + const std::string & frameId, double stamp, int width = 8, int height = 8, + uint16_t millimeters = 1500) +{ + return makeImage(frameId, stamp, + cv::Mat(height, width, CV_16UC1, cv::Scalar(millimeters)), "16UC1"); +} + +/// A mono8 image, used as a stereo left or right frame. +inline sensor_msgs::msg::Image makeMonoImage( + const std::string & frameId, double stamp, int width = 8, int height = 8, + uint8_t value = 60) +{ + return makeImage(frameId, stamp, + cv::Mat(height, width, CV_8UC1, cv::Scalar(value)), "mono8"); +} + +/// An RGB-D message with raw bgr8 color and 16UC1 depth, as rgbd_sync publishes it. +inline rtabmap_msgs::msg::RGBDImage makeRGBDImage( + const std::string & frameId, double stamp, int width = 8, int height = 8, + const cv::Scalar & rgbColor = cv::Scalar(10, 20, 30), uint16_t depthValue = 1500) +{ + rtabmap_msgs::msg::RGBDImage msg; + msg.header.frame_id = frameId; + msg.header.stamp = stampOf(stamp); + msg.rgb = makeRgbImage(frameId, stamp, width, height, rgbColor); + msg.depth = makeDepthImage(frameId, stamp, width, height, depthValue); + msg.rgb_camera_info = makeCameraInfo(frameId, stamp, width, height); + msg.depth_camera_info = makeCameraInfo(frameId, stamp, width, height); + return msg; +} + +/// A flat LaserScan of @p count equal ranges over 180 degrees. +inline sensor_msgs::msg::LaserScan makeLaserScan( + const std::string & frameId, double stamp, size_t count = 10, float range = 2.0f) +{ + sensor_msgs::msg::LaserScan scan; + scan.header.frame_id = frameId; + scan.header.stamp = stampOf(stamp); + scan.angle_min = -M_PI_2; + scan.angle_max = M_PI_2; + scan.angle_increment = count > 1 ? float(M_PI / double(count - 1)) : float(M_PI); + scan.time_increment = 0.0f; + scan.scan_time = 0.1f; + scan.range_min = 0.1f; + scan.range_max = 10.0f; + scan.ranges.assign(count, range); + return scan; +} + +/// A dense unorganized XYZ float cloud, the shape a 3D lidar driver publishes. +inline sensor_msgs::msg::PointCloud2 makeXYZCloud( + const std::string & frameId, double stamp, + const std::vector & points) +{ + sensor_msgs::msg::PointCloud2 cloud; + cloud.header.frame_id = frameId; + cloud.header.stamp = stampOf(stamp); + cloud.height = 1; + cloud.width = points.size(); + cloud.is_bigendian = false; + cloud.is_dense = true; + + cloud.fields.resize(3); + const char * names[3] = {"x", "y", "z"}; + for(int i=0; i<3; ++i) + { + cloud.fields[i].name = names[i]; + cloud.fields[i].offset = 4 * i; + cloud.fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32; + cloud.fields[i].count = 1; + } + cloud.point_step = 12; + cloud.row_step = cloud.point_step * cloud.width; + cloud.data.resize(cloud.row_step * cloud.height); + + for(size_t i=0; i(&cloud.data[i * cloud.point_step]); + p[0] = points[i].x; + p[1] = points[i].y; + p[2] = points[i].z; + } + return cloud; +} + +/// A small cloud on a line, enough to tell one scan from another. +inline sensor_msgs::msg::PointCloud2 makeScanCloud( + const std::string & frameId, double stamp, size_t count = 4) +{ + std::vector points; + points.reserve(count); + for(size_t i=0; i(base + xOffset), + *reinterpret_cast(base + yOffset), + *reinterpret_cast(base + zOffset)); +} + +} // namespace rtabmap_sync_test + +#endif /* RTABMAP_SYNC_MSG_BUILDERS_HPP_ */ diff --git a/rtabmap_sync/test/node_test_utils.hpp b/rtabmap_sync/test/node_test_utils.hpp new file mode 100644 index 00000000..ff1c0b78 --- /dev/null +++ b/rtabmap_sync/test/node_test_utils.hpp @@ -0,0 +1,208 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef RTABMAP_SYNC_NODE_TEST_UTILS_HPP_ +#define RTABMAP_SYNC_NODE_TEST_UTILS_HPP_ + +#include + +#include + +#include +#include +#include +#include +#include + +namespace rtabmap_sync_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_sync_test_helper"); + executor_->add_node(helper_); + } + + void TearDown() override + { + for(const rclcpp::Node::SharedPtr & node : nodes_) + { + executor_->remove_node(node); + } + nodes_.clear(); + executor_->remove_node(helper_); + helper_.reset(); + executor_.reset(); + } + + /// Adds a node under test to the shared executor and keeps it alive for the test. + template + std::shared_ptr addNode(const std::shared_ptr & node) + { + executor_->add_node(node); + nodes_.push_back(node); + return node; + } + + /// The helper node, used to publish inputs and subscribe to outputs. + rclcpp::Node::SharedPtr helper() { return helper_; } + + /** + * @brief Spins until @p done returns true, or the timeout elapses. + * @return true if @p done became true + */ + bool spinUntil( + const std::function & 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(); } + const MsgT & back() const { return *messages.back(); } + const MsgT & front() const { return *messages.front(); } + }; + + /// Subscribes the helper node to @p topic and records everything it receives. + template + std::shared_ptr> collect( + const std::string & topic, const rclcpp::QoS & qos = rclcpp::QoS(10)) + { + std::shared_ptr> collector = std::make_shared>(); + collector->subscription = helper_->create_subscription( + topic, qos, + [collector](const typename MsgT::ConstSharedPtr msg) { + collector->messages.push_back(msg); + }); + return collector; + } + +private: + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + rclcpp::Node::SharedPtr helper_; + std::vector nodes_; +}; + +} // namespace rtabmap_sync_test + +#endif /* RTABMAP_SYNC_NODE_TEST_UTILS_HPP_ */ diff --git a/rtabmap_sync/test/test_common_data_subscriber.cpp b/rtabmap_sync/test/test_common_data_subscriber.cpp new file mode 100644 index 00000000..bbf2a3fe --- /dev/null +++ b/rtabmap_sync/test/test_common_data_subscriber.cpp @@ -0,0 +1,315 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "common_data_subscriber_fixture.hpp" + +using namespace rtabmap_sync_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); +} + +/// The subscribe_* parameters: what they select, and how conflicts between them resolve. +/// +/// CommonDataSubscriber builds one synchronizer out of whichever inputs are asked for, +/// and several of the flags describe the same slot in that synchronizer. Rather than +/// refusing to start, it drops one of the two and says so in the log. These tests pin +/// down which one survives, because that is what decides the topics a user has to remap. +class CommonDataSubscriberConfigTest : public CommonDataSubscriberTest {}; + +TEST_F(CommonDataSubscriberConfigTest, DefaultsToAnRGBDCameraWithOdometry) +{ + std::shared_ptr sub = start(); + + EXPECT_TRUE(sub->isSubscribedToDepth()); + EXPECT_TRUE(sub->isSubscribedToRGB()); + EXPECT_TRUE(sub->isSubscribedToOdom()); + EXPECT_FALSE(sub->isSubscribedToStereo()); + EXPECT_FALSE(sub->isSubscribedToRGBD()); + EXPECT_FALSE(sub->isSubscribedToSensorData()); + EXPECT_FALSE(sub->isSubscribedToScan2d()); + EXPECT_FALSE(sub->isSubscribedToScan3d()); + EXPECT_FALSE(sub->isSubscribedToOdomInfo()); + EXPECT_TRUE(sub->isDataSubscribed()); + EXPECT_STREQ(sub->name().c_str(), "recording_subscriber"); +} + +TEST_F(CommonDataSubscriberConfigTest, TheGuiFlagSubscribesToNothingButOdometry) +{ + // rtabmap_viz passes gui=true: it renders whatever the SLAM node publishes and has + // no reason to subscribe to the raw camera topics unless asked. + std::shared_ptr sub = start({}, /*gui=*/true); + + EXPECT_FALSE(sub->isSubscribedToDepth()); + EXPECT_FALSE(sub->isSubscribedToRGB()); + EXPECT_TRUE(sub->isSubscribedToOdom()); + EXPECT_TRUE(sub->isDataSubscribed()) << "odometry alone still counts as data"; +} + +TEST_F(CommonDataSubscriberConfigTest, StereoWinsOverDepth) +{ + // Both describe the camera slot. Stereo is the more specific request, so it stays + // and depth -- along with the rgb flag that comes with it -- is dropped. + std::shared_ptr sub = start({ + rclcpp::Parameter("subscribe_depth", true), + rclcpp::Parameter("subscribe_stereo", true)}); + + EXPECT_TRUE(sub->isSubscribedToStereo()); + EXPECT_FALSE(sub->isSubscribedToDepth()); + EXPECT_FALSE(sub->isSubscribedToRGB()); +} + +TEST_F(CommonDataSubscriberConfigTest, StereoWinsOverRGB) +{ + std::shared_ptr sub = start({ + rclcpp::Parameter("subscribe_depth", false), + rclcpp::Parameter("subscribe_rgb", true), + rclcpp::Parameter("subscribe_stereo", true)}); + + EXPECT_TRUE(sub->isSubscribedToStereo()); + EXPECT_FALSE(sub->isSubscribedToRGB()); +} + +TEST_F(CommonDataSubscriberConfigTest, RGBDWinsOverDepthRGBAndStereo) +{ + // An RGBDImage already carries color, depth and calibration in one message, so it + // replaces every other way of describing the camera. + std::shared_ptr sub = start({ + rclcpp::Parameter("subscribe_depth", true), + rclcpp::Parameter("subscribe_rgb", true), + rclcpp::Parameter("subscribe_stereo", true), + rclcpp::Parameter("subscribe_rgbd", true)}); + + EXPECT_TRUE(sub->isSubscribedToRGBD()); + EXPECT_FALSE(sub->isSubscribedToDepth()); + EXPECT_FALSE(sub->isSubscribedToRGB()); + EXPECT_FALSE(sub->isSubscribedToStereo()); +} + +TEST_F(CommonDataSubscriberConfigTest, SensorDataWinsOverEveryCameraInput) +{ + // A SensorData is a whole RTAB-Map frame, images and scan together; nothing else is + // needed alongside it. + std::shared_ptr sub = start({ + rclcpp::Parameter("subscribe_depth", true), + rclcpp::Parameter("subscribe_rgb", true), + rclcpp::Parameter("subscribe_stereo", true), + rclcpp::Parameter("subscribe_sensor_data", true)}); + + EXPECT_TRUE(sub->isSubscribedToSensorData()); + EXPECT_FALSE(sub->isSubscribedToDepth()); + EXPECT_FALSE(sub->isSubscribedToRGB()); + EXPECT_FALSE(sub->isSubscribedToStereo()); +} + +TEST_F(CommonDataSubscriberConfigTest, SensorDataWinsOverRGBD) +{ + std::shared_ptr sub = start({ + rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("subscribe_sensor_data", true)}); + + EXPECT_TRUE(sub->isSubscribedToSensorData()); + EXPECT_FALSE(sub->isSubscribedToRGBD()); +} + +TEST_F(CommonDataSubscriberConfigTest, SensorDataWinsOverEveryScanInput) +{ + // The scan travels inside the SensorData, so a separate scan topic would be a second + // copy of the same measurement. + std::shared_ptr sub = start({ + rclcpp::Parameter("subscribe_sensor_data", true), + rclcpp::Parameter("subscribe_scan", true), + rclcpp::Parameter("subscribe_scan_cloud", true)}); + + EXPECT_TRUE(sub->isSubscribedToSensorData()); + EXPECT_FALSE(sub->isSubscribedToScan2d()); + EXPECT_FALSE(sub->isSubscribedToScan3d()); +} + +TEST_F(CommonDataSubscriberConfigTest, TheTwoDScanWinsOverTheThreeDOne) +{ + std::shared_ptr sub = start({ + rclcpp::Parameter("subscribe_scan", true), + rclcpp::Parameter("subscribe_scan_cloud", true)}); + + EXPECT_TRUE(sub->isSubscribedToScan2d()); + EXPECT_FALSE(sub->isSubscribedToScan3d()); +} + +TEST_F(CommonDataSubscriberConfigTest, TheScanDescriptorWinsOverBothPlainScans) +{ + // A ScanDescriptor carries the scan plus the global descriptor computed from it, so + // it supersedes the plain scan topics rather than sitting beside them. + std::shared_ptr sub = start({ + rclcpp::Parameter("subscribe_scan", true), + rclcpp::Parameter("subscribe_scan_descriptor", true)}); + EXPECT_FALSE(sub->isSubscribedToScan2d()); + + std::shared_ptr other = addNode( + std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("subscribe_scan_cloud", true), + rclcpp::Parameter("subscribe_scan_descriptor", true)}))); + EXPECT_FALSE(other->isSubscribedToScan3d()); +} + +TEST_F(CommonDataSubscriberConfigTest, AnOdomFrameIdReplacesTheOdometryTopic) +{ + // With odom_frame_id set, the pose is read from TF instead. Leaving the topic + // subscribed as well would stall the synchronizer on a topic nobody publishes. + std::shared_ptr sub = start({ + rclcpp::Parameter("subscribe_odom", true), + rclcpp::Parameter("odom_frame_id", "odom")}); + + EXPECT_FALSE(sub->isSubscribedToOdom()); +} + +TEST_F(CommonDataSubscriberConfigTest, CamerasDefaultToApproximateSync) +{ + // Color and depth come off the sensor at slightly different instants. + EXPECT_TRUE(start()->isApproxSync()); +} + +TEST_F(CommonDataSubscriberConfigTest, StereoDefaultsToExactSync) +{ + // A stereo pair is hardware-triggered, so the two frames share a stamp exactly. + std::shared_ptr sub = start({ + rclcpp::Parameter("subscribe_stereo", true)}); + + EXPECT_FALSE(sub->isApproxSync()); +} + +TEST_F(CommonDataSubscriberConfigTest, AScanOnlyPipelineDefaultsToExactSync) +{ + // With no camera in the picture the remaining inputs are the scan and the odometry + // computed from it, which carries the scan's own stamp. + std::shared_ptr sub = start({ + rclcpp::Parameter("subscribe_depth", false), + rclcpp::Parameter("subscribe_rgb", false), + rclcpp::Parameter("subscribe_scan_cloud", true)}); + + EXPECT_FALSE(sub->isApproxSync()); +} + +TEST_F(CommonDataSubscriberConfigTest, AScanNextToACameraKeepsApproximateSync) +{ + // The exact default only applies when the scan is alone; a camera in the set puts + // the default back to approximate. + std::shared_ptr sub = start({ + rclcpp::Parameter("subscribe_scan_cloud", true)}); + + EXPECT_TRUE(sub->isSubscribedToDepth()); + EXPECT_TRUE(sub->isApproxSync()); +} + +TEST_F(CommonDataSubscriberConfigTest, ApproxSyncOverridesTheDefault) +{ + // The parameter is declared after the defaults are worked out, so an explicit value + // wins in both directions. + EXPECT_FALSE(start({rclcpp::Parameter("approx_sync", false)})->isApproxSync()); + + std::shared_ptr stereo = addNode( + std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({ + rclcpp::Parameter("subscribe_stereo", true), + rclcpp::Parameter("approx_sync", true)}))); + EXPECT_TRUE(stereo->isApproxSync()); +} + +TEST_F(CommonDataSubscriberConfigTest, ReportsTheConfiguredQueueSizes) +{ + std::shared_ptr sub = start({ + rclcpp::Parameter("topic_queue_size", 3), + rclcpp::Parameter("sync_queue_size", 7)}); + + EXPECT_EQ(sub->getTopicQueueSize(), 3); + EXPECT_EQ(sub->getSyncQueueSize(), 7); +} + +TEST_F(CommonDataSubscriberConfigTest, TheDeprecatedQueueSizeFeedsSyncQueueSize) +{ + // "queue_size" was split into the two above; the old name still has to work. + std::shared_ptr sub = start({ + rclcpp::Parameter("queue_size", 4)}); + + EXPECT_EQ(sub->getSyncQueueSize(), 4); + EXPECT_EQ(sub->getTopicQueueSize(), 10) << "the topic queue keeps its own default"; +} + +TEST_F(CommonDataSubscriberConfigTest, SyncQueueSizeWinsOverTheDeprecatedName) +{ + std::shared_ptr sub = start({ + rclcpp::Parameter("queue_size", 4), + rclcpp::Parameter("sync_queue_size", 9)}); + + EXPECT_EQ(sub->getSyncQueueSize(), 9); +} + +TEST_F(CommonDataSubscriberConfigTest, CountsOneRGBDCamera) +{ + std::shared_ptr sub = start({ + rclcpp::Parameter("subscribe_rgbd", true)}); + + EXPECT_EQ(sub->rgbdCameras(), 1); +} + +TEST_F(CommonDataSubscriberConfigTest, ReportsNoRGBDCamerasOnTheRGBDImagesInterface) +{ + // rgbd_cameras=0 switches to the single RGBDImages topic, whose camera count is only + // known per message -- so there is no fixed number to report. + std::shared_ptr sub = start({ + rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("rgbd_cameras", 0)}); + + EXPECT_TRUE(sub->isSubscribedToRGBD()); + EXPECT_EQ(sub->rgbdCameras(), 0); +} + +TEST_F(CommonDataSubscriberConfigTest, ReportsNoRGBDCamerasWhenNotSubscribedToRGBD) +{ + EXPECT_EQ(start()->rgbdCameras(), 0); +} + +TEST_F(CommonDataSubscriberConfigTest, NothingIsSubscribedWhenEveryInputIsOff) +{ + std::shared_ptr sub = start({ + rclcpp::Parameter("subscribe_depth", false), + rclcpp::Parameter("subscribe_rgb", false), + rclcpp::Parameter("subscribe_odom", false)}); + + EXPECT_FALSE(sub->isDataSubscribed()); + spinFor(std::chrono::milliseconds(200)); + EXPECT_TRUE(sub->empty()); +} + +#ifndef RTABMAP_SYNC_USER_DATA +TEST_F(CommonDataSubscriberConfigTest, UserDataIsRefusedUnlessBuiltIn) +{ + // The user-data synchronizers are behind a build option, because they double the + // number of synchronizer templates the package has to compile. + std::shared_ptr sub = start({ + rclcpp::Parameter("subscribe_user_data", true)}); + + EXPECT_TRUE(sub->isSubscribedToDepth()) << "the rest of the setup must still happen"; + spinFor(std::chrono::milliseconds(100)); + EXPECT_EQ(helper()->count_publishers("/user_data"), 0u); +} +#endif + +#ifndef RTABMAP_SYNC_MULTI_RGBD +TEST_F(CommonDataSubscriberConfigTest, MoreThanOneRGBDCameraIsRefusedUnlessBuiltIn) +{ + // Synchronizing several RGBDImage topics is behind a build option for the same + // reason. Without it, nothing is subscribed -- rgbd_cameras=0 is the way out. + std::shared_ptr sub = start({ + rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("rgbd_cameras", 2)}); + + spinFor(std::chrono::milliseconds(200)); + EXPECT_EQ(helper()->count_subscribers("/rgbd_image0"), 0u); + EXPECT_EQ(helper()->count_subscribers("/rgbd_image"), 0u); + EXPECT_TRUE(sub->empty()); +} +#endif diff --git a/rtabmap_sync/test/test_common_data_subscriber_sync.cpp b/rtabmap_sync/test/test_common_data_subscriber_sync.cpp new file mode 100644 index 00000000..ba2d0f63 --- /dev/null +++ b/rtabmap_sync/test/test_common_data_subscriber_sync.cpp @@ -0,0 +1,540 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "common_data_subscriber_fixture.hpp" + +#include + +using namespace rtabmap_sync_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); +} + +/// End-to-end: real messages in on the topics each mode subscribes to, one callback out. +/// +/// Every set below is published with identical stamps, so the result does not depend on +/// which sync policy the mode defaults to. What each test pins down is the wiring: which +/// topics a given combination of subscribe_* flags listens on, which of the four +/// callbacks fires, and which slots of it are filled. +class CommonDataSubscriberSyncTest : public CommonDataSubscriberTest {}; + +TEST_F(CommonDataSubscriberSyncTest, DepthModeDeliversOneCameraToTheMultiCameraCallback) +{ + start({rclcpp::Parameter("subscribe_odom", false)}); + + rclcpp::Publisher::SharedPtr rgb = + advertise("rgb/image"); + rclcpp::Publisher::SharedPtr depth = + advertise("depth/image"); + rclcpp::Publisher::SharedPtr info = + advertise("rgb/camera_info"); + + rgb->publish(makeRgbImage("camera_link", 1000.0)); + depth->publish(makeDepthImage("camera_link", 1000.0)); + info->publish(makeCameraInfo("camera_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + + const RecordingSubscriber::Record & got = sub_->back(); + EXPECT_EQ(got.kind, RecordingSubscriber::Record::kMultiCamera); + EXPECT_EQ(got.images, 1u); + EXPECT_EQ(got.depths, 1u); + EXPECT_EQ(got.cameraInfos, 1u); + EXPECT_EQ(got.frameId, "camera_link"); + EXPECT_DOUBLE_EQ(got.stamp, 1000.0); + EXPECT_FALSE(got.hasOdom); + EXPECT_FALSE(got.hasOdomInfo); + EXPECT_FALSE(got.hasScan2d); + EXPECT_FALSE(got.hasScan3d); +} + +TEST_F(CommonDataSubscriberSyncTest, DepthModeWithOdometryWaitsForThePose) +{ + start(); // subscribe_odom defaults to true + + rclcpp::Publisher::SharedPtr rgb = + advertise("rgb/image"); + rclcpp::Publisher::SharedPtr depth = + advertise("depth/image"); + rclcpp::Publisher::SharedPtr info = + advertise("rgb/camera_info"); + rclcpp::Publisher::SharedPtr odom = + advertise("odom"); + + // The camera alone is not a complete set. + rgb->publish(makeRgbImage("camera_link", 1000.0)); + depth->publish(makeDepthImage("camera_link", 1000.0)); + info->publish(makeCameraInfo("camera_link", 1000.0)); + spinFor(std::chrono::milliseconds(300)); + EXPECT_TRUE(sub_->empty()) << "without the pose the frame cannot be placed in the map"; + + odom->publish(makeOdometry("odom", 1000.0, 1.5)); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + EXPECT_TRUE(sub_->back().hasOdom); +} + +TEST_F(CommonDataSubscriberSyncTest, DepthModeCanAlsoTakeTheOdometryInfo) +{ + start({rclcpp::Parameter("subscribe_odom", false), + rclcpp::Parameter("subscribe_odom_info", true)}); + EXPECT_TRUE(sub_->isSubscribedToOdomInfo()); + + rclcpp::Publisher::SharedPtr rgb = + advertise("rgb/image"); + rclcpp::Publisher::SharedPtr depth = + advertise("depth/image"); + rclcpp::Publisher::SharedPtr info = + advertise("rgb/camera_info"); + rclcpp::Publisher::SharedPtr odomInfo = + advertise("odom_info"); + + rgb->publish(makeRgbImage("camera_link", 1000.0)); + depth->publish(makeDepthImage("camera_link", 1000.0)); + info->publish(makeCameraInfo("camera_link", 1000.0)); + odomInfo->publish(makeOdomInfo("odom", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + + EXPECT_TRUE(sub_->back().hasOdomInfo); + EXPECT_FALSE(sub_->back().hasOdom) << "the info is not the pose"; +} + +TEST_F(CommonDataSubscriberSyncTest, DepthModeCarriesATwoDScanAlongside) +{ + start({rclcpp::Parameter("subscribe_odom", false), + rclcpp::Parameter("subscribe_scan", true)}); + + rclcpp::Publisher::SharedPtr rgb = + advertise("rgb/image"); + rclcpp::Publisher::SharedPtr depth = + advertise("depth/image"); + rclcpp::Publisher::SharedPtr info = + advertise("rgb/camera_info"); + rclcpp::Publisher::SharedPtr scan = + advertise("scan"); + + rgb->publish(makeRgbImage("camera_link", 1000.0)); + depth->publish(makeDepthImage("camera_link", 1000.0)); + info->publish(makeCameraInfo("camera_link", 1000.0)); + scan->publish(makeLaserScan("base_scan", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + + EXPECT_EQ(sub_->back().images, 1u); + EXPECT_TRUE(sub_->back().hasScan2d); + EXPECT_FALSE(sub_->back().hasScan3d); +} + +TEST_F(CommonDataSubscriberSyncTest, DepthModeCarriesAThreeDScanAlongside) +{ + start({rclcpp::Parameter("subscribe_odom", false), + rclcpp::Parameter("subscribe_scan_cloud", true)}); + + rclcpp::Publisher::SharedPtr rgb = + advertise("rgb/image"); + rclcpp::Publisher::SharedPtr depth = + advertise("depth/image"); + rclcpp::Publisher::SharedPtr info = + advertise("rgb/camera_info"); + rclcpp::Publisher::SharedPtr cloud = + advertise("scan_cloud"); + + rgb->publish(makeRgbImage("camera_link", 1000.0)); + depth->publish(makeDepthImage("camera_link", 1000.0)); + info->publish(makeCameraInfo("camera_link", 1000.0)); + cloud->publish(makeScanCloud("lidar_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + + EXPECT_TRUE(sub_->back().hasScan3d); + EXPECT_FALSE(sub_->back().hasScan2d); +} + +TEST_F(CommonDataSubscriberSyncTest, AScanDescriptorIsUnpackedIntoScanAndDescriptor) +{ + // The descriptor topic replaces the scan topic and carries the scan inside it, plus + // the global descriptor computed from that same scan. + start({rclcpp::Parameter("subscribe_odom", false), + rclcpp::Parameter("subscribe_scan_descriptor", true)}); + + rclcpp::Publisher::SharedPtr rgb = + advertise("rgb/image"); + rclcpp::Publisher::SharedPtr depth = + advertise("depth/image"); + rclcpp::Publisher::SharedPtr info = + advertise("rgb/camera_info"); + rclcpp::Publisher::SharedPtr descriptor = + advertise("scan_descriptor"); + + rgb->publish(makeRgbImage("camera_link", 1000.0)); + depth->publish(makeDepthImage("camera_link", 1000.0)); + info->publish(makeCameraInfo("camera_link", 1000.0)); + descriptor->publish(makeScanDescriptor("base_scan", 1000.0, + /*with2d=*/true, /*with3d=*/false, /*withGlobalDescriptor=*/true)); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + + EXPECT_TRUE(sub_->back().hasScan2d) << "the scan inside the descriptor must be used"; + EXPECT_EQ(sub_->back().globalDescriptors, 1u); +} + +TEST_F(CommonDataSubscriberSyncTest, AnEmptyGlobalDescriptorIsNotForwarded) +{ + // An empty descriptor is "none computed", not a descriptor of length zero. + start({rclcpp::Parameter("subscribe_odom", false), + rclcpp::Parameter("subscribe_scan_descriptor", true)}); + + rclcpp::Publisher::SharedPtr rgb = + advertise("rgb/image"); + rclcpp::Publisher::SharedPtr depth = + advertise("depth/image"); + rclcpp::Publisher::SharedPtr info = + advertise("rgb/camera_info"); + rclcpp::Publisher::SharedPtr descriptor = + advertise("scan_descriptor"); + + rgb->publish(makeRgbImage("camera_link", 1000.0)); + depth->publish(makeDepthImage("camera_link", 1000.0)); + info->publish(makeCameraInfo("camera_link", 1000.0)); + descriptor->publish(makeScanDescriptor("base_scan", 1000.0, + /*with2d=*/true, /*with3d=*/false, /*withGlobalDescriptor=*/false)); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + + EXPECT_TRUE(sub_->back().hasScan2d); + EXPECT_EQ(sub_->back().globalDescriptors, 0u); +} + +TEST_F(CommonDataSubscriberSyncTest, RGBModeDeliversNoDepth) +{ + start({rclcpp::Parameter("subscribe_depth", false), + rclcpp::Parameter("subscribe_rgb", true), + rclcpp::Parameter("subscribe_odom", false)}); + + rclcpp::Publisher::SharedPtr rgb = + advertise("rgb/image"); + rclcpp::Publisher::SharedPtr info = + advertise("rgb/camera_info"); + + rgb->publish(makeRgbImage("camera_link", 1000.0)); + info->publish(makeCameraInfo("camera_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + + EXPECT_EQ(sub_->back().images, 1u); + EXPECT_EQ(sub_->back().depths, 0u) + << "an empty depth vector is how the callback learns there is no depth"; + EXPECT_EQ(sub_->back().cameraInfos, 1u); +} + +TEST_F(CommonDataSubscriberSyncTest, StereoModeDeliversTheRightImageInTheDepthSlot) +{ + start({rclcpp::Parameter("subscribe_stereo", true), + rclcpp::Parameter("subscribe_odom", false)}); + + rclcpp::Publisher::SharedPtr left = + advertise("left/image_rect"); + rclcpp::Publisher::SharedPtr right = + advertise("right/image_rect"); + rclcpp::Publisher::SharedPtr leftInfo = + advertise("left/camera_info"); + rclcpp::Publisher::SharedPtr rightInfo = + advertise("right/camera_info"); + + left->publish(makeMonoImage("left_frame", 1000.0)); + right->publish(makeMonoImage("left_frame", 1000.0)); + leftInfo->publish(makeCameraInfo("left_frame", 1000.0)); + rightInfo->publish(makeCameraInfo("left_frame", 1000.0, 8, 8, /*tx=*/-12.0)); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + + EXPECT_EQ(sub_->back().images, 1u); + EXPECT_EQ(sub_->back().depths, 1u); + EXPECT_EQ(sub_->back().frameId, "left_frame"); +} + +TEST_F(CommonDataSubscriberSyncTest, RGBDModeUnpacksTheMessageIntoImages) +{ + start({rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("subscribe_odom", false)}); + + rclcpp::Publisher::SharedPtr rgbd = + advertise("rgbd_image"); + + rgbd->publish(makeRGBDImage("camera_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + + const RecordingSubscriber::Record & got = sub_->back(); + EXPECT_EQ(got.kind, RecordingSubscriber::Record::kMultiCamera); + EXPECT_EQ(got.images, 1u); + EXPECT_EQ(got.depths, 1u); + EXPECT_EQ(got.cameraInfos, 1u); + EXPECT_EQ(got.frameId, "camera_link"); +} + +TEST_F(CommonDataSubscriberSyncTest, RGBDModeCarriesAScanAlongside) +{ + start({rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("subscribe_odom", false), + rclcpp::Parameter("subscribe_scan_cloud", true)}); + + rclcpp::Publisher::SharedPtr rgbd = + advertise("rgbd_image"); + rclcpp::Publisher::SharedPtr cloud = + advertise("scan_cloud"); + + rgbd->publish(makeRGBDImage("camera_link", 1000.0)); + cloud->publish(makeScanCloud("lidar_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + + EXPECT_EQ(sub_->back().images, 1u); + EXPECT_TRUE(sub_->back().hasScan3d); +} + +TEST_F(CommonDataSubscriberSyncTest, TheRGBDImagesInterfaceDeliversEveryCamera) +{ + // rgbd_cameras=0 takes a pre-grouped RGBDImages -- what rgbdx_sync publishes -- so + // any number of cameras works without the multi-RGBD build option. + start({rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("rgbd_cameras", 0), + rclcpp::Parameter("subscribe_odom", false)}); + + rclcpp::Publisher::SharedPtr rgbdx = + advertise("rgbd_images"); + + rtabmap_msgs::msg::RGBDImages msg; + msg.header.frame_id = "camera0_link"; + msg.header.stamp = stampOf(1000.0); + msg.rgbd_images.push_back(makeRGBDImage("camera0_link", 1000.0)); + msg.rgbd_images.push_back(makeRGBDImage("camera1_link", 1000.0)); + msg.rgbd_images.push_back(makeRGBDImage("camera2_link", 1000.0)); + rgbdx->publish(msg); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + + const RecordingSubscriber::Record & got = sub_->back(); + EXPECT_EQ(got.images, 3u); + EXPECT_EQ(got.depths, 3u); + EXPECT_EQ(got.cameraInfos, 3u); + EXPECT_EQ(got.frameId, "camera0_link"); +} + +TEST_F(CommonDataSubscriberSyncTest, ATwoDScanAloneGoesToTheLaserScanCallback) +{ + start({rclcpp::Parameter("subscribe_depth", false), + rclcpp::Parameter("subscribe_rgb", false), + rclcpp::Parameter("subscribe_odom", false), + rclcpp::Parameter("subscribe_scan", true)}); + + rclcpp::Publisher::SharedPtr scan = + advertise("scan"); + + scan->publish(makeLaserScan("base_scan", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + + const RecordingSubscriber::Record & got = sub_->back(); + EXPECT_EQ(got.kind, RecordingSubscriber::Record::kLaserScan); + EXPECT_TRUE(got.hasScan2d); + EXPECT_FALSE(got.hasScan3d); + EXPECT_EQ(got.frameId, "base_scan"); + EXPECT_DOUBLE_EQ(got.stamp, 1000.0); +} + +TEST_F(CommonDataSubscriberSyncTest, AThreeDScanAloneGoesToTheLaserScanCallback) +{ + start({rclcpp::Parameter("subscribe_depth", false), + rclcpp::Parameter("subscribe_rgb", false), + rclcpp::Parameter("subscribe_odom", false), + rclcpp::Parameter("subscribe_scan_cloud", true)}); + + rclcpp::Publisher::SharedPtr cloud = + advertise("scan_cloud"); + + cloud->publish(makeScanCloud("lidar_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + + const RecordingSubscriber::Record & got = sub_->back(); + EXPECT_EQ(got.kind, RecordingSubscriber::Record::kLaserScan); + EXPECT_TRUE(got.hasScan3d); + EXPECT_EQ(got.frameId, "lidar_link"); +} + +TEST_F(CommonDataSubscriberSyncTest, AScanWithOdometryIsSynchronizedWithIt) +{ + start({rclcpp::Parameter("subscribe_depth", false), + rclcpp::Parameter("subscribe_rgb", false), + rclcpp::Parameter("subscribe_scan_cloud", true)}); + EXPECT_TRUE(sub_->isSubscribedToOdom()); + + rclcpp::Publisher::SharedPtr cloud = + advertise("scan_cloud"); + rclcpp::Publisher::SharedPtr odom = + advertise("odom"); + + cloud->publish(makeScanCloud("lidar_link", 1000.0)); + spinFor(std::chrono::milliseconds(300)); + EXPECT_TRUE(sub_->empty()); + + odom->publish(makeOdometry("odom", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + EXPECT_TRUE(sub_->back().hasOdom); + EXPECT_TRUE(sub_->back().hasScan3d); +} + +TEST_F(CommonDataSubscriberSyncTest, ASensorDataGoesToItsOwnCallback) +{ + start({rclcpp::Parameter("subscribe_sensor_data", true), + rclcpp::Parameter("subscribe_odom", false)}); + + rclcpp::Publisher::SharedPtr data = + advertise("sensor_data"); + + data->publish(makeSensorData("camera_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + + const RecordingSubscriber::Record & got = sub_->back(); + EXPECT_EQ(got.kind, RecordingSubscriber::Record::kSensorData); + EXPECT_EQ(got.cameraInfos, 1u); + EXPECT_EQ(got.frameId, "camera_link"); + EXPECT_FALSE(got.hasOdom); +} + +TEST_F(CommonDataSubscriberSyncTest, ASensorDataCanBeSynchronizedWithOdometry) +{ + start({rclcpp::Parameter("subscribe_sensor_data", true)}); + + rclcpp::Publisher::SharedPtr data = + advertise("sensor_data"); + rclcpp::Publisher::SharedPtr odom = + advertise("odom"); + + data->publish(makeSensorData("camera_link", 1000.0)); + odom->publish(makeOdometry("odom", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + + EXPECT_EQ(sub_->back().kind, RecordingSubscriber::Record::kSensorData); + EXPECT_TRUE(sub_->back().hasOdom); +} + +TEST_F(CommonDataSubscriberSyncTest, OdometryAloneGoesToTheOdomCallback) +{ + start({rclcpp::Parameter("subscribe_depth", false), + rclcpp::Parameter("subscribe_rgb", false)}); + + rclcpp::Publisher::SharedPtr odom = + advertise("odom"); + + odom->publish(makeOdometry("odom", 1000.0, 2.5)); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + + const RecordingSubscriber::Record & got = sub_->back(); + EXPECT_EQ(got.kind, RecordingSubscriber::Record::kOdom); + EXPECT_TRUE(got.hasOdom); + EXPECT_FALSE(got.hasOdomInfo); + EXPECT_EQ(got.frameId, "odom"); + EXPECT_DOUBLE_EQ(got.stamp, 1000.0); +} + +TEST_F(CommonDataSubscriberSyncTest, OdometryAndItsInfoAreSynchronizedTogether) +{ + start({rclcpp::Parameter("subscribe_depth", false), + rclcpp::Parameter("subscribe_rgb", false), + rclcpp::Parameter("subscribe_odom_info", true)}); + + rclcpp::Publisher::SharedPtr odom = + advertise("odom"); + rclcpp::Publisher::SharedPtr odomInfo = + advertise("odom_info"); + + odom->publish(makeOdometry("odom", 1000.0)); + spinFor(std::chrono::milliseconds(300)); + EXPECT_TRUE(sub_->empty()) << "the pair is incomplete until the info arrives"; + + odomInfo->publish(makeOdomInfo("odom", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + EXPECT_TRUE(sub_->back().hasOdom); + EXPECT_TRUE(sub_->back().hasOdomInfo); +} + +TEST_F(CommonDataSubscriberSyncTest, DeliversEveryFrameOfAStream) +{ + start({rclcpp::Parameter("subscribe_odom", false)}); + + rclcpp::Publisher::SharedPtr rgb = + advertise("rgb/image"); + rclcpp::Publisher::SharedPtr depth = + advertise("depth/image"); + rclcpp::Publisher::SharedPtr info = + advertise("rgb/camera_info"); + + for(int i=0; i<5; ++i) + { + const double stamp = 1000.0 + 0.1*double(i); + rgb->publish(makeRgbImage("camera_link", stamp)); + depth->publish(makeDepthImage("camera_link", stamp)); + info->publish(makeCameraInfo("camera_link", stamp)); + ASSERT_TRUE(spinUntil([&, i]() { return sub_->size() == size_t(i+1); })) + << "frame " << i << " never arrived"; + } + + ASSERT_EQ(sub_->size(), 5u); + for(size_t i=1; isize(); ++i) + { + EXPECT_GT(sub_->records()[i].stamp, sub_->records()[i-1].stamp); + } +} + +TEST_F(CommonDataSubscriberSyncTest, ExactSyncDropsAnIncompleteSet) +{ + // With approx_sync off every input has to carry the same stamp, which is the whole + // point of the setting -- and the most common reason a pipeline goes quiet. + start({rclcpp::Parameter("subscribe_odom", false), + rclcpp::Parameter("approx_sync", false)}); + + rclcpp::Publisher::SharedPtr rgb = + advertise("rgb/image"); + rclcpp::Publisher::SharedPtr depth = + advertise("depth/image"); + rclcpp::Publisher::SharedPtr info = + advertise("rgb/camera_info"); + + rgb->publish(makeRgbImage("camera_link", 1000.000)); + depth->publish(makeDepthImage("camera_link", 1000.002)); + info->publish(makeCameraInfo("camera_link", 1000.000)); + spinFor(std::chrono::milliseconds(400)); + EXPECT_TRUE(sub_->empty()); + + rgb->publish(makeRgbImage("camera_link", 1001.0)); + depth->publish(makeDepthImage("camera_link", 1001.0)); + info->publish(makeCameraInfo("camera_link", 1001.0)); + EXPECT_TRUE(spinUntil([&]() { return !sub_->empty(); })); +} + +TEST_F(CommonDataSubscriberSyncTest, PublishesDiagnostics) +{ + start({rclcpp::Parameter("subscribe_odom", false)}); + + std::shared_ptr> diagnostics = + collect("/diagnostics"); + + rclcpp::Publisher::SharedPtr rgb = + advertise("rgb/image"); + rclcpp::Publisher::SharedPtr depth = + advertise("depth/image"); + rclcpp::Publisher::SharedPtr info = + advertise("rgb/camera_info"); + + rgb->publish(makeRgbImage("camera_link", 1000.0)); + depth->publish(makeDepthImage("camera_link", 1000.0)); + info->publish(makeCameraInfo("camera_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !diagnostics->empty(); }, + std::chrono::milliseconds(10000))); + + bool sawInput = false; + bool sawOutput = false; + for(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr & msg : + diagnostics->messages) + { + for(const diagnostic_msgs::msg::DiagnosticStatus & status : msg->status) + { + sawInput = sawInput || status.name.find("Input Status") != std::string::npos; + sawOutput = sawOutput || status.name.find("Output Status") != std::string::npos; + } + } + EXPECT_TRUE(sawInput); + EXPECT_TRUE(sawOutput) << "tick() is what the subclass calls to report its own rate"; +} diff --git a/rtabmap_sync/test/test_rgb_sync.cpp b/rtabmap_sync/test/test_rgb_sync.cpp new file mode 100644 index 00000000..cb2902a3 --- /dev/null +++ b/rtabmap_sync/test/test_rgb_sync.cpp @@ -0,0 +1,258 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "node_test_utils.hpp" +#include "msg_builders.hpp" + +#include + +#include + +#include +#include + +using namespace rtabmap_sync_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); +} + +/// Drives rgb_sync over its two input topics and collects both outputs. +class RGBSyncTest : public NodeTest +{ +protected: + void start(const std::vector & params = {}) + { + node_ = addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(params))); + + out_ = collect("rgbd_image"); + rgbPub_ = helper()->create_publisher("rgb/image", 10); + infoPub_ = helper()->create_publisher( + "rgb/camera_info", 10); + ASSERT_TRUE(waitForSubscriber(rgbPub_)); + ASSERT_TRUE(waitForSubscriber(infoPub_)); + ASSERT_TRUE(waitForSubscribedFromNode("rgbd_image")); + } + + void collectCompressed() + { + compressed_ = collect("rgbd_image/compressed"); + ASSERT_TRUE(waitForSubscribedFromNode("rgbd_image/compressed")); + } + + /// Waits until the node under test sees a subscriber on @p topic. @see RGBDSyncTest. + bool waitForSubscribedFromNode(const std::string & topic) + { + return spinUntil([&]() { return node_->count_subscribers(topic) > 0; }); + } + + void publish(double stamp, int width = 8, int height = 8) + { + rgbPub_->publish(makeRgbImage("camera_link", stamp, width, height)); + infoPub_->publish(makeCameraInfo("camera_link", stamp, width, height)); + } + + std::shared_ptr node_; + std::shared_ptr> out_; + std::shared_ptr> compressed_; + rclcpp::Publisher::SharedPtr rgbPub_; + rclcpp::Publisher::SharedPtr infoPub_; +}; + +TEST_F(RGBSyncTest, PacksColorAndCalibrationIntoAnRGBDImage) +{ + start(); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = out_->back(); + EXPECT_EQ(got.header.frame_id, "camera_link") << "the frame comes from the camera_info"; + EXPECT_DOUBLE_EQ(rclcpp::Time(got.header.stamp).seconds(), 1000.0); + EXPECT_EQ(got.rgb.encoding, "bgr8"); + EXPECT_EQ(got.rgb.width, 8u); + EXPECT_NEAR(got.rgb_camera_info.k[0], 100.0, 1e-9); +} + +TEST_F(RGBSyncTest, LeavesDepthEmptyByDefault) +{ + // The point of this node is an RGB-only pipeline: there is no depth to carry, and a + // consumer has to be able to tell that from an all-zero depth image. + start(); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = out_->back(); + EXPECT_TRUE(got.depth.data.empty()); + EXPECT_EQ(got.depth.width, 0u); + EXPECT_EQ(got.depth_camera_info.width, 0u) << "no depth means no depth calibration"; +} + +TEST_F(RGBSyncTest, FillEmptyDepthAddsAZeroedDepthImage) +{ + // Some consumers refuse a message without depth. This gives them one that is + // entirely "no reading", which is how zero is interpreted in a depth image. + start({rclcpp::Parameter("fill_empty_depth", true)}); + + publish(1000.0, /*width=*/8, /*height=*/8); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = out_->back(); + ASSERT_FALSE(got.depth.data.empty()); + EXPECT_EQ(got.depth.encoding, "16UC1"); + EXPECT_EQ(got.depth.width, 8u); + EXPECT_EQ(got.depth.height, 8u); + for(size_t i=0; ipublish(makeRgbImage("camera_link", 1000.0)); + infoPub_->publish(makeCameraInfo("camera_link", 1000.004)); + spinFor(std::chrono::milliseconds(500)); + EXPECT_TRUE(out_->empty()) << "the default must not pair stamps 4 ms apart"; + + publish(1001.0); + EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); })); +} + +TEST_F(RGBSyncTest, ExactSyncRejectsFramesWithDifferentStamps) +{ + start({rclcpp::Parameter("approx_sync", false)}); + + rgbPub_->publish(makeRgbImage("camera_link", 1000.0)); + infoPub_->publish(makeCameraInfo("camera_link", 1000.004)); + spinFor(std::chrono::milliseconds(500)); + EXPECT_TRUE(out_->empty()); + + publish(1001.0); + EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); })); +} + +TEST_F(RGBSyncTest, ApproxSyncPairsAnImageWithANearbyCameraInfo) +{ + // A camera_info republished on its own timer does not carry the image's stamp. + start({rclcpp::Parameter("approx_sync", true)}); + + for(int i=0; i<5; ++i) + { + const double stamp = 1000.0 + 0.1*double(i); + rgbPub_->publish(makeRgbImage("camera_link", stamp)); + infoPub_->publish(makeCameraInfo("camera_link", stamp + 0.004)); + spinFor(std::chrono::milliseconds(20)); + } + EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); })); +} + +TEST_F(RGBSyncTest, CompressesColorAsJpeg) +{ + start(); + collectCompressed(); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !compressed_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = compressed_->back(); + ASSERT_FALSE(got.rgb_compressed.data.empty()); + EXPECT_NE(got.rgb_compressed.format.find("jp"), std::string::npos) + << "expected a jpeg format, got \"" << got.rgb_compressed.format << "\""; + EXPECT_TRUE(got.rgb.data.empty()) << "the compressed output carries no raw image"; + EXPECT_TRUE(got.depth_compressed.data.empty()) + << "without fill_empty_depth there is nothing to compress on the depth side"; +} + +TEST_F(RGBSyncTest, CompressesTheFakeDepthAsPng) +{ + start({rclcpp::Parameter("fill_empty_depth", true)}); + collectCompressed(); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !compressed_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = compressed_->back(); + ASSERT_FALSE(got.depth_compressed.data.empty()); + EXPECT_EQ(got.depth_compressed.format, "png"); + const cv::Mat depth = rtabmap::uncompressImage(got.depth_compressed.data); + ASSERT_FALSE(depth.empty()); + EXPECT_EQ(depth.type(), CV_16UC1); + EXPECT_EQ(cv::countNonZero(depth), 0) << "the fake depth is all zeros"; +} + +TEST_F(RGBSyncTest, CompressedRateThrottlesTheCompressedOutputOnly) +{ + // A long window (0.2 Hz = five seconds); the throttle runs off the wall clock, so a + // slow machine must not spill the four frames into a second one. + start({rclcpp::Parameter("compressed_rate", 0.2)}); + collectCompressed(); + + for(int i=0; i<4; ++i) + { + publish(1000.0 + 0.01*double(i)); + ASSERT_TRUE(spinUntil([&, i]() { return out_->size() == size_t(i+1); })); + } + + spinFor(std::chrono::milliseconds(200)); + EXPECT_EQ(out_->size(), 4u) << "the raw output is never throttled"; + EXPECT_EQ(compressed_->size(), 1u) + << "only the first of four back-to-back frames may be compressed"; +} + +TEST_F(RGBSyncTest, StaysSilentWithoutASubscriber) +{ + addNode(std::make_shared(rclcpp::NodeOptions())); + rclcpp::Publisher::SharedPtr rgbPub = + helper()->create_publisher("rgb/image", 10); + rclcpp::Publisher::SharedPtr infoPub = + helper()->create_publisher("rgb/camera_info", 10); + ASSERT_TRUE(waitForSubscriber(rgbPub)); + ASSERT_TRUE(waitForSubscriber(infoPub)); + + rgbPub->publish(makeRgbImage("camera_link", 1000.0)); + infoPub->publish(makeCameraInfo("camera_link", 1000.0)); + spinFor(std::chrono::milliseconds(300)); + + std::shared_ptr> late = + collect("rgbd_image"); + spinFor(std::chrono::milliseconds(200)); + EXPECT_TRUE(late->empty()); +} + +TEST_F(RGBSyncTest, IsNamedAfterItself) +{ + // It used to default to "rgbd_sync", which put it on top of the other node's name + // in the graph whenever both were launched without an explicit name. + start(); + EXPECT_STREQ(node_->get_name(), "rgb_sync"); +} + +TEST_F(RGBSyncTest, AcceptsTheDeprecatedQueueSizeParameter) +{ + start({rclcpp::Parameter("queue_size", 5)}); + + publish(1000.0); + EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); })); +} + +TEST_F(RGBSyncTest, SubscribesBestEffortWhenAsked) +{ + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({rclcpp::Parameter("qos", 2)}))); + + rclcpp::Publisher::SharedPtr bestEffort = + helper()->create_publisher( + "rgb/image", rclcpp::QoS(10).best_effort()); + EXPECT_TRUE(waitForSubscriber(bestEffort)); +} diff --git a/rtabmap_sync/test/test_rgbd_sync.cpp b/rtabmap_sync/test/test_rgbd_sync.cpp new file mode 100644 index 00000000..cc6c5c8d --- /dev/null +++ b/rtabmap_sync/test/test_rgbd_sync.cpp @@ -0,0 +1,530 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "node_test_utils.hpp" +#include "msg_builders.hpp" + +#include + +#include + +#include + +#include + +using namespace rtabmap_sync_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); +} + +/// Drives rgbd_sync over its three input topics and collects both outputs. +class RGBDSyncTest : public NodeTest +{ +protected: + /// Starts the node with @p params and wires up the inputs and the raw output. + void start(const std::vector & params = {}) + { + node_ = addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(params))); + + out_ = collect("rgbd_image"); + rgbPub_ = helper()->create_publisher("rgb/image", 10); + depthPub_ = helper()->create_publisher("depth/image", 10); + infoPub_ = helper()->create_publisher( + "rgb/camera_info", 10); + ASSERT_TRUE(waitForSubscriber(rgbPub_)); + ASSERT_TRUE(waitForSubscriber(depthPub_)); + ASSERT_TRUE(waitForSubscriber(infoPub_)); + ASSERT_TRUE(waitForSubscribedFromNode("rgbd_image")); + } + + /// Also subscribes to the compressed output. Call right after start(). + void collectCompressed() + { + compressed_ = collect("rgbd_image/compressed"); + ASSERT_TRUE(waitForSubscribedFromNode("rgbd_image/compressed")); + } + + /** + * @brief Waits until the node under test sees a subscriber on @p topic. + * + * Both outputs are published only when subscribed, and it is the node's own view of + * the graph that decides. Waiting on the subscriber's side instead leaves a window + * in which the test is connected but the node does not know it yet, and the first + * frame is silently dropped. + */ + bool waitForSubscribedFromNode(const std::string & topic) + { + return spinUntil([&]() { return node_->count_subscribers(topic) > 0; }); + } + + /// Publishes one set of inputs, letting each carry its own stamp. + void publishStamps(double rgbStamp, double depthStamp, double infoStamp) + { + rgbPub_->publish(makeRgbImage("camera_link", rgbStamp)); + depthPub_->publish(makeDepthImage("camera_link", depthStamp)); + infoPub_->publish(makeCameraInfo("camera_link", infoStamp)); + } + + /// Publishes one hardware-synchronized set: every input carries the same stamp. + void publish(double stamp, int width = 8, int height = 8, + uint16_t depthMillimeters = 1500) + { + rgbPub_->publish(makeRgbImage("camera_link", stamp, width, height)); + depthPub_->publish( + makeDepthImage("camera_link", stamp, width, height, depthMillimeters)); + infoPub_->publish(makeCameraInfo("camera_link", stamp, width, height)); + } + + /** + * @brief Publishes @p count frames 100 ms apart, with depth trailing color. + * + * The approximate policy cannot emit a pair the moment it arrives: it has to wait + * until a later message proves no better match is coming. Feeding it a stream is + * therefore the only way to observe approximate matching at all. + * + * @param depthOffset seconds added to the depth stamp; color and camera_info share + * the frame stamp. + */ + void publishStream(size_t count, double depthOffset, double start = 1000.0) + { + for(size_t i=0; ipublish(makeRgbImage("camera_link", stamp)); + depthPub_->publish(makeDepthImage("camera_link", stamp + depthOffset)); + infoPub_->publish(makeCameraInfo("camera_link", stamp)); + spinFor(std::chrono::milliseconds(20)); + } + } + + /// True if @p stamp is one of @p stamps, to the nanosecond the stamp was built from. + static bool isOneOf(const std::vector & stamps, double stamp) + { + for(double candidate : stamps) + { + if(std::fabs(candidate - stamp) < 1e-6) + { + return true; + } + } + return false; + } + + std::shared_ptr node_; + std::vector rgbStamps_; + std::vector depthStamps_; + std::shared_ptr> out_; + std::shared_ptr> compressed_; + rclcpp::Publisher::SharedPtr rgbPub_; + rclcpp::Publisher::SharedPtr depthPub_; + rclcpp::Publisher::SharedPtr infoPub_; +}; + +TEST_F(RGBDSyncTest, PacksTheThreeInputsIntoOneMessage) +{ + start(); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = out_->back(); + EXPECT_EQ(got.header.frame_id, "camera_link"); + EXPECT_DOUBLE_EQ(rclcpp::Time(got.header.stamp).seconds(), 1000.0); + EXPECT_EQ(got.rgb.encoding, "bgr8"); + EXPECT_EQ(got.rgb.width, 8u); + EXPECT_EQ(got.depth.encoding, "16UC1"); + EXPECT_EQ(got.depth.width, 8u); + EXPECT_NEAR(got.rgb_camera_info.k[0], 100.0, 1e-9); + EXPECT_NEAR(got.depth_camera_info.k[0], 100.0, 1e-9) + << "a single camera_info is copied into both slots"; + EXPECT_TRUE(got.rgb_compressed.data.empty()) << "the raw output carries raw images"; + EXPECT_TRUE(got.depth_compressed.data.empty()); +} + +TEST_F(RGBDSyncTest, TakesTheFrameIdFromTheCameraInfo) +{ + // The images may be stamped in an optical frame while the camera_info names the + // frame the calibration is expressed in; the latter is what the output must carry. + start(); + + rgbPub_->publish(makeRgbImage("camera_rgb_optical_frame", 1000.0)); + depthPub_->publish(makeDepthImage("camera_depth_optical_frame", 1000.0)); + infoPub_->publish(makeCameraInfo("camera_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_EQ(out_->back().header.frame_id, "camera_link"); +} + +TEST_F(RGBDSyncTest, StampsTheOutputWithTheLaterOfTheTwoImages) +{ + // Approximate sync pairs frames that are close but not equal. The output stamp is + // the later of the two, so the message is never stamped before data it contains. + start({rclcpp::Parameter("approx_sync", true)}); + + // Depth trails color by 5 ms, so every output must carry its depth frame's stamp. + publishStream(/*count=*/5, /*depthOffset=*/0.005); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + for(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr & msg : out_->messages) + { + const double stamp = rclcpp::Time(msg->header.stamp).seconds(); + EXPECT_TRUE(isOneOf(depthStamps_, stamp)) + << "expected the later (depth) stamp, got " << stamp; + EXPECT_FALSE(isOneOf(rgbStamps_, stamp)); + } +} + +TEST_F(RGBDSyncTest, ApproxSyncPairsFramesWithDifferentStamps) +{ + start({rclcpp::Parameter("approx_sync", true)}); + + publishStream(/*count=*/5, /*depthOffset=*/0.004); + EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); })) + << "approximate sync must pair inputs whose stamps only nearly agree"; +} + +TEST_F(RGBDSyncTest, ExactSyncRejectsFramesWithDifferentStamps) +{ + start({rclcpp::Parameter("approx_sync", false)}); + + publishStamps(/*rgb=*/1000.000, /*depth=*/1000.004, /*info=*/1000.008); + spinFor(std::chrono::milliseconds(500)); + EXPECT_TRUE(out_->empty()) << "exact sync must not pair mismatched stamps"; + + // The same node does produce output once the stamps agree exactly. + publish(1001.0); + EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); })); +} + +TEST_F(RGBDSyncTest, ApproxSyncMaxIntervalRejectsDistantFrames) +{ + // The guard against silently pairing a stale frame with a fresh one. + start({rclcpp::Parameter("approx_sync", true), + rclcpp::Parameter("approx_sync_max_interval", 0.01)}); + + // Depth lags by 550 ms. The frames are 100 ms apart, so no depth frame lands within + // 10 ms of any color frame -- not even a much older one. + publishStream(/*count=*/6, /*depthOffset=*/0.55); + spinFor(std::chrono::milliseconds(500)); + EXPECT_TRUE(out_->empty()) << "no pair is within the 10 ms interval"; + + publishStream(/*count=*/6, /*depthOffset=*/0.002, /*start=*/2000.0); + EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); })) + << "2 ms apart is within the interval and must still be paired"; +} + +TEST_F(RGBDSyncTest, DecimationScalesTheImagesAndTheCalibration) +{ + start({rclcpp::Parameter("decimation", 2)}); + + publish(1000.0, /*width=*/8, /*height=*/8); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = out_->back(); + EXPECT_EQ(got.rgb.width, 4u); + EXPECT_EQ(got.rgb.height, 4u); + EXPECT_EQ(got.depth.width, 4u); + EXPECT_EQ(got.depth.height, 4u); + EXPECT_NEAR(got.rgb_camera_info.k[0], 50.0, 1e-6) + << "the focal length must be halved with the image, or the cloud comes out wrong"; + EXPECT_EQ(got.rgb_camera_info.width, 4u); + EXPECT_EQ(got.depth_camera_info.width, 4u); +} + +TEST_F(RGBDSyncTest, DecimationIsDisabledWhenItWouldNotDivideTheDepthImage) +{ + // A decimation that does not divide the depth size exactly would misalign depth + // against color, so the node gives up on it rather than producing a wrong cloud. + start({rclcpp::Parameter("decimation", 3)}); + + publish(1000.0, /*width=*/8, /*height=*/8); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_EQ(out_->back().rgb.width, 8u) << "images must be passed through unresized"; + EXPECT_EQ(out_->back().depth.width, 8u); + EXPECT_NEAR(out_->back().rgb_camera_info.k[0], 100.0, 1e-9); +} + +TEST_F(RGBDSyncTest, ADecimationBelowOneIsClampedToOne) +{ + start({rclcpp::Parameter("decimation", 0)}); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + EXPECT_EQ(out_->back().rgb.width, 8u); +} + +TEST_F(RGBDSyncTest, DepthScaleMultipliesTheDepthValues) +{ + // For a driver that publishes depth in the wrong unit: 1500 in a 16UC1 image is + // 1.5 m only if the unit really is millimeters. + start({rclcpp::Parameter("depth_scale", 2.0)}); + + publish(1000.0, 8, 8, /*depthMillimeters=*/1500); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = out_->back(); + ASSERT_EQ(got.depth.encoding, "16UC1"); + ASSERT_GE(got.depth.data.size(), 2u); + EXPECT_EQ(*reinterpret_cast(got.depth.data.data()), 3000) + << "every depth pixel must be scaled"; +} + +TEST_F(RGBDSyncTest, CompressesColorAsJpegAndDepthAsPng) +{ + start(); + collectCompressed(); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !compressed_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = compressed_->back(); + EXPECT_FALSE(got.rgb_compressed.data.empty()); + EXPECT_FALSE(got.depth_compressed.data.empty()); + EXPECT_EQ(got.depth_compressed.format, "png") << "depth must stay lossless"; + EXPECT_NE(got.rgb_compressed.format.find("jp"), std::string::npos) + << "expected a jpeg format, got \"" << got.rgb_compressed.format << "\""; + EXPECT_TRUE(got.rgb.data.empty()) << "the compressed output carries no raw images"; + EXPECT_TRUE(got.depth.data.empty()); + EXPECT_EQ(got.header.frame_id, "camera_link"); +} + +TEST_F(RGBDSyncTest, TheCompressedDepthDecompressesBackToTheInput) +{ + start(); + collectCompressed(); + + publish(1000.0, 8, 8, /*depthMillimeters=*/1234); + ASSERT_TRUE(spinUntil([&]() { return !compressed_->empty(); })); + + const cv::Mat depth = + rtabmap::uncompressImage(compressed_->back().depth_compressed.data); + ASSERT_FALSE(depth.empty()); + EXPECT_EQ(depth.type(), CV_16UC1); + EXPECT_EQ(depth.cols, 8); + EXPECT_EQ(depth.rows, 8); + EXPECT_EQ(depth.at(0, 0), 1234) + << "png is lossless, so the value must survive the round trip exactly"; +} + +TEST_F(RGBDSyncTest, CompressedRateThrottlesTheCompressedOutputOnly) +{ + // Compression is expensive and the compressed topic usually feeds a slow link, so + // it can be published at a lower rate than the raw one. + // The throttle is measured against the wall clock, not the message stamps, so the + // window has to be long enough that a slow machine still gets all four frames + // inside it -- 0.2 Hz gives five seconds for what takes milliseconds when idle. + start({rclcpp::Parameter("compressed_rate", 0.2)}); + collectCompressed(); + + for(int i=0; i<4; ++i) + { + publish(1000.0 + 0.01*double(i)); + ASSERT_TRUE(spinUntil([&, i]() { return out_->size() == size_t(i+1); })); + } + + spinFor(std::chrono::milliseconds(200)); + EXPECT_EQ(out_->size(), 4u) << "the raw output is never throttled"; + EXPECT_EQ(compressed_->size(), 1u) + << "only the first of four back-to-back frames may be compressed"; +} + +TEST_F(RGBDSyncTest, PublishesEveryFrameCompressedWhenTheRateIsUnset) +{ + start(); + collectCompressed(); + + for(int i=0; i<3; ++i) + { + publish(1000.0 + 0.01*double(i)); + ASSERT_TRUE(spinUntil([&, i]() { return out_->size() == size_t(i+1); })); + } + + // The compressed message is published before the raw one but may be delivered after. + spinUntil([&]() { return compressed_->size() == 3u; }); + EXPECT_EQ(compressed_->size(), 3u) << "compressed_rate 0 means no throttling"; +} + +TEST_F(RGBDSyncTest, PublishesBothOutputsWhenBothHaveSubscribers) +{ + start(); + collectCompressed(); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty() && !compressed_->empty(); })); + + EXPECT_FALSE(out_->back().rgb.data.empty()); + EXPECT_FALSE(compressed_->back().rgb_compressed.data.empty()); + EXPECT_EQ(out_->back().header.stamp, compressed_->back().header.stamp); +} + +TEST_F(RGBDSyncTest, DoesNotCompressWhenOnlyTheRawOutputIsSubscribed) +{ + // Compression is the expensive half of this node; it must not run for nobody. + start(); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + std::shared_ptr> late = + collect("rgbd_image/compressed"); + ASSERT_TRUE(waitForPublisher(late->subscription)); + spinFor(std::chrono::milliseconds(200)); + EXPECT_TRUE(late->empty()) << "subscribing late must not deliver a back catalogue"; +} + +TEST_F(RGBDSyncTest, StaysSilentWithoutAnySubscriber) +{ + addNode(std::make_shared(rclcpp::NodeOptions())); + rclcpp::Publisher::SharedPtr rgbPub = + helper()->create_publisher("rgb/image", 10); + rclcpp::Publisher::SharedPtr depthPub = + helper()->create_publisher("depth/image", 10); + rclcpp::Publisher::SharedPtr infoPub = + helper()->create_publisher("rgb/camera_info", 10); + ASSERT_TRUE(waitForSubscriber(rgbPub)); + ASSERT_TRUE(waitForSubscriber(depthPub)); + ASSERT_TRUE(waitForSubscriber(infoPub)); + + rgbPub->publish(makeRgbImage("camera_link", 1000.0)); + depthPub->publish(makeDepthImage("camera_link", 1000.0)); + infoPub->publish(makeCameraInfo("camera_link", 1000.0)); + spinFor(std::chrono::milliseconds(300)); + + std::shared_ptr> late = + collect("rgbd_image"); + spinFor(std::chrono::milliseconds(200)); + EXPECT_TRUE(late->empty()); +} + +TEST_F(RGBDSyncTest, SyncsRepeatedFramesInOrder) +{ + start(); + + for(int i=0; i<5; ++i) + { + publish(1000.0 + 0.1*double(i)); + ASSERT_TRUE(spinUntil([&, i]() { return out_->size() == size_t(i+1); })) + << "frame " << i << " was not synchronized"; + } + + ASSERT_EQ(out_->size(), 5u); + for(size_t i=1; isize(); ++i) + { + EXPECT_GT(rclcpp::Time(out_->messages[i]->header.stamp).seconds(), + rclcpp::Time(out_->messages[i-1]->header.stamp).seconds()) + << "frames must come out in the order they went in"; + } +} + +TEST_F(RGBDSyncTest, AcceptsTheDeprecatedQueueSizeParameter) +{ + // "queue_size" was renamed to "sync_queue_size"; the old name still has to work. + start({rclcpp::Parameter("queue_size", 5)}); + + publish(1000.0); + EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); })); +} + +TEST_F(RGBDSyncTest, PublishesDiagnostics) +{ + start(); + + std::shared_ptr> diagnostics = + collect("/diagnostics"); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !diagnostics->empty(); }, + std::chrono::milliseconds(10000))); + + bool sawInput = false; + bool sawOutput = false; + for(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr & msg : + diagnostics->messages) + { + for(const diagnostic_msgs::msg::DiagnosticStatus & status : msg->status) + { + sawInput = sawInput || status.name.find("Input Status") != std::string::npos; + sawOutput = sawOutput || status.name.find("Output Status") != std::string::npos; + } + } + EXPECT_TRUE(sawInput) << "the input rate is what tells an operator a topic went quiet"; + EXPECT_TRUE(sawOutput); +} + +/// QoS of the subscriptions, which has to match the driver or nothing arrives at all. +/// +/// A reliable subscription refuses to match a best-effort publisher, while a best-effort +/// subscription matches either. Whether a connection is established at all is therefore +/// what tells us which reliability the node picked. +class RGBDSyncQosTest : public NodeTest +{ +protected: + enum Reliability { kSystemDefault = 0, kReliable = 1, kBestEffort = 2 }; + + void startSync(const std::vector & params) + { + addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(params))); + } + + template + typename rclcpp::Publisher::SharedPtr input( + const std::string & topic, Reliability reliability) + { + rclcpp::QoS qos(10); + reliability == kBestEffort ? qos.best_effort() : qos.reliable(); + return helper()->create_publisher(topic, qos); + } +}; + +TEST_F(RGBDSyncQosTest, SubscribesBestEffortWhenAsked) +{ + // The common case: a camera driver publishing sensor data best effort. + startSync({rclcpp::Parameter("qos", int(kBestEffort))}); + + EXPECT_TRUE(waitForSubscriber( + input("rgb/image", kBestEffort))); + EXPECT_TRUE(waitForSubscriber( + input("rgb/camera_info", kBestEffort))); +} + +TEST_F(RGBDSyncQosTest, QosCameraInfoOverridesQosOnTheCameraInfoOnly) +{ + // Drivers commonly publish images best effort but camera_info reliable, so the two + // have to be settable apart. + startSync({rclcpp::Parameter("qos", int(kBestEffort)), + rclcpp::Parameter("qos_camera_info", int(kReliable))}); + + rclcpp::Publisher::SharedPtr bestEffortInfo = + input("rgb/camera_info", kBestEffort); + spinFor(std::chrono::milliseconds(500)); + EXPECT_EQ(bestEffortInfo->get_subscription_count(), 0u) + << "a reliable camera_info subscription must refuse a best-effort publisher"; + + EXPECT_TRUE(waitForSubscriber(input("rgb/image", kBestEffort))) + << "the image side must have kept qos"; +} + +TEST_F(RGBDSyncQosTest, PublishesWithTheConfiguredReliability) +{ + startSync({rclcpp::Parameter("qos", int(kBestEffort))}); + + std::shared_ptr> reliable = + collect( + "rgbd_image", rclcpp::QoS(10).reliable()); + spinFor(std::chrono::milliseconds(500)); + EXPECT_EQ(reliable->subscription->get_publisher_count(), 0u) + << "the output must be best effort too, so a reliable consumer cannot match it"; + + std::shared_ptr> bestEffort = + collect( + "rgbd_image", rclcpp::QoS(10).best_effort()); + EXPECT_TRUE(waitForPublisher(bestEffort->subscription)); +} diff --git a/rtabmap_sync/test/test_rgbdx_sync.cpp b/rtabmap_sync/test/test_rgbdx_sync.cpp new file mode 100644 index 00000000..22ab6fe5 --- /dev/null +++ b/rtabmap_sync/test/test_rgbdx_sync.cpp @@ -0,0 +1,263 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "node_test_utils.hpp" +#include "msg_builders.hpp" + +#include + +#include + +#include + +#include +#include + +using namespace rtabmap_sync_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); +} + +/// Drives rgbdx_sync over the rgbd_image0..N topics for a configurable camera count. +class RGBDXSyncTest : public NodeTest +{ +protected: + /// Starts the node for @p cameras cameras and wires up one publisher per camera. + void start(int cameras, const std::vector & extra = {}) + { + std::vector params = extra; + params.push_back(rclcpp::Parameter("rgbd_cameras", cameras)); + addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(params))); + + out_ = collect("rgbd_images"); + for(int i=0; icreate_publisher( + "rgbd_image" + std::to_string(i), 10)); + ASSERT_TRUE(waitForSubscriber(pubs_.back())); + } + ASSERT_TRUE(waitForPublisher(out_->subscription)); + } + + /// Publishes one frame per camera, all carrying @p stamp. + void publish(double stamp) + { + for(size_t i=0; ipublish(makeRGBDImage( + "camera" + std::to_string(i) + "_link", stamp, 8, 8, + // A distinct color per camera, so the order can be checked. + cv::Scalar(double(10*(i+1)), 20, 30))); + } + } + + std::shared_ptr> out_; + std::vector::SharedPtr> pubs_; +}; + +TEST_F(RGBDXSyncTest, PacksTwoCamerasIntoOneMessage) +{ + start(2); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImages & got = out_->back(); + ASSERT_EQ(got.rgbd_images.size(), 2u); + EXPECT_EQ(got.header.frame_id, "camera0_link") + << "the container takes the first camera's header"; + EXPECT_DOUBLE_EQ(rclcpp::Time(got.header.stamp).seconds(), 1000.0); + EXPECT_EQ(got.rgbd_images[0].header.frame_id, "camera0_link"); + EXPECT_EQ(got.rgbd_images[1].header.frame_id, "camera1_link"); +} + +TEST_F(RGBDXSyncTest, KeepsTheCamerasInTopicOrder) +{ + // Downstream matches each image against a calibration by index, so the order of the + // array has to follow the rgbd_imageN numbering and nothing else. + start(3); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImages & got = out_->back(); + ASSERT_EQ(got.rgbd_images.size(), 3u); + for(size_t i=0; i<3; ++i) + { + ASSERT_FALSE(got.rgbd_images[i].rgb.data.empty()); + EXPECT_EQ(got.rgbd_images[i].rgb.data[0], uint8_t(10*(i+1))) + << "camera " << i << " is not where it should be"; + } +} + +TEST_F(RGBDXSyncTest, CarriesTheImagesThroughUnchanged) +{ + start(2); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & first = out_->back().rgbd_images[0]; + EXPECT_EQ(first.rgb.encoding, "bgr8"); + EXPECT_EQ(first.rgb.width, 8u); + EXPECT_EQ(first.depth.encoding, "16UC1"); + EXPECT_NEAR(first.rgb_camera_info.k[0], 100.0, 1e-9) + << "this node only groups messages; it never touches their content"; +} + +TEST_F(RGBDXSyncTest, SupportsUpToEightCameras) +{ + start(8); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + EXPECT_EQ(out_->back().rgbd_images.size(), 8u); +} + +TEST_F(RGBDXSyncTest, RejectsACameraCountBelowTwo) +{ + // One camera needs no grouping at all -- use the RGBDImage topic directly. Saying so + // at construction beats starting a node that can never publish. + EXPECT_THROW( + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({rclcpp::Parameter("rgbd_cameras", 1)}))), + UException); +} + +TEST_F(RGBDXSyncTest, RejectsACameraCountAboveEight) +{ + EXPECT_THROW( + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({rclcpp::Parameter("rgbd_cameras", 9)}))), + UException); +} + +TEST_F(RGBDXSyncTest, WaitsForEveryCamera) +{ + // A set is only published once every camera has contributed: a partial set would + // silently drop a camera's field of view from the map. + start(3); + + pubs_[0]->publish(makeRGBDImage("camera0_link", 1000.0)); + pubs_[1]->publish(makeRGBDImage("camera1_link", 1000.0)); + spinFor(std::chrono::milliseconds(500)); + EXPECT_TRUE(out_->empty()) << "two of three cameras is not a set"; + + pubs_[2]->publish(makeRGBDImage("camera2_link", 1000.0)); + EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); })); +} + +TEST_F(RGBDXSyncTest, ApproxSyncPairsCamerasWithDifferentStamps) +{ + // Separate USB cameras never share a stamp, which is why approximate is the default. + start(2); + + for(int i=0; i<5; ++i) + { + const double stamp = 1000.0 + 0.1*double(i); + pubs_[0]->publish(makeRGBDImage("camera0_link", stamp)); + pubs_[1]->publish(makeRGBDImage("camera1_link", stamp + 0.004)); + spinFor(std::chrono::milliseconds(20)); + } + EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); })); +} + +TEST_F(RGBDXSyncTest, ExactSyncRejectsCamerasWithDifferentStamps) +{ + start(2, {rclcpp::Parameter("approx_sync", false)}); + + pubs_[0]->publish(makeRGBDImage("camera0_link", 1000.000)); + pubs_[1]->publish(makeRGBDImage("camera1_link", 1000.004)); + spinFor(std::chrono::milliseconds(500)); + EXPECT_TRUE(out_->empty()); + + publish(1001.0); + EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); })); +} + +TEST_F(RGBDXSyncTest, ApproxSyncMaxIntervalRejectsDistantFrames) +{ + start(2, {rclcpp::Parameter("approx_sync_max_interval", 0.01)}); + + for(int i=0; i<6; ++i) + { + const double stamp = 1000.0 + 0.1*double(i); + pubs_[0]->publish(makeRGBDImage("camera0_link", stamp)); + pubs_[1]->publish(makeRGBDImage("camera1_link", stamp + 0.55)); + spinFor(std::chrono::milliseconds(20)); + } + spinFor(std::chrono::milliseconds(300)); + EXPECT_TRUE(out_->empty()) << "no pair is within the 10 ms interval"; + + for(int i=0; i<6; ++i) + { + const double stamp = 2000.0 + 0.1*double(i); + pubs_[0]->publish(makeRGBDImage("camera0_link", stamp)); + pubs_[1]->publish(makeRGBDImage("camera1_link", stamp + 0.002)); + spinFor(std::chrono::milliseconds(20)); + } + EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); })); +} + +TEST_F(RGBDXSyncTest, StampsTheOutputWithTheFirstCamera) +{ + // Unlike the two-image nodes, which take the later stamp, this one is a container: + // each image keeps its own stamp and the container takes camera 0's. + start(2); + + for(int i=0; i<5; ++i) + { + const double stamp = 1000.0 + 0.1*double(i); + pubs_[0]->publish(makeRGBDImage("camera0_link", stamp)); + pubs_[1]->publish(makeRGBDImage("camera1_link", stamp + 0.005)); + spinFor(std::chrono::milliseconds(20)); + } + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImages & got = out_->back(); + ASSERT_EQ(got.rgbd_images.size(), 2u); + EXPECT_EQ(got.header.stamp, got.rgbd_images[0].header.stamp); + EXPECT_NE(got.header.stamp, got.rgbd_images[1].header.stamp) + << "the second camera must keep its own stamp"; +} + +TEST_F(RGBDXSyncTest, SyncsRepeatedSetsInOrder) +{ + start(2); + + for(int i=0; i<5; ++i) + { + publish(1000.0 + 0.1*double(i)); + ASSERT_TRUE(spinUntil([&, i]() { return out_->size() == size_t(i+1); })); + } + + ASSERT_EQ(out_->size(), 5u); + for(size_t i=1; isize(); ++i) + { + EXPECT_GT(rclcpp::Time(out_->messages[i]->header.stamp).seconds(), + rclcpp::Time(out_->messages[i-1]->header.stamp).seconds()); + } +} + +TEST_F(RGBDXSyncTest, AcceptsTheDeprecatedQueueSizeParameter) +{ + start(2, {rclcpp::Parameter("queue_size", 5)}); + + publish(1000.0); + EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); })); +} + +TEST_F(RGBDXSyncTest, SubscribesBestEffortWhenAsked) +{ + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({rclcpp::Parameter("qos", 2)}))); + + rclcpp::Publisher::SharedPtr bestEffort = + helper()->create_publisher( + "rgbd_image0", rclcpp::QoS(10).best_effort()); + EXPECT_TRUE(waitForSubscriber(bestEffort)); +} diff --git a/rtabmap_sync/test/test_stereo_sync.cpp b/rtabmap_sync/test/test_stereo_sync.cpp new file mode 100644 index 00000000..384d194a --- /dev/null +++ b/rtabmap_sync/test/test_stereo_sync.cpp @@ -0,0 +1,323 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "node_test_utils.hpp" +#include "msg_builders.hpp" + +#include + +#include +#include + +using namespace rtabmap_sync_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); +} + +/// Drives stereo_sync over its four input topics and collects both outputs. +class StereoSyncTest : public NodeTest +{ +protected: + void start(const std::vector & params = {}) + { + node_ = addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(params))); + + out_ = collect("rgbd_image"); + leftPub_ = helper()->create_publisher( + "left/image_rect", 10); + rightPub_ = helper()->create_publisher( + "right/image_rect", 10); + leftInfoPub_ = helper()->create_publisher( + "left/camera_info", 10); + rightInfoPub_ = helper()->create_publisher( + "right/camera_info", 10); + ASSERT_TRUE(waitForSubscriber(leftPub_)); + ASSERT_TRUE(waitForSubscriber(rightPub_)); + ASSERT_TRUE(waitForSubscriber(leftInfoPub_)); + ASSERT_TRUE(waitForSubscriber(rightInfoPub_)); + ASSERT_TRUE(waitForSubscribedFromNode("rgbd_image")); + } + + void collectCompressed() + { + compressed_ = collect("rgbd_image/compressed"); + ASSERT_TRUE(waitForSubscribedFromNode("rgbd_image/compressed")); + } + + /// Waits until the node under test sees a subscriber on @p topic. @see RGBDSyncTest. + bool waitForSubscribedFromNode(const std::string & topic) + { + return spinUntil([&]() { return node_->count_subscribers(topic) > 0; }); + } + + /// Publishes a hardware-synchronized stereo pair, which is what this node expects. + void publish(double stamp, int width = 8, int height = 8) + { + publishStamps(stamp, stamp, width, height); + } + + /// Publishes a pair whose two images carry different stamps. + void publishStamps(double leftStamp, double rightStamp, + int width = 8, int height = 8) + { + leftPub_->publish(makeMonoImage("left_frame", leftStamp, width, height, 60)); + rightPub_->publish(makeMonoImage("right_frame", rightStamp, width, height, 90)); + leftInfoPub_->publish(makeCameraInfo("left_frame", leftStamp, width, height)); + // The right camera carries the baseline in P(0,3): -fx * baseline. + rightInfoPub_->publish( + makeCameraInfo("left_frame", rightStamp, width, height, kBaselineTx)); + } + + /// P(0,3) of the right camera for a 100 px focal length and a 12 cm baseline. + static constexpr double kBaselineTx = -12.0; + + std::shared_ptr node_; + std::shared_ptr> out_; + std::shared_ptr> compressed_; + rclcpp::Publisher::SharedPtr leftPub_; + rclcpp::Publisher::SharedPtr rightPub_; + rclcpp::Publisher::SharedPtr leftInfoPub_; + rclcpp::Publisher::SharedPtr rightInfoPub_; +}; + +constexpr double StereoSyncTest::kBaselineTx; + +TEST_F(StereoSyncTest, PacksTheStereoPairIntoTheRgbAndDepthSlots) +{ + // An RGBDImage carrying a stereo pair puts the left image where color goes and the + // right image where depth goes; the baseline in the second camera_info is what tells + // a consumer to read it that way. + start(); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = out_->back(); + EXPECT_EQ(got.header.frame_id, "left_frame"); + EXPECT_DOUBLE_EQ(rclcpp::Time(got.header.stamp).seconds(), 1000.0); + ASSERT_FALSE(got.rgb.data.empty()); + ASSERT_FALSE(got.depth.data.empty()); + EXPECT_EQ(got.rgb.encoding, "mono8"); + EXPECT_EQ(got.depth.encoding, "mono8") << "the right image is not depth"; + EXPECT_EQ(got.rgb.data[0], 60) << "rgb must be the left image"; + EXPECT_EQ(got.depth.data[0], 90) << "depth must be the right image"; +} + +TEST_F(StereoSyncTest, CarriesTheBaselineInTheSecondCameraInfo) +{ + start(); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = out_->back(); + EXPECT_DOUBLE_EQ(got.rgb_camera_info.p[3], 0.0) << "the left camera is the origin"; + EXPECT_DOUBLE_EQ(got.depth_camera_info.p[3], kBaselineTx) + << "without the baseline nothing downstream can triangulate"; +} + +TEST_F(StereoSyncTest, DefaultsToExactSync) +{ + // Stereo pairs come off hardware-triggered sensors, so the default is the exact + // policy: it is cheaper and cannot mismatch left with right. + start(); + + publishStamps(/*left=*/1000.0, /*right=*/1000.004); + spinFor(std::chrono::milliseconds(500)); + EXPECT_TRUE(out_->empty()) << "the default must not pair frames 4 ms apart"; + + publish(1001.0); + EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); })); +} + +TEST_F(StereoSyncTest, ApproxSyncPairsFramesWithDifferentStamps) +{ + // For a pair of free-running cameras, which is what approx_sync is there for. + start({rclcpp::Parameter("approx_sync", true)}); + + for(int i=0; i<5; ++i) + { + const double stamp = 1000.0 + 0.1*double(i); + publishStamps(stamp, stamp + 0.004); + spinFor(std::chrono::milliseconds(20)); + } + EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); })); +} + +TEST_F(StereoSyncTest, StampsTheOutputWithTheLaterOfTheTwoImages) +{ + start({rclcpp::Parameter("approx_sync", true)}); + + std::vector rightStamps; + for(int i=0; i<5; ++i) + { + const double stamp = 1000.0 + 0.1*double(i); + rightStamps.push_back(stamp + 0.005); + publishStamps(stamp, stamp + 0.005); + spinFor(std::chrono::milliseconds(20)); + } + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + for(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr & msg : out_->messages) + { + const double stamp = rclcpp::Time(msg->header.stamp).seconds(); + bool matched = false; + for(double candidate : rightStamps) + { + matched = matched || std::fabs(candidate - stamp) < 1e-6; + } + EXPECT_TRUE(matched) << "expected the later (right) stamp, got " << stamp; + } +} + +TEST_F(StereoSyncTest, ApproxSyncMaxIntervalRejectsDistantFrames) +{ + start({rclcpp::Parameter("approx_sync", true), + rclcpp::Parameter("approx_sync_max_interval", 0.01)}); + + // The right camera lags by 550 ms; the frames are 100 ms apart, so nothing lands + // within the interval, not even an older frame. + for(int i=0; i<6; ++i) + { + const double stamp = 1000.0 + 0.1*double(i); + publishStamps(stamp, stamp + 0.55); + spinFor(std::chrono::milliseconds(20)); + } + spinFor(std::chrono::milliseconds(300)); + EXPECT_TRUE(out_->empty()); + + for(int i=0; i<6; ++i) + { + const double stamp = 2000.0 + 0.1*double(i); + publishStamps(stamp, stamp + 0.002); + spinFor(std::chrono::milliseconds(20)); + } + EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); })); +} + +TEST_F(StereoSyncTest, CompressesBothImagesAsJpeg) +{ + // Both halves of a stereo pair are ordinary images, so both take the lossy path -- + // unlike rgbd_sync, where depth has to stay lossless. + start(); + collectCompressed(); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !compressed_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = compressed_->back(); + ASSERT_FALSE(got.rgb_compressed.data.empty()); + ASSERT_FALSE(got.depth_compressed.data.empty()); + EXPECT_NE(got.rgb_compressed.format.find("jp"), std::string::npos) + << "expected a jpeg format, got \"" << got.rgb_compressed.format << "\""; + EXPECT_NE(got.depth_compressed.format.find("jp"), std::string::npos) + << "expected a jpeg format, got \"" << got.depth_compressed.format << "\""; + EXPECT_NE(got.depth_compressed.format, "png") + << "the right image must not take the lossless depth path"; + EXPECT_TRUE(got.rgb.data.empty()) << "the compressed output carries no raw images"; + EXPECT_TRUE(got.depth.data.empty()); + EXPECT_DOUBLE_EQ(got.depth_camera_info.p[3], kBaselineTx) + << "the calibration must survive compression"; +} + +TEST_F(StereoSyncTest, CompressedRateThrottlesTheCompressedOutputOnly) +{ + // A long window (0.2 Hz = five seconds); the throttle runs off the wall clock, so a + // slow machine must not spill the four frames into a second one. + start({rclcpp::Parameter("compressed_rate", 0.2)}); + collectCompressed(); + + for(int i=0; i<4; ++i) + { + publish(1000.0 + 0.01*double(i)); + ASSERT_TRUE(spinUntil([&, i]() { return out_->size() == size_t(i+1); })); + } + + spinFor(std::chrono::milliseconds(200)); + EXPECT_EQ(out_->size(), 4u) << "the raw output is never throttled"; + EXPECT_EQ(compressed_->size(), 1u); +} + +TEST_F(StereoSyncTest, StaysSilentWithoutASubscriber) +{ + addNode(std::make_shared(rclcpp::NodeOptions())); + rclcpp::Publisher::SharedPtr leftPub = + helper()->create_publisher("left/image_rect", 10); + rclcpp::Publisher::SharedPtr rightPub = + helper()->create_publisher("right/image_rect", 10); + rclcpp::Publisher::SharedPtr leftInfo = + helper()->create_publisher("left/camera_info", 10); + rclcpp::Publisher::SharedPtr rightInfo = + helper()->create_publisher("right/camera_info", 10); + ASSERT_TRUE(waitForSubscriber(leftPub)); + ASSERT_TRUE(waitForSubscriber(rightPub)); + ASSERT_TRUE(waitForSubscriber(leftInfo)); + ASSERT_TRUE(waitForSubscriber(rightInfo)); + + leftPub->publish(makeMonoImage("left_frame", 1000.0)); + rightPub->publish(makeMonoImage("right_frame", 1000.0)); + leftInfo->publish(makeCameraInfo("left_frame", 1000.0)); + rightInfo->publish(makeCameraInfo("left_frame", 1000.0, 8, 8, kBaselineTx)); + spinFor(std::chrono::milliseconds(300)); + + std::shared_ptr> late = + collect("rgbd_image"); + spinFor(std::chrono::milliseconds(200)); + EXPECT_TRUE(late->empty()); +} + +TEST_F(StereoSyncTest, SyncsRepeatedPairsInOrder) +{ + start(); + + for(int i=0; i<5; ++i) + { + publish(1000.0 + 0.1*double(i)); + ASSERT_TRUE(spinUntil([&, i]() { return out_->size() == size_t(i+1); })); + } + + ASSERT_EQ(out_->size(), 5u); + for(size_t i=1; isize(); ++i) + { + EXPECT_GT(rclcpp::Time(out_->messages[i]->header.stamp).seconds(), + rclcpp::Time(out_->messages[i-1]->header.stamp).seconds()); + } +} + +TEST_F(StereoSyncTest, AcceptsColorInputToo) +{ + // A color stereo pair is just as valid; the encoding is carried through untouched. + start(); + + leftPub_->publish(makeRgbImage("left_frame", 1000.0)); + rightPub_->publish(makeRgbImage("left_frame", 1000.0)); + leftInfoPub_->publish(makeCameraInfo("left_frame", 1000.0)); + rightInfoPub_->publish(makeCameraInfo("left_frame", 1000.0, 8, 8, kBaselineTx)); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + EXPECT_EQ(out_->back().rgb.encoding, "bgr8"); + EXPECT_EQ(out_->back().depth.encoding, "bgr8"); +} + +TEST_F(StereoSyncTest, AcceptsTheDeprecatedQueueSizeParameter) +{ + start({rclcpp::Parameter("queue_size", 5)}); + + publish(1000.0); + EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); })); +} + +TEST_F(StereoSyncTest, SubscribesBestEffortWhenAsked) +{ + addNode(std::make_shared(rclcpp::NodeOptions() + .parameter_overrides({rclcpp::Parameter("qos", 2)}))); + + rclcpp::Publisher::SharedPtr bestEffort = + helper()->create_publisher( + "left/image_rect", rclcpp::QoS(10).best_effort()); + EXPECT_TRUE(waitForSubscriber(bestEffort)); +} diff --git a/rtabmap_sync/test/test_sync_diagnostic.cpp b/rtabmap_sync/test/test_sync_diagnostic.cpp new file mode 100644 index 00000000..dfa6dffc --- /dev/null +++ b/rtabmap_sync/test/test_sync_diagnostic.cpp @@ -0,0 +1,301 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include "node_test_utils.hpp" +#include "msg_builders.hpp" + +#include + +#include + +#include + +#include +#include + +using namespace rtabmap_sync_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); + +/// A DiagnosticTask that only exists to be recognized by name in the output. +class NamedTask : public diagnostic_updater::DiagnosticTask +{ +public: + explicit NamedTask(const std::string & name) : DiagnosticTask(name) {} + void run(diagnostic_updater::DiagnosticStatusWrapper & stat) override + { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "reporting for duty"); + } +}; +} // namespace + +/** + * @brief Drives a SyncDiagnostic directly and reads what it publishes on /diagnostics. + * + * The class is what every node in this package reports through: it watches the rate of + * the messages going into a synchronizer and the rate coming out, so that "the map + * stopped updating" can be told apart from "one camera went quiet". + */ +class SyncDiagnosticTest : public NodeTest +{ +protected: + /** + * @brief Creates and initializes the diagnostic, then starts spinning its node. + * + * The node joins the executor only once the diagnostic has created its publisher and + * its timers, and /diagnostics is subscribed only after that. Everything the tests + * then see is a periodic update; the one-off "Node starting up" notices the updater + * emits as each task is added are over with before anyone is listening. + */ + void start(const std::string & topic, + double tolerance = 0.5, int windowSize = 5, + std::vector otherTasks = {}) + { + node_ = std::make_shared("sync_diagnostic_test_node"); + diagnostic_ = std::make_unique( + node_.get(), tolerance, windowSize); + diagnostic_->init(topic, "nothing received", otherTasks); + addNode(node_); + + out_ = collect("/diagnostics"); + ASSERT_TRUE(waitForPublisher(out_->subscription)); + } + + void TearDown() override + { + diagnostic_.reset(); + node_.reset(); + NodeTest::TearDown(); + } + + /// The most recent status named @p name, or nullptr if none was ever published. + const diagnostic_msgs::msg::DiagnosticStatus * latest(const std::string & name) const + { + const diagnostic_msgs::msg::DiagnosticStatus * found = nullptr; + for(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr & msg : + out_->messages) + { + for(const diagnostic_msgs::msg::DiagnosticStatus & status : msg->status) + { + if(status.name.find(name) != std::string::npos) + { + found = &status; + } + } + } + return found; + } + + /// Spins until a status named @p name has been published at least once. + bool waitForStatus(const std::string & name) + { + return spinUntil([&]() { return latest(name) != nullptr; }, + std::chrono::milliseconds(10000)); + } + + /** + * @brief Ticks the input at @p hertz in real time, with stamps advancing to match. + * + * Both halves matter: the target rate is learned from the gaps between stamps, while + * the rate that is checked against it is measured off the wall clock. + */ + void tickInputFor(int count, double hertz) + { + const double period = 1.0/hertz; + const double start = nowSeconds(); + for(int i=0; itickInput(stampOf(start + period*double(i))); + spinFor(std::chrono::milliseconds(int(period*1000.0))); + } + } + + /** + * @brief The node's clock, which is where the stamps in these tests start. + * + * Each status also carries a TimeStampStatus, which fails a stamp more than a few + * seconds away from now -- the diagnostic for the unsynchronized-clock case. Stamps + * out of a fixed epoch would trip it and mask whatever the test was about. + */ + double nowSeconds() const { return node_->now().seconds(); } + + rclcpp::Node::SharedPtr node_; + std::unique_ptr diagnostic_; + std::shared_ptr> out_; +}; + +TEST_F(SyncDiagnosticTest, PublishesAnInputAndAnOutputStatus) +{ + // Two statuses, not one: a node can be receiving everything it asked for and still + // publish nothing, and the pair is what tells those apart. + start("/camera/rgb/image"); + + ASSERT_TRUE(waitForStatus("Input Status")); + EXPECT_TRUE(waitForStatus("Output Status")); +} + +TEST_F(SyncDiagnosticTest, DerivesTheHardwareIdFromTheTopic) +{ + // The last two segments of an image topic are the image and its side, so dropping + // them leaves the device: /back_camera/left/image belongs to "back_camera". + start("/back_camera/left/image"); + + ASSERT_TRUE(waitForStatus("Input Status")); + EXPECT_EQ(latest("Input Status")->hardware_id, "back_camera"); +} + +TEST_F(SyncDiagnosticTest, KeepsTheNamespaceOfADeeperTopic) +{ + start("/robot/front_camera/rgb/image_raw"); + + ASSERT_TRUE(waitForStatus("Input Status")); + EXPECT_EQ(latest("Input Status")->hardware_id, "robot/front_camera"); +} + +TEST_F(SyncDiagnosticTest, ReportsNoHardwareIdWhenThereIsNoTopicToNameIt) +{ + // The nodes that synchronize several topics at once pass an empty name, because no + // single one of them identifies the device. + start(""); + + ASSERT_TRUE(waitForStatus("Input Status")); + EXPECT_EQ(latest("Input Status")->hardware_id, "none"); +} + +TEST_F(SyncDiagnosticTest, AddsTheTasksItIsHandedAlongsideItsOwn) +{ + // rtabmap_slam adds its own task this way, so that the rate and the SLAM state come + // out in one /diagnostics message instead of two. + NamedTask task("Extra Task"); + start("/camera/rgb/image", 0.5, 5, {&task}); + + ASSERT_TRUE(waitForStatus("Extra Task")); + EXPECT_EQ(latest("Extra Task")->message, "reporting for duty"); + EXPECT_TRUE(waitForStatus("Input Status")) << "its own tasks must still be there"; +} + +TEST_F(SyncDiagnosticTest, ReportsAnErrorBeforeAnythingHasArrived) +{ + // A node that has never received a message is the failure this exists to surface. + start("/camera/rgb/image"); + + ASSERT_TRUE(waitForStatus("Input Status")); + EXPECT_NE(latest("Input Status")->level, diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(SyncDiagnosticTest, LearnsTheRateFromTheStampsAndReportsOkAtThatRate) +{ + start("/camera/rgb/image", /*tolerance=*/0.5, /*windowSize=*/5); + + // 20 Hz, with the stamps advancing 50 ms per tick to match. + tickInputFor(/*count=*/25, /*hertz=*/20.0); + + ASSERT_TRUE(waitForStatus("Input Status")); + EXPECT_EQ(latest("Input Status")->level, diagnostic_msgs::msg::DiagnosticStatus::OK) + << "status was: " << latest("Input Status")->message; +} + +TEST_F(SyncDiagnosticTest, ComplainsOnceAKnownInputGoesQuiet) +{ + start("/camera/rgb/image", /*tolerance=*/0.5, /*windowSize=*/5); + + tickInputFor(/*count=*/25, /*hertz=*/20.0); + ASSERT_TRUE(waitForStatus("Input Status")); + ASSERT_EQ(latest("Input Status")->level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + // The camera stops. The learned rate stays, so the measured one now falls short. + // The updater is built with a 2 s period, so this has to span more than one of them. + out_->messages.clear(); + spinFor(std::chrono::milliseconds(3000)); + + ASSERT_NE(latest("Input Status"), nullptr); + EXPECT_NE(latest("Input Status")->level, diagnostic_msgs::msg::DiagnosticStatus::OK) + << "a silent camera must not keep reporting OK"; +} + +TEST_F(SyncDiagnosticTest, TheOutputStatusFollowsTheInputRateByDefault) +{ + // A synchronizer that drops nothing publishes as fast as it receives, so the input + // rate is the right expectation for the output side until told otherwise. + start("/camera/rgb/image", /*tolerance=*/0.5, /*windowSize=*/5); + + const double period = 1.0/20.0; + const double start = nowSeconds(); + for(int i=0; i<25; ++i) + { + const rclcpp::Time stamp = stampOf(start + period*double(i)); + diagnostic_->tickInput(stamp); + diagnostic_->tickOutput(stamp); + spinFor(std::chrono::milliseconds(50)); + } + + ASSERT_TRUE(waitForStatus("Output Status")); + EXPECT_EQ(latest("Output Status")->level, diagnostic_msgs::msg::DiagnosticStatus::OK) + << "status was: " << latest("Output Status")->message; +} + +TEST_F(SyncDiagnosticTest, AnOutputSlowerThanItsInputIsReported) +{ + // The case worth catching: everything arrives, but the node only manages to produce + // half of it -- a dropped frame is invisible on the input side alone. + start("/camera/rgb/image", /*tolerance=*/0.5, /*windowSize=*/5); + + // Both sides declare 20 Hz, but only every fourth frame makes it out. + const double period = 1.0/20.0; + const double start = nowSeconds(); + for(int i=0; i<25; ++i) + { + const rclcpp::Time stamp = stampOf(start + period*double(i)); + diagnostic_->tickInput(stamp, /*expectedFrequency=*/20.0); + if(i % 4 == 0) + { + diagnostic_->tickOutput(stamp, /*expectedFrequency=*/20.0); + } + spinFor(std::chrono::milliseconds(50)); + } + + ASSERT_TRUE(waitForStatus("Output Status")); + EXPECT_NE(latest("Output Status")->level, diagnostic_msgs::msg::DiagnosticStatus::OK) + << "status was: " << latest("Output Status")->message; + EXPECT_EQ(latest("Input Status")->level, diagnostic_msgs::msg::DiagnosticStatus::OK) + << "the input side is healthy and must say so"; +} + +TEST_F(SyncDiagnosticTest, AnExplicitRateOverridesTheLearnedOne) +{ + // A node that knows its own target rate -- a throttled or decimated output -- says + // so rather than letting the stamps imply a rate it was never going to reach. + start("/camera/rgb/image", /*tolerance=*/0.5, /*windowSize=*/5); + + // Ticking at 5 Hz while declaring 5 Hz is fine, even though the stamps say 20 Hz. + const double start = nowSeconds(); + for(int i=0; i<10; ++i) + { + diagnostic_->tickInput(stampOf(start + 0.05*double(i)), /*expectedFrequency=*/5.0); + spinFor(std::chrono::milliseconds(200)); + } + + ASSERT_TRUE(waitForStatus("Input Status")); + EXPECT_EQ(latest("Input Status")->level, diagnostic_msgs::msg::DiagnosticStatus::OK) + << "status was: " << latest("Input Status")->message; +} + +TEST_F(SyncDiagnosticTest, RejectsAWindowSizeBelowOne) +{ + // The window is averaged over, so an empty one would divide by zero. + rclcpp::Node::SharedPtr node = + addNode(std::make_shared("sync_diagnostic_bad_window")); + EXPECT_THROW( + rtabmap_sync::SyncDiagnostic(node.get(), 0.2, /*windowSize=*/0), + UException); +} + +TEST_F(SyncDiagnosticTest, ASingleSampleWindowIsAccepted) +{ + rclcpp::Node::SharedPtr node = + addNode(std::make_shared("sync_diagnostic_small_window")); + EXPECT_NO_THROW(rtabmap_sync::SyncDiagnostic(node.get(), 0.2, /*windowSize=*/1)); +} diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt index 9c287b4a..9104af74 100644 --- a/rtabmap_util/CMakeLists.txt +++ b/rtabmap_util/CMakeLists.txt @@ -308,8 +308,17 @@ if(BUILD_TESTING) # Each node gets its own test binary: a crash or a stuck executor in one node cannot # take the others down, and every binary starts with a clean DDS graph. + # + # 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 -- rgb/image, + # rgbd_image, odom -- with rtabmap_sync's. On a shared domain they discover each other's + # publishers, and assertions then see traffic the test never sent. rtabmap_sync numbers + # its own from 50; keep the two ranges apart. + set(rtabmap_util_test_domain_id 30) macro(rtabmap_util_add_node_test test_name) - ament_add_gtest(${test_name} test/${test_name}.cpp) + ament_add_gtest(${test_name} test/${test_name}.cpp + ENV ROS_DOMAIN_ID=${rtabmap_util_test_domain_id}) + math(EXPR rtabmap_util_test_domain_id "${rtabmap_util_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_util_plugins rtabmap_util) diff --git a/rtabmap_util/README.md b/rtabmap_util/README.md index 8eccf4b5..398c7bf5 100644 --- a/rtabmap_util/README.md +++ b/rtabmap_util/README.md @@ -58,9 +58,3 @@ A few things recur across these nodes. **`fixed_frame_id`.** Where a node has to account for the robot moving between two stamps, it does so by asking TF how a frame moved relative to a fixed one — usually `odom`. Leaving it empty disables the compensation rather than erroring, so a moving robot then gets subtly misplaced data. **`Grid/*` parameters.** Nodes that segment or assemble maps use RTAB-Map's own [`LocalGridMaker`](https://introlab.github.io/rtabmap/api/latest/classrtabmap_1_1LocalGridMaker.html), and expose its parameters directly under their RTAB-Map names. Their meanings and defaults are in RTAB-Map's [parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html), which is the source of truth for them. One to know about: `Grid/RangeMax` is not unlimited by default, so distant points are dropped before anything else happens. - -## Building the documentation - -```bash -rosdoc2 build --package-path rtabmap_util --output-directory doc_output -``` diff --git a/rtabmap_util/doc/imu_to_tf.md b/rtabmap_util/doc/imu_to_tf.md index 8802e830..6d2d69bc 100644 --- a/rtabmap_util/doc/imu_to_tf.md +++ b/rtabmap_util/doc/imu_to_tf.md @@ -91,15 +91,45 @@ Node( remappings=[('scan_cloud', '/lidar/points/deskewed')]) ``` -The three nodes chain into a single TF tree: +The three nodes chain into a single TF tree. In terms of data, this node's job is to turn the IMU's orientation into a *frame* that `lidar_deskewing` and `icp_odometry` can look up — while the IMU topic itself still goes straight to the SLAM nodes, which use it for their own purposes: -```text -map rtabmap -└── icp_odom icp_odometry - └── base_link_stabilized imu_to_tf - └── base_link - ├── lidar_link robot description (static) - └── imu_link +```mermaid +flowchart LR + IMU["IMU driver"] + IMUT(["imu/data"]) + I2T["imu_to_tf"] + LIDAR["lidar driver"] + DESKEW["lidar_deskewing"] + ICP["icp_odometry"] + MAP["rtabmap"] + TF(["tf: base_link_stabilized"]) + DESKEWED(["/lidar/points/deskewed"]) + IMU --> IMUT + IMUT --> I2T & ICP & MAP + I2T --> TF + LIDAR -->|/lidar/points| DESKEW + DESKEW --> DESKEWED + DESKEWED -->|scan_cloud| ICP & MAP + ICP -->|odom| MAP + TF -.-> DESKEW + TF -.-> ICP +``` + +And the frames themselves: + +```mermaid +flowchart TB + MAP("map") + ICPODOM("icp_odom") + STAB("base_link_stabilized") + BASE("base_link") + LIDAR("lidar_link") + IMULINK("imu_link") + MAP -->|rtabmap| ICPODOM + ICPODOM -->|icp_odometry| STAB + STAB -->|imu_to_tf| BASE + BASE -->|robot description| LIDAR + BASE -->|robot description| IMULINK ``` | Edge | Published by | diff --git a/rtabmap_util/doc/map_assembler.md b/rtabmap_util/doc/map_assembler.md index fe0fb62b..a6f5cbf7 100644 --- a/rtabmap_util/doc/map_assembler.md +++ b/rtabmap_util/doc/map_assembler.md @@ -24,6 +24,23 @@ ComposableNode( 'cloud_output_voxelized': True}]) ``` +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 +flowchart LR + subgraph ROBOT["robot"] + SLAM["rtabmap"] + end + subgraph REMOTE["remote computer"] + ASM["map_assembler"] + RVIZ["RViz"] + end + SLAM -->|mapData| ASM + ASM -->|cloud_map| RVIZ + ASM -->|map| RVIZ + ASM -->|octomap_binary| RVIZ +``` + ## Subscribed Topics | Topic | Type | Description | diff --git a/rtabmap_util/doc/obstacles_detection.md b/rtabmap_util/doc/obstacles_detection.md index 4fcadbfa..f98e95df 100644 --- a/rtabmap_util/doc/obstacles_detection.md +++ b/rtabmap_util/doc/obstacles_detection.md @@ -89,6 +89,20 @@ The `marking`/`clearing` split is the whole point. Ground points only clear: the Feeding the raw cloud in as a single source cannot do this — every floor point would mark an obstacle and the robot would refuse to move. Working from [`turtlebot3_rgbd.launch.py`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py) and its [nav2 parameters](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/params/turtlebot3_rgbd_nav2_params.yaml) will save some time. +A depth camera turned into ground and obstacle clouds for a costmap: + +```mermaid +flowchart LR + CAM["camera driver"] + XYZ["point_cloud_xyz"] + OBST["obstacles_detection
frame_id: base_link"] + NAV["nav2 costmap"] + CAM -->|"depth/image,
camera_info"| XYZ + XYZ -->|cloud| OBST + OBST -->|ground| NAV + OBST -->|obstacles| NAV +``` + ## Subscribed Topics | Topic | Type | Description | diff --git a/rtabmap_util/doc/point_cloud_aggregator.md b/rtabmap_util/doc/point_cloud_aggregator.md index a310c448..49995394 100644 --- a/rtabmap_util/doc/point_cloud_aggregator.md +++ b/rtabmap_util/doc/point_cloud_aggregator.md @@ -33,6 +33,27 @@ With 2D or 3D lidars, feed the aggregator **deskewed** clouds: run a [lidar_desk The two nodes correct different motions and you generally want both. Deskewing removes the distortion *within* each sweep, point by point, because a spinning lidar measures each point from a slightly different pose. `fixed_frame_id` here places whole clouds relative to each other, because the sensors did not fire at the same instant. Merging raw sweeps only merges their distortions. +One deskewing node per sensor, then this node, and optionally back to a `LaserScan`. Every stage that compensates motion needs the same fixed frame: + +```mermaid +flowchart LR + L0["lidar_front driver"] + L1["lidar_left driver"] + L2["lidar_right driver"] + D0["lidar_deskewing
fixed_frame_id: odom"] + D1["lidar_deskewing
fixed_frame_id: odom"] + D2["lidar_deskewing
fixed_frame_id: odom"] + AGG["point_cloud_aggregator
count: 3
frame_id: base_link
fixed_frame_id: odom"] + SCAN["pointcloud_to_laserscan
optional"] + L0 -->|/lidar_front/points| D0 + L1 -->|/lidar_left/points| D1 + L2 -->|/lidar_right/points| D2 + D0 -->|cloud1| AGG + D1 -->|cloud2| AGG + D2 -->|cloud3| AGG + AGG -->|combined_cloud| SCAN +``` + ## Subscribed Topics | Topic | Type | Description | diff --git a/rtabmap_util/doc/point_cloud_assembler.md b/rtabmap_util/doc/point_cloud_assembler.md index 55caefbf..a960805a 100644 --- a/rtabmap_util/doc/point_cloud_assembler.md +++ b/rtabmap_util/doc/point_cloud_assembler.md @@ -44,6 +44,26 @@ Node( ('odom', 'icp_odom')]), ``` +The deskewing and `icp_odometry` both measure motion against the `odom` frame, while the assembler takes the pose from the `icp_odom` topic instead; the assembled cloud, not the raw sweep, is what `rtabmap` stores: + +```mermaid +flowchart LR + LIDAR["lidar driver"] + DESKEW["lidar_deskewing
fixed_frame_id: odom"] + ICP["icp_odometry
guess_frame_id: odom"] + ASM["point_cloud_assembler
fixed_frame_id: ''"] + MAP["rtabmap"] + DESKEWED(["deskewed cloud"]) + ICPODOM(["icp_odom"]) + LIDAR -->|points| DESKEW + DESKEW --> DESKEWED + DESKEWED -->|scan_cloud| ICP + DESKEWED -->|cloud| ASM + ICP --> ICPODOM + ICPODOM -->|odom| ASM & MAP + ASM -->|assembled_cloud| MAP +``` + Note `fixed_frame_id: ''`. Clearing it switches the node from TF to the `odom` topic, pairing each cloud with the exact odometry message that goes with it rather than an interpolated TF lookup — see [Where the poses come from](#where-the-poses-come-from). Feeding the result to `rtabmap` as `scan_cloud` means the assembled cloud, not the raw sweep, is what gets stored. ### Widening a narrow field of view @@ -62,10 +82,24 @@ Node( remappings=[('cloud', '/camera/scan/deskewed')]), ``` +Here the pose comes from the robot's wheel odometry, through the `odom` frame in TF. Nothing is being deskewed — `lidar_deskewing` is in the chain purely because this node takes `PointCloud2` and `depthimage_to_laserscan` emits a `LaserScan`: + +```mermaid +flowchart LR + D2S["depthimage_to_laserscan"] + CONV["lidar_deskewing
LaserScan → PointCloud2"] + ASM["point_cloud_assembler
circular_buffer
max_clouds: 20
frame_id: base_link
fixed_frame_id: odom"] + MAP["rtabmap"] + D2S -->|input_scan| CONV + CONV -->|cloud| ASM + ASM -->|assembled_cloud| MAP +``` + `circular_buffer` is what makes this work as a live input: the window rolls, so every incoming scan produces a full assembled cloud rather than one per twenty. `linear_update` and `angular_update` stop a stationary robot from filling the buffer with twenty copies of the same view, which would leave it with nothing but the current scan the moment it moved off again. The cloud goes to `rtabmap` as `scan_cloud`, with `scan_cloud_is_2d` set since the points all came from one row of pixels. + ## Subscribed Topics | Topic | Type | Description | diff --git a/rtabmap_util/doc/pointcloud_to_depthimage.md b/rtabmap_util/doc/pointcloud_to_depthimage.md index 106e0df3..329be95b 100644 --- a/rtabmap_util/doc/pointcloud_to_depthimage.md +++ b/rtabmap_util/doc/pointcloud_to_depthimage.md @@ -28,6 +28,23 @@ ComposableNode( ('camera_info', '/camera/color/camera_info')]) ``` +The lidar supplies the geometry, the camera supplies the pose and the color, and the result joins an ordinary RGB-D pipeline: + +```mermaid +flowchart LR + LIDAR["lidar driver"] + CAM["camera driver"] + P2D["pointcloud_to_depthimage
fixed_frame_id: odom"] + SYNC["rgbd_sync"] + MAP["rtabmap"] + LIDAR -->|cloud| P2D + CAM -->|camera_info| P2D + CAM -->|rgb/image| SYNC + CAM -->|rgb/camera_info| SYNC + P2D -->|image_raw as depth/image| SYNC + SYNC -->|rgbd_image| MAP +``` + ## Subscribed Topics | Topic | Type | Description | diff --git a/rtabmap_util/doc/rgbd_relay.md b/rtabmap_util/doc/rgbd_relay.md index 1b2b8cb7..35ed0cc0 100644 --- a/rtabmap_util/doc/rgbd_relay.md +++ b/rtabmap_util/doc/rgbd_relay.md @@ -25,6 +25,32 @@ ComposableNode( remappings=[('rgbd_image', '/camera/rgbd_image')]) ``` + +A relay at each end of the link, so only the compressed form crosses it. Once it is raw again it feeds the SLAM node directly, and [rgbd_split](rgbd_split.md) unpacks it into the plain `Image` topics RViz can display: + +```mermaid +flowchart LR + subgraph ROBOT["robot"] + CAM["camera driver"] + SYNC["rgbd_sync"] + RELAY1["rgbd_relay
compress: true"] + end + subgraph REMOTE["remote computer"] + RELAY2["rgbd_relay
uncompress: true"] + RAW(["rgbd_image_relay"]) + MAP["rtabmap"] + SPLIT["rgbd_split"] + RVIZ["RViz"] + end + CAM -->|"rgb, depth,
camera_info"| SYNC + SYNC -->|rgbd_image| RELAY1 + RELAY1 -->|compressed| RELAY2 + RELAY2 --> RAW + RAW --> MAP + RAW --> SPLIT + SPLIT -->|rgb, depth| RVIZ +``` + ## Subscribed Topics | Topic | Type | Description | @@ -62,6 +88,17 @@ ros2 run rtabmap_util rgbd_relay --ros-args \ -p qos_pub:=1 ``` +```mermaid +flowchart LR + CAM["camera driver
publishes best effort"] + SYNC["rgbd_sync"] + RELAY["rgbd_relay
qos_sub: 2, qos_pub: 1"] + MAP["rtabmap
needs reliable"] + CAM -->|"rgb, depth,
camera_info"| SYNC + SYNC -->|rgbd_image| RELAY + RELAY -->|rgbd_image_relay| MAP +``` + Both parameters default to `qos`, so setting `qos` alone configures both sides at once. Reliability is all that is bridged — durability is left at the default, so a transient-local publisher is not converted. The queue depths are separate too, through `queue_sub` and `queue_pub`. diff --git a/rtabmap_util/doc/rgbd_split.md b/rtabmap_util/doc/rgbd_split.md index f968a31a..1e8a8c0e 100644 --- a/rtabmap_util/doc/rgbd_split.md +++ b/rtabmap_util/doc/rgbd_split.md @@ -20,6 +20,18 @@ ComposableNode( remappings=[('rgbd_image', '/camera/rgbd_image')]) ``` +Unpacking a bundle for RViz: + +```mermaid +flowchart LR + RGBD(["/camera/rgbd_image"]) + SPLIT["rgbd_split"] + RVIZ["RViz"] + RGBD --> SPLIT + SPLIT -->|"rgb/image,
rgb/camera_info"| RVIZ + SPLIT -->|"depth/image,
depth/camera_info"| RVIZ +``` + ## Subscribed Topics | Topic | Type | Description | From 73c98f87a807dd48d3f7d67ff18d77b4c69ac56c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 13 Sep 2026 12:20:17 -0700 Subject: [PATCH 12/17] rtabmap_python tests and doc (#1455) --- .github/workflows/coverage.yml | 41 +++- README.md | 2 +- codecov.yml | 9 +- rtabmap_python/README.md | 49 +++++ rtabmap_python/package.xml | 3 + rtabmap_python/rosdoc2.yaml | 29 +++ .../rtabmap_python/cv_compression.py | 53 ++++++ rtabmap_python/test/test_cv_compression.py | 178 ++++++++++++++++++ 8 files changed, 355 insertions(+), 9 deletions(-) create mode 100644 rtabmap_python/README.md create mode 100644 rtabmap_python/rosdoc2.yaml create mode 100644 rtabmap_python/test/test_cv_compression.py diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml index 2ade89c0..39b6a390 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 + package-name: rtabmap_conversions rtabmap_util rtabmap_sync 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. @@ -75,6 +75,9 @@ jobs: { "build": { "mixin": ["coverage-gcc"] + }, + "test": { + "pytest-with-coverage": true } } # Pinned so a change in the mixin repository cannot break this job. @@ -115,6 +118,8 @@ jobs: # `source` does not exist. GitHub picks sh whenever it cannot find # bash in the image's PATH, and says so in the log ("shell: sh -e"). . /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" # Baseline from the .gcno files. Without it a source file that no test # ever loaded is missing from the report altogether rather than @@ -129,10 +134,34 @@ jobs: '*CompilerId*' '*/CMakeFiles/*' || true test -s lcov/total_coverage.info - # Fails the job if the step above produced nothing -- the failure mode - # this workflow has hit twice already is a green run that measured zero. - - name: Coverage summary - run: lcov --summary ros_ws/lcov/total_coverage.info + # Python coverage is a separate mechanism: the coverage-gcc mixin only adds + # --coverage to the compiler, which does nothing for an ament_python package. + # colcon test --pytest-with-coverage (set above) writes a Cobertura report into + # the package's own build directory instead. + # + # Its paths are relative to the package rather than the repository, so + # cv_compression.py arrives as "rtabmap_python/cv_compression.py" -- one level + # short of where it really lives. Rewrite them here rather than leave Codecov to + # guess, which it does by suffix and can get wrong. + - name: Python coverage report + working-directory: ros_ws + run: | + python3 - <<'EOF' + import pathlib + import xml.etree.ElementTree as ET + p = pathlib.Path('build/rtabmap_python/coverage.xml') + if not p.is_file(): + print('::warning::no python coverage produced for rtabmap_python') + raise SystemExit(0) + tree = ET.parse(p) + root = tree.getroot() + for source in root.iter('source'): + source.text = '.' + for cls in root.iter('class'): + cls.set('filename', 'rtabmap_python/' + cls.get('filename')) + tree.write(p, xml_declaration=True, encoding='utf-8') + print('rewrote', p, 'to repository-relative paths') + EOF # colcon lcov-result runs genhtml itself, into the same lcov/ directory. - name: Upload HTML coverage artifact @@ -146,7 +175,7 @@ jobs: if: ${{ env.CODECOV_TOKEN != '' }} uses: codecov/codecov-action@v5 with: - files: ros_ws/lcov/total_coverage.info + files: ros_ws/lcov/total_coverage.info,ros_ws/build/rtabmap_python/coverage.xml # Upload ONLY the aggregated lcov file. By default the CLI also walks # the tree and runs gcov over every .gcno it finds, which re-adds the # test sources the --filter above just dropped. diff --git a/README.md b/README.md index 0be9987b..51bb9c3c 100644 --- a/README.md +++ b/README.md @@ -52,7 +52,7 @@ The stack is split into small packages so a pipeline only pulls in what it uses. |---|---| | `rtabmap_msgs` | Message, service and action definitions used across the stack. | | [`rtabmap_conversions`](rtabmap_conversions/README.md) | C++ library converting between RTAB-Map library types and ROS 2 messages. | -| `rtabmap_python` | Python helpers, currently image compression matching RTAB-Map's own format. | +| [`rtabmap_python`](rtabmap_python/README.md) | Python helpers for RTAB-Map's own binary formats, currently the compressed matrices carried in `rtabmap_msgs` fields and database blobs. | ### Visualization diff --git a/codecov.yml b/codecov.yml index a826350c..2e315d02 100644 --- a/codecov.yml +++ b/codecov.yml @@ -56,13 +56,18 @@ component_management: name: rtabmap_sync paths: - rtabmap_sync/** + - component_id: rtabmap_python + name: rtabmap_python + paths: + - rtabmap_python/** comment: layout: "condensed_header, diff, components, files" behavior: default require_changes: true # stay quiet when coverage doesn't move -# Only rtabmap_conversions, rtabmap_util and rtabmap_sync have tests today, so +# Only rtabmap_conversions, rtabmap_util, rtabmap_sync 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. @@ -73,8 +78,8 @@ ignore: - "rtabmap_launch/**" - "rtabmap_msgs/**" - "rtabmap_odom/**" - - "rtabmap_python/**" - "rtabmap_rviz_plugins/**" - "rtabmap_slam/**" - "rtabmap_viz/**" - "**/test/**" + - "**/setup.py" # packaging scaffolding, not code under test diff --git a/rtabmap_python/README.md b/rtabmap_python/README.md new file mode 100644 index 00000000..a57dbf58 --- /dev/null +++ b/rtabmap_python/README.md @@ -0,0 +1,49 @@ +# rtabmap_python + +Python helpers for reading and writing the binary formats [RTAB-Map](https://github.com/introlab/rtabmap) uses. + +RTAB-Map is a C++ library, and the data it hands to ROS is not always plain ROS types. Several `rtabmap_msgs` fields — and every blob in an `.db` database — carry a matrix in RTAB-Map's own compressed encoding rather than as a `sensor_msgs/Image` or an array. This package is the Python side of that encoding, for scripts that read those fields without going through the C++ library. + +There are no nodes here. It is an `ament_python` package that installs one importable module. + +## Module + +`rtabmap_python.cv_compression` — a single-channel `cv::Mat` to and from bytes. + +| Function | Description | +|---|---| +| `compress(data)` | 1-D or 2-D numpy array → `bytearray`. | +| `uncompress(data)` | those bytes → 2-D numpy array. | + +```python +import numpy as np +from rtabmap_python.cv_compression import compress, uncompress + +scan = np.zeros((360, 2), dtype=np.float32) +blob = compress(scan) # what the message field carries +restored = uncompress(blob) # (360, 2) float32 +``` + +The encoding is a zlib stream followed by a 12-byte trailer holding rows, cols and the element type as three `int32`. It matches `compressData()` and `uncompressData()` in RTAB-Map's `corelib/src/Compression.cpp` byte for byte, so either side can read what the other wrote. The [module docstring](rtabmap_python/cv_compression.py) has the exact layout, and the generated [Python API reference](https://docs.ros.org/en/jazzy/p/rtabmap_python/) renders it alongside the two functions. + +## Things worth knowing + +**The result is read-only.** `uncompress` views the decompressed buffer instead of copying it, so the array it returns has `writeable=False` and assigning into it raises. Call `.copy()` if you need to modify it. + +**Single-channel only.** The C++ encoder packs the channel count into the type code; the tables here cover the single-channel depths `CV_8U` through `CV_64F`. A multi-channel matrix written by the C++ side raises `KeyError` rather than decoding wrongly. So does an unsupported dtype on the way in — `int64` and `float16` have no encoding. + +**The trailer is host-endian**, because the C++ side writes raw `int`s. A blob is not portable between machines of opposite endianness. + +## Building and testing + +```bash +colcon build --packages-select rtabmap_python +colcon test --packages-select rtabmap_python +colcon test-result --verbose +``` + +The tests cover the round trip for every supported depth, the exact trailer bytes against the codes RTAB-Map's `serializeMatType()` produces, and each of the behaviours above. Alongside them, the standard `ament_copyright`, `ament_flake8` and `ament_pep257` linters run over the package. + +## License + +BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license). diff --git a/rtabmap_python/package.xml b/rtabmap_python/package.xml index 4a5d7e84..c8054481 100644 --- a/rtabmap_python/package.xml +++ b/rtabmap_python/package.xml @@ -10,6 +10,8 @@ https://github.com/introlab/rtabmap_ros/issues https://github.com/introlab/rtabmap_ros + python3-numpy + ament_copyright ament_flake8 ament_pep257 @@ -17,5 +19,6 @@ ament_python + rosdoc2.yaml diff --git a/rtabmap_python/rosdoc2.yaml b/rtabmap_python/rosdoc2.yaml new file mode 100644 index 00000000..ae1e28c9 --- /dev/null +++ b/rtabmap_python/rosdoc2.yaml @@ -0,0 +1,29 @@ +## Configuration for rosdoc2, the documentation generator used by docs.ros.org. +## Regenerate the annotated default with: +## rosdoc2 default_config --package-path rtabmap_python +## Build the docs locally with: +## rosdoc2 build --package-path rtabmap_python --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_python package: there are no C/C++ headers to parse, and the + ## API reference comes from the module docstrings via sphinx-apidoc. + always_run_doxygen: false + always_run_sphinx_apidoc: true + +builders: + ## Sphinx renders the landing page and the autodoc pages sphinx-apidoc produces + ## from rtabmap_python/. + - sphinx: { + name: 'rtabmap_python', + output_dir: '' + } diff --git a/rtabmap_python/rtabmap_python/cv_compression.py b/rtabmap_python/rtabmap_python/cv_compression.py index 0b8e1604..82769d89 100644 --- a/rtabmap_python/rtabmap_python/cv_compression.py +++ b/rtabmap_python/rtabmap_python/cv_compression.py @@ -27,6 +27,35 @@ # POSSIBILITY OF SUCH DAMAGE. +""" +Compress numpy arrays into RTAB-Map's ``cv::Mat`` wire format. + +RTAB-Map stores and transmits matrices -- images, laser scans, descriptors -- as a zlib +payload followed by a 12-byte trailer recording the shape and the element type. Database +blobs and the compressed fields of ``rtabmap_msgs`` messages both use it. + +The layout is:: + + [ zlib stream of the elements in C order ][ rows ][ cols ][ type ] + int32 int32 int32 + +The three trailer fields are written with ``struct`` format ``'iii'`` -- native byte order +and size, matching the C++ side's raw ``int`` writes. That makes the encoding +**host-endian**, so a blob does not travel between machines of opposite endianness. + +``type`` is the OpenCV depth of the elements: 0 ``CV_8U``, 1 ``CV_8S``, 2 ``CV_16U``, +3 ``CV_16S``, 4 ``CV_32S``, 5 ``CV_32F``, 6 ``CV_64F``. + +These two functions are the Python side of that format. They match ``compressData()`` and +``uncompressData()`` in RTAB-Map's ``corelib/src/Compression.cpp`` byte for byte, so a +matrix written by either side can be read by the other. + +Single-channel matrices only. The C++ encoder packs the channel count into the type code +alongside the depth; the tables here cover the single-channel depths ``CV_8U`` through +``CV_64F``, which is what the codes 0 to 6 mean. +""" + + import struct import zlib @@ -34,6 +63,20 @@ import numpy as np def compress(data): + """ + Compress a 1-D or 2-D array into RTAB-Map's format. + + :param data: a single-channel array whose dtype is one of ``uint8``, ``int8``, + ``uint16``, ``int16``, ``int32``, ``float32`` or ``float64``. A 1-D array of + length ``n`` is recorded as a 1-by-``n`` matrix, which is the shape + :func:`uncompress` gives back. Any memory layout is accepted; the bytes are + always written in C order. + :returns: a ``bytearray`` holding the zlib payload followed by the trailer described + in the module docstring. + :raises AssertionError: if ``data`` has more than two dimensions. + :raises KeyError: if its dtype is not one of the seven above -- ``int64`` and + ``float16`` have no encoding in this format. + """ assert data.ndim == 1 or data.ndim == 2 dim1 = 1 @@ -63,6 +106,16 @@ def compress(data): def uncompress(data): + """ + Restore an array written by :func:`compress` or by RTAB-Map's C++ side. + + :param data: a bytes-like object laid out as :func:`compress` returns. + :returns: a 2-D array of the recorded shape and dtype. It is 2-D even when + :func:`compress` was handed a 1-D array, and it is **read-only**: it views the + decompressed buffer instead of copying it, so call ``.copy()`` before writing. + :raises KeyError: if the trailer's type code is not a single-channel depth 0 to 6, + which is what a multi-channel matrix from the C++ side encodes to. + """ cvtype_to_numpy_type = { 0: 'uint8', 1: 'int8', diff --git a/rtabmap_python/test/test_cv_compression.py b/rtabmap_python/test/test_cv_compression.py new file mode 100644 index 00000000..55d46c47 --- /dev/null +++ b/rtabmap_python/test/test_cv_compression.py @@ -0,0 +1,178 @@ +# Copyright 2025 matlabbe +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the matlabbe nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +"""Tests for :mod:`rtabmap_python.cv_compression`.""" + +import struct +import zlib + +import numpy as np +import pytest + +from rtabmap_python.cv_compression import compress, uncompress + + +# The single-channel depths the format encodes, with the codes RTAB-Map's +# serializeMatType() gives them: CV_8U, CV_8S, CV_16U, CV_16S, CV_32S, CV_32F, CV_64F. +SUPPORTED_TYPES = [ + ('uint8', 0), + ('int8', 1), + ('uint16', 2), + ('int16', 3), + ('int32', 4), + ('float32', 5), + ('float64', 6), +] + +# Three int32: rows, cols, type code. +TRAILER_SIZE = 3 * 4 + + +@pytest.mark.parametrize('dtype,code', SUPPORTED_TYPES) +def test_roundtrip_preserves_shape_dtype_and_values(dtype, code): + """Every supported depth survives a compress/uncompress cycle unchanged.""" + data = np.arange(12, dtype=dtype).reshape(3, 4) + + result = uncompress(compress(data)) + + assert result.shape == (3, 4) + assert result.dtype == np.dtype(dtype) + assert np.array_equal(result, data) + + +@pytest.mark.parametrize('dtype,code', SUPPORTED_TYPES) +def test_trailer_records_rows_cols_and_type_code(dtype, code): + """The last 12 bytes are rows, cols and the type code, as the C++ side writes them.""" + data = np.zeros((3, 4), dtype=dtype) + + trailer = bytes(compress(data)[-TRAILER_SIZE:]) + + assert trailer == struct.pack('iii', 3, 4, code) + + +def test_payload_is_plain_zlib_of_the_c_order_bytes(): + """Everything before the trailer is a zlib stream, so the C++ side can inflate it.""" + data = np.arange(6, dtype=np.uint8) + + payload = bytes(compress(data)[:-TRAILER_SIZE]) + + assert zlib.decompress(payload) == data.tobytes() + + +def test_compress_returns_a_bytearray(): + """The return type is a bytearray, which is what the message fields expect.""" + assert isinstance(compress(np.zeros(4, dtype=np.uint8)), bytearray) + + +def test_one_dimensional_input_comes_back_as_a_single_row(): + """A 1-D array is recorded as 1-by-n, so the roundtrip is not shape-preserving.""" + data = np.arange(5, dtype=np.float32) + + result = uncompress(compress(data)) + + assert result.shape == (1, 5) + assert np.array_equal(result.ravel(), data) + + +@pytest.mark.parametrize('shape', [(1, 5), (5, 1), (2, 3)]) +def test_two_dimensional_shapes_are_preserved_exactly(shape): + """Rows and cols are recorded separately, so no 2-D shape is transposed or flattened.""" + data = np.arange(5 if 1 in shape else 6, dtype=np.uint8).reshape(shape) + + assert uncompress(compress(data)).shape == shape + + +def test_non_contiguous_input_roundtrips(): + """A transposed view is written in C order, so it reads back as the same matrix.""" + data = np.arange(12, dtype=np.int16).reshape(3, 4).T + assert not data.flags.c_contiguous + + result = uncompress(compress(data)) + + assert result.shape == (4, 3) + assert np.array_equal(result, data) + + +def test_empty_array_roundtrips_as_an_empty_row(): + """An empty array is not a special case; it comes back as a 1-by-0 matrix.""" + result = uncompress(compress(np.array([], dtype=np.uint8))) + + assert result.shape == (1, 0) + assert result.dtype == np.uint8 + + +def test_uncompressed_array_is_read_only(): + """uncompress() views the decompressed buffer rather than copying it.""" + result = uncompress(compress(np.arange(4, dtype=np.uint8))) + + assert not result.flags.writeable + with pytest.raises(ValueError): + result[0, 0] = 1 + # .copy() is the way out, as the docstring says. + assert result.copy().flags.writeable + + +def test_accepts_bytes_as_well_as_bytearray(): + """uncompress() reads whatever compress() produced, converted or not.""" + data = np.arange(8, dtype=np.uint16).reshape(2, 4) + + result = uncompress(bytes(compress(data))) + + assert np.array_equal(result, data) + + +def test_larger_matrix_roundtrips(): + """A matrix big enough to actually exercise zlib, with non-trivial content.""" + rng = np.random.default_rng(42) + data = rng.integers(0, 255, size=(120, 160), dtype=np.uint8) + + assert np.array_equal(uncompress(compress(data)), data) + + +def test_rejects_more_than_two_dimensions(): + """The format has no encoding for a third dimension, so compress() refuses one.""" + with pytest.raises(AssertionError): + compress(np.zeros((2, 2, 2), dtype=np.uint8)) + + +@pytest.mark.parametrize('dtype', ['int64', 'uint64', 'float16']) +def test_rejects_unsupported_dtype(dtype): + """Depths outside CV_8U..CV_64F have no type code and raise rather than truncate.""" + with pytest.raises(KeyError): + compress(np.zeros(4, dtype=dtype)) + + +def test_rejects_unknown_type_code(): + """A trailer from a multi-channel C++ matrix encodes a code this table lacks.""" + payload = bytes(compress(np.zeros((2, 2), dtype=np.uint8))[:-TRAILER_SIZE]) + # What serializeMatType() returns for a 3-channel CV_8U matrix: depth + ((cn - 1) << 3). + forged = bytearray(payload) + struct.pack('iii', 2, 2, 0 + ((3 - 1) << 3)) + + with pytest.raises(KeyError): + uncompress(forged) From 11edc01d6adc0f4fe1e2a5336e1841d3754352eb Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 21 Sep 2026 17:02:45 -0700 Subject: [PATCH 13/17] rtabmap_odom tests and doc (#1456) * rtabmap_odom tests and doc * opengv note * added ci checks or humble-latest flaky dep cmake errors * Added real data tests for rgbd_odom and stereo_odom * added real data for icp_odometry's deskewing test * fixing json cmake error on lyrical/rolling * test 2d icp odom deskewing branch * first review of existing OdometryROS tests * testing with imu used as guess * tested imu arrivals sync * Fixed odom reset on right pose when guess frame id is used * fixing header errors in ci >=lyrical * Added support for input rgbd_image topic with features for odom, added multicam rgbd_odometry test * Added stereo odom support for features-only frames. Added multicam stereo tests. * forcing latest rtabmap version * updated OdometryROS API * ci: dont build non-latest docker in pull requests * splitting docker jobs * doc edit * Making publish_null_when_lost:=false continous when guess is provided (using guess covariance when we cannot register yet) * updated stereo doc * ficing rolling ci (rviz Ogre header) * Added test coverage of alll rgbd_image callbacks * fixing rolling ci * making docker ci build/run the tests on pull requests * fixing ros2 ci testing * improved sync callback coverage * improving stereo_odometry test coverage * improved icp_odometry test coverage * lyrical voxel_grid ptr error * make multicam tests working as well without opengv * removing deps of missing packages on rolling * PCL empty cloud conversion compiler errors fix * fixing icp_odometry test failure on ci witohut libpointmatcher * fixing nav2 costmap plugin build on lyrical * joining thread when exiting * updating icp test to work the same on pcl 1.15 (lyrical) * Fix parallel tests seg fault --------- Co-authored-by: mathieu86 --- .github/workflows/coverage.yml | 4 +- .github/workflows/docker-ros2.yml | 92 +- .github/workflows/ros2.yml | 24 +- README.md | 11 +- codecov.yml | 9 +- docker/humble/latest/Dockerfile | 24 +- docker/iron/latest/Dockerfile | 12 +- docker/jazzy/latest/Dockerfile | 24 +- docker/kilted/latest/Dockerfile | 24 +- docker/lyrical/latest/Dockerfile | 26 +- docker/verify_deps.sh | 63 + rtabmap_conversions/CMakeLists.txt | 2 +- rtabmap_conversions/README.md | 15 +- .../PointCloudConversion.h | 75 + rtabmap_conversions/src/MsgConversion.cpp | 5 +- rtabmap_costmap_plugins/CMakeLists.txt | 1 + .../rtabmap_costmap_plugins/voxel_layer.hpp | 9 + rtabmap_costmap_plugins/src/voxel_layer.cpp | 115 +- rtabmap_odom/CMakeLists.txt | 48 + rtabmap_odom/README.md | 273 +++ rtabmap_odom/doc/icp_odometry.md | 238 ++ rtabmap_odom/doc/rgbd_odometry.md | 211 ++ rtabmap_odom/doc/stereo_odometry.md | 219 ++ .../include/rtabmap_odom/OdometryROS.h | 87 + .../include/rtabmap_odom/icp_odometry.hpp | 11 + .../include/rtabmap_odom/rgbd_odometry.hpp | 25 +- .../include/rtabmap_odom/stereo_odometry.hpp | 24 +- rtabmap_odom/package.xml | 6 + rtabmap_odom/rosdoc2.yaml | 35 + rtabmap_odom/src/OdometryROS.cpp | 278 ++- rtabmap_odom/src/nodelets/icp_odometry.cpp | 16 +- rtabmap_odom/src/nodelets/rgbd_odometry.cpp | 281 ++- rtabmap_odom/src/nodelets/stereo_odometry.cpp | 264 ++- rtabmap_odom/test/bag_playback.hpp | 79 + rtabmap_odom/test/camera_rig.hpp | 319 +++ rtabmap_odom/test/data/README.md | 103 + .../data/lidar/ouster_half_turn/metadata.yaml | 52 + .../ouster_half_turn/ouster_half_turn.mcap | Bin 0 -> 857414 bytes rtabmap_odom/test/data/rgbd/calib/154.yaml | 16 + rtabmap_odom/test/data/rgbd/calib/17.yaml | 16 + rtabmap_odom/test/data/rgbd/depth/154.png | Bin 0 -> 123097 bytes rtabmap_odom/test/data/rgbd/depth/17.png | Bin 0 -> 116855 bytes rtabmap_odom/test/data/rgbd/rgb/154.jpg | Bin 0 -> 78853 bytes rtabmap_odom/test/data/rgbd/rgb/17.jpg | Bin 0 -> 82283 bytes .../test/data/stereo/raw/left/420.jpg | Bin 0 -> 59318 bytes .../test/data/stereo/raw/left/425.jpg | Bin 0 -> 60695 bytes .../test/data/stereo/raw/right/420.jpg | Bin 0 -> 54879 bytes .../test/data/stereo/raw/right/425.jpg | Bin 0 -> 55597 bytes .../test/data/stereo/raw/stereo_left.yaml | 34 + .../test/data/stereo/raw/stereo_pose.yaml | 30 + .../test/data/stereo/raw/stereo_right.yaml | 34 + .../test/data/stereo/rect/left/50.jpg | Bin 0 -> 68515 bytes .../test/data/stereo/rect/left/60.jpg | Bin 0 -> 67197 bytes .../test/data/stereo/rect/right/50.jpg | Bin 0 -> 67018 bytes .../test/data/stereo/rect/right/60.jpg | Bin 0 -> 65240 bytes .../test/data/stereo/rect/stereo_left.yaml | 24 + .../test/data/stereo/rect/stereo_right.yaml | 24 + rtabmap_odom/test/msg_builders.hpp | 362 +++ rtabmap_odom/test/node_test_utils.hpp | 256 ++ rtabmap_odom/test/scan_scenes.hpp | 115 + rtabmap_odom/test/test_data.hpp | 268 +++ rtabmap_odom/test/test_icp_odometry.cpp | 2103 +++++++++++++++++ rtabmap_odom/test/test_odometry_ros.cpp | 1350 +++++++++++ rtabmap_odom/test/test_rgbd_odometry.cpp | 800 +++++++ rtabmap_odom/test/test_stereo_odometry.cpp | 1272 ++++++++++ rtabmap_python/README.md | 16 +- rtabmap_rviz_plugins/src/MapCloudDisplay.cpp | 5 +- rtabmap_rviz_plugins/src/MapGraphDisplay.cpp | 12 + rtabmap_sync/README.md | 18 +- rtabmap_sync/doc/rgb_sync.md | 10 + rtabmap_sync/doc/rgbd_sync.md | 12 + rtabmap_sync/doc/rgbdx_sync.md | 11 + rtabmap_sync/doc/stereo_sync.md | 11 + rtabmap_util/README.md | 6 + rtabmap_util/doc/db_player.md | 12 +- rtabmap_util/doc/disparity_to_depth.md | 8 + rtabmap_util/doc/imu_to_tf.md | 16 +- rtabmap_util/doc/lidar_deskewing.md | 11 + rtabmap_util/doc/map_assembler.md | 11 + rtabmap_util/doc/obstacles_detection.md | 11 + rtabmap_util/doc/point_cloud_aggregator.md | 11 + rtabmap_util/doc/point_cloud_assembler.md | 14 + rtabmap_util/doc/point_cloud_xyz.md | 9 + rtabmap_util/doc/point_cloud_xyzrgb.md | 9 + rtabmap_util/doc/pointcloud_to_depthimage.md | 11 + rtabmap_util/doc/rgbd_relay.md | 9 + rtabmap_util/doc/rgbd_split.md | 9 + rtabmap_util/src/MapsManager.cpp | 17 +- .../src/nodelets/obstacles_detection.cpp | 9 +- rtabmap_util/src/nodelets/point_cloud_xyz.cpp | 5 +- .../src/nodelets/point_cloud_xyzrgb.cpp | 5 +- 91 files changed, 9736 insertions(+), 350 deletions(-) create mode 100755 docker/verify_deps.sh create mode 100644 rtabmap_conversions/include/rtabmap_conversions/PointCloudConversion.h create mode 100644 rtabmap_odom/README.md create mode 100644 rtabmap_odom/doc/icp_odometry.md create mode 100644 rtabmap_odom/doc/rgbd_odometry.md create mode 100644 rtabmap_odom/doc/stereo_odometry.md create mode 100644 rtabmap_odom/rosdoc2.yaml create mode 100644 rtabmap_odom/test/bag_playback.hpp create mode 100644 rtabmap_odom/test/camera_rig.hpp create mode 100644 rtabmap_odom/test/data/README.md create mode 100644 rtabmap_odom/test/data/lidar/ouster_half_turn/metadata.yaml create mode 100644 rtabmap_odom/test/data/lidar/ouster_half_turn/ouster_half_turn.mcap create mode 100644 rtabmap_odom/test/data/rgbd/calib/154.yaml create mode 100644 rtabmap_odom/test/data/rgbd/calib/17.yaml create mode 100644 rtabmap_odom/test/data/rgbd/depth/154.png create mode 100644 rtabmap_odom/test/data/rgbd/depth/17.png create mode 100644 rtabmap_odom/test/data/rgbd/rgb/154.jpg create mode 100644 rtabmap_odom/test/data/rgbd/rgb/17.jpg create mode 100644 rtabmap_odom/test/data/stereo/raw/left/420.jpg create mode 100644 rtabmap_odom/test/data/stereo/raw/left/425.jpg create mode 100644 rtabmap_odom/test/data/stereo/raw/right/420.jpg create mode 100644 rtabmap_odom/test/data/stereo/raw/right/425.jpg create mode 100644 rtabmap_odom/test/data/stereo/raw/stereo_left.yaml create mode 100644 rtabmap_odom/test/data/stereo/raw/stereo_pose.yaml create mode 100644 rtabmap_odom/test/data/stereo/raw/stereo_right.yaml create mode 100644 rtabmap_odom/test/data/stereo/rect/left/50.jpg create mode 100644 rtabmap_odom/test/data/stereo/rect/left/60.jpg create mode 100644 rtabmap_odom/test/data/stereo/rect/right/50.jpg create mode 100644 rtabmap_odom/test/data/stereo/rect/right/60.jpg create mode 100644 rtabmap_odom/test/data/stereo/rect/stereo_left.yaml create mode 100644 rtabmap_odom/test/data/stereo/rect/stereo_right.yaml create mode 100644 rtabmap_odom/test/msg_builders.hpp create mode 100644 rtabmap_odom/test/node_test_utils.hpp create mode 100644 rtabmap_odom/test/scan_scenes.hpp create mode 100644 rtabmap_odom/test/test_data.hpp create mode 100644 rtabmap_odom/test/test_icp_odometry.cpp create mode 100644 rtabmap_odom/test/test_odometry_ros.cpp create mode 100644 rtabmap_odom/test/test_rgbd_odometry.cpp create mode 100644 rtabmap_odom/test/test_stereo_odometry.cpp diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml index 39b6a390..03d5ee80 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_python + package-name: rtabmap_conversions rtabmap_util rtabmap_sync rtabmap_odom 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,7 +120,7 @@ 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" + PKGS="rtabmap_conversions rtabmap_util rtabmap_sync rtabmap_odom" # 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. diff --git a/.github/workflows/docker-ros2.yml b/.github/workflows/docker-ros2.yml index 6c9f7957..541de0e0 100644 --- a/.github/workflows/docker-ros2.yml +++ b/.github/workflows/docker-ros2.yml @@ -12,56 +12,37 @@ concurrency: cancel-in-progress: ${{ github.event_name == 'pull_request' }} jobs: + # Images built from this tree (docker/*/latest), the ones a change here can break. docker: # A manual dispatch is honored only on ros2, the only ref we push from. if: ${{ github.event_name != 'workflow_dispatch' || github.ref == 'refs/heads/ros2' }} runs-on: ubuntu-latest - + strategy: fail-fast: false matrix: - docker_tag: [humble, humble-latest, jazzy, jazzy-latest, kilted, kilted-latest, lyrical-latest] + docker_tag: [humble-latest, jazzy-latest, kilted-latest, lyrical-latest] include: - - docker_tag: humble - docker_path: 'humble' - docker_platforms: | - linux/amd64 - docker_tag: humble-latest docker_path: 'humble/latest' docker_platforms: | linux/amd64 linux/arm64 - - docker_tag: jazzy - docker_path: 'jazzy' - docker_platforms: | - linux/amd64 - linux/arm64 - docker_tag: jazzy-latest docker_path: 'jazzy/latest' docker_platforms: | linux/amd64 linux/arm64 - - docker_tag: kilted - docker_path: 'kilted' - docker_platforms: | - linux/amd64 - linux/arm64 - docker_tag: kilted-latest docker_path: 'kilted/latest' docker_platforms: | linux/amd64 - #Disabled till rtabmap_ros is released on lyrical - #- docker_tag: lyrical - # docker_path: 'lyrical' - # docker_platforms: | - # linux/amd64 - # linux/arm64 - docker_tag: lyrical-latest docker_path: 'lyrical/latest' docker_platforms: | linux/amd64 linux/arm64 - + steps: - name: Checkout @@ -90,8 +71,73 @@ jobs: context: . push: ${{ github.event_name != 'pull_request' }} platforms: ${{ github.event_name == 'pull_request' && 'linux/amd64' || matrix.docker_platforms }} + # Run the test suites inside the image being built. Nothing of them is kept, and + # the build fails if one does, so no image is published from a tree that fails. + build-args: | + RUN_TESTS=1 file: ./docker/${{ matrix.docker_path }}/Dockerfile tags: introlab3it/rtabmap_ros:${{ matrix.docker_tag }} no-cache: true cache-to: type=inline + # Images that install a released rtabmap_ros from apt (docker/): they hold + # nothing from the tree under review, so building them on a pull request would only + # report that the release still installs. Left to the pushes that publish them. + docker-released: + if: ${{ github.event_name != 'pull_request' && (github.event_name != 'workflow_dispatch' || github.ref == 'refs/heads/ros2') }} + runs-on: ubuntu-latest + + strategy: + fail-fast: false + matrix: + docker_tag: [humble, jazzy, kilted, lyrical] + include: + - docker_tag: humble + docker_path: 'humble' + docker_platforms: | + linux/amd64 + - docker_tag: jazzy + docker_path: 'jazzy' + docker_platforms: | + linux/amd64 + linux/arm64 + - docker_tag: kilted + docker_path: 'kilted' + docker_platforms: | + linux/amd64 + linux/arm64 + - docker_tag: lyrical + docker_path: 'lyrical' + docker_platforms: | + linux/amd64 + linux/arm64 + + steps: + - + name: Checkout + uses: actions/checkout@v4 + - + name: Set up QEMU + uses: docker/setup-qemu-action@v3 + with: + platforms: all + - + name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 + - + name: Login to DockerHub + uses: docker/login-action@v3 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + - + name: Build and push + uses: docker/build-push-action@v6 + with: + context: . + push: true + platforms: ${{ matrix.docker_platforms }} + file: ./docker/${{ matrix.docker_path }}/Dockerfile + tags: introlab3it/rtabmap_ros:${{ matrix.docker_tag }} + no-cache: true + cache-to: type=inline diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 0723b173..6e0cf1f5 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -25,24 +25,26 @@ jobs: include: - ros_distro: humble skip_keys: '' - packages: 'rtabmap_ros rtabmap_conversions' + packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python' - ros_distro: jazzy skip_keys: '' - packages: 'rtabmap_ros rtabmap_conversions' + packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python' - ros_distro: kilted skip_keys: 'grid_map_ros' - packages: 'rtabmap_ros rtabmap_conversions' + packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python' - ros_distro: lyrical - skip_keys: 'nav2_bringup nav2_msgs velodyne grid_map_ros realsense2_camera libpointmatcher' - # rtabmap_costmap_plugins cannot be built, missing nav2 on lyrical, build other packages: - packages: 'rtabmap_launch rtabmap_demos rtabmap_python rtabmap_examples rtabmap_rviz_plugins rtabmap_conversions' + skip_keys: 'velodyne grid_map_ros libpointmatcher' + packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python' - ros_distro: rolling - # libpointmatcher and gtsam are rtabmap deps not yet available on rolling - # (same skip keys as introlab/rtabmap's cmake-ros workflow) - skip_keys: 'nav2_bringup nav2_msgs velodyne libpointmatcher gtsam' + # 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 + # to rtabmap, which just builds without them. + skip_keys: 'velodyne libpointmatcher gtsam grid_map_ros' use_ros2_testing: true # Rolling is using ros2-testing (nightly) - # rtabmap_costmap_plugins cannot be built, missing nav2 on rolling, build other packages: - packages: 'rtabmap_launch rtabmap_demos rtabmap_python rtabmap_examples rtabmap_rviz_plugins rtabmap_conversions' + # nav2 is missing from resolute too, and it is not optional: rtabmap_slam and + # rtabmap_costmap_plugins need it to compile at all, and the rtabmap_ros + # metapackage pulls both in. So rolling names the packages it can build. + packages: 'rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python' fail-fast: false container: image: osrf/ros:${{ matrix.ros_distro }}-desktop-full diff --git a/README.md b/README.md index 51bb9c3c..0ede8901 100644 --- a/README.md +++ b/README.md @@ -36,7 +36,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_odom` | Odometry nodes — `rgbd_odometry`, `stereo_odometry` and `icp_odometry`. Any external odometry can be used instead. | +| [`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`. | ### Sensor processing @@ -100,6 +100,15 @@ sudo apt install ros-$ROS_DISTRO-rtabmap-ros colcon build --symlink-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DRTABMAP_SYNC_USER_DATA=ON -DCMAKE_BUILD_TYPE=Release ``` +### Testing + +```bash +cd ~/ros2_ws +colcon build --base-paths src/rtabmap_ros +colcon test --base-paths src/rtabmap_ros +colcon test-result --verbose +``` + # Usage * For sensor integration examples (stereo and RGB-D cameras, 3D LiDAR), see [rtabmap_examples](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) sub-folder. diff --git a/codecov.yml b/codecov.yml index 2e315d02..fe2e77e4 100644 --- a/codecov.yml +++ b/codecov.yml @@ -56,6 +56,10 @@ component_management: name: rtabmap_sync paths: - rtabmap_sync/** + - component_id: rtabmap_odom + name: rtabmap_odom + paths: + - rtabmap_odom/** - component_id: rtabmap_python name: rtabmap_python paths: @@ -66,8 +70,8 @@ comment: behavior: default require_changes: true # stay quiet when coverage doesn't move -# Only rtabmap_conversions, rtabmap_util, rtabmap_sync and rtabmap_python have tests -# today, so +# Only rtabmap_conversions, rtabmap_util, rtabmap_sync, rtabmap_odom 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. @@ -77,7 +81,6 @@ ignore: - "rtabmap_examples/**" - "rtabmap_launch/**" - "rtabmap_msgs/**" - - "rtabmap_odom/**" - "rtabmap_rviz_plugins/**" - "rtabmap_slam/**" - "rtabmap_viz/**" diff --git a/docker/humble/latest/Dockerfile b/docker/humble/latest/Dockerfile index fcbb8551..cf80889e 100644 --- a/docker/humble/latest/Dockerfile +++ b/docker/humble/latest/Dockerfile @@ -4,14 +4,32 @@ RUN mkdir -p ros2_ws/src COPY . ros2_ws/src/rtabmap_ros +# No rosdep "-r": a bad mirror must fail here. RUN source /ros_entrypoint.sh && \ cd ros2_ws && \ - export MAKEFLAGS="-j2" && \ + echo 'Acquire::Retries "5";' > /etc/apt/apt.conf.d/80-retries && \ rosdep init && \ rosdep update && \ apt-get update && \ - rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap" && \ - apt-get clean && rm -rf /var/lib/apt/lists/ && \ + rosdep install --from-paths src --ignore-src -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap" && \ + bash src/rtabmap_ros/docker/verify_deps.sh && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# Tests run in the build layer, before the workspace is deleted, so none of them reach +# the image. CI asks for them; a target that is not the architecture being built on +# skips them, that one being emulated. Only buildx sets BUILDPLATFORM, so anything +# else building with RUN_TESTS=1 runs them. +ARG RUN_TESTS=0 +ARG BUILDPLATFORM +ARG TARGETPLATFORM + +RUN source /ros_entrypoint.sh && \ + cd ros2_ws && \ + export MAKEFLAGS="-j2" && \ colcon build --executor sequential --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ + if [ "$RUN_TESTS" = "1" ] && [ "${BUILDPLATFORM:-$TARGETPLATFORM}" = "$TARGETPLATFORM" ]; then \ + colcon test --executor sequential --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install && \ + colcon test-result --verbose; \ + fi && \ cd && \ rm -rf ros2_ws diff --git a/docker/iron/latest/Dockerfile b/docker/iron/latest/Dockerfile index 3051956f..87b5919b 100644 --- a/docker/iron/latest/Dockerfile +++ b/docker/iron/latest/Dockerfile @@ -6,15 +6,21 @@ RUN source /ros_entrypoint.sh && \ COPY . ros2_ws/src/rtabmap_ros +# No rosdep "-r": a bad mirror must fail here. RUN source /ros_entrypoint.sh && \ cd ros2_ws && \ - export MAKEFLAGS="-j1" && \ + echo 'Acquire::Retries "5";' > /etc/apt/apt.conf.d/80-retries && \ rosdep init && \ rosdep update && \ apt-get update && \ - rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool && \ + rosdep install --from-paths src --ignore-src -y -t build_export -t test -t build -t buildtool_export -t buildtool && \ apt remove ros-$ROS_DISTRO-rtabmap* -y && \ - apt-get clean && rm -rf /var/lib/apt/lists/ && \ + bash src/rtabmap_ros/docker/verify_deps.sh && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +RUN source /ros_entrypoint.sh && \ + cd ros2_ws && \ + export MAKEFLAGS="-j1" && \ colcon build --event-handlers console_direct+ --install-base /opt/ros/iron --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ cd && \ rm -rf ros2_ws diff --git a/docker/jazzy/latest/Dockerfile b/docker/jazzy/latest/Dockerfile index 62259cc4..95c67b07 100644 --- a/docker/jazzy/latest/Dockerfile +++ b/docker/jazzy/latest/Dockerfile @@ -6,14 +6,32 @@ RUN source /ros_entrypoint.sh && \ COPY . ros2_ws/src/rtabmap_ros +# No rosdep "-r": a bad mirror must fail here. RUN source /ros_entrypoint.sh && \ cd ros2_ws && \ - export MAKEFLAGS="-j2" && \ + echo 'Acquire::Retries "5";' > /etc/apt/apt.conf.d/80-retries && \ rosdep init && \ rosdep update && \ apt-get update && \ - rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap grid_map_ros" && \ - apt-get clean && rm -rf /var/lib/apt/lists/ && \ + rosdep install --from-paths src --ignore-src -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap grid_map_ros" && \ + bash src/rtabmap_ros/docker/verify_deps.sh && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# Tests run in the build layer, before the workspace is deleted, so none of them reach +# the image. CI asks for them; a target that is not the architecture being built on +# skips them, that one being emulated. Only buildx sets BUILDPLATFORM, so anything +# else building with RUN_TESTS=1 runs them. +ARG RUN_TESTS=0 +ARG BUILDPLATFORM +ARG TARGETPLATFORM + +RUN source /ros_entrypoint.sh && \ + cd ros2_ws && \ + export MAKEFLAGS="-j2" && \ colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ + if [ "$RUN_TESTS" = "1" ] && [ "${BUILDPLATFORM:-$TARGETPLATFORM}" = "$TARGETPLATFORM" ]; then \ + colcon test --executor sequential --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install && \ + colcon test-result --verbose; \ + fi && \ cd && \ rm -rf ros2_ws diff --git a/docker/kilted/latest/Dockerfile b/docker/kilted/latest/Dockerfile index 685cbec1..3190fbc1 100644 --- a/docker/kilted/latest/Dockerfile +++ b/docker/kilted/latest/Dockerfile @@ -6,14 +6,32 @@ RUN source /ros_entrypoint.sh && \ COPY . ros2_ws/src/rtabmap_ros +# No rosdep "-r": a bad mirror must fail here. RUN source /ros_entrypoint.sh && \ cd ros2_ws && \ - export MAKEFLAGS="-j2" && \ + echo 'Acquire::Retries "5";' > /etc/apt/apt.conf.d/80-retries && \ rosdep init && \ rosdep update && \ apt-get update && \ - rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap nav2_bringup realsense2_camera nav2_msgs grid_map_ros" && \ - apt-get clean && rm -rf /var/lib/apt/lists/ && \ + rosdep install --from-paths src --ignore-src -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap nav2_bringup realsense2_camera nav2_msgs grid_map_ros" && \ + bash src/rtabmap_ros/docker/verify_deps.sh && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# Tests run in the build layer, before the workspace is deleted, so none of them reach +# the image. CI asks for them; a target that is not the architecture being built on +# skips them, that one being emulated. Only buildx sets BUILDPLATFORM, so anything +# else building with RUN_TESTS=1 runs them. +ARG RUN_TESTS=0 +ARG BUILDPLATFORM +ARG TARGETPLATFORM + +RUN source /ros_entrypoint.sh && \ + cd ros2_ws && \ + export MAKEFLAGS="-j2" && \ colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ + if [ "$RUN_TESTS" = "1" ] && [ "${BUILDPLATFORM:-$TARGETPLATFORM}" = "$TARGETPLATFORM" ]; then \ + colcon test --executor sequential --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install && \ + colcon test-result --verbose; \ + fi && \ cd && \ rm -rf ros2_ws diff --git a/docker/lyrical/latest/Dockerfile b/docker/lyrical/latest/Dockerfile index 6517694d..0551179a 100644 --- a/docker/lyrical/latest/Dockerfile +++ b/docker/lyrical/latest/Dockerfile @@ -6,14 +6,32 @@ RUN source /ros_entrypoint.sh && \ COPY . ros2_ws/src/rtabmap_ros +# No rosdep "-r": a bad mirror must fail here. RUN source /ros_entrypoint.sh && \ cd ros2_ws && \ - export MAKEFLAGS="-j2" && \ + echo 'Acquire::Retries "5";' > /etc/apt/apt.conf.d/80-retries && \ rosdep init && \ rosdep update && \ apt-get update && \ - rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap nav2_bringup realsense2_camera nav2_msgs grid_map_ros nav2_costmap_2d" && \ - apt-get clean && rm -rf /var/lib/apt/lists/ && \ - colcon build --packages-skip rtabmap_costmap_plugins rtabmap_ros --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ + rosdep install --from-paths src --ignore-src -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap nav2_bringup realsense2_camera nav2_msgs grid_map_ros" && \ + bash src/rtabmap_ros/docker/verify_deps.sh && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# Tests run in the build layer, before the workspace is deleted, so none of them reach +# the image. CI asks for them; a target that is not the architecture being built on +# skips them, that one being emulated. Only buildx sets BUILDPLATFORM, so anything +# else building with RUN_TESTS=1 runs them. +ARG RUN_TESTS=0 +ARG BUILDPLATFORM +ARG TARGETPLATFORM + +RUN source /ros_entrypoint.sh && \ + cd ros2_ws && \ + export MAKEFLAGS="-j2" && \ + colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \ + if [ "$RUN_TESTS" = "1" ] && [ "${BUILDPLATFORM:-$TARGETPLATFORM}" = "$TARGETPLATFORM" ]; then \ + colcon test --executor sequential --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install && \ + colcon test-result --verbose; \ + fi && \ cd && \ rm -rf ros2_ws diff --git a/docker/verify_deps.sh b/docker/verify_deps.sh new file mode 100755 index 00000000..d59648c5 --- /dev/null +++ b/docker/verify_deps.sh @@ -0,0 +1,63 @@ +#!/usr/bin/env bash +# +# Sanity-check the build sysroot right after "rosdep install". +# +# apt/dpkg can be left in a half-applied state, most often on the QEMU-emulated +# arm64 CI leg (ports.ubuntu.com is a single, frequently desynced mirror). CMake +# does not notice, because both PCL and VTK look their files up in ways that +# degrade silently: +# +# * PCLConfig.cmake resolves each component with +# find_library(... HINTS ${PCL_LIBRARY_DIRS} NO_DEFAULT_PATH), so a missing +# libpcl_*.so only yields "Could NOT find PCL_COMMON (missing: +# PCL_COMMON_LIBRARY)" on stderr and configuring still succeeds. +# +# * VTK-targets.cmake creates every VTK::* imported target, then loads their +# IMPORTED_LOCATION from the per-configuration files it picks up with +# file(GLOB VTK-targets-*.cmake). An empty glob is not an error, so the +# targets survive with no location at all and the build only dies at the +# generate step with "IMPORTED_LOCATION not set for imported target +# VTK::CommonCore configuration Release". +# +# Both surface hours into the build, in whichever package first links those +# targets (rtabmap_odom, via pcl_ros). Fail here instead, where the cause is +# still readable. + +set -euo pipefail +shopt -s nullglob + +status=0 + +fail() { + echo "verify_deps: $*" >&2 + status=1 +} + +# PCLConfig.cmake and the libpcl_*.so development symlinks both ship in +# libpcl-dev, so finding the config without them means the package is not +# fully installed. +for config in /usr/lib/*/cmake/pcl/PCLConfig.cmake /usr/lib/cmake/pcl/PCLConfig.cmake; do + # nullglob only drops patterns, not wildcard-free words. + [ -e "${config}" ] || continue + libdir=${config%/cmake/pcl/PCLConfig.cmake} + for component in common io kdtree search surface filters registration \ + sample_consensus segmentation visualization; do + if [ ! -e "${libdir}/libpcl_${component}.so" ]; then + fail "${libdir}/libpcl_${component}.so is missing while ${config} is installed (libpcl-dev is incomplete)" + fi + done +done + +for targets in /usr/lib/*/cmake/vtk-*/VTK-targets.cmake /usr/lib/cmake/vtk-*/VTK-targets.cmake; do + [ -e "${targets}" ] || continue + if ! compgen -G "${targets%.cmake}-*.cmake" > /dev/null; then + fail "no VTK-targets-.cmake next to ${targets}, every VTK::* target would have no IMPORTED_LOCATION (libvtk-dev is incomplete)" + fi +done + +if [ "${status}" -ne 0 ]; then + echo "verify_deps: dependency installation left an inconsistent sysroot, aborting before the build" >&2 + exit 1 +fi + +echo "verify_deps: PCL and VTK sysroot look consistent" diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index 7300b3ea..f0db191f 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.10 REQUIRED) +find_package(RTABMap 0.23.12 REQUIRED) # libraries SET(Libraries diff --git a/rtabmap_conversions/README.md b/rtabmap_conversions/README.md index 91e04544..2909600e 100644 --- a/rtabmap_conversions/README.md +++ b/rtabmap_conversions/README.md @@ -6,6 +6,13 @@ This package is a library only — it contains no nodes, no launch files and no You only need it directly if you are writing your own node against RTAB-Map's C++ API and want to publish or subscribe to `rtabmap_msgs`. +## Contents + +- [Usage](#usage) +- [What it covers](#what-it-covers) +- [Conventions worth knowing](#conventions-worth-knowing) +- [License](#license) + ## Usage Add the dependency to your `package.xml` and `CMakeLists.txt`: @@ -56,14 +63,6 @@ These cut across the whole API and are not obvious from the signatures. Per-func **`CameraInfo` matrices are fixed-size arrays.** `k`, `r` and `p` are `std::array`, so they are never "empty" — an unset matrix is all zeros. `cameraModelFromROS()` treats a zero `k[0]`/`p[0]` (the focal length) as absent. -## Building and testing - -```bash -colcon build --packages-select rtabmap_conversions -colcon test --packages-select rtabmap_conversions -colcon test-result --verbose -``` - ## License BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license). diff --git a/rtabmap_conversions/include/rtabmap_conversions/PointCloudConversion.h b/rtabmap_conversions/include/rtabmap_conversions/PointCloudConversion.h new file mode 100644 index 00000000..95d898a5 --- /dev/null +++ b/rtabmap_conversions/include/rtabmap_conversions/PointCloudConversion.h @@ -0,0 +1,75 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#ifndef RTABMAP_CONVERSIONS_POINTCLOUDCONVERSION_H_ +#define RTABMAP_CONVERSIONS_POINTCLOUDCONVERSION_H_ + +#include + +#include +#include + +/** + * @file + * @brief pcl::toROSMsg and pcl::fromROSMsg, minus their empty-cloud crash. + * + * Both take the address of the first point, and of the first byte of the output, before + * checking that there is one (see pcl/conversions.h): for an empty cloud that indexes + * past the end of an empty vector. Nothing notices while the standard library does not + * check, which is why it went unseen for years -- Ubuntu enables those checks from + * resolute on, and then the process aborts outright. + * + * An empty cloud is ordinary here rather than exceptional: a scan whose points were all + * filtered out, a frame with no obstacles in it, an occupancy grid with nothing new. Each + * of those still has to be published, so the conversions are used through this. + */ + +namespace rtabmap_conversions { + +/** + * @brief @p cloud as a PointCloud2 message. + * + * An empty cloud is converted as a single point and emptied afterwards, so the message + * still carries the field layout the installed PCL would have given it. + */ +template +void toPointCloud2Msg( + const pcl::PointCloud & cloud, sensor_msgs::msg::PointCloud2 & msg) +{ + if(!cloud.empty()) + { + pcl::toROSMsg(cloud, msg); + return; + } + + pcl::PointCloud onePoint; + onePoint.header = cloud.header; + onePoint.is_dense = cloud.is_dense; + onePoint.push_back(PointT()); + pcl::toROSMsg(onePoint, msg); + msg.width = 0; + msg.height = 1; + msg.row_step = 0; + msg.data.clear(); +} + +/// @brief @p msg as a point cloud, an empty message included. +template +void fromPointCloud2Msg( + const sensor_msgs::msg::PointCloud2 & msg, pcl::PointCloud & cloud) +{ + if(msg.data.empty()) + { + cloud.clear(); + cloud.is_dense = msg.is_dense; + pcl_conversions::toPCL(msg.header, cloud.header); + return; + } + pcl::fromROSMsg(msg, cloud); +} + +} // namespace rtabmap_conversions + +#endif /* RTABMAP_CONVERSIONS_POINTCLOUDCONVERSION_H_ */ diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 1f2c4685..9e00d0a1 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include #include "rtabmap_conversions/MsgConversion.h" #include @@ -2808,7 +2809,7 @@ bool convertScanMsg( if(hasIntensity) { pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(scanOut, *pclScan); + rtabmap_conversions::fromPointCloud2Msg(scanOut, *pclScan); pclScan->is_dense = true; data = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom).data(); // put back in laser frame format = rtabmap::LaserScan::kXYI; @@ -2816,7 +2817,7 @@ bool convertScanMsg( else { pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(scanOut, *pclScan); + rtabmap_conversions::fromPointCloud2Msg(scanOut, *pclScan); pclScan->is_dense = true; data = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom).data(); // put back in laser frame format = rtabmap::LaserScan::kXY; diff --git a/rtabmap_costmap_plugins/CMakeLists.txt b/rtabmap_costmap_plugins/CMakeLists.txt index d592a057..eab748d7 100644 --- a/rtabmap_costmap_plugins/CMakeLists.txt +++ b/rtabmap_costmap_plugins/CMakeLists.txt @@ -45,6 +45,7 @@ IF("$ENV{ROS_DISTRO}" STRLESS "lyrical") IF("$ENV{ROS_DISTRO}" STRLESS "jazzy") target_compile_definitions(rtabmap_costmap_plugins PRIVATE -DPRE_ROS_JAZZY) ENDIF() + target_compile_definitions(rtabmap_costmap_plugins PRIVATE -DPRE_ROS_LYRICAL) ament_target_dependencies(rtabmap_costmap_plugins ${AmentLibraries}) ELSE() target_link_libraries(rtabmap_costmap_plugins PRIVATE ${Libraries}) diff --git a/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp b/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp index affc632d..5e1115d1 100644 --- a/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp +++ b/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp @@ -284,6 +284,15 @@ protected: rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector parameters); + /** + * @brief Declares one of this layer's parameters and returns its value + * @param node the node the layer runs in + * @param name the parameter's name, without the layer prefix + * @param defaultValue the value to use when nothing else sets it + */ + template + T declareOrGetParameter(NodeT & node, const std::string & name, const T & defaultValue); + // Dynamic parameters handler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; }; diff --git a/rtabmap_costmap_plugins/src/voxel_layer.cpp b/rtabmap_costmap_plugins/src/voxel_layer.cpp index dcb6ca3f..27a99792 100644 --- a/rtabmap_costmap_plugins/src/voxel_layer.cpp +++ b/rtabmap_costmap_plugins/src/voxel_layer.cpp @@ -58,42 +58,76 @@ using rcl_interfaces::msg::ParameterType; namespace rtabmap_costmap_plugins { +namespace +{ + +/// nav2's Observation::cloud_ used to be a raw pointer and is now the cloud itself, so +/// it is reached through this rather than dereferenced directly. +inline const sensor_msgs::msg::PointCloud2 & cloudOf( + const sensor_msgs::msg::PointCloud2 & cloud) +{ + return cloud; +} + +inline const sensor_msgs::msg::PointCloud2 & cloudOf( + const sensor_msgs::msg::PointCloud2 * cloud) +{ + return *cloud; +} + +/// nav2 hands out observations by value up to kilted and by shared pointer after it. +inline const nav2_costmap_2d::Observation & obsOf( + const nav2_costmap_2d::Observation & observation) +{ + return observation; +} + +inline const nav2_costmap_2d::Observation & obsOf( + const std::shared_ptr & observation) +{ + return *observation; +} + +} // namespace + +/// nav2 declared a layer's parameters through Layer::declareParameter up to kilted and +/// through the node itself after it. +template +T VoxelLayer::declareOrGetParameter( + NodeT & node, const std::string & name, const T & defaultValue) +{ +#ifdef PRE_ROS_LYRICAL + declareParameter(name, rclcpp::ParameterValue(defaultValue)); + T value = defaultValue; + node->get_parameter(name_ + "." + name, value); + return value; +#else + return node->declare_or_get_parameter(name_ + "." + name, defaultValue); +#endif +} + void VoxelLayer::onInitialize() { nav2_costmap_2d::ObstacleLayer::onInitialize(); - declareParameter("enabled", rclcpp::ParameterValue(true)); - declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true)); - declareParameter("min_obstacle_height", rclcpp::ParameterValue(0.0)); - declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0)); - declareParameter("z_voxels", rclcpp::ParameterValue(10)); - declareParameter("origin_z", rclcpp::ParameterValue(0.0)); - declareParameter("z_resolution", rclcpp::ParameterValue(0.2)); - declareParameter("unknown_threshold", rclcpp::ParameterValue(15)); - declareParameter("mark_threshold", rclcpp::ParameterValue(0)); - declareParameter("combination_method", rclcpp::ParameterValue(1)); - declareParameter("publish_voxel_map", rclcpp::ParameterValue(false)); - declareParameter("robot_base_frame", rclcpp::ParameterValue("base_link")); - auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; } - node->get_parameter(name_ + "." + "enabled", enabled_); - node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); - node->get_parameter(name_ + "." + "min_obstacle_height", min_obstacle_height_); - node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_); - node->get_parameter(name_ + "." + "z_voxels", size_z_); - node->get_parameter(name_ + "." + "origin_z", origin_z_); - node->get_parameter(name_ + "." + "z_resolution", z_resolution_); - node->get_parameter(name_ + "." + "unknown_threshold", unknown_threshold_); - node->get_parameter(name_ + "." + "mark_threshold", mark_threshold_); - node->get_parameter(name_ + "." + "publish_voxel_map", publish_voxel_); - node->get_parameter(name_ + "." + "robot_base_frame", robot_base_frame_); + enabled_ = declareOrGetParameter(node, "enabled", true); + footprint_clearing_enabled_ = declareOrGetParameter(node, "footprint_clearing_enabled", true); + min_obstacle_height_ = declareOrGetParameter(node, "min_obstacle_height", 0.0); + max_obstacle_height_ = declareOrGetParameter(node, "max_obstacle_height", 2.0); + size_z_ = declareOrGetParameter(node, "z_voxels", 10); + origin_z_ = declareOrGetParameter(node, "origin_z", 0.0); + z_resolution_ = declareOrGetParameter(node, "z_resolution", 0.2); + unknown_threshold_ = declareOrGetParameter(node, "unknown_threshold", 15); + mark_threshold_ = declareOrGetParameter(node, "mark_threshold", 0); + publish_voxel_ = declareOrGetParameter(node, "publish_voxel_map", false); + robot_base_frame_ = declareOrGetParameter(node, "robot_base_frame", std::string("base_link")); - int combination_method_param{}; - node->get_parameter(name_ + "." + "combination_method", combination_method_param); + const int combination_method_param = declareOrGetParameter(node, "combination_method", 1); #ifdef PRE_ROS_JAZZY combination_method_ = combination_method_param; #else @@ -169,7 +203,11 @@ void VoxelLayer::updateBounds( useExtraBounds(min_x, min_y, max_x, max_y); bool current = true; +#ifdef PRE_ROS_LYRICAL std::vector observations, clearing_observations; +#else + std::vector observations, clearing_observations; +#endif // get the marking observations current = getMarkingObservations(observations) && current; @@ -182,16 +220,15 @@ void VoxelLayer::updateBounds( // raytrace freespace for (unsigned int i = 0; i < clearing_observations.size(); ++i) { - raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y); + raytraceFreespace(obsOf(clearing_observations[i]), min_x, min_y, max_x, max_y); } // place the new obstacles into a priority queue... each with a priority of zero to begin with - for (std::vector::const_iterator it = observations.begin(); it != observations.end(); - ++it) + for (auto it = observations.begin(); it != observations.end(); ++it) { - const nav2_costmap_2d::Observation & obs = *it; + const nav2_costmap_2d::Observation & obs = obsOf(*it); - const sensor_msgs::msg::PointCloud2 & cloud = *(obs.cloud_); + const sensor_msgs::msg::PointCloud2 & cloud = cloudOf(obs.cloud_); double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_; double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_; @@ -277,7 +314,9 @@ void VoxelLayer::raytraceFreespace( { auto clearing_endpoints_ = std::make_unique(); - if (clearing_observation.cloud_->height == 0 || clearing_observation.cloud_->width == 0) { + const sensor_msgs::msg::PointCloud2 & clearing_cloud = cloudOf(clearing_observation.cloud_); + + if (clearing_cloud.height == 0 || clearing_cloud.width == 0) { return; } @@ -311,8 +350,8 @@ void VoxelLayer::raytraceFreespace( } clearing_endpoints_->data.clear(); - clearing_endpoints_->width = clearing_observation.cloud_->width; - clearing_endpoints_->height = clearing_observation.cloud_->height; + clearing_endpoints_->width = clearing_cloud.width; + clearing_endpoints_->height = clearing_cloud.height; clearing_endpoints_->is_dense = true; clearing_endpoints_->is_bigendian = false; @@ -331,9 +370,9 @@ void VoxelLayer::raytraceFreespace( double map_end_y = origin_y_ + getSizeInMetersY(); double map_end_z = origin_z_ + getSizeInMetersZ(); - sensor_msgs::PointCloud2ConstIterator iter_x(*(clearing_observation.cloud_), "x"); - sensor_msgs::PointCloud2ConstIterator iter_y(*(clearing_observation.cloud_), "y"); - sensor_msgs::PointCloud2ConstIterator iter_z(*(clearing_observation.cloud_), "z"); + sensor_msgs::PointCloud2ConstIterator iter_x(clearing_cloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(clearing_cloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(clearing_cloud, "z"); for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { double wpx = *iter_x; @@ -431,7 +470,7 @@ void VoxelLayer::raytraceFreespace( if (publish_clearing_points) { clearing_endpoints_->header.frame_id = global_frame_; - clearing_endpoints_->header.stamp = clearing_observation.cloud_->header.stamp; + clearing_endpoints_->header.stamp = clearing_cloud.header.stamp; clearing_endpoints_pub_->publish(std::move(clearing_endpoints_)); } diff --git a/rtabmap_odom/CMakeLists.txt b/rtabmap_odom/CMakeLists.txt index 7b10ad25..4a1a72e6 100644 --- a/rtabmap_odom/CMakeLists.txt +++ b/rtabmap_odom/CMakeLists.txt @@ -157,4 +157,52 @@ install(DIRECTORY include/ FILES_MATCHING PATTERN "*.h" ) +############# +## Testing ## +############# +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(OpenCV REQUIRED COMPONENTS core imgcodecs) + # Recorded sensor input is replayed by the tests themselves, not by "ros2 bag play". + find_package(rosbag2_cpp REQUIRED) + + # Real frames the visual odometry tests register against, read from the source tree: + # these binaries are never installed, and the fixtures are not either. See + # test/data/README.md for where they come from. + set(rtabmap_odom_test_data_root "${CMAKE_CURRENT_SOURCE_DIR}/test/data") + + # Each node gets its own test binary: a crash or a stuck executor in one node cannot + # take the others down, and every binary starts with a clean DDS graph. + # + # 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, + # rgbd_image, scan_cloud -- with rtabmap_sync's and rtabmap_util's. On a shared domain + # they discover each other's publishers and assertions then see traffic the test never + # sent. rtabmap_util numbers from 30 and rtabmap_sync from 50; keep the ranges apart. + set(rtabmap_odom_test_domain_id 70) + macro(rtabmap_odom_add_node_test test_name) + ament_add_gtest(${test_name} test/${test_name}.cpp + ENV ROS_DOMAIN_ID=${rtabmap_odom_test_domain_id} + TIMEOUT 300) + math(EXPR rtabmap_odom_test_domain_id "${rtabmap_odom_test_domain_id} + 1") + if(TARGET ${test_name}) + target_include_directories(${test_name} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test) + target_compile_definitions(${test_name} PRIVATE + RTABMAP_ODOM_TEST_DATA_ROOT="${rtabmap_odom_test_data_root}") + target_link_libraries(${test_name} rtabmap_odom_plugins rtabmap_odom + opencv_core opencv_imgcodecs rosbag2_cpp::rosbag2_cpp rtabmap::core) + if("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(${test_name} ${AmentLibraries}) + else() + target_link_libraries(${test_name} ${Libraries} ${PublicLibraries}) + endif() + endif() + endmacro() + + rtabmap_odom_add_node_test(test_odometry_ros) + rtabmap_odom_add_node_test(test_rgbd_odometry) + rtabmap_odom_add_node_test(test_stereo_odometry) + rtabmap_odom_add_node_test(test_icp_odometry) +endif() + ament_package() diff --git a/rtabmap_odom/README.md b/rtabmap_odom/README.md new file mode 100644 index 00000000..9c77e6cf --- /dev/null +++ b/rtabmap_odom/README.md @@ -0,0 +1,273 @@ +# rtabmap_odom + +Odometry for [RTAB-Map](https://github.com/introlab/rtabmap): where the robot is relative to a local fixed frame, estimated from its own sensors — a pose that moves continuously and never jumps, but drifts over time. + +SLAM needs a pose for every measurement it maps. These nodes produce one by registering each new frame against the last — visually from an RGB-D or stereo camera, or geometrically from a lidar — and integrating the result into a [`nav_msgs/msg/Odometry`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Odometry.html) and a TF. + +## Contents + +- [Nodes](#nodes) + - [Choosing a sensor modality for the environment](#choosing-a-sensor-modality-for-the-environment) +- [Library](#library) +- [Conventions](#conventions) + - [Frames and TF](#frames-and-tf) + - [RTAB-Map's own parameters](#rtab-maps-own-parameters) + - [Feeding in an external guess](#feeding-in-an-external-guess) + - [IMU](#imu) + - [Update rates and dropped frames](#update-rates-and-dropped-frames) + - [Lost frames, resets and new maps](#lost-frames-resets-and-new-maps) +- [Services](#services) +- [Published topics](#published-topics) + - [Outputting filtered scans and features](#outputting-filtered-scans-and-features) +- [Diagnostics](#diagnostics) +- [License](#license) + +## Nodes + +| Node | Description | +|---|---| +| [rgbd_odometry](doc/rgbd_odometry.md) | Visual odometry from a color image and a depth image registered to it. | +| [stereo_odometry](doc/stereo_odometry.md) | Visual odometry from a stereo pair. | +| [icp_odometry](doc/icp_odometry.md) | Geometric odometry from a 2D or 3D lidar. | + +Every node is a [composable node](https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Composition.html) as well as a standalone executable. + +### Choosing a sensor modality for the environment + +Which to use is a question about the **environment**, not about which sensor is better. A camera tracks visual texture; a lidar tracks geometry. Each fails where its own cue is missing, and the two failures do not overlap much. + +| Environment | Use | Why | +|---|---|---| +| Visually textured and well lit — offices, cluttered rooms, daylight outdoors | **Camera** | Plenty of features to match, and appearance gives loop closure for free. | +| Textureless but geometrically rich — bare corridors with doorways and furniture, warehouse aisles | **Lidar** | Blank walls give a camera nothing; the shape of the space still constrains ICP. | +| Dark, or lighting that changes abruptly | **Lidar** | A camera is simply blind. Lidar does not care. | +| Geometrically plain but visually rich — a large open hall with a patterned floor, textured flat walls | **Camera** | [Degenerate geometry](doc/icp_odometry.md#degenerate-geometry) defeats ICP here, while the texture is exactly what a camera needs. | +| Both plain and textureless — an empty warehouse, a long featureless tunnel | **Wheel odometry**, with either as a corrector | Neither cue is present. This is the case where wheel odometry carries the pose. | +| Repetitive and self-similar — tiled floors, rows of racking, a long colonnade | **Wheel odometry as the guess**, with either on top | Both cues are present but *ambiguous*: a camera matches the wrong copy of a feature, a lidar the wrong bay of shelving. See [Repetitive patterns](doc/rgbd_odometry.md#repetitive-patterns). | +| Outdoors at range | **Stereo camera or 3D lidar** | RGB-D depth stops working outdoors; both of these keep going. | + +**Do not underestimate wheel odometry.** On a wheeled robot it is locally excellent and only drifts over distance — the opposite failure from both of the above, which are locally noisy but not systematically biased. It is also the only one of the three that keeps working when the environment offers no cue at all — and, because it is indifferent to what the scene *looks* like, the only one that is not fooled when the scene repeats itself. + +**Fuse the wheels with an IMU before feeding them in.** [`robot_localization`](https://docs.ros.org/en/jazzy/p/robot_localization/) is the standard way: its EKF combines wheel odometry with IMU orientation and angular rates into one filtered `odom` topic, which is a markedly better guess than the wheels alone. [FusionCore](https://github.com/manankharwar/fusioncore) is another EKF that does the same job. The IMU fixes exactly what encoders are worst at — yaw through a turn, and wheel slip, which encoders report as motion that never happened. Where the camera or lidar fails outright, that filtered estimate is what carries the robot through, and a pipeline built this way degrades instead of breaking. + +Which is why the robust arrangement is rarely one of them alone: feed wheel odometry in as `guess_frame_id` and the registration starts near the answer every frame. That covers the camera's fast-motion and blank-wall failures and the lidar's degenerate-corridor failure, while the camera or lidar in turn corrects the wheels' drift. See [Feeding in an external guess](#feeding-in-an-external-guess). + +**For 2D indoor odometry a lidar usually costs less computation.** Registering a few hundred scan points is far less CPU than detecting, describing and matching visual features on every frame, and it needs no GPU — which is what decides whether odometry keeps up on the small onboard computers these robots carry. + +With both a camera and a lidar, the usual arrangement is `icp_odometry` for the pose and the camera for appearance — see [Combining a camera and a lidar](doc/icp_odometry.md#combining-a-camera-and-a-lidar). + +## Library + +The package installs a C++ library, documented in the [C++ API reference](https://docs.ros.org/en/jazzy/p/rtabmap_odom/generated/index.html) generated from the headers. + +**`OdometryROS`** is the base class all three nodes derive from, and it is where most of this package's behaviour actually lives. It owns the RTAB-Map `Odometry` object, the pose integration, the TF broadcast, the IMU intake, the services and the diagnostics. Each node subclasses it to do one thing: turn its own topics into a `rtabmap::SensorData` and hand it over. That is why the three nodes share nearly all of their parameters and publish exactly the same topics. + +It also runs the registration on **its own thread**. A frame arriving while the previous one is still being processed does not block the subscription callback; see [Update rates and dropped frames](#update-rates-and-dropped-frames). + +## Conventions + +These apply to all three nodes. + +### Frames and TF + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `frame_id` | `string` | `"base_link"` | The robot frame being tracked. The pose published is this frame's, not the sensor's — the sensor-to-robot transform is read from TF. | +| `odom_frame_id` | `string` | `"odom"` | The fixed frame the pose is expressed in. | +| `publish_tf` | `bool` | `true` | Broadcast the pose on TF. **Turn this off if something else already publishes that transform**, or the two fight and TF alternates between them. What exactly is broadcast depends on `guess_frame_id`: without it, `odom_frame_id` → `frame_id`; with it, a correction `odom_frame_id` → `guess_frame_id` ([why](#it-also-keeps-tf-alive-through-a-failure)). | +| `wait_for_transform` | `double` | `0.1` | Seconds to wait for a needed transform before giving up on the frame. | +| `initial_pose` | `string` | `""` | Starting pose, `"x y z roll pitch yaw"`. Also settable at runtime through `reset_odom_to_pose`. | +| `ground_truth_frame_id` | `string` | `""` | When set, the pose is taken from this TF instead of being computed — for replaying a dataset with a known trajectory. | +| `ground_truth_base_frame_id` | `string` | value of `frame_id` | The robot frame within the ground truth TF tree. | +| `guess_frame_id` | `string` | `""` | A frame carrying another odometry source, used as the initial guess for each registration. Documented with its companions under [Feeding in an external guess](#feeding-in-an-external-guess) -- **the highest-value parameter here for a wheeled robot**. | + +The sensor must be connected to `frame_id` in TF **before the first frame arrives**, or that frame is dropped with a warning. A static publisher is the usual answer. + +### RTAB-Map's own parameters + +Everything in RTAB-Map's odometry parameter set is exposed as a ROS parameter **under its RTAB-Map name**, so tuning is done directly: + +```bash +ros2 run rtabmap_odom rgbd_odometry --ros-args \ + -p "Odom/Strategy:='1'" \ + -p "Vis/MinInliers:='15'" \ + -p "Odom/ResetCountdown:='1'" +``` + +**Note the quoting.** 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. Writing `-p Odom/Strategy:=1` makes ROS infer an integer, and the node throws on startup rather than starting with the wrong value: + +``` +parameter 'Odom/Strategy' has invalid type: Wrong parameter type, +parameter {Odom/Strategy} is of type {string}, setting it to {integer} is not allowed. +``` + +The inner quotes are what keeps it a string. Shell quotes alone do not help, since the value is parsed as YAML after the shell is done with it. In a launch file the same rule reads naturally: `{'Odom/Strategy': '1'}`. + +This applies **only** to RTAB-Map's own parameters. The nodes' ROS parameters -- `frame_id`, `publish_tf`, `scan_voxel_size`, `approx_sync` -- are declared with their real types and take plain values. + +Which parameters exist depends on the node: each declares the set matching its sensor, so `Vis/*` appears on the visual nodes and `Icp/*` only on `icp_odometry`. `ros2 param list` on a running node is the authoritative list; the meaning of each is in [RTAB-Map's parameter reference](https://github.com/introlab/rtabmap/blob/master/corelib/include/rtabmap/core/Parameters.h). + +The two worth knowing before anything else: + +- **`Odom/Strategy`** selects the registration algorithm — `0` frame-to-map (default, more accurate), `1` frame-to-frame (cheaper), and others for the external VO libraries RTAB-Map can be built against. +- **`Odom/ResetCountdown`** automatically resets odometry after this many consecutive lost frames instead of staying lost forever. `0` disables it, which is the default. + +`config_path` loads the same parameters from an INI file; only the odometry ones are taken from it. + +### Feeding in an external guess + +Registration works far better when it starts near the answer. Two ways to supply one: + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `guess_frame_id` | `string` | `""` | A TF frame carrying another odometry source — wheels, IMU-integrated, a base driver. Its motion between frames becomes the initial guess. | +| `guess_min_translation` | `double` | `0.0` | Skip frames whose guessed motion is below this, in meters. `0` disables. | +| `guess_min_rotation` | `double` | `0.0` | Same, in radians. | +| `guess_min_time` | `double` | `0.0` | Same, in seconds. | +| `guess_linear_variance` | `double` | `0.001` | Covariance of the published pose when the guess is used directly. | +| `guess_angular_variance` | `double` | `0.001` | Same, rotational. | + +**`guess_frame_id` is the single biggest improvement available to a wheeled robot.** Wheel odometry is locally excellent and globally hopeless; visual and ICP registration is the reverse. Giving the registration a wheel-odometry guess makes it converge more often, faster, and survive the frames where the camera sees nothing. + +The `guess_min_*` parameters additionally suppress processing while the robot is stationary, which stops a static scene from accumulating drift and saves the CPU. + +#### It also keeps TF alive through a failure + +Setting `guess_frame_id` also changes *how* the pose is broadcast. This is worth understanding before odometry fails on a real robot, because it decides what the rest of the system sees while it is lost. + +With a guess frame configured, the node no longer publishes `odom_frame_id` → `frame_id` directly. It publishes a **correction** instead, `odom_frame_id` → `guess_frame_id`. + +So the guess source keeps the conventional `odom` frame -- whatever produces it, the robot's driver or [`robot_localization`](https://docs.ros.org/en/jazzy/p/robot_localization/) -- and **this node takes a different name for its own `odom_frame_id`**. The examples in this repository use `vo` for the visual nodes and `icp_odom` for the lidar one: + +```mermaid +flowchart TD + ODOM(["/vo
odom_frame_id"]) + GUESS(["/odom
guess_frame_id"]) + BASE(["/base_link
frame_id"]) + SENSOR(["/camera or /lidar
the sensor's header.frame_id"]) + ODOM -->|correction, this node
e.g. ~10 Hz, ~50 ms delay| GUESS + GUESS -->|robot driver or robot_localization
e.g. ~50 Hz, ~1 ms delay| BASE + BASE -->|static| SENSOR +``` + +*The rates and delays above are examples only — yours depend on the sensor, the base driver and the computer.* + +**That chain keeps being published while registration is lost.** The correction freezes at the last successfully computed pose composed with the motion the guess has accumulated since, so `base_link` keeps moving in TF at the guess source's rate, driven entirely by the guess. Nothing downstream stalls or jumps; the pose just accumulates that source's drift until registration recovers. Without `guess_frame_id` there is no correction to publish and **no TF at all is broadcast while lost**, which is what breaks the tree. + +Pair it with `Odom/ResetCountdown` and the recovery is complete. Take a robot turning to face a white wall: visual odometry loses tracking, TF keeps flowing from the wheels, and after the configured number of failed frames the odometry resets — not to where it was when it got lost, but to `last computed pose × guess motion`, which is where the wheels say the robot has got to in the meantime. Registration restarts from there and the trajectory carries on with only the drift the wheels accumulated. + +What it resets to depends on what is available, in this order: + +1. **A guess** — resets to the last pose composed with the guess motion, as above. +2. **No guess, but `odom_frame_id` → `frame_id` exists in TF at the sensor frame's stamp** — resets to that pose. This is the `publish_tf:=false` arrangement: this node publishes only its odometry topic, [`robot_localization`](https://docs.ros.org/en/jazzy/p/robot_localization/) fuses that topic with the wheels and the IMU, and the filter owns the transform. The reset therefore lands on the filter's current estimate — the odometry gets restarted from where the fused solution says the robot is, having contributed to that solution itself while it was working. `publish_tf` has to be off for this to mean anything, and **`publish_null_when_lost` should be off too**: the null pose is a signal for consumers that read it as one, and a filter fusing this topic is not — it would be handed an invalid pose to fuse. With it off the node simply stops publishing while lost, and the filter carries on from its other inputs until registration recovers. +3. **Neither** — resets to the last computed pose, so the robot resumes believing it never moved while lost. + +After an automatic reset the countdown is left armed, so if odometry still cannot initialize on the next frames it keeps re-resetting to the latest guess rather than getting stuck. + +### IMU + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `wait_imu_to_init` | `bool` | `false` | Hold off until an IMU message has arrived, so gravity is known from the first frame. | +| `imu_queue_size` | `int` | `200` | Depth of the IMU buffer. IMUs run far faster than cameras; this is why it is large. | +| `qos_imu` | `int` | value of `qos` | Reliability of the `imu` subscription. | +| `always_check_imu_tf` | `bool` | `false` | Re-read the IMU-to-robot transform every message rather than caching it. | + +The `imu` topic is optional on all three nodes. Supplying it lets odometry know which way is down, which constrains roll and pitch — worth doing on any robot that has an IMU, and close to mandatory for a handheld or aerial one. + +**With no `guess_frame_id`, the IMU also supplies the rotation half of each frame's guess.** The rotation measured between the previous frame and this one becomes the guess's orientation, leaving the translation to the motion model. That is often the difference between tracking a fast turn and losing it, since rotation is what breaks feature matching first. An external guess takes precedence when there is one: `guess_frame_id` is used whole, and the IMU is not consulted for the guess at all. + +### Update rates and dropped frames + +Registration runs on its own thread, so a slow frame does not block the subscription. What happens to the frames arriving meanwhile is a choice: + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `always_process_most_recent_frame` | `bool` | `true` | Drop frames that arrive while registration is still running and take the newest. `false` registers every frame in order, on the subscription thread. | +| `expected_update_rate` | `double` | `0.0` | The rate frames are expected at, in Hz. Used only when `max_update_rate` is unset, and it is a **ceiling**: a frame arriving sooner than `1/expected_update_rate` after the last one is skipped, with a warning that the input is faster than expected. `0` disables. | +| `max_update_rate` | `double` | `0.0` | Throttle registration to at most this rate, skipping frames silently. Takes precedence over `expected_update_rate`. `0` disables. | +| `min_update_rate` | `double` | `0.0` | Treat odometry as **lost and reset it** when more than `1/min_update_rate` passes between updates — the motion assumption no longer holds across a gap that long. `0` disables. | + +**`always_process_most_recent_frame` already bounds the delay.** A frame arriving while registration is still running is dropped on the spot rather than queued, so the worker always picks up the newest frame and the published pose is at most one registration behind the sensor. No backlog ever forms. `/diagnostics` reports how many frames went this way. + +That is why **`max_update_rate` is about CPU, not latency**: given the skipping above, the worst-case delay is roughly the same whether it is set or not. What it changes is how many frames get registered at all. Set it to give the rest of the robot its cores back — not to make the pose fresher, which it will not do. + +Setting `always_process_most_recent_frame:=false` is the opposite trade: every frame is registered, in order, on the subscription thread. That is what you want when replaying a bag, where dropping frames loses data you meant to process. + +### Lost frames, resets and new maps + +A frame that cannot be registered is *lost*: the node publishes an all-zero pose with `9999` down the diagonal of both covariance matrices, which says there is no pose here to use. + +The first frame after a reset — from `reset_odom`, `reset_odom_to_pose` or `Odom/ResetCountdown` — carries the same `9999` for a different reason. It is an *initialization* rather than a registration: nothing to measure against, no velocity to carry over. Its pose is real and meant to be used; what the covariance says is that it does not continue the last valid one. + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `publish_null_when_lost` | `bool` | `true` | Publish a null pose, with `9999` on the covariance diagonals, when a frame cannot be registered. `false` publishes nothing. | + +**Leave it on, unless a filter is consuming this topic.** A consumer that sees the null message knows odometry is lost; one that sees nothing cannot tell that apart from a node that died or a topic that was never connected. `rtabmap` relies on it to know the frame should not be mapped. + +`rtabmap` reads an identity pose, or both covariances at `9999`, as a discontinuity, and starts a **new map** rather than deforming the graph across a jump the robot never made: + +``` +Odometry is reset (identity pose or high variance detected). Increment map id! +``` + +While it is lost, `publish_null_when_lost:=true` publishes a null pose and no velocity for every frame, both marked `9999`. With `:=false` it publishes nothing — except that a guess frame keeps it going: every frame that re-initialises the map, which `Odom/ResetCountdown` makes frequent, is published with the guess's pose and confidence, so the topic has no gap for as long as the guess is there. What differs between configurations is the first frame after the reset, and where it restarts from: + +| | First frame after the reset | Second frame | TF while lost | `rtabmap` | +|---|---|---|---|---| +| `publish_null_when_lost:=true` (default), with or without a guess | recovered pose, `9999` on both pose and velocity | registered, from the recovered pose | unbroken with a guess, absent without one | new map | +| `publish_null_when_lost:=false` with `guess_frame_id` | recovered pose and the guess's velocity, both with the guess's covariance | registered, from the recovered pose | unbroken | one session | +| `publish_null_when_lost:=false`, `publish_tf:=false`, another node publishing `odom` → `base_link` | not published | registered, from the recovered pose | unbroken, published by the other node | one session | +| `publish_null_when_lost:=false` with neither | not published | registered, from the pose held before the loss | absent until it recovers | one session, across the gap | + +*Registered* is the ordinary case: a pose and a velocity measured against the previous valid frame, with the covariance the registration computed. + +The two middle rows are the ones to build on. Either an external source is named through `guess_frame_id`, and the restarting frame is published as a continuation of the trajectory — the poses *and* the covariances staying continuous for as long as that guess is published — or a filter such as `robot_localization` owns `odom` → `base_link`, and the reset adopts whatever pose it holds. + +**The last row is a trap.** With nothing to say where the robot went while the odometry was lost, the reset resumes at the pose from before it, that motion is dropped from the trajectory, and since no `9999` ever reaches `rtabmap` the map is deformed across the gap rather than split at it. The node reports that combination as an error at startup. + +For finer control, put an intermediate node between this one and `rtabmap` and let it set the covariances itself. It decides what counts as a discontinuity, instead of that being inferred from the reset alone — starting a new map when the guess frame has gone quiet, say, and the newly computed pose may be wrong even though registration reported success. + +Recovering from lost is what `Odom/ResetCountdown` is for, or the `reset_odom` service. Combined with `guess_frame_id` it also keeps the TF tree intact throughout — see [It also keeps TF alive through a failure](#it-also-keeps-tf-alive-through-a-failure). + +## Services + +| Service | Type | Description | +|---|---|---| +| `reset_odom` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Drop the internal local map and restart the pose — at the identity, or, with `guess_frame_id` configured, at whatever pose the guess frame currently holds, so that odometry restarts where the other source says the robot is. | +| `reset_odom_to_pose` | [`rtabmap_msgs/srv/ResetPose`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/ResetPose.html) | Reset to a given `x y z roll pitch yaw`. | +| `pause_odom` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Stop processing incoming frames. | +| `resume_odom` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Resume. | +| `log_debug`, `log_info`, `log_warning`, `log_error` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Change RTAB-Map's own log level at runtime. | + +## Published topics + +Common to all three nodes. **Every one of them, `odom` included, is published only when something is subscribed** -- the work of building each message is skipped otherwise. The TF broadcast is not gated this way and happens whenever `publish_tf` is on -- except while registration is lost with no guess frame configured, when there is nothing to broadcast. + +| Topic | Type | Description | +|---|---|---| +| `odom` | [`nav_msgs/msg/Odometry`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Odometry.html) | The pose and velocity. Covariance is meaningful: it grows with registration uncertainty, and is `9999` on the diagonal when lost. | +| `odom_info` | [`rtabmap_msgs/msg/OdomInfo`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/OdomInfo.html) | Everything about how the frame was registered — inlier count, matches, features, timings. The first thing to look at when odometry misbehaves. | +| `odom_info_lite` | [`rtabmap_msgs/msg/OdomInfo`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/OdomInfo.html) | The same without the per-feature arrays, for logging or a slow link. | +| `odom_local_map` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The feature map the current frame was registered against. Visual paths only — built from the frame's visual words, so `icp_odometry` never fills it. | +| `odom_local_scan_map` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The scan map, the ICP path's equivalent of `odom_local_map`. | +| `odom_last_frame` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The current frame's **features**, in the odom frame — not its scan or its pixels. Visual paths only, for the same reason as `odom_local_map`; for the filtered scan see `odom_sensor_data/*`. | +| `odom_rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The frame **as odometry processed it**, not the input as it arrived. See [Outputting filtered scans and features](#outputting-filtered-scans-and-features). | +| `odom_sensor_data/raw`, `/features`, `/compressed` | [`rtabmap_msgs/msg/SensorData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/SensorData.html) | The same frame as `SensorData`. `/features` strips the images and scan and keeps only the extracted features; `/compressed` carries JPEG/PNG images instead of raw. | + +### Outputting filtered scans and features + +`odom_rgbd_image` and `odom_sensor_data/*` republish the frame **after** odometry has worked on it, which is the point of them — they are what odometry actually registered, not a copy of the input: + +- **Features are included.** Registration writes the keypoints, their 3D positions and their descriptors back into the frame, so these topics carry them. `odom_sensor_data/features` is that alone, with the images and scan removed. +- **The scan is the filtered one.** `icp_odometry` builds the frame after deskewing, voxelization, range filtering and normal estimation, so what comes out here is the decimated cloud ICP saw — not the raw sweep the lidar published. Subscribe to the driver's topic if you want the original. +- **Images are converted.** `rgbd_odometry` hands over grayscale unless `keep_color` is set, so that is what these carry too. + +## Diagnostics + +All three publish to `/diagnostics`: the input rate, the output rate, and how many frames were processed versus dropped. A healthy input rate with a low output rate means frames are arriving but not registering — check `odom_info` before touching anything else. + +## License + +BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license). diff --git a/rtabmap_odom/doc/icp_odometry.md b/rtabmap_odom/doc/icp_odometry.md new file mode 100644 index 00000000..1877614d --- /dev/null +++ b/rtabmap_odom/doc/icp_odometry.md @@ -0,0 +1,238 @@ +# icp_odometry + +Odometry from a 2D or 3D lidar, by registering each scan against the previous one with ICP. + +No features and no appearance: the motion is whatever transform best aligns this scan's points with the last. That makes it indifferent to lighting and texture — it works in the dark, and on the blank white corridor where [rgbd_odometry](rgbd_odometry.md) has nothing to track. + +What it is sensitive to instead is **geometry**. ICP can only recover motion that the scene's shape constrains, and a scene can fail to constrain it: see [Degenerate geometry](#degenerate-geometry), which is the failure mode worth understanding before deploying this. + +The shared parameters — frames, TF, guesses, the IMU, RTAB-Map's own, the services — are in the [package README](../README.md#conventions). This page covers what is specific to this node. + +## Contents + +- [Usage](#usage) +- [Subscribed Topics](#subscribed-topics) +- [Published Topics](#published-topics) + - [Reusing the filtered scan downstream](#reusing-the-filtered-scan-downstream) +- [Parameters](#parameters) + - [Where these defaults come from](#where-these-defaults-come-from) + - [Making the correspondence ratio mean something](#making-the-correspondence-ratio-mean-something) +- [Preparing the scan](#preparing-the-scan) +- [Deskewing](#deskewing) +- [Degenerate geometry](#degenerate-geometry) +- [Combining a camera and a lidar](#combining-a-camera-and-a-lidar) +- [When it loses track](#when-it-loses-track) + +## Usage + +2D lidar: + +```bash +ros2 run rtabmap_odom icp_odometry --ros-args \ + -r scan:=/scan \ + -p frame_id:=base_link +``` + +3D lidar: + +```bash +ros2 run rtabmap_odom icp_odometry --ros-args \ + -r scan_cloud:=/velodyne_points \ + -p frame_id:=base_link \ + -p "Icp/PointToPlane:='true'" \ + -p scan_normal_k:=10 \ + -p scan_voxel_size:=0.1 +``` + +```python +ComposableNode( + package='rtabmap_odom', + plugin='rtabmap_odom::ICPOdometry', + name='icp_odometry', + parameters=[{'frame_id': 'base_link', + 'scan_voxel_size': 0.1, + 'scan_normal_k': 10, + 'Icp/PointToPlane': 'true'}], + remappings=[('scan_cloud', '/velodyne_points')]) +``` + +## Subscribed Topics + +One of the two scan topics, not both. + +| Topic | Type | Description | +|---|---|---| +| `scan` | [`sensor_msgs/msg/LaserScan`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/LaserScan.html) | A 2D lidar. | +| `scan_cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | A 3D lidar, or a 2D one already converted to a cloud. | +| `imu` | [`sensor_msgs/msg/Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Optional, and more useful here than elsewhere — it pins roll and pitch, which a lidar alone constrains poorly. | + +## Published Topics + +Most are common to all three nodes; see [the README](../README.md#published-topics). Two belong to this path: + +| Topic | Type | Description | +|---|---|---| +| `odom_local_scan_map` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The accumulated scan map the current scan was registered against. | +| `odom_filtered_input_scan` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The input scan **after** deskewing, voxelization, range filtering and normal estimation — exactly what ICP registered, carrying the original header. | + +### Reusing the filtered scan downstream + +Remap `odom_filtered_input_scan` onto `rtabmap`'s `scan_cloud` and the map is built from the scan this node already prepared, rather than from the raw sweep. + +```mermaid +flowchart LR + LIDAR["lidar"] + ICP["icp_odometry"] + MAP["rtabmap
subscribe_scan_cloud:=true"] + LIDAR -->|scan_cloud| ICP + ICP -->|odom_filtered_input_scan| MAP + ICP -->|odom + TF| MAP +``` + +That skips the expensive half twice over. Voxelization and normal estimation are not repeated, since the cloud arrives already decimated and carrying `normal_*` fields, and the deskewing this node did is carried along with it. + +The alternative for deskewing is to do it **before** odometry, with [`lidar_deskewing`](../../rtabmap_util/doc/lidar_deskewing.md), and fan the corrected cloud out to both nodes: + +```mermaid +flowchart LR + LIDAR["lidar"] + DESKEW["lidar_deskewing"] + ICP["icp_odometry"] + MAP["rtabmap
subscribe_scan_cloud:=true"] + LIDAR -->|scan_cloud| DESKEW + DESKEW -->|deskewed cloud| ICP & MAP + ICP -->|odom + TF| MAP +``` + +That is the only way to give `rtabmap` **every point of the sweep**. `odom_filtered_input_scan` carries the decimated cloud ICP registered, so a map built from it inherits whatever voxelization odometry applied — and outdoors that is 30 to 50 cm. Deskewing upstream separates the two: odometry can filter as hard as it likes while the map is built from the full-resolution cloud, at the cost of an extra node and an extra copy of every sweep. + +## Parameters + +Specific to this node: the scan is filtered **before** ICP sees it, and these control that. The registration itself is tuned through RTAB-Map's `Icp/*` parameters. + +These two sets overlap, and the node resolves the overlap for you — see [Where these defaults come from](#where-these-defaults-come-from), because the defaults below are not what the source's initializers suggest. + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `scan_voxel_size` | `double` | `0.05` | Downsample to one point per voxel of this size, in meters. From `Icp/VoxelSize`. `0` disables it here. | +| `scan_downsampling_step` | `int` | `1` | Keep every Nth point. From `Icp/DownsamplingStep`. Cheaper than voxelization but density-dependent; prefer `scan_voxel_size`. | +| `scan_range_min` | `double` | `0.0` | Drop points closer than this, in meters. From `Icp/RangeMin`. `0` disables. Useful against the robot's own body. | +| `scan_range_max` | `double` | `0.0` | Drop points farther than this. From `Icp/RangeMax`. `0` disables. | +| `scan_normal_k` | `int` | `5` | Estimate each point's normal from this many neighbours. From `Icp/PointToPlaneK`. **Point-to-plane ICP needs normals**; without them it has nothing to work with. | +| `scan_normal_radius` | `double` | `0.0` | Estimate normals from neighbours within this radius instead. From `Icp/PointToPlaneRadius`. `0` disables. | +| `scan_normal_ground_up` | `double` | `0.0` | Force normals to point upward when within this dot-product threshold of vertical. From `Icp/PointToPlaneGroundNormalsUp`. Helps on ground planes. | +| `scan_cloud_max_points` | `int` | `-1` | How many points a full sweep of the lidar holds. It is the **denominator of the correspondence ratio** — see [Making the correspondence ratio mean something](#making-the-correspondence-ratio-mean-something). `-1` leaves it unset; an organized cloud fills it in automatically. | +| `scan_cloud_is_2d` | `bool` | `false` | Treat `scan_cloud` as a planar scan even though it carries a `z` field, so it is registered as a 2D scan rather than a 3D one. For a 2D lidar already converted to a cloud. | +| `deskewing` | `bool` | `false` | Correct for motion during the sweep. See [Deskewing](#deskewing). | +| `deskewing_slerp` | `bool` | `false` | Interpolate the deskewing transform rather than looking up TF per point. Faster, slightly less accurate. | +| `topic_queue_size` | `int` | `1` | Queue depth of the scan subscription. Deliberately small: a stale scan is worse than a dropped one. | + +### Where these defaults come from + +Filtering a scan and filtering it again inside ICP would be wasted work, so at startup the node moves each filter from RTAB-Map's parameter to its own: + +> `IcpOdometry: Transferring value 5 of "Icp/PointToPlaneK" to ros parameter "scan_normal_k" for convenience.` + +That log line is normal, not a warning about your configuration. Each `scan_*` parameter above **takes its default from the matching `Icp/*` parameter**, and the `Icp/*` one is then set to `0` so the filter runs once, here, rather than twice. This is why `scan_voxel_size` is `0.05` and `scan_normal_k` is `5` out of the box rather than disabled. + +Setting the ROS parameter explicitly wins: the transfer is skipped and the `Icp/*` value is zeroed instead. Setting **both** is the case to avoid — the node warns that both are set, and the scan is then filtered twice: + +``` +IcpOdometry: Both parameter "Icp/VoxelSize" and ros parameter "scan_voxel_size" are set. +``` + +So tune through `scan_*` **or** through `Icp/*`, not both. + +### Making the correspondence ratio mean something + +`Icp/CorrespondenceRatio` decides whether a registration is trustworthy: the points ICP managed to pair, over the points it could have paired. `scan_cloud_max_points` is what sets that second number. Set it to the theoretical maximum points per sweep. No need to set it explicitly for organized clouds though, `width × height` will be used as maximum points. + +Left at `-1`, the denominator becomes the size of the larger of the two scans being matched (for dense clouds). Take two scans that came back with 30 and 50 points — a lidar staring at open space, where most rays returned nothing. Dividing by 50 says "we matched most of what we saw", and the ratio looks healthy. But the sensor emits 10000 rays a sweep, so 50 returns means almost nothing was in range, and the registration is resting on nearly no evidence. Told that a full sweep is 10000 points, ICP divides by that instead and the ratio collapses to what the overlap actually was, so the threshold rejects the frame. + +## Preparing the scan + +ICP cost grows with the number of points, and a 3D lidar produces far more than registration needs — a 64-beam sensor is a hundred thousand points per sweep, and aligning them all is both slow and *no more accurate* than aligning a well-spread subset. Voxelization is therefore on by default at 5 cm. + +What it buys beyond speed is even density, which matters more than the point count: a raw lidar sweep is dense near the sensor and sparse far away, so an unvoxelized ICP is dominated by whatever is closest — often the robot itself or the ground right under it. Size `scan_voxel_size` to the environment: **0.05 to 0.2 m indoors**, and **0.3 to 0.5 m outdoors**, where the scene is far larger and the extra resolution buys nothing but CPU. + +**Move `Icp/MaxCorrespondenceDistance` with it — a good rule of thumb is ten times the voxel size.** The two are coupled: voxelizing at 0.3 m leaves neighbouring points that far apart, so a correspondence distance of 0.1 m cannot pair anything and ICP returns nothing at all. + +**Point-to-plane ICP converges better than point-to-point** on the flat surfaces that dominate most environments, and it is what the default `scan_normal_k` of 5 is there to support: + +```bash +-p "Icp/PointToPlane:='true'" -p scan_normal_k:=10 +``` + +The quoting is not optional: RTAB-Map parameters are strings, and an unquoted `true` makes the node throw on startup ([why](../README.md#rtab-maps-own-parameters)). + +Whether it is on by default depends on how RTAB-Map was built — `Icp/PointToPlane` defaults to `true` only with libpointmatcher available, and `false` otherwise — so set it explicitly if you care. + +`scan_range_min` is worth setting on any robot whose lidar can see parts of itself. Those points are perfectly self-consistent between scans, so they pull the alignment toward "no motion" — a bias that looks like the robot under-travelling rather than like an error. + +## Deskewing + +A spinning lidar measures its points over a whole revolution, not at an instant. If the robot moves during that revolution, the scan is a smear — the points are in a frame that no longer exists by the time the sweep ends. At walking pace with a 10 Hz lidar this is centimeters; on a fast vehicle it dominates the error budget. + +`deskewing:=true` corrects it. It needs the cloud to carry **per-point timestamps**, in a field named `t`, `time`, `stamps` or `timestamp`; without one the node logs an error and drops the frame rather than guessing. + +There are two ways it gets the motion to correct with, and which one is used depends on whether you gave it an external guess: + +- **With `guess_frame_id` set** — the motion comes from that TF. This is the accurate route, and the reason to pair deskewing with wheel odometry or an IMU-integrated frame. +- **Without it** — a constant-velocity model from the previous frame's estimate. It cannot deskew the very first frame, and it degrades exactly when velocity changes fastest, which is when deskewing matters most. + +`deskewing_slerp` interpolates between the sweep's endpoints rather than looking up a transform per point. Much cheaper, and accurate enough unless the motion within one sweep is strongly non-linear. + +## Degenerate geometry + +This is the failure that matters, and it is not a bug. ICP recovers only the motion the scene constrains, and some scenes do not constrain all of it: + +- **A long featureless corridor** does not constrain motion *along* the corridor. The walls look identical a meter forward, so ICP happily reports no motion while the robot drives. The map then folds the corridor up into a fraction of its length. +- **A large open space** with everything out of range constrains nothing at all. +- **A flat plane** — a warehouse floor to a horizontal 2D lidar — constrains height and tilt but not translation. + +RTAB-Map detects this rather than walking into it, but only on the point-to-plane path. The defences, in order of effectiveness: + +1. **`guess_frame_id` with wheel odometry.** The guess supplies the motion ICP cannot see, and ICP corrects the part it can. This turns the corridor case from a failure into a non-issue, and it is why lidar odometry on a wheeled robot should essentially always have it. +2. **The structural complexity check**, which is the built-in one. With `Icp/PointToPlane` on, a scan whose normals fail to span the space — the definition of a corridor — scores below `Icp/PointToPlaneMinComplexity` (`0.02`) and is handled by `Icp/PointToPlaneLowComplexityStrategy` instead of being trusted: + + | Value | Behaviour | + |---|---| + | `0` | Reject the transform outright: the frame is reported lost. | + | `1` *(default)* | Recompute with point-to-point and constrain the correction to the axes that *are* observable — in a corridor, y and yaw are kept and **x is taken from the guess**. | + | `2` | Recompute with point-to-point and accept the result as is. | + | `3` | Keep the point-to-plane transform, with the same axis-constrained projection as `1`. | + + The default pairs with defence 1: it detects the unobservable axis and hands that axis to the guess. Without a `guess_frame_id` there is nothing to hand it to, which is why the two belong together. +3. **A 3D lidar instead of a 2D one**, which sees ceiling, floor and doorways that a horizontal slice misses. +4. **`Icp/CorrespondenceRatio`** to reject registrations supported by too few correspondences, so a bad frame is reported lost rather than silently accepted. + +With `Icp/PointToPlane` off, none of the complexity machinery runs: there are no normals to measure, so a degenerate scan is registered and trusted like any other. + +## Combining a camera and a lidar + +With both sensors, the usual arrangement is `icp_odometry` for the pose and the camera for appearance: + +```mermaid +flowchart LR + LIDAR["lidar"] + CAM["RGB-D camera"] + ICP["icp_odometry"] + SYNC["rgbd_sync"] + ODOM(["odom + TF"]) + MAP["rtabmap
subscribe_rgbd + subscribe_scan_cloud"] + LIDAR --> ICP --> ODOM --> MAP + LIDAR --> MAP + CAM --> SYNC --> MAP +``` + +Lidar geometry is the more reliable pose source, while loop closure detection is appearance-based and wants the images. `rtabmap` then subscribes to the camera, the scan and this node's odometry together. + +## When it loses track + +`odom_info` carries the ICP result. The numbers to look at are the correspondence count and ratio: too few correspondences means the scans do not overlap enough, whether because the robot moved too far between them, the range filters are too aggressive, or the scene genuinely changed. + +- **Scans too far apart** — the lidar rate is too low for the speed, or `max_update_rate` is throttling too hard. +- **`Icp/MaxCorrespondenceDistance` too small** — ICP never associates the points at all. It has to be larger than the motion between scans; too large and it associates the wrong things. +- **Everything filtered away** — check `scan_range_min`/`scan_range_max` and `scan_voxel_size` against the actual scale of the environment. + +As everywhere else in this package, `Odom/ResetCountdown` recovers automatically from a lost state instead of staying lost. diff --git a/rtabmap_odom/doc/rgbd_odometry.md b/rtabmap_odom/doc/rgbd_odometry.md new file mode 100644 index 00000000..5ba9220f --- /dev/null +++ b/rtabmap_odom/doc/rgbd_odometry.md @@ -0,0 +1,211 @@ +# rgbd_odometry + +Visual odometry from a color image, a registered depth image and a calibration. + +Each frame's visual features are matched against the previous frame — or against a small local map of recent features — and the camera motion that best explains the matches becomes the pose. Depth turns the 2D feature matches into 3D correspondences, which is what makes the scale real rather than arbitrary. + +Use it when an RGB-D camera is the main sensor. For a stereo pair use [stereo_odometry](stereo_odometry.md); for a lidar, [icp_odometry](icp_odometry.md). All three publish the same topics and share the parameters in the [package README](../README.md#conventions), which covers frames, TF, the RTAB-Map parameters, guesses, the IMU and the services. This page covers what is specific to this node. + +## Contents + +- [Pipeline arrangements](#pipeline-arrangements) +- [Usage](#usage) +- [Subscribed Topics](#subscribed-topics) +- [Published Topics](#published-topics) +- [Parameters](#parameters) +- [Synchronization](#synchronization) +- [Several cameras](#several-cameras) +- [Repetitive patterns](#repetitive-patterns) +- [When it loses track](#when-it-loses-track) + +## Pipeline arrangements + +A camera-only pipeline, with the camera synchronized once and fanned out -- the arrangement [Synchronization](#synchronization) recommends: + +```mermaid +flowchart LR + CAM["RGB-D camera"] + SYNC["rgbd_sync"] + ODOM["rgbd_odometry
subscribe_rgbd:=true"] + MAP["rtabmap
subscribe_rgbd:=true"] + CAM -->|rgb/image
depth/image
rgb/camera_info| SYNC + SYNC -->|rgbd_image| ODOM & MAP + ODOM -->|odom + TF| MAP +``` + +Without `rgbd_sync`, both nodes subscribe to the three raw topics and each synchronizes them independently -- which works, but lets the two settle on different pairings. + +Another arrangement drops `rgbd_sync` altogether and feeds `rtabmap` from **this node's own output** instead: + +```mermaid +flowchart LR + CAM["RGB-D camera"] + ODOM["rgbd_odometry"] + MAP["rtabmap
subscribe_rgbd or subscribe_sensor_data"] + CAM -->|rgb/image
depth/image
rgb/camera_info| ODOM + ODOM -->|odom_rgbd_image
or odom_sensor_data| MAP + ODOM -->|odom + TF| MAP +``` + +Remap `rtabmap`'s `rgbd_image` to `odom_rgbd_image`, or set `subscribe_sensor_data` and remap to `odom_sensor_data/raw`. Two things come for free: + +- **No separate synchronization.** This node already matched the three topics to register the frame, and republishes the result, so there is no `rgbd_sync` to run and no second synchronizer to agree with. +- **The features are reused.** `odom_sensor_data` carries the keypoints, their 3D positions and their descriptors that odometry extracted; they survive the conversion back into RTAB-Map on the other side, so `rtabmap` does not redo feature detection and descriptor extraction. + +## Usage + +Against a camera's raw topics: + +```bash +ros2 run rtabmap_odom rgbd_odometry --ros-args \ + -r rgb/image:=/camera/color/image_raw \ + -r depth/image:=/camera/depth/image_rect_raw \ + -r rgb/camera_info:=/camera/color/camera_info \ + -p frame_id:=base_link +``` + +Against an [`rgbd_sync`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_sync) output, which is the better arrangement when anything else consumes the same camera: + +```bash +ros2 run rtabmap_odom rgbd_odometry --ros-args \ + -p subscribe_rgbd:=true \ + -r rgbd_image:=/camera/rgbd_image \ + -p frame_id:=base_link +``` + +```python +ComposableNode( + package='rtabmap_odom', + plugin='rtabmap_odom::RGBDOdometry', + name='rgbd_odometry', + parameters=[{'frame_id': 'base_link', 'subscribe_rgbd': True}], + remappings=[('rgbd_image', '/camera/rgbd_image')]) +``` + +## Subscribed Topics + +Which topics are used depends on `subscribe_rgbd` and `rgbd_cameras`. + +**Default** — `subscribe_rgbd:=false`: + +| Topic | Type | Description | +|---|---|---| +| `rgb/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Color or mono image. Goes through `image_transport`. | +| `depth/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Depth registered to the color camera. `16UC1` in millimeters or `32FC1` in meters. | +| `rgb/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Calibration of the color camera. | + +**With `subscribe_rgbd:=true`**, one pre-synchronized message instead of three topics: + +| `rgbd_cameras` | Topic | Type | +|---|---|---| +| `1` (default) | `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | +| `2`–`6` | `rgbd_image0` … `rgbd_image5` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | +| `0` | `rgbd_images` | [`rtabmap_msgs/msg/RGBDImages`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImages.html) | + +`rgbd_cameras:=0` takes any number of cameras in a single message, which is what [`rgbdx_sync`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_sync) produces — the route that needs no rebuild and the only one that goes past six. + +| Topic | Type | Description | +|---|---|---| +| `imu` | [`sensor_msgs/msg/Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Optional. Constrains roll and pitch; see [the README](../README.md#imu). | + +## Published Topics + +`odom`, `odom_info`, `odom_local_map`, `odom_last_frame`, `odom_rgbd_image` and the rest are common to all three nodes and documented in [the README](../README.md#published-topics). + +## Parameters + +Specific to this node. The shared ones — `frame_id`, `publish_tf`, `guess_frame_id`, `max_update_rate`, all of RTAB-Map's own — are in [the README](../README.md#conventions). + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `subscribe_rgbd` | `bool` | `false` | Take a pre-synchronized `RGBDImage` instead of three raw topics. | +| `rgbd_cameras` | `int` | `1` | Number of `RGBDImage` topics. `0` means one `RGBDImages` topic carrying any number. Only with `subscribe_rgbd:=true`. | +| `approx_sync` | `bool` | `true` | Match the raw topics by nearest stamp. See [Synchronization](#synchronization). | +| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. | +| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. | +| `sync_queue_size` | `int` | `5` | Queue depth of the synchronizer. | +| `queue_size` | `int` | — | **Deprecated**, renamed to `sync_queue_size`. Copied to it with a warning. | +| `qos_camera_info` | `int` | value of `qos` | Reliability of the `rgb/camera_info` subscription alone. | +| `keep_color` | `bool` | `false` | Keep the color image in the data handed to the odometry instead of converting to grayscale. Registration is grayscale either way; this only matters for what downstream consumers of `odom_rgbd_image` receive. | +| `image_transport` | `string` | `"raw"` | Transport for `rgb/image`, e.g. `compressed`. | +| `depth_transport` | `string` | `"raw"` | Transport for `depth/image`, e.g. `compressedDepth`. | +| `rgb_transport` | `string` | — | **Deprecated**, renamed to `image_transport`. | + +## Synchronization + +With `subscribe_rgbd:=false` this node runs its own synchronizer over the three raw topics, and the same trade-off applies as everywhere else in this stack: **exact matching is cheaper and cannot mismatch, but publishes nothing at all if the stamps differ by a nanosecond**. The default here is approximate because many RGB-D cameras do not stamp color and depth identically. + +When several nodes consume the same camera — odometry and `rtabmap`, usually — **synchronize once with `rgbd_sync` and set `subscribe_rgbd:=true` on both**. Two independent approximate synchronizers over the same three topics can settle on different pairings, and then `rtabmap` maps a frame at a pose computed from a different one. Feeding both from one `RGBDImage` removes the possibility. + +`approx_sync_max_interval` is worth setting whenever approximate matching stays on: without it, a camera that stalls and resumes silently pairs a fresh color frame with a stale depth frame. A tenth of the frame period is a reasonable start. + +## Several cameras + +More cameras means more of the scene is textured enough to track, which is the usual reason visual odometry fails indoors. Point them in different directions rather than overlapping. + +Two routes, both requiring the frames to be synchronized and each camera to be in TF: + +- **`rgbd_cameras:=2..6`** subscribes to `rgbd_image0`…`rgbd_imageN` and synchronizes them here. +- **`rgbd_cameras:=0`** takes one `rgbd_images` topic from `rgbdx_sync`, which has no upper limit and needs no rebuild. + +**RTAB-Map has to be built with OpenGV for this.** The default motion estimation is PnP (`Vis/EstimationType=1`, 3D→2D), and the multi-camera version of it lives in OpenGV. Without that dependency the registration refuses to run and says so: + +``` +Multi-camera 2D-3D PnP registration is only available if rtabmap is built with +OpenGV dependency. Use 3D-3D registration approach instead for multi-camera. +``` + +Check with `rtabmap --version`, which prints a `With OpenGV:` line. If it says `false`, either rebuild RTAB-Map against OpenGV or switch to `Vis/EstimationType:='0'` (3D→3D), which needs no extra dependency but registers point cloud to point cloud rather than reprojecting, and is the weaker estimator when depth is noisy. + +**Hardware-synchronize the cameras if you can.** The node treats the set as one rigid observation at one timestamp: features from every camera are registered together, with the extrinsics from TF held fixed. There is no equivalent of lidar deskewing here — it cannot estimate the motion that happened *within* the rig between one camera's exposure and the next. If the cameras fire at different instants while the robot moves, that motion is absorbed as though the rig had flexed, and the registration is pulled off by however far the robot travelled in between. + +Synchronizing the topics is not the same thing: `approx_sync` only decides which frames are grouped, it cannot undo an exposure that happened 20 ms later than its neighbour's. + +Calibration matters more with several cameras than with one for the same reason: the extrinsics between them come from TF, and an error there shows up as a constant bias in the estimated motion rather than as an obvious failure. + +## Features computed elsewhere + +`RGBDImage` has fields for local features — `key_points`, `points` and `descriptors` — and when a frame arrives with them filled, this node hands them to the odometry as they are instead of detecting and describing anything. RTAB-Map extracts features only from a frame that brought none, so nothing is recomputed. + +This is for a camera, or a driver, that already does the extraction: on a multi-camera rig it is most of the per-frame work, and it can be done once and shared with `rtabmap` rather than repeated in each node. + +What a publisher has to get right: + +- **One entry per camera**, in the same order as the images, for `rgbd_cameras:=0` as well as the numbered topics. +- **Keypoints in their own camera's image coordinates.** The node stitches the images side by side and shifts each camera's keypoints by the images that precede it. +- **3D points in that camera's optical frame.** They are brought into `frame_id` with the camera's transform from TF, the same one used for the calibration. +- **Descriptors compressed** with `rtabmap::compressData()`, one row per keypoint, the same type for every camera. +- **Equal counts.** `points` and `descriptors` may be left empty, but if they are there, they must have as many entries as there are keypoints. A frame whose three disagree has its features dropped with an error rather than used out of step. + +Both images may be left out entirely: the depth image's job was to give the keypoints their depth and they arrive with it, and the color image's was to have features found in it. A frame is then its calibration and its features, which is the whole point — the images are nearly all of the bandwidth. The `camera_info` of each camera has to be there either way, as it is what says how big the image would have been and, through its `frame_id`, where the camera is. + +What stops applying, since nothing is extracted: `Vis/MaxFeatures`, `Vis/DepthAsMask`, the detector chosen with `Kp/DetectorStrategy`, and the depth bounds `Vis/MinDepth` and `Vis/MaxDepth`. Whatever is published is what gets registered, so the publisher owns those decisions. `Vis/CorType` must stay at `0` (feature matching); optical flow (`1`) reads the images themselves and has nothing to work with here. + +## Repetitive patterns + +Not every failure announces itself. A scene full of identical detail — a tiled floor, rows of identical shelving, a patterned carpet, a brick wall — hands the matcher plenty of features and plenty of confident matches, just not always the *right* ones. One tile matched to its neighbour looks like a perfectly good inlier, and the pose comes out shifted by exactly one tile. Inlier counts stay healthy, nothing is reported lost, and the trajectory drifts in steps. + +The defence is to constrain **where** a match is allowed to come from: + +- **`guess_frame_id`** gives each feature a predicted image position, from wheel odometry or another external source. +- **`Vis/CorGuessWinSize`** bounds the search around that prediction — 40 pixels by default. Reducing it, to 10 or 20, means a feature can only match something close to where the guess says it should be, so the identical neighbour one tile away is never a candidate. + +## When it loses track + +Visual odometry fails when there is nothing to match: a blank wall, a dark room, motion blur, or a scene where everything moved. The node then publishes a null pose (see [the README](../README.md#lost-frames-resets-and-new-maps)) and `odom_info` says why. + +Look at `odom_info` first — `inliers` is the number that matters: + +```bash +ros2 topic echo /odom_info --field inliers +``` + +Inliers falling below `Vis/MinInliers` (default 20) is the definition of a lost frame. Whether the fix is more features, a better guess or a different strategy depends on which part is short: + +- **Few features detected at all** — the scene is untextured or too dark. Lowering `Vis/MinInliers` lets frames register on fewer matches, but a pose resting on a handful of inliers is poorly constrained and drifts badly — it buys continuity at the price of accuracy. The real fixes are physical. If it is dark, add a light — a spotlight on the robot restores texture the camera can track, and costs far less than changing sensor. Otherwise it is **more field of view**: a wider lens, or [several cameras](#several-cameras) pointed in different directions. It only takes one textured patch somewhere in view to track, so a blank wall filling a narrow FOV stops being a problem the moment the rig can also see the ceiling or a doorway. Failing that, the camera is the wrong sensor here — a lidar if the scene has geometry, wheel odometry if it has neither. See [Choosing a sensor modality for the environment](../README.md#choosing-a-sensor-modality-for-the-environment). +- **Features detected but few matched** — motion is too fast for the search window, or the frame rate is too low. A `guess_frame_id` from wheel odometry is what helps most here. +- **Matched but rejected as outliers** — usually a moving scene, or depth that does not agree with the color image. Check that depth really is registered to color. + +`Odom/ResetCountdown` gets the node out of a lost state automatically instead of leaving it lost until something calls `reset_odom`. + +**A camera alone is not a great odometry source on a wheeled robot.** If the base publishes wheel odometry, feeding it in through `guess_frame_id` is worth more than any amount of tuning here. diff --git a/rtabmap_odom/doc/stereo_odometry.md b/rtabmap_odom/doc/stereo_odometry.md new file mode 100644 index 00000000..d39a7062 --- /dev/null +++ b/rtabmap_odom/doc/stereo_odometry.md @@ -0,0 +1,219 @@ +# stereo_odometry + +Visual odometry from a stereo pair. + +Features are found in the left image and matched into the right one to get their depth by disparity, then matched against the previous frame to get the motion. It is the same registration as [rgbd_odometry](rgbd_odometry.md); only the source of depth differs — computed here from the pair rather than measured by the sensor. + +That difference is the reason to choose it. A stereo pair works outdoors and at range, where the projected-pattern depth of an RGB-D camera returns nothing, and its accuracy degrades gracefully with distance instead of cutting off. The cost is that depth is only available where there is texture to match, and that it depends on a good stereo calibration. + +The shared parameters — frames, TF, guesses, the IMU, RTAB-Map's own parameters, the services — are in the [package README](../README.md#conventions). This page covers what is specific to this node. + +## Contents + +- [Pipeline arrangements](#pipeline-arrangements) +- [Usage](#usage) +- [Subscribed Topics](#subscribed-topics) +- [Published Topics](#published-topics) +- [Parameters](#parameters) +- [Synchronization](#synchronization) +- [Getting the scale right](#getting-the-scale-right) +- [Features computed elsewhere](#features-computed-elsewhere) +- [Repetitive patterns](#repetitive-patterns) +- [When it loses track](#when-it-loses-track) + +## Pipeline arrangements + +A typical stereo pipeline, rectification included: + +```mermaid +flowchart LR + CAM["stereo driver"] + PROC["stereo_image_proc"] + SYNC["stereo_sync"] + ODOM["stereo_odometry"] + MAP["rtabmap"] + CAM -->|left/image_raw
right/image_raw
camera_info x2| PROC + PROC -->|left/image_rect
right/image_rect
camera_info x2| SYNC + SYNC -->|rgbd_image| ODOM & MAP + ODOM -->|odom + TF| MAP +``` + +`stereo_image_proc` can be dropped when the driver already publishes rectified images: + +```mermaid +flowchart LR + CAM["stereo driver
publishing rectified images"] + SYNC["stereo_sync"] + ODOM["stereo_odometry"] + MAP["rtabmap"] + CAM -->|left/image_rect
right/image_rect
camera_info x2| SYNC + SYNC -->|rgbd_image| ODOM & MAP + ODOM -->|odom + TF| MAP +``` + +The same shortcut as on the RGB-D side is available here: drop `stereo_sync` and feed `rtabmap` from **this node's own output**. + +```mermaid +flowchart LR + CAM["stereo driver
publishing rectified images"] + ODOM["stereo_odometry"] + MAP["rtabmap
subscribe_rgbd or subscribe_sensor_data"] + CAM -->|left/image_rect
right/image_rect
camera_info x2| ODOM + ODOM -->|odom_rgbd_image
or odom_sensor_data| MAP + ODOM -->|odom + TF| MAP +``` + +Remap `rtabmap`'s `rgbd_image` to `odom_rgbd_image`, or set `subscribe_sensor_data` and remap to `odom_sensor_data/raw`. The stereo pair survives the trip intact -- the left image, the right image and both calibrations travel in the one message, exactly as `stereo_sync` would have packed them -- and the features this node extracted come with it, so `rtabmap` does not redo feature detection and descriptor extraction. + +Everything above hands the node rectified images. It can also take the raw pair, straight from the driver: + +```mermaid +flowchart LR + CAM["stereo driver"] + ODOM["stereo_odometry
Rtabmap/ImagesAlreadyRectified:=false"] + MAP["rtabmap
subscribe_rgbd or subscribe_sensor_data"] + CAM -->|left/image_raw
right/image_raw
camera_info x2| ODOM + ODOM -->|odom_rgbd_image
or odom_sensor_data| MAP + ODOM -->|odom + TF| MAP +``` + +Two different things can make that work: + +- **The odometry rectifies the pair itself.** With `Rtabmap/ImagesAlreadyRectified:=false` it builds a rectification map from the calibration and applies it to every frame, saying so once: + + ``` + Rtabmap/ImagesAlreadyRectified parameter is set to false but the selected odometry + approach cannot process raw stereo images. We will rectify them for convenience. + ``` + + It needs the geometry between the two cameras to do that — the right `camera_info` carrying `P(0,3)`, or TF between the two camera frames. If a rectification map cannot be built from what the calibration says, the frame is refused rather than registered wrong. + +- **The odometry takes them raw.** A few approaches do their own undistortion and want the unrectified images: `Odom/Strategy` `6` (OKVIS), `8` (MSCKF), `9` (VINS-Fusion) and `10` (OpenVINS), each available only if RTAB-Map was built against that library. Nothing rectifies anything then, and `Rtabmap/ImagesAlreadyRectified:=false` simply tells the pipeline to leave the images alone. + +Which of the two applies decides what `rtabmap` needs when it is fed from this node's output. If the odometry rectified the pair, the rectified images are what travels on -- they replace the raw ones in the frame -- and `rtabmap` keeps `Rtabmap/ImagesAlreadyRectified` at its default `true`. If the odometry took them raw, they arrive raw, and `rtabmap` needs `Rtabmap/ImagesAlreadyRectified:=false` of its own to rectify them again on its side. Set `Mem/UseOdomFeatures:=false` along with it, since it defaults to `true`: `rtabmap` rectifies a stereo pair but does not currently map features that travelled with the frame into the rectified image, so any it reused would be read against the wrong one. + +Rectifying here costs what `stereo_image_proc` would have cost, but only on the frames the odometry actually registers. When it runs slower than the camera — throttled by `max_update_rate`, or dropping frames that arrive while a registration is still running — the rectification happens at the odometry's rate instead of the camera's, and every frame `stereo_image_proc` would have rectified for nothing is saved. Where something else needs the whole stream rectified, the first arrangement is still the one to use. + +## Usage + +```bash +ros2 run rtabmap_odom stereo_odometry --ros-args \ + -r left/image_rect:=/stereo/left/image_rect \ + -r right/image_rect:=/stereo/right/image_rect \ + -r left/camera_info:=/stereo/left/camera_info \ + -r right/camera_info:=/stereo/right/camera_info \ + -p frame_id:=base_link +``` + +```python +ComposableNode( + package='rtabmap_odom', + plugin='rtabmap_odom::StereoOdometry', + name='stereo_odometry', + parameters=[{'frame_id': 'base_link'}], + remappings=[('left/image_rect', '/stereo/left/image_rect'), + ('right/image_rect', '/stereo/right/image_rect'), + ('left/camera_info', '/stereo/left/camera_info'), + ('right/camera_info', '/stereo/right/camera_info')]) +``` + +**The images are normally rectified**, which is what the `image_rect` topic names assume, and what [`stereo_image_proc`](https://docs.ros.org/en/jazzy/p/stereo_image_proc/) produces when the driver does not. + +They do not have to be. RTAB-Map can rectify them itself from the calibration -- set `Rtabmap/ImagesAlreadyRectified:=false` and feed it the raw pair with distortion coefficients in the `camera_info`. What does not work is the silent middle case: **unrectified images with that parameter left at its default of `true`**. Nothing fails loudly; disparity is computed across rows that no longer correspond, giving depths that are wrong in a smoothly varying way and a trajectory that is wrong without looking broken. + +## Subscribed Topics + +**Default** — `subscribe_rgbd:=false`: + +| Topic | Type | Description | +|---|---|---| +| `left/image_rect` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Rectified left image, color or mono. | +| `right/image_rect` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Rectified right image, color or mono. | +| `left/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Left calibration. | +| `right/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Right calibration. Its `P` matrix carries the baseline, which sets the scale of the whole trajectory. | + +**With `subscribe_rgbd:=true`**, one pre-synchronized message from [`stereo_sync`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_sync): + +| `rgbd_cameras` | Topic | Type | +|---|---|---| +| `1` (default) | `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | +| `2`–`6` | `rgbd_image0` … `rgbd_image5` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | +| `0` | `rgbd_images` | [`rtabmap_msgs/msg/RGBDImages`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImages.html) | + +| Topic | Type | Description | +|---|---|---| +| `imu` | [`sensor_msgs/msg/Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Optional. See [the README](../README.md#imu). | + +## Published Topics + +Common to all three nodes; see [the README](../README.md#published-topics). + +## Parameters + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `subscribe_rgbd` | `bool` | `false` | Take a pre-synchronized `RGBDImage` from `stereo_sync` instead of four raw topics. | +| `rgbd_cameras` | `int` | `1` | Number of `RGBDImage` topics. `0` means one `RGBDImages` topic. Only with `subscribe_rgbd:=true`. More than one needs RTAB-Map built with OpenGV, and the cameras hardware-synchronized — see [Several cameras](rgbd_odometry.md#several-cameras). | +| `approx_sync` | `bool` | `false` | Match the raw topics by nearest stamp. **Defaults to exact**, unlike `rgbd_odometry` — see [Synchronization](#synchronization). | +| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. Only used when `approx_sync` is on. | +| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. | +| `sync_queue_size` | `int` | `5` | Queue depth of the synchronizer. | +| `queue_size` | `int` | — | **Deprecated**, renamed to `sync_queue_size`. | +| `qos_camera_info` | `int` | value of `qos` | Reliability of the two `camera_info` subscriptions. | +| `keep_color` | `bool` | `false` | Keep the left image in color in the data passed on, rather than converting to grayscale. Registration is grayscale either way. | +| `image_transport` | `string` | `"raw"` | Transport for both image topics. | + +## Synchronization + +**This node defaults to exact matching**, because a stereo pair is normally hardware-triggered and the two images therefore carry identical stamps. That is the right default: exact matching is cheaper and cannot pair the left image with the wrong right one — and a mismatched stereo pair does not produce an error, it produces wrong disparities and a wrong trajectory. + +The failure mode to recognize is the other one: if the stamps are *not* identical, **nothing is ever published and nothing says why**. Check before assuming the node is broken: + +```bash +ros2 topic echo --once /stereo/left/image_rect --field header.stamp +ros2 topic echo --once /stereo/right/image_rect --field header.stamp +``` + +If they differ, set `approx_sync:=true` and set `approx_sync_max_interval` to something tight — a stereo pair whose images are more than a fraction of a frame apart is not usable for disparity regardless. + +## Getting the scale right + +Everything about a stereo trajectory's scale comes from the **baseline**, which this node reads from the right camera's `P` matrix (`P[3] = -fx * baseline`). Two consequences: + +- A `right/camera_info` whose `P` matrix is all zeros — which some drivers publish before calibration is loaded — gives a zero baseline and no usable depth at all. +- A calibration whose baseline is off by a few percent produces a trajectory off by the same few percent, consistently, with nothing else looking wrong. + +If the map comes out uniformly too large or too small, check the baseline before anything else. + +## Features computed elsewhere + +`RGBDImage` has fields for local features — `key_points`, `points` and `descriptors` — and when a frame arrives with them filled, this node hands them to the odometry as they are instead of detecting and describing anything. RTAB-Map extracts features only from a frame that brought none, so nothing is recomputed, and neither is the disparity search that would otherwise give each feature its depth. + +This is for a camera, or a driver, that already does the extraction: on a multi-camera rig it is most of the per-frame work, and it can be done once and shared with `rtabmap` rather than repeated in each node. + +What a publisher has to get right: + +- **One entry per camera**, in the same order as the images, for `rgbd_cameras:=0` as well as the numbered topics. +- **Keypoints in their own camera's left image.** The node stitches the left images side by side and shifts each camera's keypoints by the images that precede it. A stereo pair's features belong to the left image; nothing is expected in the right one. +- **3D points in that camera's left optical frame.** They are brought into `frame_id` with the camera's transform from TF, the same one used for the calibration. +- **Descriptors compressed** with `rtabmap::compressData()`, one row per keypoint, the same type for every camera. +- **Equal counts.** `points` and `descriptors` may be left empty, but if they are there, they must have as many entries as there are keypoints. A frame whose three disagree has its features dropped with an error rather than used out of step. + +Both images may be left out entirely — the left image's job was to have features found in it, the right one's to give them their disparity, and they arrive with their 3D positions already. A frame is then its two calibrations and its features, which is the whole point: the images are nearly all of the bandwidth. Both `camera_info` still have to be there, the left one saying how big the image would have been and where the camera is, the right one carrying the baseline in `P(0,3)` — without it there is no scale, features or not. + +What stops applying, since nothing is extracted: `Vis/MaxFeatures`, the detector chosen with `Kp/DetectorStrategy`, and the depth bounds `Vis/MinDepth` and `Vis/MaxDepth`. Whatever is published is what gets registered, so the publisher owns those decisions. `Vis/CorType` must stay at `0` (feature matching); optical flow (`1`) reads the images themselves and has nothing to work with here. + +## Repetitive patterns + +Identical detail repeated across the scene — a tiled floor, rows of shelving, a brick wall — lets the matcher pair a feature with the wrong copy of itself, which drifts the trajectory by exactly one repeat while the inlier count stays healthy and nothing is reported lost. It bites stereo twice over, since the same ambiguity also misplaces the left/right match that sets the depth. + +The fix is the same as for [rgbd_odometry](rgbd_odometry.md#repetitive-patterns): an external guess through `guess_frame_id`, with `Vis/CorGuessWinSize` reduced so a match has to come from close to where the guess predicts. `Stereo/WinWidth` and `Stereo/WinHeight` matter here too — a correlation window smaller than the repeating pattern has nothing unique to lock onto. + +## When it loses track + +The same diagnosis as [rgbd_odometry](rgbd_odometry.md#when-it-loses-track) — `odom_info`'s `inliers` is the number to watch — with two failure modes specific to stereo: + +- **Poor rectification.** Matched features should lie on the same image row. If they do not, either the pair is unrectified while `Rtabmap/ImagesAlreadyRectified` is `true`, or the calibration itself is off. +- **Untextured scene.** With no texture there is nothing to match *between* left and right either, so there is no depth at all — worse than the RGB-D case, where the sensor still measures wrong depth on a blank wall. + +`Stereo/*` parameters tune the disparity matching itself: `Stereo/MaxDisparity` bounds how close a point can be, `Stereo/WinWidth` and `Stereo/WinHeight` the correlation window. They are listed by `ros2 param list` like every other RTAB-Map parameter. diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index ca0df5bc..b0e8328c 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -58,57 +58,144 @@ namespace rtabmap { class Odometry; } +/** + * @file + * @brief The node the three odometry nodes of this package are built on. + */ + namespace rtabmap_odom { +/** + * @brief Runs RTAB-Map's odometry as a ROS node: everything but the subscriptions. + * + * `rgbd_odometry`, `stereo_odometry` and `icp_odometry` differ only in what they listen + * to. Each turns its own topics into a rtabmap::SensorData and hands it to processData(); + * from there on this class does the work -- registration, pose integration, the `odom` + * topic and its TF, the IMU intake, the services, the diagnostics, the reset policy when + * tracking is lost. That is why the three nodes share nearly all of their parameters and + * publish the same topics. + * + * A subclass is expected to: + * - call init() from its constructor, saying which families of RTAB-Map parameters it + * accepts, which decides both the defaults and what the node will accept being set; + * - create its subscriptions in onOdomInit() and describe them with initDiagnosticMsg(); + * - call tick() when a message arrives and processData() once a frame is complete; + * - implement flushCallbacks(), so that a reset can drop whatever its synchronizer holds. + * + * The class is also a UThread. By default the frame handed to processData() is passed to + * that thread and the callback returns at once, so a slow registration cannot block the + * executor; a frame arriving while the thread is busy is dropped rather than queued. With + * `always_process_most_recent_frame:=false` it is registered on the calling thread + * instead, which keeps every frame at the cost of holding up the executor. + * + * @see the package README for the parameters and topics these nodes have in common. + */ class OdometryROS : public rclcpp::Node, public UThread { public: + /// Constructs the node under its default name. explicit OdometryROS(const rclcpp::NodeOptions & options); + /// Constructs the node under @p name, which is what the three nodes use. explicit OdometryROS(const std::string & name, const rclcpp::NodeOptions & options); virtual ~OdometryROS(); + /** + * @brief Hands a complete frame to the odometry; called by a subclass's callback. + * @param[in,out] data the frame to register, which comes back carrying the + * features the odometry ended up using + * @param[in] header stamp and frame of the data, used to publish the result + * + * The frame is either queued for the worker thread or registered right here, + * depending on `always_process_most_recent_frame`. Either way, a frame that arrives + * while the previous one is still being registered is dropped: the odometry stays on + * the newest data rather than falling behind. + */ void processData(rtabmap::SensorData & data, const std_msgs::msg::Header & header); + /// `reset_odom` service: starts a new map at the origin, or at the guess frame's pose. void resetOdom(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + /// `reset_odom_to_pose` service: starts a new map at the pose given in the request. void resetToPose(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + /// `pause_odom` service: keeps the subscriptions but stops registering what arrives. void pause(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + /// `resume_odom` service: registers again, starting from the next frame. void resume(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + /// `log_debug` service: raises RTAB-Map's own log level to debug at runtime. void setLogDebug(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + /// `log_info` service; see setLogDebug(). void setLogInfo(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + /// `log_warning` service; see setLogDebug(). void setLogWarn(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + /// `log_error` service; see setLogDebug(). void setLogError(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + /// The robot frame the odometry is computed for, `frame_id`. const std::string & frameId() const {return frameId_;} + /// The frame the estimated poses are expressed in, `odom_frame_id`. const std::string & odomFrameId() const {return odomFrameId_;} + /// The frame an external motion guess is read from, `guess_frame_id`; empty if unused. const std::string & guessFrameId() const {return guessFrameId_;} + /// The RTAB-Map parameters this node was configured with, defaults included. const rtabmap::ParametersMap & parameters() const {return parameters_;} + /// Whether the `pause_odom` service has been called and not resumed since. bool isPaused() const {return paused_;} protected: + /** + * @brief Declares the node's parameters and creates the odometry; call it last in the + * subclass constructor. + * @param[in] stereoParams true if the node takes RTAB-Map's stereo parameters + * @param[in] visParams true if it takes the visual registration ones + * @param[in] icpParams true if it takes the scan matching ones + * + * The three flags decide which RTAB-Map parameters the node declares, and so which + * ones it accepts being set: `icp_odometry` refuses a `Vis/` parameter and the other + * two refuse an `Icp/` one. onOdomInit() is called at the end, for the subclass to + * create its subscriptions. + */ void init(bool stereoParams, bool visParams, bool icpParams); + /// The reliability the subclass should give its own subscriptions, from `qos`. rmw_qos_reliability_policy_t qos() const {return qos_;} + /** + * @brief Starts the diagnostics, once the subclass knows what it subscribed to. + * @param[in] subscribedTopicsMsg the human readable list logged at startup and + * repeated in the "no data received" warning + * @param[in] approxSync whether the subclass matches stamps approximately, + * which that warning mentions as a likely cause + * @param[in] subscribedTopic the one topic whose rate is watched, if any + */ void initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic = ""); + /// Drops whatever the subclass's synchronizer holds; called when the odometry resets. virtual void flushCallbacks() {}; + /// The node's TF buffer, for the subclass to look up its sensors' frames. tf2_ros::Buffer & tfBuffer() {return *tfBuffer_;} + /// How long a TF lookup may block, from `wait_for_transform`. const double & waitForTransform() const {return waitForTransform_;} + /// The velocity of the last registered frame, null when there is no estimate yet. rtabmap::Transform velocityGuess() const; + /// Stamp of the last registered frame, 0 before the first one. double previousStamp() const {return previousStamp_;} + /// Called after a frame has been registered and published, for a subclass to add to it. virtual void postProcessData(const rtabmap::SensorData & /*data*/, const std_msgs::msg::Header & /*header*/) const {} private: void processData(); virtual void mainLoop(); virtual void mainLoopKill(); + /// Lets a subclass adjust the RTAB-Map parameters before the odometry is created. virtual void updateParameters(rtabmap::ParametersMap &) {} + /// Called at the end of init(), where a subclass creates its subscriptions. virtual void onOdomInit() {} void callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg); void reset(const rtabmap::Transform & pose = rtabmap::Transform::getIdentity()); protected: + /// The callback group the subclass's sensor subscriptions belong to. rclcpp::CallbackGroup::SharedPtr dataCallbackGroup_; + /// Reports the arrival of an input message to the diagnostics, before anything else. void tick(const rclcpp::Time & stamp); private: diff --git a/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp index dc50b450..589866b1 100644 --- a/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp +++ b/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp @@ -44,6 +44,17 @@ using namespace rtabmap; namespace rtabmap_odom { +/** + * @brief Odometry from a laser scanner, 2D or 3D, by scan matching. + * + * Takes a sensor_msgs::msg::LaserScan or a sensor_msgs::msg::PointCloud2 and registers + * each scan against the previous ones with ICP. A cloud whose points carry their own + * timestamps is deskewed first, using TF or the last known velocity, since a scan taken + * while the robot moves is not one rigid observation. + * + * @see doc/icp_odometry.md for the topics, the parameters and the shapes it can and + * cannot constrain. + */ class ICPOdometry : public rtabmap_odom::OdometryROS { public: diff --git a/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp index 52649daf..b6046da0 100644 --- a/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp +++ b/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp @@ -38,6 +38,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include +#include #include #include @@ -50,6 +52,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap_odom { +/** + * @brief Odometry from an RGB-D camera, or from several on one rig. + * + * Takes either the three raw topics of a camera (`rgb/image`, `depth/image`, + * `rgb/camera_info`) or pre-synchronized rtabmap_msgs::msg::RGBDImage messages, one per + * camera, and registers each frame's visual features against a local feature map. A + * frame that arrives with its own keypoints, 3D points and descriptors is registered + * with those rather than having them extracted again. + * + * @see doc/rgbd_odometry.md for the topics, the parameters and what to do when it loses + * tracking. + */ class RGBDOdometry : public rtabmap_odom::OdometryROS { public: @@ -61,10 +75,19 @@ private: virtual void updateParameters(rtabmap::ParametersMap & parameters); virtual void onOdomInit(); + /** + * Local features, when the input topic carries them, are indexed per camera like + * the images are: one entry per camera, in the same order. They are optional, and + * a frame that comes without them is processed exactly as before, the features + * being extracted from the images downstream. + */ void commonCallback( const std::vector & rgbImages, const std::vector & depthImages, - const std::vector& cameraInfos); + const std::vector& cameraInfos, + const std::vector > & localKeyPointsMsgs = {}, + const std::vector > & localPoints3dMsgs = {}, + const std::vector & localDescriptorsMsgs = {}); void callback( const sensor_msgs::msg::Image::ConstSharedPtr image, diff --git a/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp index 5b47c8d0..96cdfbf5 100644 --- a/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp +++ b/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp @@ -43,12 +43,25 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif #include +#include +#include #include #include namespace rtabmap_odom { +/** + * @brief Odometry from a stereo pair, or from several on one rig. + * + * Takes either the four raw topics of a stereo camera (left and right image plus their + * calibrations) or pre-synchronized rtabmap_msgs::msg::RGBDImage messages carrying the + * pair, one per camera. The right camera's `P(0,3)` is what gives the trajectory its + * scale. A frame that arrives with its own keypoints, 3D points and descriptors is + * registered with those rather than having them extracted again. + * + * @see doc/stereo_odometry.md for the topics, the parameters and the scale it depends on. + */ class StereoOdometry : public rtabmap_odom::OdometryROS { public: @@ -60,11 +73,20 @@ private: virtual void updateParameters(rtabmap::ParametersMap & parameters); virtual void onOdomInit(); + /** + * Local features, when the input topic carries them, are indexed per camera like + * the images are: one entry per camera, in the same order, and placed in that + * camera's left image. They are optional, and a frame that comes without them is + * processed exactly as before, the features being extracted downstream. + */ void commonCallback( const std::vector & leftImages, const std::vector & rightImages, const std::vector& leftCameraInfos, - const std::vector& rightCameraInfos); + const std::vector& rightCameraInfos, + const std::vector > & localKeyPointsMsgs = {}, + const std::vector > & localPoints3dMsgs = {}, + const std::vector & localDescriptorsMsgs = {}); void callback( const sensor_msgs::msg::Image::ConstSharedPtr imageRectLeft, diff --git a/rtabmap_odom/package.xml b/rtabmap_odom/package.xml index 14870d32..292eeec8 100644 --- a/rtabmap_odom/package.xml +++ b/rtabmap_odom/package.xml @@ -30,8 +30,14 @@ rtabmap_util rtabmap_sync + ament_cmake_gtest + + rosbag2_cpp + rosbag2_storage_mcap + ament_cmake + rosdoc2.yaml diff --git a/rtabmap_odom/rosdoc2.yaml b/rtabmap_odom/rosdoc2.yaml new file mode 100644 index 00000000..6e3ef7d9 --- /dev/null +++ b/rtabmap_odom/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_odom +## Build the docs locally with: +## rosdoc2 build --package-path rtabmap_odom --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_odom 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_odom', + doxygen_xml_directory: 'generated/doxygen/xml', + output_dir: '' + } diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 5a270c75..40ec056f 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include #include "rtabmap_odom/OdometryROS.h" #include @@ -61,6 +62,44 @@ using namespace rtabmap; namespace rtabmap_odom { +namespace { + +/** + * @brief The covariance of a pose that came from the guess frame instead of registration. + * + * Used wherever the guess is what the published pose rests on: a frame the odometry did + * not update because it had not moved enough, and the frame that restarts the map after + * a reset. Nothing was measured in either case, so the confidence is the one the guess + * was declared to have rather than anything the registration computed. + */ +cv::Mat guessCovariance(double linearVariance, double angularVariance) +{ + cv::Mat covariance = cv::Mat::zeros(6,6,CV_64FC1); + covariance.at(0,0) = linearVariance; // xx + covariance.at(1,1) = linearVariance; // yy + covariance.at(2,2) = linearVariance; // zz + covariance.at(3,3) = angularVariance; // rr + covariance.at(4,4) = angularVariance; // pp + covariance.at(5,5) = angularVariance; // yawyaw + return covariance; +} + +/** + * @brief The velocity a motion implies, for a frame with no registration to measure one. + * + * Named apart from the guess itself so that it can be called where a `guessVelocity` + * variable is in scope. + */ +rtabmap::Transform velocityFrom(const rtabmap::Transform & motion, double dt) +{ + UASSERT(dt > 0.0); + float x,y,z,roll,pitch,yaw; + motion.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + return rtabmap::Transform(x/dt, y/dt, z/dt, roll/dt, pitch/dt, yaw/dt); +} + +} // namespace + OdometryROS::OdometryROS(const rclcpp::NodeOptions & options) : OdometryROS("odometry", options) {} @@ -83,6 +122,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o publishNullWhenLost_(true), publishCompressedSensorData_(false), qos_(RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT), + bufferedDataToProcess_(false), paused_(false), resetCountdown_(0), resetCurrentCount_(0), @@ -194,6 +234,17 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o "are the same frame (value=\"%s\"). \"guess_frame_id\" is disabled.", odomFrameId_.c_str()); guessFrameId_.clear(); } + if(!publishNullWhenLost_ && guessFrameId_.empty() && publishTf_) + { + RCLCPP_ERROR(this->get_logger(), "\"publish_null_when_lost\" is false, but nothing can " + "say where odometry restarts after being lost: \"guess_frame_id\" is not set and " + "\"publish_tf\" is true, so the %s->%s fallback returns this node's own pose. " + "Whatever the robot did while lost will be silently dropped from the trajectory " + "and mapped across. Set \"guess_frame_id\", or set \"publish_tf\" to false if " + "another node (e.g. robot_localization) publishes %s->%s, or leave " + "\"publish_null_when_lost\" true.", + odomFrameId_.c_str(), frameId_.c_str(), odomFrameId_.c_str(), frameId_.c_str()); + } RCLCPP_INFO(this->get_logger(), "Odometry: frame_id = %s", frameId_.c_str()); RCLCPP_INFO(this->get_logger(), "Odometry: odom_frame_id = %s", odomFrameId_.c_str()); RCLCPP_INFO(this->get_logger(), "Odometry: publish_tf = %s", publishTf_?"true":"false"); @@ -628,8 +679,18 @@ void OdometryROS::processData() imuProcessed_ = true; } + // Whether this is a frame at all, as opposed to an IMU-only update. Neither the image + // nor the features answer that on their own: a frame that brings its own features has + // no image, and a frame of an empty scene has no feature. The calibration does, being + // there whenever a camera produced the data -- the same rule RTAB-Map's own + // Odometry::process() applies before registering anything. + const bool isFrame = !data.imageRaw().empty() || + !data.cameraModels().empty() || + !data.stereoCameraModels().empty() || + !data.laserScanRaw().isEmpty(); + Transform groundTruth; - if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()) + if(isFrame) { // Detect time jump in the past double clockNow = now().seconds(); @@ -688,7 +749,7 @@ void OdometryROS::processData() { groundTruth = rtabmap_conversions::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, header.stamp, *tfBuffer_, waitForTransform_); - if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()) + if(isFrame) { // Use only XYZ to handle the case odometry was previously initialized with IMU, // we assume that the ground truth contains also a real initial orientation @@ -716,39 +777,6 @@ void OdometryROS::processData() } } - bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_ > 1.0/minUpdateRate_; - if(tooOldPreviousData) - { - RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset because last update " - "is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.", - rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp)); - - if(!guess_.isNull()) - { - RCLCPP_WARN(this->get_logger(), "Odometry automatically reset based on latest guess available from TF (%s->%s, moved %s since got lost)!", - guessFrameId_.c_str(), frameId_.c_str(), guess_.prettyPrint().c_str()); - odometry_->reset(odometry_->getPose() * guess_); - guess_.setNull(); - guessPreviousPose_.setNull(); - } - else - { - // Check TF to see if sensor fusion is used (e.g., the output of robot_localization) - Transform tfPose = rtabmap_conversions::getTransform(odomFrameId_, frameId_, header.stamp, *tfBuffer_, waitForTransform_); - if(tfPose.isNull()) - { - RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest computed pose!"); - odometry_->reset(odometry_->getPose()); - } - else - { - RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!", - odomFrameId_.c_str(), frameId_.c_str()); - odometry_->reset(tfPose); - } - } - } - bool skipOdometryUpdate = false; rtabmap::Transform pose; @@ -756,23 +784,36 @@ void OdometryROS::processData() rtabmap::Transform guessVelocity; Transform guessCurrentPose; + // Whether the guess has a previous pose to be relative to, which decides how the pose + // is seeded from it further down. It is cleared by reset(), so a reset asked for + // through a service restarts at the guess frame while an automatic one continues from + // the pose it has just carried forward. + bool guessIsTheFirstOne = false; if(!guessFrameId_.empty()) { guessCurrentPose = rtabmap_conversions::getTransform(guessFrameId_, frameId_, header.stamp, *tfBuffer_, waitForTransform_); Transform previousPose = guessPreviousPose_; - if(guessPreviousPose_.isNull()) + guessIsTheFirstOne = guessPreviousPose_.isNull(); + if(guessIsTheFirstOne) { previousPose = guessCurrentPose; - if(!guessCurrentPose.isNull() && odometry_->getPose().isIdentity()) - { - RCLCPP_INFO(get_logger(), "Odometry: init pose with guess %s", guessCurrentPose.prettyPrint().c_str()); - odometry_->reset(guessCurrentPose); - } } if(!previousPose.isNull() && !guessCurrentPose.isNull()) { + // What the guess frame says the robot is doing. This is what gets published + // for a frame with no registration behind it -- one skipped for not having + // moved enough, or one starting a new map, whose twist would otherwise be + // unknown although its pose comes from the guess. It is dropped further down + // as soon as the registration has a velocity of its own to report. + if(previousStamp_ > 0.0 && + rtabmap_conversions::timestampFromROS(header.stamp) > previousStamp_) + { + guessVelocity = velocityFrom(previousPose.inverse() * guessCurrentPose, + rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_); + } + if(guess_.isNull()) { guess_ = previousPose.inverse() * guessCurrentPose; @@ -791,20 +832,7 @@ void OdometryROS::processData() { // Ignore odometry update, we didn't move enough pose = odometry_->getPose() * guess_; - info.reg.covariance = cv::Mat::zeros(6,6,CV_64FC1); - info.reg.covariance.at(0,0) = guessLinearVariance_; // xx - info.reg.covariance.at(1,1) = guessLinearVariance_; // yy - info.reg.covariance.at(2,2) = guessLinearVariance_; // zz - info.reg.covariance.at(3,3) = guessAngularVariance_; // rr - info.reg.covariance.at(4,4) = guessAngularVariance_; // pp - info.reg.covariance.at(5,5) = guessAngularVariance_; // yawyaw - //set velocity - double dt = rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_; - UASSERT(dt>0.0); - // use part of guess matching dt - (previousPose.inverse() * guessCurrentPose).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); - guessVelocity = rtabmap::Transform(x/dt, y/dt, z/dt, roll/dt, pitch/dt, yaw/dt); skipOdometryUpdate = true; } } @@ -817,16 +845,117 @@ void OdometryROS::processData() } } + // Handled here rather than before the guess is computed: guess_ only holds the motion + // since the previous frame once the block above has run, and resetting without it + // throws away everything the guess source measured across the gap -- which is exactly + // what this reset is supposed to carry over. + bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_ > 1.0/minUpdateRate_; + if(tooOldPreviousData) + { + RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset because last update " + "is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.", + rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp)); + + if(!guess_.isNull()) + { + RCLCPP_WARN(this->get_logger(), "Odometry automatically reset based on latest guess available from TF (%s->%s, moved %s since got lost)!", + guessFrameId_.c_str(), frameId_.c_str(), guess_.prettyPrint().c_str()); + odometry_->reset(odometry_->getPose() * guess_); + // Cleared because it has just been applied: the odometry now starts from a + // pose that already includes it, and leaving it would have the registration + // apply it a second time on the frame that initialises the new map. + // guessPreviousPose_ is kept, so the next frame measures its motion from this + // one rather than starting over and losing a frame of it. + guess_.setNull(); + } + else + { + // Check TF to see if sensor fusion is used (e.g., the output of robot_localization) + Transform tfPose = rtabmap_conversions::getTransform(odomFrameId_, frameId_, header.stamp, *tfBuffer_, waitForTransform_); + if(tfPose.isNull()) + { + RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest computed pose!"); + odometry_->reset(odometry_->getPose()); + } + else + { + RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!", + odomFrameId_.c_str(), frameId_.c_str()); + odometry_->reset(tfPose); + } + } + } + // process data rclcpp::Time timeStart = rclcpp::Clock().now(); if(!groundTruth.isNull()) { data.setGroundTruth(groundTruth); } + // Set when the guess has already been folded into the pose below, so that a reset + // later in this frame does not go looking for a fallback that is no longer needed. + bool poseCarriedByGuess = false; + // Set when this frame starts a new map and the guess frame says where, which is what + // makes the trajectory it starts continuous with the one before it. + bool initialisedOnGuess = false; if(!skipOdometryUpdate) { + // This frame will initialise the odometry's map whenever no frame has been + // registered since the last reset -- at startup, after a service reset, or on + // recovery from an automatic one. Registration then returns no motion, so the pose + // has to be put where the guess says the robot is *before* the frame is processed: + // afterwards the map is already anchored in the wrong place, and the next + // registration measures the difference against that anchor and takes the correction + // straight back out. Resetting here costs nothing, the map being empty either way. + // + // There are two ways to be right, depending on what the guess can say: + initialisedOnGuess = odometry_->framesProcessed() == 0 && !guessCurrentPose.isNull(); + if(initialisedOnGuess) + { + if(guessIsTheFirstOne) + { + // Nothing to be relative to. Adopt the guess source's own coordinates, so + // that odometry restarts where the guess says it is rather than at the + // origin. A pose asked for explicitly through reset_odom_to_pose is left + // alone: only an odometry still sitting at the identity is seeded this way. + if(odometry_->getPose().isIdentity()) + { + RCLCPP_INFO(get_logger(), "Odometry: init pose with guess %s", + guessCurrentPose.prettyPrint().c_str()); + odometry_->reset(guessCurrentPose); + } + } + else if(!guess_.isNull() && !guess_.isIdentity()) + { + // There is a previous guess pose, so the guess describes real motion since + // the frame before this one -- which an automatic reset has just carried the + // pose through. Advance by it and the trajectory stays continuous; drop it + // and the new map is anchored a frame behind, once per reset, accumulating. + RCLCPP_DEBUG(this->get_logger(), "Odometry: advancing the pose by the guess " + "(%s) before the map is initialised, so the motion measured since the " + "previous frame is not lost.", guess_.prettyPrint().c_str()); + odometry_->reset(odometry_->getPose() * guess_); + guess_.setNull(); + poseCarriedByGuess = true; + } + } pose = odometry_->process(data, guess_, &info); } + + // 9999 on both covariances is how rtabmap is told a frame starts a new map. When the + // guess frame says where it starts, and publish_null_when_lost says this consumer + // wants poses rather than the news of a reset, it goes out as a continuation instead. + const bool publishAsContinuation = initialisedOnGuess && !publishNullWhenLost_ && !pose.isNull(); + if(skipOdometryUpdate || publishAsContinuation) + { + // Both rest on the guess rather than on a registration: its confidence, its velocity. + info.reg.covariance = guessCovariance(guessLinearVariance_, guessAngularVariance_); + } + else + { + // The registration measured this one, so its velocity is the one to publish. + guessVelocity.setNull(); + } if(!pose.isNull()) { if(!skipOdometryUpdate) { @@ -909,11 +1038,12 @@ void OdometryROS::processData() if(setTwist) { float x,y,z,roll,pitch,yaw; - if(skipOdometryUpdate) { - UASSERT(!guessVelocity.isNull()); - guessVelocity.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); - } else { + // Whatever is left of the two: the registration's own velocity, or the + // guess's where the frame had no registration to give one. + if(guessVelocity.isNull()) { odometry_->getVelocityGuess().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + } else { + guessVelocity.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); } odom.twist.twist.linear.x = x; odom.twist.twist.linear.y = y; @@ -931,7 +1061,7 @@ void OdometryROS::processData() odom.twist.covariance.at(35) = setTwist?info.reg.covariance.at(5,5):BAD_COVARIANCE; // yawyaw //publish the message - if(setTwist || publishNullWhenLost_) + if(setTwist || publishNullWhenLost_ || publishAsContinuation) { odomPub_->publish(odom); } @@ -953,7 +1083,7 @@ void OdometryROS::processData() cloud.push_back(pt); } sensor_msgs::msg::PointCloud2 cloudMsg; - pcl::toROSMsg(cloud, cloudMsg); + rtabmap_conversions::toPointCloud2Msg(cloud, cloudMsg); cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image cloudMsg.header.frame_id = odomFrameId_; odomLocalMap_->publish(cloudMsg); @@ -976,7 +1106,7 @@ void OdometryROS::processData() } sensor_msgs::msg::PointCloud2 cloudMsg; - pcl::toROSMsg(cloud, cloudMsg); + rtabmap_conversions::toPointCloud2Msg(cloud, cloudMsg); cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image cloudMsg.header.frame_id = odomFrameId_; odomLastFrame_->publish(cloudMsg); @@ -996,7 +1126,7 @@ void OdometryROS::processData() cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z)); } sensor_msgs::msg::PointCloud2 cloudMsg; - pcl::toROSMsg(cloud, cloudMsg); + rtabmap_conversions::toPointCloud2Msg(cloud, cloudMsg); cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image cloudMsg.header.frame_id = odomFrameId_; odomLastFrame_->publish(cloudMsg); @@ -1010,22 +1140,22 @@ void OdometryROS::processData() if(info.localScanMap.hasNormals() && info.localScanMap.hasIntensity()) { pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloudINormal(info.localScanMap, info.localScanMap.localTransform()); - pcl::toROSMsg(*cloud, cloudMsg); + rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg); } else if(info.localScanMap.hasNormals()) { pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloudNormal(info.localScanMap, info.localScanMap.localTransform()); - pcl::toROSMsg(*cloud, cloudMsg); + rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg); } else if(info.localScanMap.hasIntensity()) { pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloudI(info.localScanMap, info.localScanMap.localTransform()); - pcl::toROSMsg(*cloud, cloudMsg); + rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg); } else { pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloud(info.localScanMap, info.localScanMap.localTransform()); - pcl::toROSMsg(*cloud, cloudMsg); + rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg); } cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image @@ -1106,6 +1236,17 @@ void OdometryROS::processData() odometry_->reset(odometry_->getPose() * guess_); guess_.setNull(); } + else if(poseCarriedByGuess) + { + // The guess was folded into the pose before this frame was processed, so the + // pose already covers the motion since the last one. Going to TF for a + // fallback here would block for wait_for_transform on every lost frame and + // answer a question that has already been answered. + RCLCPP_WARN(this->get_logger(), "Odometry automatically reset, carrying the " + "latest guess from TF (%s->%s) that was already applied to the pose!", + guessFrameId_.c_str(), frameId_.c_str()); + odometry_->reset(odometry_->getPose()); + } else { // Check TF to see if sensor fusion is used (e.g., the output of robot_localization) @@ -1319,6 +1460,11 @@ void OdometryROS::reset(const Transform & pose) UScopeMutex lock(dataMutex_); odometry_->reset(pose); guess_.setNull(); + // Clearing this is what tells the next frame to restart from the guess frame rather + // than continue from here: the seeding step below cannot tell a reset asked for + // through a service from one the node decided on its own, and reads this instead. The + // automatic resets deliberately leave it alone, so that they carry on from the pose + // they just moved. guessPreviousPose_.setNull(); previousStamp_ = 0.0; previousClockTime_ = 0.0; diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp index 2281e7fb..f67453c8 100644 --- a/rtabmap_odom/src/nodelets/icp_odometry.cpp +++ b/rtabmap_odom/src/nodelets/icp_odometry.cpp @@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include #include #include @@ -69,6 +70,7 @@ ICPOdometry::ICPOdometry(const rclcpp::NodeOptions & options) : ICPOdometry::~ICPOdometry() { + this->join(true); } void ICPOdometry::onOdomInit() @@ -399,12 +401,12 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan if(hasIntensity) { - pcl::fromROSMsg(scanOut, *pclScanI); + rtabmap_conversions::fromPointCloud2Msg(scanOut, *pclScanI); pclScanI->is_dense = true; } else { - pcl::fromROSMsg(scanOut, *pclScan); + rtabmap_conversions::fromPointCloud2Msg(scanOut, *pclScan); pclScan->is_dense = true; } @@ -519,7 +521,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr pointCloudMsg) { UASSERT_MSG(pointCloudMsg->data.size() == pointCloudMsg->row_step*pointCloudMsg->height, - uFormat("data=%d row_step=%d height=%d", pointCloudMsg->data.size(), pointCloudMsg->row_step, pointCloudMsg->height).c_str()); + uFormat("data=%d row_step=%d height=%d", (int)pointCloudMsg->data.size(), (int)pointCloudMsg->row_step, (int)pointCloudMsg->height).c_str()); if(scanReceived_) { @@ -668,7 +670,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p if(hasNormals && hasIntensity) { pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(*cloudMsg, *pclScan); + rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan); if(pclScan->size() && scanDownsamplingStep_ > 1) { pclScan = util3d::downsample(pclScan, scanDownsamplingStep_); @@ -686,7 +688,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p else if(hasNormals) { pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(*cloudMsg, *pclScan); + rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan); if(pclScan->size() && scanDownsamplingStep_ > 1) { pclScan = util3d::downsample(pclScan, scanDownsamplingStep_); @@ -704,7 +706,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p else if(hasIntensity) { pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(*cloudMsg, *pclScan); + rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan); if(pclScan->size() && scanDownsamplingStep_ > 1) { pclScan = util3d::downsample(pclScan, scanDownsamplingStep_); @@ -750,7 +752,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p else { pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(*cloudMsg, *pclScan); + rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan); if(pclScan->size() && scanDownsamplingStep_ > 1) { pclScan = util3d::downsample(pclScan, scanDownsamplingStep_); diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index 97d48f6e..5f43acab 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp @@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap_conversions/MsgConversion.h" #include +#include #include #include #include @@ -73,6 +74,8 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) : RGBDOdometry::~RGBDOdometry() { + this->join(true); + delete approxSync_; delete exactSync_; delete approxSync2_; @@ -435,40 +438,58 @@ void RGBDOdometry::updateParameters(ParametersMap & parameters) void RGBDOdometry::commonCallback( const std::vector & rgbImages, const std::vector & depthImages, - const std::vector& cameraInfos) + const std::vector& cameraInfos, + const std::vector > & localKeyPointsMsgs, + const std::vector > & localPoints3dMsgs, + const std::vector & localDescriptorsMsgs) { UASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size()); rclcpp::Time higherStamp; UASSERT_MSG(rgbImages[0], "RGB image is null!"); - int imageWidth = rgbImages[0]->image.cols; - int imageHeight = rgbImages[0]->image.rows; UASSERT_MSG(depthImages[0], "Depth image is null!"); + + // The images are what the local features would otherwise be extracted from, so a frame + // that brings its own can leave them out -- which is nearly all of the bandwidth. It + // then describes itself with its calibration alone: how big the image would have been, + // where the camera is, what it sees. (An RGB-D message with no depth image at all also + // used to divide by zero below.) + const bool hasRgb = !rgbImages[0]->image.empty(); + const bool hasDepth = !depthImages[0]->image.empty(); + + int imageWidth = hasRgb?rgbImages[0]->image.cols:(int)cameraInfos[0].width; + int imageHeight = hasRgb?rgbImages[0]->image.rows:(int)cameraInfos[0].height; int depthWidth = depthImages[0]->image.cols; int depthHeight = depthImages[0]->image.rows; - UASSERT_MSG( - imageWidth/depthWidth == imageHeight/depthHeight, - uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str()); + if(hasDepth) + { + UASSERT_MSG( + imageWidth/depthWidth == imageHeight/depthHeight, + uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str()); + } int cameraCount = rgbImages.size(); cv::Mat rgb; cv::Mat depth; std::vector cameraModels; + std::vector keypoints; + std::vector points3d; + cv::Mat descriptors; for(unsigned int i=0; iencoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || + if((hasRgb && !(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 || rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 || rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 || - rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) || - !(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 || + rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0)) || + (hasDepth && !(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 || depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 || - depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)) + depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))) { RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8 and " "image_depth=32FC1,16UC1,mono16. Current rgb=%s and depth=%s", @@ -476,20 +497,33 @@ void RGBDOdometry::commonCallback( depthImages[i]->encoding.c_str()); return; } - UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight, - uFormat("imageWidth=%d vs %d imageHeight=%d vs %d", - imageWidth, - rgbImages[i]->image.cols, - imageHeight, - rgbImages[i]->image.rows).c_str()); - UASSERT_MSG(depthImages[i]->image.cols == depthWidth && depthImages[i]->image.rows == depthHeight, - uFormat("depthWidth=%d vs %d depthHeight=%d vs %d", - depthWidth, - depthImages[i]->image.cols, - depthHeight, - depthImages[i]->image.rows).c_str()); + if(hasRgb) + { + UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight, + uFormat("imageWidth=%d vs %d imageHeight=%d vs %d", + imageWidth, + rgbImages[i]->image.cols, + imageHeight, + rgbImages[i]->image.rows).c_str()); + } + if(hasDepth) + { + UASSERT_MSG(depthImages[i]->image.cols == depthWidth && depthImages[i]->image.rows == depthHeight, + uFormat("depthWidth=%d vs %d depthHeight=%d vs %d", + depthWidth, + depthImages[i]->image.cols, + depthHeight, + depthImages[i]->image.rows).c_str()); + } - rclcpp::Time stamp = rtabmap_conversions::timestampFromROS(rgbImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(depthImages[i]->header.stamp)?rgbImages[i]->header.stamp:depthImages[i]->header.stamp; + // An image that is not there carries no header either, so a frame that has none is + // stamped and placed by its calibration, which is all it has. + const std::string & cameraFrameId = hasRgb?rgbImages[i]->header.frame_id:cameraInfos[i].header.frame_id; + rclcpp::Time stamp = cameraInfos[i].header.stamp; + if(hasRgb || hasDepth) + { + stamp = rtabmap_conversions::timestampFromROS(rgbImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(depthImages[i]->header.stamp)?rgbImages[i]->header.stamp:depthImages[i]->header.stamp; + } if(i == 0) { @@ -500,7 +534,7 @@ void RGBDOdometry::commonCallback( higherStamp = stamp; } - Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), rgbImages[i]->header.frame_id, stamp, tfBuffer(), waitForTransform()); + Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), cameraFrameId, stamp, tfBuffer(), waitForTransform()); if(localTransform.isNull()) { return; @@ -528,53 +562,75 @@ void RGBDOdometry::commonCallback( } } - cv_bridge::CvImageConstPtr ptrImage = rgbImages[i]; - if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 && - rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0) + if(hasRgb) { - if(keepColor_ && rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0) + cv_bridge::CvImageConstPtr ptrImage = rgbImages[i]; + if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 && + rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0) { - ptrImage = cv_bridge::cvtColor(rgbImages[i], "bgr8"); + if(keepColor_ && rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0) + { + ptrImage = cv_bridge::cvtColor(rgbImages[i], "bgr8"); + } + else + { + ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8"); + } + } + + // initialize + if(rgb.empty()) + { + rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type()); + } + + if(ptrImage->image.type() == rgb.type()) + { + ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight))); } else { - ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8"); + RCLCPP_ERROR(this->get_logger(), "Some RGB images are not the same type! %d vs %d", ptrImage->image.type(), rgb.type()); + return; } } - cv_bridge::CvImageConstPtr ptrDepth = depthImages[i]; + if(hasDepth) + { + cv_bridge::CvImageConstPtr ptrDepth = depthImages[i]; + if(depth.empty()) + { + depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrDepth->image.type()); + } - // initialize - if(rgb.empty()) - { - rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type()); - } - if(depth.empty()) - { - depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrDepth->image.type()); - } - - if(ptrImage->image.type() == rgb.type()) - { - ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight))); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Some RGB images are not the same type! %d vs %d", ptrImage->image.type(), rgb.type()); - return; - } - - if(ptrDepth->image.type() == depth.type()) - { - ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight))); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Some Depth images are not the same type! %d vs %d", ptrDepth->image.type(), depth.type()); - return; + if(ptrDepth->image.type() == depth.type()) + { + ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight))); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Some Depth images are not the same type! %d vs %d", ptrDepth->image.type(), depth.type()); + return; + } } cameraModels.push_back(rtabmap_conversions::cameraModelFromROS(cameraInfos[i], localTransform)); + + // The images of all cameras are stitched side by side above, so the keypoints of + // camera i are shifted by as many images as come before it, and their 3D points, + // which arrive in that camera's optical frame, are brought back to the base frame. + if(localKeyPointsMsgs.size() == rgbImages.size()) + { + rtabmap_conversions::keypointsFromROS(localKeyPointsMsgs[i], keypoints, imageWidth*i); + } + if(localPoints3dMsgs.size() == rgbImages.size()) + { + rtabmap_conversions::points3fFromROS(localPoints3dMsgs[i], points3d, localTransform); + } + if(localDescriptorsMsgs.size() == rgbImages.size()) + { + descriptors.push_back(localDescriptorsMsgs[i]); + } } rtabmap::SensorData data( @@ -584,9 +640,30 @@ void RGBDOdometry::commonCallback( 0, rtabmap_conversions::timestampFromROS(higherStamp)); + // Features that came with the frame are used as they are: the odometry then skips + // detection, description and the depth lookup that would otherwise rebuild them + // (see RegistrationVis, which extracts only when the frame carries no keypoints). + // They are dropped rather than trusted if the three of them disagree, as using them + // out of step would silently mismatch keypoints with their descriptors or 3D points. + if(!keypoints.empty()) + { + if((!points3d.empty() && points3d.size() != keypoints.size()) || + (!descriptors.empty() && descriptors.rows != (int)keypoints.size())) + { + RCLCPP_ERROR(this->get_logger(), "Ignoring the local features received with this frame: " + "%d keypoints, %d 3D points and %d descriptors, which should be the same count " + "(or none at all for the 3D points and the descriptors).", + (int)keypoints.size(), (int)points3d.size(), descriptors.rows); + } + else + { + data.setFeatures(keypoints, points3d, descriptors); + } + } + std_msgs::msg::Header header; header.stamp = higherStamp; - header.frame_id = rgbImages.size()==1?rgbImages[0]->header.frame_id:""; + header.frame_id = rgbImages.size()==1?(hasRgb?rgbImages[0]->header.frame_id:cameraInfos[0].header.frame_id):""; this->processData(data, header); } @@ -627,6 +704,27 @@ void RGBDOdometry::callback( } } +namespace { + +/** + * @brief Collects the local features one camera's image carries, cameras in order. + * + * An image that carries none pushes empty entries rather than nothing, so that the + * per-camera indexing still lines up with the images. + */ +void appendLocalFeatures( + const rtabmap_msgs::msg::RGBDImage & image, + std::vector > & keyPoints, + std::vector > & points3d, + std::vector & descriptors) +{ + keyPoints.push_back(image.key_points); + points3d.push_back(image.points); + descriptors.push_back(rtabmap::uncompressData(image.descriptors)); +} + +} // namespace + void RGBDOdometry::callbackRGBDX( const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images) { @@ -642,13 +740,17 @@ void RGBDOdometry::callbackRGBDX( std::vector imageMsgs(images->rgbd_images.size()); std::vector depthMsgs(images->rgbd_images.size()); std::vector infoMsgs; + std::vector > localKeyPoints; + std::vector > localPoints3d; + std::vector localDescriptors; for(size_t i=0; irgbd_images.size(); ++i) { rtabmap_conversions::toCvShare(images->rgbd_images[i], images, imageMsgs[i], depthMsgs[i]); infoMsgs.push_back(images->rgbd_images[i].rgb_camera_info); + appendLocalFeatures(images->rgbd_images[i], localKeyPoints, localPoints3d, localDescriptors); } - this->commonCallback(imageMsgs, depthMsgs, infoMsgs); + this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors); } } @@ -665,7 +767,12 @@ void RGBDOdometry::callbackRGBD( rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]); infoMsgs.push_back(image->rgb_camera_info); - this->commonCallback(imageMsgs, depthMsgs, infoMsgs); + std::vector > localKeyPoints; + std::vector > localPoints3d; + std::vector localDescriptors; + appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors); + + this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors); } } @@ -685,7 +792,13 @@ void RGBDOdometry::callbackRGBD2( infoMsgs.push_back(image->rgb_camera_info); infoMsgs.push_back(image2->rgb_camera_info); - this->commonCallback(imageMsgs, depthMsgs, infoMsgs); + std::vector > localKeyPoints; + std::vector > localPoints3d; + std::vector localDescriptors; + appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors); + + this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors); } } @@ -708,7 +821,14 @@ void RGBDOdometry::callbackRGBD3( infoMsgs.push_back(image2->rgb_camera_info); infoMsgs.push_back(image3->rgb_camera_info); - this->commonCallback(imageMsgs, depthMsgs, infoMsgs); + std::vector > localKeyPoints; + std::vector > localPoints3d; + std::vector localDescriptors; + appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors); + + this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors); } } @@ -734,7 +854,15 @@ void RGBDOdometry::callbackRGBD4( infoMsgs.push_back(image3->rgb_camera_info); infoMsgs.push_back(image4->rgb_camera_info); - this->commonCallback(imageMsgs, depthMsgs, infoMsgs); + std::vector > localKeyPoints; + std::vector > localPoints3d; + std::vector localDescriptors; + appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors); + + this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors); } } @@ -763,7 +891,16 @@ void RGBDOdometry::callbackRGBD5( infoMsgs.push_back(image4->rgb_camera_info); infoMsgs.push_back(image5->rgb_camera_info); - this->commonCallback(imageMsgs, depthMsgs, infoMsgs); + std::vector > localKeyPoints; + std::vector > localPoints3d; + std::vector localDescriptors; + appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image5, localKeyPoints, localPoints3d, localDescriptors); + + this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors); } } @@ -795,7 +932,17 @@ void RGBDOdometry::callbackRGBD6( infoMsgs.push_back(image5->rgb_camera_info); infoMsgs.push_back(image6->rgb_camera_info); - this->commonCallback(imageMsgs, depthMsgs, infoMsgs); + std::vector > localKeyPoints; + std::vector > localPoints3d; + std::vector localDescriptors; + appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image5, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image6, localKeyPoints, localPoints3d, localDescriptors); + + this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors); } } diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp index f56b2cc3..72cd9cb0 100644 --- a/rtabmap_odom/src/nodelets/stereo_odometry.cpp +++ b/rtabmap_odom/src/nodelets/stereo_odometry.cpp @@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include using namespace rtabmap; @@ -73,6 +74,8 @@ StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) : StereoOdometry::~StereoOdometry() { + this->join(true); + delete approxSync_; delete exactSync_; delete approxSync2_; @@ -407,29 +410,46 @@ void StereoOdometry::commonCallback( const std::vector & leftImages, const std::vector & rightImages, const std::vector& leftCameraInfos, - const std::vector& rightCameraInfos) + const std::vector& rightCameraInfos, + const std::vector > & localKeyPointsMsgs, + const std::vector > & localPoints3dMsgs, + const std::vector & localDescriptorsMsgs) { UASSERT(leftImages.size() > 0 && leftImages.size() == rightImages.size() && leftImages.size() == leftCameraInfos.size() && rightImages.size() == rightCameraInfos.size()); rclcpp::Time higherStamp; - int leftWidth = leftImages[0]->image.cols; - int leftHeight = leftImages[0]->image.rows; + + // The images are what the local features would otherwise be extracted from, so a frame + // that brings its own can leave them out -- which is nearly all of the bandwidth. It + // then describes itself with its calibration alone: how big the left image would have + // been, where the rig is, how far apart the two cameras are. + const bool hasImages = !leftImages[0]->image.empty() && !rightImages[0]->image.empty(); + + int leftWidth = hasImages?leftImages[0]->image.cols:(int)leftCameraInfos[0].width; + int leftHeight = hasImages?leftImages[0]->image.rows:(int)leftCameraInfos[0].height; int rightWidth = rightImages[0]->image.cols; int rightHeight = rightImages[0]->image.rows; - UASSERT_MSG( - leftWidth == rightWidth && leftHeight == rightHeight, - uFormat("left=%dx%d right=%dx%d", leftWidth, leftHeight, rightWidth, rightHeight).c_str()); + if(hasImages) + { + UASSERT_MSG( + leftWidth == rightWidth && leftHeight == rightHeight, + uFormat("left=%dx%d right=%dx%d", leftWidth, leftHeight, rightWidth, rightHeight).c_str()); + } int cameraCount = leftImages.size(); cv::Mat left; cv::Mat right; std::vector cameraModels; + std::vector keypoints; + std::vector points3d; + cv::Mat descriptors; for(unsigned int i=0; iencoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || + if(hasImages && + (!(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || leftImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || @@ -442,14 +462,21 @@ void StereoOdometry::commonCallback( rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 || rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 || - rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0)) + rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))) { RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)", leftImages[i]->encoding.c_str(), rightImages[i]->encoding.c_str()); return; } - rclcpp::Time stamp = rtabmap_conversions::timestampFromROS(leftImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(rightImages[i]->header.stamp)?leftImages[i]->header.stamp:rightImages[i]->header.stamp; + // An image that is not there carries no header either, so a frame that has none is + // stamped and placed by its calibration, which is all it has. + const std::string & cameraFrameId = hasImages?leftImages[i]->header.frame_id:leftCameraInfos[i].header.frame_id; + rclcpp::Time stamp = leftCameraInfos[i].header.stamp; + if(hasImages) + { + stamp = rtabmap_conversions::timestampFromROS(leftImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(rightImages[i]->header.stamp)?leftImages[i]->header.stamp:rightImages[i]->header.stamp; + } if(i == 0) { @@ -460,7 +487,7 @@ void StereoOdometry::commonCallback( higherStamp = stamp; } - Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), leftImages[i]->header.frame_id, stamp, tfBuffer(), waitForTransform()); + Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), cameraFrameId, stamp, tfBuffer(), waitForTransform()); if(localTransform.isNull()) { return; @@ -488,7 +515,13 @@ void StereoOdometry::commonCallback( } } - if(!leftImages[i]->image.empty() && !rightImages[i]->image.empty()) + if(hasImages != (!leftImages[i]->image.empty() && !rightImages[i]->image.empty())) + { + RCLCPP_ERROR(this->get_logger(), "Odom: camera %d of this frame has images while " + "another one doesn't (or the other way around)?!?", i); + return; + } + { bool alreadyRectified = true; Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified); @@ -602,62 +635,76 @@ void StereoOdometry::commonCallback( shown = true; } } - cv_bridge::CvImageConstPtr ptrLeft = leftImages[i]; - if(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 && - leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0) + if(hasImages) { - if(keepColor_ && leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0) + cv_bridge::CvImageConstPtr ptrLeft = leftImages[i]; + if(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 && + leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0) { - ptrLeft = cv_bridge::cvtColor(leftImages[i], "bgr8"); + if(keepColor_ && leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0) + { + ptrLeft = cv_bridge::cvtColor(leftImages[i], "bgr8"); + } + else + { + ptrLeft = cv_bridge::cvtColor(leftImages[i], "mono8"); + } + } + cv_bridge::CvImageConstPtr ptrRight = rightImages[i]; + if(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 && + rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0) + { + ptrRight = cv_bridge::cvtColor(rightImages[i], "mono8"); + } + + // initialize + if(left.empty()) + { + left = cv::Mat(leftHeight, leftWidth*cameraCount, ptrLeft->image.type()); + } + if(right.empty()) + { + right = cv::Mat(rightHeight, rightWidth*cameraCount, ptrRight->image.type()); + } + + if(ptrLeft->image.type() == left.type()) + { + ptrLeft->image.copyTo(cv::Mat(left, cv::Rect(i*leftWidth, 0, leftWidth, leftHeight))); } else { - ptrLeft = cv_bridge::cvtColor(leftImages[i], "mono8"); + RCLCPP_ERROR(this->get_logger(), "Some left images are not the same type! %d vs %d", ptrLeft->image.type(), left.type()); + return; } - } - cv_bridge::CvImageConstPtr ptrRight = rightImages[i]; - if(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 && - rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0) - { - ptrRight = cv_bridge::cvtColor(rightImages[i], "mono8"); - } - // initialize - if(left.empty()) - { - left = cv::Mat(leftHeight, leftWidth*cameraCount, ptrLeft->image.type()); - } - if(right.empty()) - { - right = cv::Mat(rightHeight, rightWidth*cameraCount, ptrRight->image.type()); - } - - if(ptrLeft->image.type() == left.type()) - { - ptrLeft->image.copyTo(cv::Mat(left, cv::Rect(i*leftWidth, 0, leftWidth, leftHeight))); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Some left images are not the same type! %d vs %d", ptrLeft->image.type(), left.type()); - return; - } - - if(ptrRight->image.type() == right.type()) - { - ptrRight->image.copyTo(cv::Mat(right, cv::Rect(i*rightWidth, 0, rightWidth, rightHeight))); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Some right images are not the same type! %d vs %d", ptrRight->image.type(), right.type()); - return; + if(ptrRight->image.type() == right.type()) + { + ptrRight->image.copyTo(cv::Mat(right, cv::Rect(i*rightWidth, 0, rightWidth, rightHeight))); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Some right images are not the same type! %d vs %d", ptrRight->image.type(), right.type()); + return; + } } cameraModels.push_back(stereoModel); } - else + + // The left images of all cameras are stitched side by side above, so the keypoints + // of camera i are shifted by as many images as come before it, and their 3D points, + // which arrive in that camera's optical frame, are brought back to the base frame. + if(localKeyPointsMsgs.size() == leftImages.size()) { - RCLCPP_ERROR(this->get_logger(), "Odom: input images empty?!?"); - return; + rtabmap_conversions::keypointsFromROS(localKeyPointsMsgs[i], keypoints, leftWidth*i); + } + if(localPoints3dMsgs.size() == leftImages.size()) + { + rtabmap_conversions::points3fFromROS(localPoints3dMsgs[i], points3d, localTransform); + } + if(localDescriptorsMsgs.size() == leftImages.size()) + { + descriptors.push_back(localDescriptorsMsgs[i]); } } @@ -669,9 +716,30 @@ void StereoOdometry::commonCallback( 0, rtabmap_conversions::timestampFromROS(higherStamp)); + // Features that came with the frame are used as they are: the odometry then skips + // detection, description and the disparity search that would otherwise rebuild them + // (see RegistrationVis, which extracts only when the frame carries no keypoints). + // They are dropped rather than trusted if the three of them disagree, as using them + // out of step would silently mismatch keypoints with their descriptors or 3D points. + if(!keypoints.empty()) + { + if((!points3d.empty() && points3d.size() != keypoints.size()) || + (!descriptors.empty() && descriptors.rows != (int)keypoints.size())) + { + RCLCPP_ERROR(this->get_logger(), "Ignoring the local features received with this frame: " + "%d keypoints, %d 3D points and %d descriptors, which should be the same count " + "(or none at all for the 3D points and the descriptors).", + (int)keypoints.size(), (int)points3d.size(), descriptors.rows); + } + else + { + data.setFeatures(keypoints, points3d, descriptors); + } + } + std_msgs::msg::Header header; header.stamp = higherStamp; - header.frame_id = leftImages.size()==1?leftImages[0]->header.frame_id:""; + header.frame_id = leftImages.size()==1?(hasImages?leftImages[0]->header.frame_id:leftCameraInfos[0].header.frame_id):""; this->processData(data, header); } @@ -715,6 +783,27 @@ void StereoOdometry::callback( } } +namespace { + +/** + * @brief Collects the local features one camera's frame carries, cameras in order. + * + * A frame that carries none pushes empty entries rather than nothing, so that the + * per-camera indexing still lines up with the images. + */ +void appendLocalFeatures( + const rtabmap_msgs::msg::RGBDImage & image, + std::vector > & keyPoints, + std::vector > & points3d, + std::vector & descriptors) +{ + keyPoints.push_back(image.key_points); + points3d.push_back(image.points); + descriptors.push_back(rtabmap::uncompressData(image.descriptors)); +} + +} // namespace + void StereoOdometry::callbackRGBD( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image) { @@ -730,7 +819,12 @@ void StereoOdometry::callbackRGBD( leftInfoMsgs.push_back(image->rgb_camera_info); rightInfoMsgs.push_back(image->depth_camera_info); - this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs); + std::vector > localKeyPoints; + std::vector > localPoints3d; + std::vector localDescriptors; + appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors); + + this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors); } } @@ -750,14 +844,18 @@ void StereoOdometry::callbackRGBDX( std::vector rightMsgs(images->rgbd_images.size()); std::vector leftInfoMsgs; std::vector rightInfoMsgs; + std::vector > localKeyPoints; + std::vector > localPoints3d; + std::vector localDescriptors; for(size_t i=0; irgbd_images.size(); ++i) { rtabmap_conversions::toCvShare(images->rgbd_images[i], images, leftMsgs[i], rightMsgs[i]); leftInfoMsgs.push_back(images->rgbd_images[i].rgb_camera_info); rightInfoMsgs.push_back(images->rgbd_images[i].depth_camera_info); + appendLocalFeatures(images->rgbd_images[i], localKeyPoints, localPoints3d, localDescriptors); } - this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs); + this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors); } } @@ -780,7 +878,13 @@ void StereoOdometry::callbackRGBD2( rightInfoMsgs.push_back(image->depth_camera_info); rightInfoMsgs.push_back(image2->depth_camera_info); - this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs); + std::vector > localKeyPoints; + std::vector > localPoints3d; + std::vector localDescriptors; + appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors); + + this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors); } } @@ -807,7 +911,14 @@ void StereoOdometry::callbackRGBD3( rightInfoMsgs.push_back(image2->depth_camera_info); rightInfoMsgs.push_back(image3->depth_camera_info); - this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs); + std::vector > localKeyPoints; + std::vector > localPoints3d; + std::vector localDescriptors; + appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors); + + this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors); } } @@ -838,7 +949,15 @@ void StereoOdometry::callbackRGBD4( rightInfoMsgs.push_back(image3->depth_camera_info); rightInfoMsgs.push_back(image4->depth_camera_info); - this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs); + std::vector > localKeyPoints; + std::vector > localPoints3d; + std::vector localDescriptors; + appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors); + + this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors); } } @@ -873,7 +992,16 @@ void StereoOdometry::callbackRGBD5( rightInfoMsgs.push_back(image4->depth_camera_info); rightInfoMsgs.push_back(image5->depth_camera_info); - this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs); + std::vector > localKeyPoints; + std::vector > localPoints3d; + std::vector localDescriptors; + appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image5, localKeyPoints, localPoints3d, localDescriptors); + + this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors); } } @@ -912,7 +1040,17 @@ void StereoOdometry::callbackRGBD6( rightInfoMsgs.push_back(image5->depth_camera_info); rightInfoMsgs.push_back(image6->depth_camera_info); - this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs); + std::vector > localKeyPoints; + std::vector > localPoints3d; + std::vector localDescriptors; + appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image5, localKeyPoints, localPoints3d, localDescriptors); + appendLocalFeatures(*image6, localKeyPoints, localPoints3d, localDescriptors); + + this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors); } } diff --git a/rtabmap_odom/test/bag_playback.hpp b/rtabmap_odom/test/bag_playback.hpp new file mode 100644 index 00000000..aa636031 --- /dev/null +++ b/rtabmap_odom/test/bag_playback.hpp @@ -0,0 +1,79 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#ifndef RTABMAP_ODOM_BAG_PLAYBACK_HPP_ +#define RTABMAP_ODOM_BAG_PLAYBACK_HPP_ + +#include +#include + +#include +#include + +#include "test_data.hpp" + +/** + * @file + * @brief Reads recorded messages out of a bag, for tests that need real sensor input. + * + * The messages are read and replayed by the test itself rather than by `ros2 bag play`: + * no second process, no wall-clock pacing, and the test controls exactly when each + * message reaches the node. What the recording provides is the part that cannot be + * written by hand -- a real driver's cloud layout and a dense TF history around it. + */ + +namespace rtabmap_odom_test { + +/** + * @brief The Ouster recording in test/data/lidar; see that directory's README. + * + * Two sweeps half a mast turn apart, from a platform that never moves: the only thing + * between them is the mast's rotation, which TF describes in full. + */ +inline std::string ousterHalfTurnBag() +{ + return testDataRoot() + "/lidar/ouster_half_turn"; +} + +/** + * @brief Every message recorded on @p topic, deserialized. + * + * Returns an empty vector if the bag or the topic is missing, which the caller is + * expected to assert on -- a silently empty fixture would make a test pass for the wrong + * reason. + */ +template +std::vector readBagMessages(const std::string & bagPath, const std::string & topic) +{ + std::vector messages; + rosbag2_cpp::Reader reader; + try + { + reader.open(bagPath); + } + catch(const std::exception & e) + { + return messages; + } + + rclcpp::Serialization serialization; + while(reader.has_next()) + { + const std::shared_ptr message = reader.read_next(); + if(message->topic_name != topic) + { + continue; + } + rclcpp::SerializedMessage serialized(*message->serialized_data); + MsgT deserialized; + serialization.deserialize_message(&serialized, &deserialized); + messages.push_back(deserialized); + } + return messages; +} + +} // namespace rtabmap_odom_test + +#endif /* RTABMAP_ODOM_BAG_PLAYBACK_HPP_ */ diff --git a/rtabmap_odom/test/camera_rig.hpp b/rtabmap_odom/test/camera_rig.hpp new file mode 100644 index 00000000..77979a07 --- /dev/null +++ b/rtabmap_odom/test/camera_rig.hpp @@ -0,0 +1,319 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#ifndef RTABMAP_ODOM_CAMERA_RIG_HPP_ +#define RTABMAP_ODOM_CAMERA_RIG_HPP_ + +#include + +#include + +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include "msg_builders.hpp" + +/** + * @file + * @brief A camera rig in a world of points, for the multi-camera odometry tests. + * + * Several cameras looking outward from one body is the case that cannot be covered with + * recorded frames: it needs a calibrated rig, a scene all of them can see, and ground + * truth for where the body went. So the scene is made rather than recorded -- points + * scattered around the rig, projected into each camera at each pose along a known + * trajectory, which is the approach of RTAB-Map's own multi-camera tests + * (`EstimateMotion3DTo2DMultiCam*` in corelib/test/test_util3d_motion_estimation.cpp and + * the four-camera rig of test_optimizer.cpp). + * + * What comes out is the *features*, not imagery: keypoints, their 3D positions and a + * descriptor per point, which is what a camera driver doing its own feature extraction + * would publish on `rgbd_images`, and all it needs to publish. The frames carry no image + * at all, so a run that recovers the trajectory can only have done it from them. + */ + +namespace rtabmap_odom_test { + +/// Descriptors are matched between frames, so the same point has to keep the same one. +const int kRigDescriptorSize = 32; + +/** + * @brief Cameras looking outward from one body, and the points they see. + * + * The cameras are spread evenly around the body and mounted on its rim, so opposite + * cameras are a real distance apart rather than sharing an optical centre. + */ +struct CameraRig +{ + int width = 0; + int height = 0; + double fx = 0.0; + + /// base_link -> camera i, optical frame, in the order the cameras are published. + std::vector localTransforms; + std::vector frameIds; + + /// The world: points in the odometry frame, and one descriptor row per point. + std::vector points; + cv::Mat descriptors; + + size_t cameras() const { return localTransforms.size(); } + + /// Camera @p i as RTAB-Map sees it, intrinsics and mounting together. + rtabmap::CameraModel model(size_t i) const + { + return rtabmap::CameraModel(fx, fx, width/2.0, height/2.0, + localTransforms[i], 0, cv::Size(width, height)); + } +}; + +/** + * @brief A rig of @p cameras cameras in a world of @p numPoints points. + * + * The horizontal field of view is 360/cameras degrees, up to 90, so the cameras tile as + * much of the circle as they can without overlapping: a point is then seen by at most one + * of them, which keeps every descriptor unique within a frame. Two cameras seeing the same + * point would put two identical descriptors in the same frame, and the ratio test that + * accepts a match only when the best candidate is clearly better than the second would + * throw both away. + * + * The points sit in a box wider than the trajectory, minus a hole around it: something + * closer than @p minRange would swing through a camera's field of view, or behind it, + * over the course of the run. + */ +inline CameraRig makeCameraRig( + int cameras = 4, + int numPoints = 300, + float boxXY = 8.0f, + float boxZ = 1.5f, + float minRange = 2.5f, + int width = 160, + int height = 120, + float rimRadius = 0.175f, + float rimHeight = 0.05f, + uint32_t seed = 7) +{ + CameraRig rig; + rig.width = width; + rig.height = height; + // Half the horizontal field of view spans half the angle between two cameras, capped + // at 45 degrees: one or two cameras would otherwise be asked for a 360 or 180 degree + // view, which no pinhole model has. The cap only widens the gaps between cameras, so + // a point is still seen by at most one of them. + rig.fx = (width/2.0) / std::tan(std::min(M_PI/double(cameras), M_PI/4.0)); + + for(int i=0; i distXY(-boxXY, boxXY); + std::uniform_real_distribution distZ(-boxZ, boxZ); + std::normal_distribution distDescriptor(0.0f, 1.0f); + + rig.descriptors = cv::Mat(numPoints, kRigDescriptorSize, CV_32FC1); + for(int i=0; i(i, c) = distDescriptor(rng); + } + ++i; + } + return rig; +} + +/// The mounting of each camera, as the rig's driver would publish it on /tf_static. +inline std::vector cameraRigTransforms( + const CameraRig & rig, const rclcpp::Time & stamp, + const std::string & baseFrame = "base_link") +{ + std::vector transforms; + for(size_t i=0; i > keyPoints; + std::vector > points; + std::vector descriptors; +}; + +/** + * @brief What the rig sees from @p pose, one entry per camera. + * + * Each point is given to the first camera that has it in view, so no point is reported + * twice. The keypoints are in their own camera's image, the 3D points in their own + * camera's optical frame, and the descriptors in the order of the keypoints -- which is + * how a driver publishes them, and what the node has to reassemble. + */ +inline RigObservations observeCameraRig(const CameraRig & rig, const rtabmap::Transform & pose) +{ + const size_t cameras = rig.cameras(); + std::vector models; + std::vector worldToCamera; + for(size_t i=0; i= float(rig.width) || v >= float(rig.height)) + { + continue; + } + + rtabmap_msgs::msg::KeyPoint keyPoint; + keyPoint.pt.x = u; + keyPoint.pt.y = v; + keyPoint.size = 3; + keyPoint.response = 1.0f; + seen.keyPoints[i].push_back(keyPoint); + + rtabmap_msgs::msg::Point3f point; + point.x = inCamera.x; + point.y = inCamera.y; + point.z = inCamera.z; + seen.points[i].push_back(point); + + seen.descriptors[i].push_back(rig.descriptors.row(int(p))); + break; + } + } + return seen; +} + +/** + * @brief The rig's observations from @p pose as RGB-D frames, one per camera. + * + * @p withImages attaches a blank image to each camera. There is nothing to find in it, + * but the odometry only takes the paths that touch images when one is there. + */ +inline rtabmap_msgs::msg::RGBDImages cameraRigFrame( + const CameraRig & rig, const rtabmap::Transform & pose, double stamp, + bool withImages = false) +{ + const RigObservations seen = observeCameraRig(rig, pose); + + rtabmap_msgs::msg::RGBDImages msg; + msg.header.stamp = stampOf(stamp); + msg.header.frame_id = rig.frameIds[0]; + for(size_t i=0; i + `box_link` does not translate by a single millimetre over the whole recording -- the + answer is known: the transform between the two is the identity. That is what the test + measures against, rather than a value taken from a previous run. + + TF runs from 0.1 s before each sweep to 0.1 s past its end, with nothing in between: the + gap holds transforms nobody looks up, and keeping them would have tripled the file. +- **`rgbd`** -- `17` and `154`, two frames of a hand-held Kinect sequence, far enough apart + that losing tracking between them is a legitimate outcome. + +In every set the left image is color and the right one grayscale, as the cameras recorded +them. + +Both stereo sets come from the same 640x480 rig, but **not** from the same calibration, and +the two are not interchangeable: + +| | `stereo/rect` | `stereo/raw` | +| --- | --- | --- | +| `distortion_coefficients` | zeros | the lens's real plumb_bob values (~-0.34) | +| `rectification_matrix` | identity | the rotation into the rectified frame | +| `projection_matrix` fx | 487.61 | 500.22 | +| baseline (`-Tx/fx`) | 0.1197 m | 0.1197 m | + +Rectification is what leaves a calibration with no distortion and an identity rotation, so +the rectified file describes the output of `stereo_image_proc`, not what the camera +produced. Handing it to a raw pair claims a distortion-free lens the images do not have: +doing that costs roughly a third of the inliers (110 against 214) and triples the reported +standard deviation. `stereo_pose.yaml` holds the rig's measured extrinsics -- ~12 cm along +x plus a few milliradians of rotation -- which the tests publish as the TF between +`camera_left` and `camera_right`, the transform the node looks up when it has to rectify +the pair itself. + +## File formats + +The images are copied as they are. The calibration files differ from their originals by one +line: the format directive is commented out (`#%YAML:1.0`, with no `---`), which is the ROS +flavour of the same file. `%YAML:1.0` is not a valid YAML directive, so plain YAML parsers +-- `camera_info_manager`, `rosparam`, PyYAML -- reject the original form. +`test/test_data.hpp` puts the directive back in memory before handing the text to +`cv::FileStorage`, so the files stay readable by both. + +Note that they carry no OpenCV `dt` field either, so `>> cv::Mat` cannot read them; +`rows`/`cols`/`data` are read element by element, as RTAB-Map's own `CameraModel::load` +does. + +The depth images are 16-bit millimetres. `rgbd/calib/*.yaml` also carries a +`local_transform` (the optical-frame-to-robot transform RTAB-Map stores with the camera +model); the ROS nodes take that from TF instead, so the tests publish it as a `base_link` +-> `camera` static transform rather than reading it here. + +## Using them + +`test/test_data.hpp` loads these into `cv::Mat`, `sensor_msgs/CameraInfo` and +`geometry_msgs/Transform`. CMake passes the directory as `RTABMAP_ODOM_TEST_DATA_ROOT`, +pointing into the source tree: the test binaries are not installed, and neither are these +files. diff --git a/rtabmap_odom/test/data/lidar/ouster_half_turn/metadata.yaml b/rtabmap_odom/test/data/lidar/ouster_half_turn/metadata.yaml new file mode 100644 index 00000000..c24a12b9 --- /dev/null +++ b/rtabmap_odom/test/data/lidar/ouster_half_turn/metadata.yaml @@ -0,0 +1,52 @@ +rosbag2_bagfile_information: + compression_format: '' + compression_mode: '' + custom_data: null + duration: + nanoseconds: 5066021600 + files: + - duration: + nanoseconds: 5066021600 + message_count: 120 + path: ouster_half_turn.mcap + starting_time: + nanoseconds_since_epoch: 1613418430206475184 + message_count: 120 + relative_file_paths: + - ouster_half_turn.mcap + ros_distro: rosbags + starting_time: + nanoseconds_since_epoch: 1613418430206475184 + storage_identifier: mcap + topics_with_message_count: + - message_count: 2 + topic_metadata: + name: /tf_static + offered_qos_profiles: "- avoid_ros_namespace_conventions: false\n deadline: + {nsec: 0, sec: 0}\n depth: 10\n durability: 1\n history: 1\n lifespan: + {nsec: 0, sec: 0}\n liveliness: 1\n liveliness_lease_duration: {nsec: 0, + sec: 0}\n reliability: 1" + serialization_format: cdr + type: tf2_msgs/msg/TFMessage + type_description_hash: RIHS01_e369d0f05a23ae52508854b66f6aa0437f3449d652e8cbf22d5abe85d020f087 + - message_count: 116 + topic_metadata: + name: /tf + offered_qos_profiles: "- avoid_ros_namespace_conventions: false\n deadline: + {nsec: 0, sec: 0}\n depth: 10\n durability: 2\n history: 1\n lifespan: + {nsec: 0, sec: 0}\n liveliness: 1\n liveliness_lease_duration: {nsec: 0, + sec: 0}\n reliability: 1" + serialization_format: cdr + type: tf2_msgs/msg/TFMessage + type_description_hash: RIHS01_e369d0f05a23ae52508854b66f6aa0437f3449d652e8cbf22d5abe85d020f087 + - message_count: 2 + topic_metadata: + name: /os_cloud_node/points + offered_qos_profiles: "- avoid_ros_namespace_conventions: false\n deadline: + {nsec: 0, sec: 0}\n depth: 10\n durability: 2\n history: 1\n lifespan: + {nsec: 0, sec: 0}\n liveliness: 1\n liveliness_lease_duration: {nsec: 0, + sec: 0}\n reliability: 1" + serialization_format: cdr + type: sensor_msgs/msg/PointCloud2 + type_description_hash: RIHS01_9198cabf7da3796ae6fe19c4cb3bdd3525492988c70522628af5daa124bae2b5 + version: 8 diff --git a/rtabmap_odom/test/data/lidar/ouster_half_turn/ouster_half_turn.mcap b/rtabmap_odom/test/data/lidar/ouster_half_turn/ouster_half_turn.mcap new file mode 100644 index 0000000000000000000000000000000000000000..7e25ec581d27bb02cc8c8fa6f3bde5748f531fd9 GIT binary patch literal 857414 zcmV)(K#RYLO+!IYFbxU;Bme*a000001ONa4a&L1o7ytkOa&%#0ZDDX|xi776-3Xu_MAn5_Y%s=n>1vB%9 zy0b`H5|8q>+5jg#B%^|jIU931Z_e2*EeTLMwp9c@siQkaI|6qE<_>sg!|Y9Xv#q>V zX{8XL5{V=d$#;FuR%!PV6>Y069#T#$7BCKC9F*OM&;&cP5=T8py{y6R?3xl!$hH*}2RtR&7UU|(RnEZZth9t4C)-K_ zAZ&$zNUQ%0=x=q#V0RYN#EY_R1>zArU=X2J>d%S(Rw~m`I(vD-YtXiW#DfW#$u4yK zdDHJgM`k^oHS!bF3ERrf3iyB&5i`pCs?|TEOx~Q%N@-E7-Pu-dn!pO!IzGt!dCl*G zOx7UIVzyRgsK8u?A}Bif^Q2$VNrr|x3x<=ZZ3QO@`~?Lth=V`7`GYvfb05x@X;XyI z+iJ}bI1CTMqnJlAxq_&((iCe(Z7Vp3BUJgwVJC;3EH~op6}}FLwiTfUa2cTuKGyhH zljHJPFD>5|v2CTN1e^wGM{-MYE5GrbRnzv34cS(Kpm5usRAQjSKzTx_vz6@wwH+Ys z2K)wD28i09HGd##*{#J{F9GVhz!h@1DL((~=~sN_Hs@I_tqQabI{DKEZ}trqqDfPxO_Hs);YrM&vtJ3g4aMW006n{@a$8EIP_+#XB%L;KV!@U%>=Uq ztFx*I0)ab&vIN&nz?5J_f{`47#@9|Fn%Hesm4i7zLNbQn&z$}-1bIyB>!A^Xcw0To z1a~+z);88Qmc?+KH9#T_$5!<&z##l!rG-ihmBY5qT9Js`wn{<~Jc3CYTo<@5`9Y7f z5B6MG*s7o;*o2J_X*JSnWCse)0?>Pv?6&%M1@lpe8yhh;B6|t>TBwAY2U~S~0I#6) zgK+52Pkso8a+iXyflT6wKof2MJaZtg-1rCWte}4{L3(NR+z7gR3Pr{?63!3 zoYGRu`m>*3Eh}S1jLV9Knp9MyWkgdz1%^`MSB{6P)ct5eIfGyTAtJ&s}~t zC|Q8mS@o~b#CE!&!H-ygWs%AvWveb<3$fS|w4G<@V8}2A&?wO;b5+>aM=StM*v>L5 z969IFSc$O`8335CY+9gU+|D>0;3A)7K;Q#`Po9eK71n_?1Z=1D4?LOWC-)`ym8Hm> z%@WiTvYl}7U?k8zwghYmvNP@LCKoJrY-gkuu#!k8XHtJ&@}EiNEd3g2P)fv>Lg5Bi zu1>?#-S~eWvjU4+~Ne+esO6obyZ-(HCa(wmD zqb=_#tvc|WHy57C{A>u0Xw+9XJs4f?DYZ7(&Ja~ilbWU<%JEf=j}sX7)YJkvQxc!G zZEd@ud^Y>ogl13O39M(x1q^H$*sjUrER`ajygem{0Pch2eJ=D<=g*{i zMKGXz2rU3wfDiKV)yoesQTDTr30TmckLSn#ZO{ctecb~Dg57=^@c%=4X}$n$M5 zoh@;sjcZp;5fcNbqg5fXLgLv-e!ZLoFlbjDNsOypFt{r9Z_v9MOf!*z%fccmOXCGM7d3RQhHL-)X zC=*3Y6g>=@vlggidhM(}%ot2dF-lmJu!9*pOGm}Yk&QLsF<2YMNG_3Fe#z#n8kGWG z_5d@P;;;o!cx>|6{1>*fT$Veowt%=fao7q} z0A>Lm<-*rKC|#6vC_$p)2njSq*o&}tSMzHgs0)^2GBp8etQz|#cwcKk!IDRNip&s8 zm>~FE*90M!_Sco5i^ff{iO)`z?4V#z(+MMSqzm|?0LHvfVSU{vP z{XmT22FqwowWgP(^bKRd6G~(HF(;O5B#D6(22viWT zVhl;>c*%Lm-I4L@!3?UQHBosIYseEufg}Zz-uUlKF@B6_O@Bbec6HWhVbH>G#vtD= z8M`K0QwGVnLmQB`9BnziDD{=@OqL6p(;A%^L>o4Fk@6z_$K=-~XTB)VnVejT^@c1q z_|o9Z6{)`eXlbD>n$sYUxI_UW&p>`H2k+s2N5)>O-n5q?KEdwoFto$a+vnFoY$hw{ zP5`dNeVdQgBw3T>JEWYI@S#m-q9hsn9nwJ-LKa?!)z?jI*uwNBl`mE?0LJr}pU;Nl zz;gDj&6c086ap?@!ADUa%RZLBQFS)%vQJAA@Wy`&u)v65M6VI%Yy>u6RfX`_DOsOZnXDO^%+oBH%DiQzP@bcW}hqHJ@183LN zlyGWI16AU`GrH$9{~$b<0iUId`xMfGyoiYZGz2+Ta;zRB?CgXWRQgXKrTFg!lIJhK zWu_OZIja>@FrxYNVa0!a)aT28Cpcl**Svf+mA4{q#RucBhI;C((TMm= z;~XkZ=v~pf@)v4fYw%f9r4Qw?i+R8Z^gQGL(B^_`z7GDuQA8gSA|UQ@x#StjujlB2 zZ(j!m#ps|94cdqm_e^-^^Pfm^7Hq!OAk+y@8)D-X1HruXOy!5;bHM#|OVdlAo>H)i z84)mwDn=E*0?t>nLR*3sL?cqX2pK9yT8y--xbQWJrdpT=R6tS8IODBPK%ap3-T9is z5S%Ca&zz>Xu@BLOHy7TVZ^KtNL}A8sA1`IG;~_%NJ^m6*jsnnEJ4`_2^daX}d?eCX zot8Q+KM~^V*QUNSdfEw7jARNBgb5HPUSj$c4pWu}IuM>6@#JNM=O#aeAKxSKJ>B%A zKpP@c5;u{uAs0q2%ts{pZe+-?QtaMsttZwK7s2A|oj@-2bT1l52_yHc=2v8Ky+>aW zpI@%l6o4tV46q2w04M_v!o}AlPXK{*AU>O7OV-Xan7>QjJvjP0g-{aR$A?vH`6m{p zM3@rKL;R{`iw~Vgj4!stp+d`~mdWEe`Z{GxK{nEL5;2M`sg!y4^82y*osX}Ej3_l} zFK&Esq#`xXV*V3)&Vka`KSqb4v=&E6v7{E1#UYEsZ;<@j;wjKp3ayDHqk)c&ugmnc zk5P0{Tdvbsl18SCHZs~g!{k>;M(PI*1;~O}QZj=arX1!kaQb>qRUqgqd{pAf6QiDq z{1Ng_SNUy4hoUs7uNbHlKk>BUdCIRs@T;h=bc|f|6|z0?l&`Z5nHw^{Ex&5zM3%Ir z5(x2>KTr1bp1yW|RRdF^p#<6!TL#v7F8n!&I5f*wEjBaVWDt+o(oEXk-QL~jtiGyk zwGE0^LdPt&^aQ1%M@8=yx_k}PV~=GV`E_YWW=D3p1YhY4X;#uXdgwSZ0~;P;Ji-q5 z@Dw@s{i+Bel3(r^n2m;QA<}1|{G#ZL|Ddy3ZHes+FH`&O zdVIF=x78g?&sVcIm5KU%IpQ9Nq8#LKkmC0q8~FJq69;Wd^bU;>u@633 zo-O|ajn1{=J2K9mHPJQEDzT!7s%IsCmzgh^e#4QI#!+<4*_8N4?s_)zf5lyajPJ}j zj5&&BDk%Oz$pAqS1Vx@;(RU;r#u`y4Qlv2uJPM5nbbNo=;m{T>a>q`*2*knjs(m2t`-;O)SI-l;F# zZO~(mW}VOwM|#0{#_}UT@&KE@4W~A0KqFz;i;I-8^ep*X>ABV8_4E*p98fUOM)8pW zlAhoHO2Mf@DeB=uX|!}PWtNoZA-{k-2XN&3i(q(xs$_C8}ra1^-+9H*Dk@|5d54 z2w7yjPr{wojE2~Yb1Q=#rUCvH67u~e77ni9J1k85f8gq!cOYejNSIi-!q5gH9N-}f z7KMN?{0QOZ+$g-wv9;;#4t5wQ%(1mim}6_x!_(jvqHT`CnjW48wh(J`aC&$ezTf~4 z2?7875dHqXP?&(DQ7FR@5b=-?kx;PEkkC)?j}WkhpbEZ2!LPx>LKRkre}jO6EWS{f z_!|Ti#9$SO_$NpMF|dUoVBoLrVf#xg9Aps*1q<{3GjK{dqmfML$m(0|2vvzCVI(M$ zhWR6l88tYWj%-=DmM1cw~~PqUELSi-JmHM45SML%~38Ocu<%wly`GV{}LvMq2Ra zfT|HmAtqvsj3Dr6wx|T8F>`s2XLl8poE#EJE^Qj7^EeY-(m14kIy}?5N};8f7yxvk{4j$OlZL zAskk8sbr7MI90q(rZEN|XfnJ9UeYNIl(&H$Zce(BO6 zYjO*kEqRM`YGx&_V7H0 zba=@k#=y&sHEm#db}CgVWEic~MaBapAYM^AbN z8LbqJCHz@X$O;lRkd84532wQc3_uuuEB*@*)6>8Umaj*Vst+jvtgrYkSFxm`lt%eD^S6m7x~KxhoFNZq&`<{GROJMYuaPNlOJ-Q1i4IXBhmz5d zB#12h0uQssiU`Q;lW^xTTB%XjpfsR|tBs@_g=xm-e!xRN6?}t$H3VV!5!(3skM;jA z2n7fEY80yYUp3Z9Bd<@woySMdKUyiuV%lD#W&s&~e}aQmkt_`RB;0AEl^*4hhXTuz zNDOI^20Em5Jm^GiRKAS>oDuUU!Vm=TUq;~}A>bk3Vc~zP2Sm^+^+~wX{#XBe1bp1Y{vND5DVZP%!Z8%WCaQ z&IY33pr8sr8Hs;iUsh^sUvhznhC05?)V}0a03H(J_>u!86de5X1N{5?vI0CD^keZQ zcSu+$h)4)HgRpP~AK(fQeVzR$bO<;o$f8h&A>UzP48A`= z8GtNU6#m!x?@7krivP|Pf{1>IhkPs$)BamMX9QAKG-}+iQRB;v0-|5xAPXzRLK$C# zg@S}K01^EXJrEWOBKi%s$tXlz0VrYx;9()5A7P@^3c$lJwl+jUK`TQTeu#%(fG`C0 z(hdPv__2@yh)5{NM<|FmD43|i%IZ&$4-kf*3>JKUgMxu8zFH6p%0N7{3jC4*_~!>$ z_lJ0B^(B`9#GTcg)gj^+3KM^ThA{jPLBNnv$l{RDPllci8Hj)|3ISXE&O$8?VXz$* z%J>feu!Y~@A>bVby|fmGFwzcN9KzsZV}bY|!Ski6oB@YA#75#vREsjzqEm&scA;E= z&H%9}o-9#kz@22~w2#*~%}(6BNZ)}Nc-Qw=E83k%Uavp5M15C>-D12c3X zODfU~8e9-FY4ji~%bycHXJxtb-&qMVyn=%4#Fb!ZNL|QM7hEGhGQ2Jx#l&}boKraR z6plyH@iw`GCBDmqIbdzT+B4IW?@J4O0uUz|^Gr}6L4oHbO5b^Z1S*DfO_W@$q=*-| zLAb%EB`IH}rreO`nW2i6bowg5qX3U*C8=+lMv{7kg= zo#mtmmlO(~WqdcvYzoj6py#3YJL?G)OR7c8MvNzgz{3!m6_^#+QxpDH>tfkOx`%H^ zyrR$;tVXQHXQ8+6wu{@8v`{D>@d__A2x0{Bb5NXbI9oPJ9l-#MRnWavZB=bO1y%U| z;&K;9n(1<}UlImp0NJ^A8T4V$=lN%U)pBxKCcPwKP;3_oz!Nf0$UOVxIs4&BVxd%2 z4668!Oeo4L%Bv?I4reb+?tq<|Vews;J4XJD{5|#XIlG_4!30-rL5T09Z$j0C>Y0bc z**hx&@D++B@tvRuTS{yx@yrx-R>fTjU$B870^++lVJ6ci)1G%!oL${KC%i@GOMDlH zDN?0Km1iA4XQQr>0B{!oulP<71r~EG=ALv0&pxLx!eC-m;=9W)k&#b1gua82*p&8R z>0%X7mUG544vueEA7F$@>IO*xzg&IZ82VQGp&xp>@$sz|&Es69gHLS0KL)UU`aIb< z`qt4jQ5ez^l|Nt@5zwSad9GpnZljVCDoM+(G{J5*KxL{-^^7S+gT%=s6*IsGCi}t1 zL*?;=AHO=W?2>|Ms)NBkY33%+cT)5Rh#%6i$PqA@heAJ`XBr~kaH2jQHKk;1C*ZF- zXY}-*XEuH7sEx83(y&l?@RlbraLhzr!&u0Kx}I7YqvRT%6yv;ht$>lD1e1!&7c7Tf ztcq~iGGq(ZmMoY@uxwq!QLLP~b}6fbUAKysv6U%m*KT5zHb|i8>sIoKoNE};$=&s| zt<1cJl`uhZ?dDJd=o$u+Wbpe?I4gnPuP{JfGGlO}P)@I5uLOr)%sL#ElQO>haE!fU zXjX3-5h#WjIdlydNxXJ@L>{_!ABhxRyGek&(XY$bjhaqThA6s*i98a1>n)Cou3IaO zhSzW~FJrU$~u7seIo3nvp7#~O;h+GWrL=UqYK$(!a84w1&>*CWvArK`8Z zd*#LalL$_%^pym3-C^kMOAPUfnOdZQh~z7r?%?6&qq6m0Dq0gL70P~m);^kupT z?DF+cni_(4godtn2^kwd@N98*77JR27Wabia`R5YIS2v6d@UpaL5vr~1~9LYNr8WC zUpFCWO}==Ryj~*C7?LWaXSKi-J>R>#Ht*w=BoBUYXBRji(e`qnx_X10$)x$zoK@h3 zD8joTP{_Mzj zduW04+six)#>E8B%b|&$x7Bi2frQ;zwGD3i%~1jiR0N*03${2>cgO@Nv;Vl{Z10LJ z#ti=pJU0|N%eDcAqSsH4B?t{bzJ}s($MWqvGPxo{qOg<%2nyj}j%Jp9G5rs~!$yYm80EE1C z2OPa?B{I)AsiH(+Yb7d5Yu-8)HeRTXokRSmdQ!;xx(7p+qc_e| z9V2d4u38=gBqZ+I=#p8}(E`=DD@uTIWko1n)KcJb?E!z~)*%3wX$C(}krs zU_hMfHcJ4BYrWuUrRUmxR=K!_p8)<`yU!^a*X>g(kb-LeQlI!--lm*vqfGCropnzSs zPnuk}V4O&})_*G9Ygm)X+I6eMy>2}vX}fM0QBL_YIpNyXWF$+aA{4ISNGsPaB_eLu z?vO0ky3Qke-4b%tlBqERBStLOZk)vp*KnI3Rjyk>Qi^%sZ!JmnpwdpI+P$`s)Q&3G zZ7Zci#C0o$?EUdEei_)`oL*KMalJl8Pc zh-*EF=NfMFg73N&6m;#rfiJGzIV=!e!!TfBq!o;=;Y}(GuUl!~qw6*kUybV)1*U6P z&Iue@Q;AO3E~mX2B(exZM^YiEu3ae*iPx>vhpX#0TawDdaqV{Kx^C|jH|-K^@NwO0 z0K>}Ac&#yr(MevrRqWYLa;>{GoD`JFH9TkG+EpvK*IEvGt>K_t!vfy)s0mW8H77R> z*DhO@>(**5@B3S6nAfcpTHdu=WtQgJO)K+o4M!k*;}s#qwOa>)PHrUT;kwNN%X6*2 z9^6&<67=Qe;rne323_kaE)v)9qy>bob(?||*D!+wiLP5X1SquDp=)=5l!3nM_DzaF zBL-4gF0S=erfV3&fEU*-i2$OmTMt%$*Rb5LT?N=olNTvtZ@FmJ?_l_kAa41E&ZUs$Z z+l;j3da4?L6gSEd=C$T(v}RZ&#!ZoiYgY_WnrjW<5Z>BU&b6!A;kqT%=i1dAr1zaQ zC{Up5whf>T*Kig;npgVVUAts^7@dW1qs*I^gA1AlhLF|6;p|qP12E7vC4Bzpu>S&_ zcov;)0KgBZw?mZ~D4S*@w9E}>!>A@eUpy-+pl)sM)Y#FyZlQHJoEqZGpDY zVA(l|&Q?JIG??*fkPrq+!HLuaIUT-+f>vRvd+k8UK`rpnfdYrowfpC}|5na7cu=c9 zJFlGpDo~wfII|nGk2i3bc{)da&XI zx@YG=)XdM002}b;eOiQ?c=P3d*7|?INyY@UKp^9bm!|@10*i$Ix#~Xy=NRYNAUDu< zc<~-Ng7N_s8oQSNYR=2Q zvw9v!@f|UQ5gKCvh_*K87XfDvwIK>)Kx0(-0R0d5AL{Hf<8)MCFt}e3m-awqXmUaK zzwJMrlZ%1VC1@3VLGyZOLj;v!r$mSlPAx!A*LE&X2Hp^MIiNSB*pVRN%#z@AZ@K|y zl2=a@4XBP5gANa!SBjkO1YG#(HL;?F{&-L(MhvGFuG5itP-pHfkwOy+ZxtvSDoK=Ia zsViQ;7DJ#*1aL`^z^UZz?3G`v1+CP;2mt~o5;A82u+boV`Sy&0R(?V>@aMY!2Aq^| zUmHQ&BGq{N7zlwriHj8G2)xAz^t#m|Ip=y>CQ@IwRv`0SyD^5WLI_>AYL4r*YZqkE zwH{>D*DVSlnXcXSN8NR+=R;Fm54>x=$z#(s9B4-eE;%$^w>xlvxSl4lvFRGFtkHMf zTF%G>CK0g6A*LBJT9h#ib?sIb4qi_w33K9LjO+Hvi|6(9h+vItxD6C=-7>PsF$GZT z8m3&)cs>21f-6cN6Cr#Kz@Wgg<^PgU>z2me+4a=Sh&QeXwiDm>t zQ(U*0(M{K|6sQO>7CK$GLn?$w5+_O#j)V^ip|0I66Z)>(%C^mG*9jYoAZx#dg`^!0 zq5-&eCC%FF)=8OEA(@bCSTaJ@b&CW-lWSLsn1bsm4lpX$u962}*R7Q=%xf2m*V#4f zqk;6g?ZGwS+KpV$=6dQy!Q6E_ncxl>1a+5Mn%6F*feY8|7Xq5su4ESU8V+(r=(?pb zK<*mG5ny`lqOf|qZoyD3x^|@iaJz=dJP}>DN~9=UyG7JR*KnjWUubiTLUi3yZfdxO zt%OQ+-9mx_aSbKhP-s=72@|=o)VF>RnGE(PX-Ivz!sprA(%4_y?I0 z*Y2a3OE$s)Uc)XhJzcw%mx$LerI%1wy3~-dWz@ABMN5zySx)4Nq!KGrFcl@&ZKb8v zwJSwRl4}@G6s+skhYRJ(69v~Ul_plMVH=eVu3b1wv`8v>aNXiTQsx?tDaWqeEl1$% zR%%4-+HE5#=eh+H`QX~!19|7V9rEDX6(hvY7e}>gJsCvMwX24a!nNK4WYBfnqVf%i&1)CT2q)LA7ZFm|8i}5J4W|J?wduhUPoC9v8wT&nwTtNvs}F}SnH#ng z>%?5QP%?mB!&ok;p`?rOVvWOOOSpEMMB~7Knq9*-EZn(nxm1X|b|d5RSb>WMp`hm) zrZR!Oc9*o-xrQs*I3Q7*bM0bDv%7|+oUyrfxtQ5WCA>*PW_R7P8FO>(MnbT=hS7w* z!367|$HAQIR{F)^T3_j+b3H}UfWtMMh5;Nq3-qpABwKQ>T{j6#TT4zO%uG>*P4vCs{?S=~FL(Te>$&e?;>oyZ7P1o%v$ZPjcrcc)z5hlq+mPB2{ zd@32qk$w%U1td{I8pY_*k!u%?BLUYi9!5;AT}X?#u63Y9O|D%`h#*|Iu%RW_Et~0> zWg#Wk?ja$vYuAgQCfBVSXpP99D2{$8^x#wF8iulm0+Bpeu3bbt6N22)at(X2BXtHii&^Os21ZAMe$+E0{B*KS4x<;J!9Wd985f3o?Brf);4M269}5uGW@Da!0* z$9EDCKPWd9o2lsGrzsHkD$FXj-B|w@hxC@+6M44nBm?v8fVmL?4i_lMqEhlzha%S=ECe1e?ljNK|Xd zU)qYa6}gCxvtWCR(9KuRTGv`vmRxXF!i6_&9|aqzq8u#9E|FczM0T9bx)3RIpYc#S zG%U$p{CUn#_98Yf*FDia(4~kVbm4>&uX^FilK)9~qXG}=kLOL$XZ+VYlJ6X!zLpF#Z>x-t#OvmZV*V%k)99=v;W(ADLr%aLcW zojvgnP}ohSC&WwXP)r9Qe@66o5RwszoQ?58(2z|f$$|8FC#M&v7nl#*&hj`2wotoj zON+Dss5(1Oe_r(4dCD;y&kFv-r9zuZlT!K>eR`<+^Yr&nl?~m_CLY8AKbvY%0_oYD zx(Z1uB*}!5XWO8h*;Q$dq!q|)H3Mn}G7Pk{Ss>&D*;Q{nq#8t&`12>fK=QQ@h(S`j z3ay7!gk~ZvC0I(bi?pwB2eBz?SEVJ9{^f8&g@Fn~4t(+rYY>=1?5eCp(in;)>1nZc zt#JTGk@I)fKDSY^<+`0hG=(VprG2{}?kL&-=!8j?ctP{D^R$!sF25u5+z@+euaP>@ zB5BQ35DpF8Y!woP)D`sOYR@My&vt!71Wr5OVIieLLk}Jv9$q#<;cVM(xWcy8 zOeWnzXGz5j6*Kaln6pv7!Oe)BE%8Y)rG#8Oaq%RRuyB@3%RzvKoj+xfn&}(EB9TR6 z7IEfm6%K`cTRl)HjlaZL8o!$1XKCaR`YegN3V^mV!69iJ4RjVQ|Lo{rw9FvboK+J5 z@HoA5e*|9>ZO+iS19Ub46q4`Vo9+$BA29UO&F4y-685^o-%Mgs;sF)*Ixib5yfyGYh5-|9(s=xe7o@tqq~;nh&2 z&XaY#o>t{oJ$Iz9k>E6~d83nb4xU=_+HKpm!l>(3fn1a8R`B^(dJfbIMekf4HZO%J zVg$TN(R=>YlS0;6xfP7=rGNp@i05ZL-Kv}oaSR4Vy>DUmxDC`CoV~GO(Da?~Ax$e^ z>xuTBZCeFy?}kN1FWVeB{XBzk4ZA2Fyw~ltZCkg6UBhP>*6|EyH_J2Z!maD>w?CfY zoz+TthFjNdpWCV13VCi@7lvEK=eC{k+`6r+(s*{&56`XZ7Cvj&b=wwp)xhW0b=4Ek z8lJk@gq7>$cj>u&Ux2Zrd8qYK&*NMSPyR8pgA$YIvTS zc%Hs^hT*B(x@{}?tlJ#VI>_^qXTRsx$g|`#-1yAp+10I$=Y99trF=f~yt9Tp!)cyf z7|C<%K6%z+o~P=YXC3)GZJ9h%d47CucRaUWo}E1JdwDMM4Ee0(xoy?OXV+B;pJ90N z8N_qjGCsG5&u!)NhG$r}E}Zv#EEaF>lhlW|PLm7hSY4p3HWzA5jdz0~Pf!V?)~6m0 zTzD2;yJ49du3Z-^A4|iWsKfQeu>OIr!AZo*$67TczCkG%nlWI2Q%;AoV}~Q^t~ZT z`~rD->>^Zy6pXC=EVrtu!WV>DhT`eXL6DE&1}6jW9ULc=10)`)rt7vnkbgi#fCF=0 z5P_9Y5q34QgCA#J5__i56g#r8^0Ay}LY&PKA*TT|j(SrO1I1Ti+fdzB`jJidI$DV~4k%#z^jn3q^IgZ2o8IpNP=&!6DD zQsnHOEj*1ciAZXoKB{1PdOnohq@c%#mxSXV=#Q*j_aD&n>js=e8aPlOfJo@&>xJnB zjrkq11%)p9kYxB@Ki_b}y+$a$DDuofA7>QOk*~uUMeKBbUR?s;6J0hzhX4tq;Q5YD zcgyJz+Y@3KL5Yl@0{Nfn`H;>hWT(@i`PSYNC;~x=MA4d@bT+v>og(}-2En_BHU*9B zVkAitXC>O{fZPF#=}Y2K610d{86!rVOTe6t#}Qn4M{*oIUV0c@CXE@lyUg~cF$*YQo^0?4{U~oSC0xB6v+a$OPxpH z(>?jp@#xiykp`Ni5|Dq8(+Ko*a^MD>D!p}SV9+8|nOj><$vLM}GzF66{b1(<$^?d@ z1q+;!`gDzS2S9|NOtNrcz`zNa=X8$x#LmFHca-ixEAh?{AaD*@Pq(P$kYf|Hk`Xg& zWi6)=LZ?#%hw-`dYKW`=Z31BpB}z`m^>kuf33?3P4vNY^r_^cY{crO8JZF$ar&H4) z=H4o@VXBZR@tJtFwl+3%j{(_jL=yFr3}2VYh|d?6z%g-8Sp&wi$-q z*3B)Pr*P}S8g^ZG>b5NmyRfU4J-ZopTezKS*v)P0x^4?+ciOtGglE@n+bNvg7It&% z!mT@PUFh?)ZQZkHAJ6-3Zd(=Y*>z#f=UL}=+PZC<-Dz&&tlc*2+`?|_x~|+4P1i67 zCp)gEf0dxFrz;a3*Dj7FQrE6tk{s792_ylo;lD{7*YJ-RxNe=3zZ&A%Ihl*-_||b+ zR`yYxK{wrKNo|qZk|h^>w{U!YdXyeN&_r9jrZW91-}6+aOk~G549BF-epe*u96XT( zP!T{SLo)cP2KU9y{@WSozpH{VMxh;4I$| z^0g1q!KU)DEPHWZ;MLe<)d?gx`i_4g>1f^&kb%&&@gCM%PHU`Y$0_=?aLp)JR7 z{MyOT=B8@IAoU7FRTdzZ1$drrP`jxTwWMbi5QSL^v$Q6nbHli)^r)nALWGEY{c4A2 zv9C4Jg}xD+O3M{#oue5Ab$&JRSwS5q7Z&GiDoi%1h^{_XY^>Nwu}#?2&p8YOjwCniPL&;ghNn8NI!j6LOIkJJev8CiF~di9)jr>CRGvz>7rq)@mNXiJdW z5}e&a+2zg-JV`B;=)e#KLzF@bD8P0G+mdQ&aK(qkhvl{wltk_Eh)3QdZ>M&SN}8P$Pe|Ro(QDG_SO1=y zbb8ck;7Z$7ZBHqnDg=(uIYK9YsM9bfwyU~2NT-Z*jeoiEZ;pl^kh|)ErIgVT1(Gu) zXXFleXZvUkmj-uLbdq!uo*9c8@}dSd8+zznB}0RBlLbH?n!lR&%tO=620c}Fl?^UZ zQ7T9v>GG>Bo{@AJv}U!d7#F~*-JKs=`G;24V0Tt}4qa62sw;}{)Zv`4aKa){%fS-f zu7WZKR}Fc=c!Kdn#$acM_hnaMdBOwGu0RTb6f&o^>lA8NDXN4CP%(ir24zgq9gPs% zDtm#o+<;+{fk}oz?Hyz0uEL`R-kLDj2%`~(n)YrFnOzm91Iz%AVC2@XB6yD6%Fs|} z=iq@Lu&btHV6Vw5BWaAJxnoGtw#rCgus|J=oN^?mTtU=XIWyk2RoD%9j1|HtB{NDv zV+Gv+mu#TBNmQWkmI2} zu&oN~U<#5Ppp5v{70*yc2x>iaqO`4OsDR~EC{6;I1d`jFXXUgXSk&28Gl0O*GD5B& z*H2zMJPWA(Fl28lx-9SpGsTY1jxMWJoduDX1z=Ns;Dguq!(NfSBCXvR0{eL<5?%{c zFhRit1t9H)z75;i$qHr%@d3L8b}5$~e*Kf-Z_LhMq%hrs85Ueva4{i6Bgmc8Fv1!5 zP*U(i!B37r<7`-+)YQ%`Ak4wb(*zI`K;$v4vtD%^40leE1ja*>MM0&YW-%ORzv>>n zJ0k*tKfDl(LlB2x%#Fm(oGQQ~JY7+9%xaGLL65ToD0OX>ffrmN6PdNDyjFE(VQS}1 zXmE-fV`)@!8kOuNEp_+YYnvo_N}72`;dV5Gd9**ER!m=%8k+E{4D} zN`SRpwOtM9FzRuuHGsf3R5;P-epSIU8eKlX^>s7PNTIDB%E3E0fTfMe2E3eAJ-A@7 z)yXGt54ySu&I!(3!0Rm52O4!-{qq7=WE`XUXg)bB%vm%L1h%%CX$A)J14VQ|bU?-$ zI~xH69u!;EB!d~3R7#^tqsjxueC?!niDWyE1vkRMgn$JCmMlQ*tDF!<@N8$-99T%F zB4tp@pk%8qUk`~$46vOQO2Cj3ey}OBDdwuMv+4+Kx1DusaAXH5Y%bVbWB_2!Wc9!del&|fC;{eG2X=L+3V|vFx~R3YS!rx+sVoz4lpuSe5ONg4LG3x4K*zhK z)GP3mFpSV>WHcIs`u1*YDW?}qrIVfw5H>(~CqZY?{E{TtQWf67SAyWZsc-70TY@cR z#S5%O#R{#Ip_Q&_(OCkLH@4J>Lzv435o&Q(iwnt7vZa0`g1rzTw6L><{Y#k6QUSi< zu%#aVU`#faLn4RdmO7kuvd~1_Qc&{XFtM9CsX3`r;&c{37lADm2M$c8j}P02%=TfB zAPllC)kzaRgLO%*QLXWjLVeBS;zo!qH3|<-^9ZF-fI@*rFBpC7r?UYJTj^lq!^Y>& z;F7cb1n>u|A<|ZXtOD5^*&y^VWj|R=!EK&oL6bw1JEQy+E@VsWrw36moB@<=nc6b- zF}%>YpL9`RIgXDkM6wX+h(>+w6ax>|e)d=Z*Wsb4mPaiQo5K+Z_HzswoGByDmxnJ8 zHjxT9dJOET((}w52j+!H6c6NYlz`THmwz_3{Sh-F8-C11s_Zg2X2hpROnI zte6fOm$p<3La-yvKnw&K2p&(UuXL?lsQm=-2v34>M$0g389r0T*Q|EU)_xYa0#}*@ zVZr8t&D-(x)k2R@Z~Ljl3(jN+2N)7CB(J06D`J|TJ*5>2?sNxIoTwZp>T@)GW%J`P z%bpS=28${nilt;JFQ>=XLqImD?Wrza;v3);*D0>k;b{5_1F-{OPrbN{a|=E;KQ_O= zY5Y26anahIDpMCb?Ub~^^wkRE4aGgR<4i2|h0=m|!5bk+9433}Qyfp>fxc0Bqe2Nm zZxiUViVJKynDQo1dGmP5*%Ol*ZfpXS9O460CY+dXV&cPO8?8+sY%504B7>4qN=9j* ztVClI*yD@0Zis>cg#yjfNbr2w1l9u-T?JcIT^GH>07G{S-6`O}(2X?Gh=jz@-5v4{ z-3>~2gR}^UGIWSE(nv{3mw>3>d_Ukm_uO;#*|GLs>t0}VYbcZKvf}J!0NbmW;w5oi z8wEJaj|jZ zlS@-l(c54;%*Eqx$Bfut7e_{0d;SoFv#6kyI5OEXrE*%D!p&J-c@*=#FtlxK;xCQM zEL-nasPFB&E9-8z`4yuPU=;Qv{;E;;s8+`6Si%q zrrN4`pv8S^EvQ&D&%D6=nqHz{E?2ze!Waaz59>V&6}_V4vwdcxKg*GV5VO+OEh4|x zAh)d6OeraOy8eF1R<+8hcvkjDjw^-gD~$XEjzH%=SRZVQn{s_RyWo1r9%)S=F1$IN zPX9numT6q&^c>2#)2_jb^hVgnF`n|9{L(^}C%rqkK-LyrNki@fGH2W^Q(c4wW zGGd0O2+_f6%5J)=Lek6WG<#FSZ+<%D0jjXew97=3?-o)tU-71Mr6dmHjy{@PNq}^DUJn>P=sRjjuns{vwa$gM?D{ooC`{*yvyBxixAUq*v^*Y-K0aX#bJRq7 zUto)bZ6^)VNZ6z`MH<;IO}`uMAFSox6FLk1mwjrYWxt&BY%xzj(?(7t=jpdZKZchMBg4{1&;8?C z5{R|O>Gy3XHTaBrB9e1?B6+~wqBy3rjA*`FlYSXoVups$W@X+T3={ILgDuA%TCQ}I{(Y!1hQc&E1+zUo)4c?cM#(vEYaZ2bD z2U*X@uec8CEh+MwJWtsko5D`6Nwab7!mw6tDc^7kFA~7ypwN+^cqF^8Co1YyVvI^` zdMSMY%tzpXhThl~Tb7L$#AQb4ooVVRLAIvbBQ+yh0!3rvM zEa_{XPUIM1|5S5XHqOdWa$6Ro$Y;VTIGCr{5R7c)Jm5D7ZLuW+1$WTs1|%N4cwYdXoJ zc6Em>{iJ-)?taPfhVnVXFv=1Rg{L>!Yzo%Dbme2l3rh8YD!K&P#3QIq*KWTO2YL3I zkPK?}1xBUybKYbo+E7Q&lgEc_>Yv>4S%TTM&&BcJDgE5R0wR8Fm>7bjx}{6pEqWQV za7a9MnKCTJtj!)Xhm=|OxsHLg)9?$(bRZnD1^)pR0qqRY81Frsoolh;_ zPr^1!f9-WxGrH2omy#%6d}ut=wkOz{VS*J8N^ie#?^}x4%J3>XDX|tzRpji@*nLM9 z;B$UrK_blT4iffi9_oA3R(f9~sxmu6r?lPiJQ0zNSXdnXY>{U&`)uU!UFQkOX2YOK zJU?8E*-=zWVw;K-9+8LRTr*+(*9k6wz1`tW7AZ||xz_Mij0PuEZky^R4;P1w3Zzpivbf>CG)n+_j;I@7~ zX{7>j^IT#Gvv=Xim_vv_N3VO~y?hO*#4kQ%loaC^_%t?JAkcT%nd~=7qAwzw!T3*K z%$b~*&5RZ(VNkY!9gxcGn|%B^8NilYm6BXjt`XT{4aOxFz5;6%?!{PVrIkpulu=O< zwt7eM{b`-zE|Tzg+&P{7`PJgA>t*Nw%Xv~q`ZMmoR);^zzL@!1aKE1sg6$2;n~7on zm)lpIEy_8=A4{-3#D-`k^LbhJS&$8HXi{b3An<+V+Yi^QU|eM`C)lt8tW*K^hBWED zepFd9!rpoWl8vcti^0`S$udI+?;K1u3?eAW1qqL3w%1rjVO*psjbp_Gg>SX_cjnFz zu(8{Wsze&6(coViz18iE&#NpYSIuyu3e3dsnX&3D!+e;v9)sB8VA^AZ6E`2eN*<4S z*6+(K(3;a%uXuapSwG_Op$VG;-m9_CdtBFiRCg{by>0@FuAk#M?mv}1?uVJ})!`4E zQ^Zuy@icUwU-1&?EPSaqpxg+kKAMj<_gm5aXja&5uypV5*KbJe4>DW#r1ZC{j;i;> z|9Ji~vf4nTZ=sfaz_6npwzTk?$;fK)Fu)Jbp|;aBfU>Cjh+@!ii6?;4Kj2OE;nGdJ z;k{X5e`B{?boEh1eKTIYO`||J-bWb0nNIcGYvW*%al_8zy44zuB(*o`rv=@0r=teW zp}mA!-`;AZGW7<7lJ_Xih8)F&>?QayEVianI~JC4 z68{%~Stkrr5I$jIE*B?juEqOZNZa@_+R)i&;6pLhQW>m+BJecubTqrg zL)Q18tva}dec8^dHb2%?eLvcACVE_PWpU+Q4*l=WiDz$uF_SDhb6?{dRXZ#Dnhh9b z;Kv=qfHB}<$>Jk@RG|6C;A9zb%)@T~LVe4~g4;|DcY$dx@&Ka`rYj4zOACb@NKL>5 z4QSoeYqbo99mSt-^clXE=q(6YVh62Ek$Y^2dg-q0oVj|5oTS%l>CA;`IEB7-t@29j zR&FC~YqR%Qo#`mH*Y2?bJVy2I3xkY))Y#IdIs95p3tiC>^*&<|RomVj92Ys2q@$7FApg(cpnP2iGOKU$tie+xvXI&x8+Gi!t)b7yYk$xh=CEzp z-9SHZ;hOMK-DGJ>F->2jo zLtJ5YuV`g1gOl{U=F9W9Zzlrovk9>v&u+7dv*&Rc6~Xh^HMQ^DFxIpM#q_zeCYz`% z`7d!Nn&Ky8YNpFwOVTvD48;8<{Ao5PGAkKxM$#zoiJL#vf2hCVi!0bjy5_jqE=~FR zP>9#qYVvft@!-0q@5!v7rI-9*cwDB^?-|w9S)XTlF*DJm@+yG3ce^y%b&U>wg)0)F zu=Q7ayq?=^9M_)ULsq5ctaQ>hvkmi&AH;su_<-UWj~GKmWKZA`*?s_7=L(1RWWJ9i zZ_aqpGd3RO!5RnU|(8YaUnnr^p8Ihs% z`B}OY-t;hs_cc-D|_qo309P=9CIs$IczR) zD@hOWl&u+f&@HLkD<%|wUk)%#DAw^d3c1aqlsKQSAZPdICHtuSjy{n#1P@N!fd^Jp z22&E|rp%h?z1^|ys{V}V12>k;v>pbOnJ={Rv@vS$&l~6wpfFJw$+cHf3ymqYnPSy* zST!Vz4mw)*k?&z5aeZWw*&=ashKG0(m?u0OWt;EBRE3mANyesitJ@-;w3n_JWP~HI zy%wm&d*Q)1_yy9xwtS~v)im4cn8$-d7=owCRG#OcVX(suQz*H$&bwuBWbP-PHY59| zUy{Ym4PCb8Hj4VqK#C-7<@hzZZ=L&tUykYUci#*Nn5 zB9fQ-wL4Ih*SgiF64tNuP{`UC6Q9KuCAr=fjh(Y*V2aV_l8x>bJx-Dv4Ka_o^K1W_ zpGJL_vEv|;w))DZ&ZAz-b=0t85CB7Y*(9a8g;mGar@0*~;h)^TVAN^GQq*+j`#afC zFGAI&#PZ~rhs&b*w0W58PobC5eMO4fwa$EzHyqq17*XKpTb6F)Vl|R^LLiLOWG#9UBY_uO`;eJQH*FL>BU8JV z(@wu3NGY>$e;O$@&BIHlC;IGDInH0$(TwS)Y01lIb*TTFUChKgKZYG6Q zP)KKc@>|YhFNJbqe{Eq9B&Mtkq1q<^M|gJN<2^s2SHzQsI?U(B!igujW9kd>1+5ep zaj<0ZLe080<|eZoVb=6qt85*rG~qu?FoM7COy&rfmFv;b(}{~(e$?7{+0-v-Cf}bO z{$%IP=;prbF1Q&- zp@v^Rtv)9i7jU$6w9FOKylgX+da9{3X`Gw$ooCZIH%Hk|dcGqeCYs}fLSOBoKBG}z zExnqx;L|MfrA@u#B!Vq9?*&p1G>k!5+UdGLgl`axI~*N{pI+$XKt0pI>ldm3ktLe>1dn9eN;*= zdqw<#v1=e!cGX0UETunu6t5FQcGRZCQA*_FCQq^wZ`OSwR^vi7(U1i3(g7FgXdT$9)NW z2n@d9@&|O}MT6due=rN~yq=ez;rIZrBFUM|!zQ8?cLd_M#}fg} zi{lj2YEm91{ec3gp9zcE%CdN2QqT2}H`kuOGafQbk8Q}`TaE#1u?eZvh17+_y*%W< zFPI)x&&iEBnl)5ku$QcjXoB&}wpqfHF-0w2c;c)QheSb^LDrloO(2KT)Ge5TQO($g zFWRWkXgzNyRbxYT5%VrBMx(ja@|`zj!!hZ(_t0OiEv4i@U(kc$*LrhgQSHtX*2FP! znFM`2`@wzW%z`klqp7XcZ2sFUgeoinhI3vY*(ijQX0+#N8BXzr9eey`Rd=@278cg? z-_BN^9MVyEI8-DRtfBHY4o}pZtNO@I%EBBX>rKl1tNV4&C+W*Pt2J_P8UJ%wSinpr zwavv(|rG2 ze|amZ+O$8Jb3!GB43*=P>D$1ufJ%Cx#C4z6LAsL1K70iOEkV^p8fY8bUimCJq5f1& z7FYV8=>6Zh{?|+T|4~~%QDT++!&_DFSXfkbB)6F8>eo~PSP+9p{nx8 z^VP#=@@6Ht6vDE?vO~4q3vy)EU>{5_oE&D9;aYq9LOo{7HySd}EYB?CusFeGlgO$& zU@v<)&$7@$@VKuDyF2)H(~L8eux!*R zf~d0csrIHS1tvF13FhClW|c^NHVO|=0^7^``n1Lb!`na1{V+SQ|)7E$q*6^+1lt}Q*CVpJgka(H$ zzLk4S!y_0>6QbUq=Xp~f(x~03(O<(=B_cyROf)LQ-#OhMhpXg}XFwq*PRiY&ZCuE# zD@aRTPy|gFy80bXIviU!0>whxw#C4swe|tJ>5@$z@E&ns^pGh2}UfOqNsw;uh zZ{}&duowam!eQ^{3}LiZS&`s&4%sefosm6qtSPM985#eUo#pu8Id1ddE5UH1a7zx~ zg?dzU)Pu(R!9ljyxEZlIM7p?wbk?Guc>}JUH2>Qe^kWl7*4`Bw8}+?#lu)Oe&z&F2 z8Hj#H@SwR4S8lKBlkdHJKW;t~qM4 z_|lj0=|@Ts?rGN6n9t-1D-5ys^NCLfmdw1%z*ciuxg6$L@qEW~U+7@8n1wP?pH{Uw znx-r!i5w!Yw0!ToK+f@*FV4N4Q<1o(m`xO0@{xDX5X@k2yG5d8$=Z`-IBHy05t%_` zXac)-($E&(p|my3s|w>F0N)koY8l z&++S`$|Dy}Ruxtr)wOv%XdzO`G5#N`U0t4m2;?GxG6y?r8LErI)y#mYU$$+tI?Ecd z4k4{Q1W&^pkoAJ6Lhq7h%WrnwhWXChr?qCZSgdoSRQ4t2IjC&6Yp}8{7kuAiW%ug- z*0^_<5Gkikux4TrXkdP&aElY5TS9GeN%}9ao^*HYEYyPV67wr94@2-w`PV?b;Vk@m z%SzvcXvzu;U$h9D?P&TljA%^OEmyA+>igxSXnNS7>X53PI7Fj{`owBjaknZmJTlzK zYwIVmR5_KC3NFP^I{Ajw0RsimR3y2n9^2fE&0 zq);W#*UjbBT{wy9yCwtGm+S5%n8d;9IiCZ^=Cu+f`1jJLG1;oA^-|*U)z}8pej6$q z*^K^S!FX0h&9O}O(cibi@Xt|Iy^|rGsM0`W5o^$}jsQ!u?sdI?W2 z;Zkm!^|ggZ?9u9$gzm%XZfso|XXfhQ^Wo-;-#fqdqnJotX*IV?bH-jVVCs-ayFH08 zi*th2MK8q@h=(sWee23Yxwt$tp}(Qlh-nF7jbwRa`N?O21M8?S!>PB z8cQrc5zj0}R>Hiz#Nx!jgc^QES{dOm${P^^(sWwd=V>38$OOL*v(Uws`C=WZ$?|TG z!l{DeUdSlLJtY9=T_hVG*Qn?-y#2oH_dex$A+5HCaQTisfz#Vg#i^#0?rudLW&zEV z1BK5WIjRxV?n%AmeTHHxVj{XxXPc0Ja#^3-SVn2m+lj=P_irrfRK_=~ z-Vcy$`gSyK>7YD^#NYu)4n)O;EXoz!!^Fy9jDg8W& zO68CYFM?+7(NNxKM7Xv;XWG)GW}8un$dBews1@GzBCm}4g>_s=4+d*Z@A#oJreEbz z#5YBkA#sY|HQV!*KLThwS1Ros{Nr0{ngj~sU#F$*wmvRM8+GxCtwrn7Yb}UEeoM-EhhS9ZX69y|pIuvKv}1Uzm6cd2lI{(Kj3|>@E;$HN9oc&% zFNg=8lkSBXPA%a0{KB{}JR)<^d|@z7`7g)dHe$4C*vOJ32rOsWg=nN9UH2J&VnH7{ z^Y{IKE}WdD*fU}#B^TDQ`!Pzr2|tcvlWZ7omlkyj=bUf6lmg_y^LjzxO=%mUl zwwt|dXc(x9EtP#K6Q^>tearOc9fy*i3z4G3OLQZdTDnmcp(4dt2Z@w^#H`SDeR88S zjgY2|8rwt8;jrR^?T3$+Zr%$g?h6@s!SNj)nS!LBSzibD`k)qMKZ)S1(Z*qD#|)+W zkm@7;*5%jpz!sS<2(f1f{XF@27H%r<>fbho@dTT&#Tp2^&6t`+5n*Dgh8a#8VyY4> zpF5I@G%LYHy)nY`H=DM*jyOB<9LvATbDg|fQmc~1r43uStivRn6HU>6V3sA349}mV(s~^YkEHNB-X(qZ&ptNxyyH!> z-IA1c4k8cn!RB{_ac9kHG?!AuW`(pF3&yHjZV@$<-wy%LAG_n~QMK177JB&1x#Q2T z-|{CEYF`m531$k$zNs+NfvedCzRW0FVZG5Op;}d6Dd-P>mfC)zZWdZSQhlVffa_Kf z$NTPMf5KT5z-a7?m?59d_7EX?USV#`-a;_~Hi@fK`QGKu$Q3m;3j z_n#%#>9SV(Xit&c6+`-i1saaPbQR&$@^Rv{=o_h1$i&tvidp&`mZy#viyt1S%$mCd zX(86px^sa7+OyJ5z71sx%==|N7bXWM-R4?qTT0}E^up>|>p9&QaEgi`&V~3BvaW8- zL$Y0-n*L=4ao&n(M!CU&7Q!Vw*B`i}lgo@JOwVRjJo4`Z5_5!7?HG&yyEUQ9eWoTT zAvhSIzx1u*^Gh3d!lEiCslT(d&Iiv0ha7EP74lT$m=6v|qeI@aMSJr8!*_mGJ7zi#>Vl+|u_ap;^#pkI&*D z$NnImshn1_Pp7EI8l_u7@`fU!JW`4}2(5B)^W+H1l$N0I8M(EIG1a%fnVDREu)Txl z1f|D^(b2x+JLrnJa52yF($dlp9Vc0{-=17sgIDNrZrsEaX#EAbthnb)+`ewO;uA z6YvXc9^-AsPPa?~i;pqf)!$9RIqllJ3;W&b*G2cM%gfIA4(PI@s8?{mugeInd~RL!;rwm@wtyWpc*-egIImmB!Y!ZEPYyWdxB zR`)yu=Q?DjCb_oGJF~6rJ`dq@RaE;TYL1ZIvUb<}GfSOjP@njiiOE|H-X4~hB*DGR z!GrzYrS65tJt4{)8_yLvaYzCeY4Ja?qUeLWxIHN#q+`o`+$P+3XAGIxXIcHUrlz^< zwlp_;dVQhQ!nslGDw%BZzq;R}KN`)^=Y}EWja@m(0s6t#!y&PS!8WnyVdghahxPiKc1E+*LI-sC3>PtUIaZ)g_{A{D1#yafM4NgR3;(o*Q!u&+{DB6>#gz5RiXQp!^ zRIsz_{Gn4}Q2ui`4y2*K$1m@1k8Ig10;pODwuaVI;z1gbTcN2*I|aO(SwEo=7bUz% zK*55w=WQc?me|EPW<)3^>rx9zQz?-=?^Le_$qj7?I1$E@rn9MqPjlG;4C*rX0u1xd z@)yqt8Nx#`JXmXbrYQt0WMQ~zQ&SrF`5$Vc_KD^c^@NxOAbx12JP-gV(gGAn1q$U1 zZk|$E3M95=Od||CddS)n%!7l62SD-qNPs@{-x1WXo7*svKtx(hL{31SxSH1~D3>O& zZ7z)|(i5$yWl4lnferi)r9Z7e?QUG!Ft>mZq^Zsw5J2jFw+$OfDJgvdz?f6l!okP~ z86lPSy{t|5q!%_o&Id5jum)$s=uPOwFQ8;N6|PiRoXfIXr!D9m3^ZHDerEz+o=+SV zyG)9?NdZfViY@MVHp#OfL`lJu;cg+#hX6pAY5yH^ol!O9U862kRKAdB- zN)0#gX|b&W$8gcC_~6h(4Vcd)v?HT$7=0a1NQ}VpzlpgLPo@kfKnST~NhK9;4NPLR zNRGIsEzn%7nvxbN4J8!@Y7&Vu9A;di61%xW&8|Az6<-Y|f? z`b4n-zug8t&SRKT{eJ`;Jwp=`xKV)+{VoE)|AvQHpqNx$S?IBvO9dVk2tXgp)g%GF zYnB)3Im51{eh}cdHzpY|;BuW4Tbg-xcUMi;D3fQh@L-vf++~poY{E!IVjqw>&Y=a z9Fg|CpJ9+75@jWT4|?`^3>&Px*%2l|Y|I}zlT8}X7+yKopA{To=2~olf3~Dvl^AJik?pXr7 z^~7M50BX?u7Dk|3T(bd!^p#iaBVmNoq=S)?0c8J<_pv1>2!Va330H1NClT~JZ={o} zJ`#k242%*BmSzcxsVEAL;?eV^Q5RL;LOnFhaEwYBmM-CQ1!BY7$Pdc}|cxZRu}iTud&Fj)j8;@Ow}k0lsd zV)*X}M{4({7`;k^dxvAved=t=`)_G8w*X>93c}hV(s-a&IJK?EKy^n1oMB#VnHO~a zKz#WxkzqS2(i6Q+xo~0d$4?;`+9^C*i$6nXg9Y+cckELU%?+0TLWgMXe*8aR+Cb=X zTFo3VNIf%3!j$=sraLr_WZe!!S8SEu9N;iW|1`u)L33Ir*>v0HfhcrM^_ zz3x-QCtXiP06h85%Sipb5vud;Q#O|>ncDVQrcPzV!hN(8oW2f8X&4HZ)XO&CF#xXq&_B`2kHQH*K1_aI*uP!!hv=xWo$at;Y~=%86-lhQj_>amLNgGg zEN9n9jW9s(G$9YzPvYcGiYz?u%@pFJSBg*fCN=khu_M74Vh0 ztA48waE=(3L?aGJHK_Gd-c+1DhWs&F;X#-bay^2lU!#HxQ{1q^hRg9EuK@ZEFVF z3KNS#C9i&z_jI%uM@Norx)KurKeO>1av?&eIZ9}&#im)cokih@8_T`he~b?Y$=j{c zUx_mUw}hK{q+k?_XV!@2N$JE5{X*yj;K?Qw8ukr@sKRoNVFU!803#L-K+fK>v%H)j@ zq(edwSo#ZN3jv_jTD!7&k;hAaMg&x%?ov9wl5_t6&P#<4NZgl0K44Z50*kUG^x@>5 z?Ny&@GX;eoLChA2hL|*xa#fy_MmR?u`S68s==kTopjZL|3=nm`@ht!op|?@e2cx*H zQEi=*@axfB5JKDawnb8vTNuc4_yidVhVO0zX`zt$ytN6b1I8aSV#4t~AXMY=SSf!` z8$FQs9?)X;tojK-5+@V}c=?&NfHr+$l2;DgP7y7gjZ6UFRL<8{5Q2xuc}bKx69^J)^X^56rWV8RD;^IYkBLJfHJZHrhfKFwLjQj^U28GUgS~A9_^K#b-x?v-~ zs$F;qaD<-3BBQ#Tx*^B{ec7o7A*@s|43O@v4jX%)9-dpszi-_C6N3;)HcMxe0r6Gf{I*Z{t&<>k6o~ogXPW) zKf1c#09`3bA&F~ofDrhEdhqjc`v5Z;i&i2jysm-e3t;K!ZdaDGy$ehXYW&20IQ|!C z`<&V+%&&N>W_LQoajRi)TW6V`J|OYh7aM6({wtUG3kYE-{Ts?K!i?VM0G~E^!hH|5 ziPlBg&gCO_&fPnBRyG**O|Z~Jh~za9@Ly5~)bInj+~YG9u6__=IPY%#L+V;@g?8?t z7W+k+P^UjOvJ*aEw6z-b#1RM#$`uM(pKVa1vv1kwteW-ebT#Q6n3)d5HfD0G!A9mf z0nuioYd;5sh~j3Yz3lgF12}mEg`_ZgL}zlCfXPLk*!^a5FiIlZWb=D%f}1p+ZUBNb z9-_K?&(?wVN-y)Mzb~*K^2Yn!qDCyAZDyz zhC9%x3}%9)wt^tc=IY?`@h%f`Y~~I#>N;0+7yFCI&ExETx=fsRS~ZM&Ci+H zicooQQXS7Sb`J03L1_tpK3ZZRGFH{5!X6U&qn;DL1B76uBY5T^M05^8_NEED{)J;B zLx(RNkNbH4q+d1|R-TN}YS8>@@PzpKUOv@Y!;$lWpoiRJJnY>8-zD!4$IaHnDN zG5{xL^Eu#JmuZZ5VIGG4{ej-aDRF+y9%Bx> zcFkG6(t`ds2@OxP{2Eih@&X0j*=NhQbinBY1uc81Fjv^WH6PYs3D_vo&mEHK0jSAU zvmC;0d}zC=;M;z8|(jWPip-K1EbqNEffI zw#Np)tRuyC%)pKG$%qc#`ytWEK=}`X{0EXL@X=`>yu$bbz`jHH6o}5#enXJIWt44+ z#Zo;m3pX@8h%N_xj{N20Y1Vi>dO(AiGkSyf+>;~3Y!hw{Y$Y%Ldz{W?0I0Ug1;^<0 z-m?YEa##7S=jwMskP?4!pllCWM~_f@G4X!Qe_LCE(><@PT3`Ix?wK=Ge)PNv54cL& zx4bSWHnpG?r|c5esJRpTokdJ06T?+?uovpk9xu$zz`^!aO6761eC;nEPLh`qVptnx z{B=o?=WfWR+H05^EX75O9x_|pPabef^6E{mDhe(rlO|~mjA6Saq~Xf~qjV*Ey&1kg z_Pc@YG3U|wNUYtNpQ?0MYK~|Ic5d8UKVKq zF_Jc6+9?2tE4uoMli-oW`uIA@V^mnv_*4omJ!mAtrjiFjWD@m3(Rrpx5COojg}Dnx ztw|FV?i;ky=U#KP%G0j%Dipwf34LMkS3bWZjCPF4{UOj3>PYOs9Z0DppicmFk(JNJ zr{2R-)Rp-N90MnyU{sJ4mddCPAWp3sFN5Dj!y5fJciZcZPg;Id=y%p9O=-fAOEGr6 zGgBEp)xZm}s`ru~t*iKCrDtJ40a57EUM`Ia$0w&yey`{>FgeuFm1T3n%x2nDM3lZC zMz9a2^Ap$8;rk>Ge=ecx86#4TsRgea?ijFBqIiskG#fIC9t$h~JKi8BM~uge62G(kj(w1vou|DIsEda0_D`BYnX1A64N)PVz>ykNJ0p0;A?6@@Ijx zXCvy@t4_-x#8TnhAI4(;oc9+5^!BuHc3T*o_~Y){H=x-X!bt=~o(~3^%W=@)+2}iC zBip3uJ3h2^D;qq*#hgc*(FOM_>08-7;+N5KM?~q>2!dIIghTiib)`A{*U%-60}-KD zJI8i)@;XIGDj~Tfv3#G}%Z`}bE_rrcxj%s!CfYK-hU>cLUjOXxY)Ez{q%fUu)k0sN_8n z!YD8*uOJr+#8epYRolpRPtu%G#g1>`Bp`XfwdkjB$U8@eE1X@licwDqq+(kWdy)x= zzS0FtJ(@~|y2ihNOFqa1QP9^@5Keo<9JZY&ZZwAfv&#_#e{LX^s$OUP8RyV!jBQ>&AI6f=%ydj8LWSx@J*(6O*t&gpk8L1eqPYCxI{bQ|gUK z`Je-!_6HnB4Dg@f1j8<}(Cd72hr=$L6@M6zL7VT5_VgJik4beobm@PDw9EbfI3i7* zR0yY{bb=l;aNo{{_!muDmD&B0kcI;UsGR{*3xo=k9$;LgNhBCST%g|-f+GzOoT@)Z z{M+)O$4Am?QTUCc(D>96QQ{2HJUEF%2NV_fT>Rf2RD6Iy=h%y7!M`H$vLfQ&tYP$< zk38h7U{p8LB>Pm~cQYA<>+oCg_51UjaWD!-2(LD@wQgcc8Ywn>gA-yb0dNkI46OGr z-D*M({|xWQJ|en{;6=E#GC)Jh08|}@NFCt%rBXM>i$#(oQQ6DkK#~AP6)@`CfKmMg zp_8gHc@jXumvl@hAlrZ5=24E#^X?j&d-7rWTzDQ9+3}hm)?Z-sw{NMYKPd(cj{Nke zp8;Tl+ACv`0|fd5wRAp=3yYur|lLjVw4O+3020lys zppvmvFF!!-9eGKJ?7;V1nq1O~T%9+qWkbVp|lOSdWBFpoOSbOVHd6w*XW=;6$eJpx% zf_}!tW%4!{SjsWDQlU=4i-$+lM~%?G>!Ia@fKjaC;^Lo}?&&J)R0lYDY0IxH?-0~a@vCs+!6{BLI)MQZy`(Hp> z>psTc-_E@D%8B`~K`@F$=1dT1zaxDn0vBwB*lY{nk;m48Hl7+SHzLx(n-AV^N88T2=55&fbC zOfMxiN)GZnE8l#{dd;Py)#htfF)~%A9X5w(6CVEQzo^Jw^~iFD*;$l0m6!5fi<*dO zDNn2T;$(tUU;%NuwF>ir!UrU;ysKYePkzSJ8syIpitDqGPF|>XA0)8!2Xe{>+{9r` zR(CgT{Hn}a3|v^(XdEyVyKYYcFDTWwN9sr4@yai#Q<78q@#9DJ+e*x1;P&=aTFio8nStIw3>H4qq6b66c-tz{r z=sJ#g689WS35{`#VliM`bAtz@7NUCD3a+9Fej!uNyL1?mGJc+3M491gs-iCy#<&Qti)|KtkYt#XL4kq57ABwl@{bCT*` z=Su6In#hsw!+c#3N%!)joXibyUexw_ubo|G5F1IiqK&}u&*MZ%>@e&|9|O~_$zI&# z#)DQbO}~TXtNTCCOwt42B7EZCgGnjC@~nhFdVJbqpnA)O=vk7hp#whKOq4pj*#clew0#$mpv~(yJ2Mp6NG5#fNCH*GB{1D` zEoROCn70So9Gw2A&L}fwuqLz9Kn#31xsZB_W3nv~wrBcfBDahI=;lcZ7H6>Vq!z_^&iXv_OK@PVDwqfTWr)X&j^Oqb3R@iRxrP9Lvxi!A~}>NnD$i z^MLtF0|T@a3S^`aV2r&U;9>o3$^c+gl_X!P@y>vlpRD=hH(yvau`w0hh#bw+IOFUS z0Rg3wHA!(QXd~zOBO}W|E+30aU+Q zz2Iz4K#rK$*}#rwbp-3SEjDbGZnYn@$%2{P)hy7%)hh^GkA)YH9Dz31fw|oUFp5m* zvM{Iz&pE7I@k4s}pI$jklDk&bM*S7B(HM12xX{~AvXfYSuw<|*ubEBPZv&}=XJP_a z3^yVwm<;cS+`N*$qhXC7uM}yd#OaRZllxg55vB^`IE}B5s+3*e5zqf^o593@H35kG zXrKH<4~iDpXz@XHPrN3P{3^Aw+!A38x?w)AG|Ai3Y)aK4U;PKb zT;6ZAk5_Vx`~ygy2zB7j1F(LOr?X}-RSxdyrkgVuEbmph(jr}B1pg%tU=~EnS|WE; zX~k^8*mw`X`TGwdx9F@n@FtyDLl$@>(-MTC&u2Ou%7(T?ccc?%B?K9LS<8|Nm)3td z%AFNuU!`E|y;f1ZB_@_iq3=dypcor+CKQ~3_hqen2A1p-RkfEog^!lR%72FIIvr57 zm<#R#A?iTG4M*5WtPY;;9vMFalqsPG95{|kk`0ptdfeJX{Lr!LSajdMlG-Pb7O-E7 zybe0C!lQ^7M^WL6T{*cP1P)>9uSnr+244k<5glDT@i$LcGT4ex&4^xeqQUxH~pL@jwKsebzvJWw^EMOtZ>wET$$+;7LioyUj&VbfHXqHCUtk$tCgamrUl9K5 z?BmLKrQI-83+^^Z2czs>r%YWs`Pbv(X`QUuPp_-!`2d{@G2hdOt?_9EH~RAaGzLbh zvz01N!Rz>oiVW1uk~ob7hyil7ox<%L9a(XkaXR$1dQIcH%?Oaq@f1J4sa`$=rb#8H zD}JMIaJxNT8fbIupQ}fzJU21)o zfi@YK%9mq72!LKK!sujEjsai{*%6L{0;0@m$tA)7!4scI|B5P?)#v}12Y_k!h7j>3 zu)P1Ev%QcrhR?JT#|XNR?7vPGehPj@OXZ9h24K2GS}Sj>=FpkwS1i3EuGoweN4i}p zpgr1!#tJ=rknO*Y^w}sOMdRos0#AkQ^L~L4i{Jipz5>fzls4%71Dc!0MV-FHKR{fQSeKn%!UM2^)u4|huWbOt;a04&Y7JHNC?SHnPnzx}H1 zzJzLf5s29g=5!nnLKs^)qEX~c8r$1|kpns+AoEs8i=2#Kh~EH)l&D2R#ff&?B?6J# z!|goErz!r&LpoE7ANH?!h=G6B@{v#|5J5*79QEW&62s@rgosJ)BnEs^0>ERpU?-m)|8&2S zSu16x800M+jE>?c*XfZM(D`fTYQ@t`KAz_SUFf5*iJr4fr=%1Xpq!bxkqh?n zTUXy3J#JY@ljCR-0KAEpucOXPe9z z7;G+UQLkc@`HiG)OTMTuu``~$oOqL-OcwTOqKjUH(LK(2{*HL(pYC~eJUgzg><&e^ zc#O8Kk}N?CyF+nTVNr-BLzgRoaC%Kw8t?Z0i%AjH1fCF%-?l_w#^-zj>HdUYg^Tv8 zdRIJTo10sOf+ghT0yjIsW?@8AlN-INaf{afsqIdzi&-r6=-ny(>K5Z-g;vxD$@(DX zTr;;)VWY2dJLW~Cf)5L~1eezTP*Z64vqUFJ?S>%VdlU7k6bsj6y-D#bdJJi!73CEh z+uY;*u}`#pp?lCi(7-~EGpUiWePxX#v9IZGKB@bcW90emw*mEv-@IPCLyHafpc`q< z6+lSLIHJOT6>@Ne1;tz!NaLpjDWjl(}3c8t*B4KJz}QIn zv6EefYu*~*;}z%q^NLJh^wSZJ{}Iy7Ono-Ku#?{&+~IZD$9pq3NGs1{wC3@VGQXlW z^r?igXvR{ef7JWDu{(B=zK8_obTAi@U{yF=AjF*!fhYjR4bKabXD*ZL$j)#$5HiBj zR?Wh5d2qNk7hSSyjtDMRUgL7G-6n)j*g|&IVd_n>3B} z0wbx$?l*Brs8^ zxpJLnC+VR$bM0x*BRX3;X1^27eM$qNn=3G)31fw(fcYgO_-KBPp53nZIz|XgIdhF( zq%$|KLIP}O$^?mm!5w%2c(B2fKCasw&*OL0hXEY3WW_MJkq^RNE#)WAhv2(Ei#_pu z>~4785gye~W%Ns2CVzqDk$Rt<9}V!PxdbgIm^g4e!SFj~(snD%(<3Iv)X5ESv-Tj9 zj4Gelk8am{xV<`%GQ3{p?ANx^%!fSQM2oVn9&F-A7zw?1r^2&{Q&Pm91~t>?tuYKM z3!;s zn`vEl^(A|P^IdJ3ZdvDbJ@{81wuuv^QRaWDO=^wQpERmp4)vd1{D5Ydw9x6JYe7K& zInaNw#p5vl`Bwn{HospOc;etT5=s?QgF0njWYb0-LnG<*f(7Ghd<3Wa(=+W(Yth=l2f>n#w%z;^K9Ul#Th>g2^cEj%;K;YUU-Gte3QhxG6QdXHp)^^By6 zMEL@9poVm~_W8}=7jYh;;Jxs6zFd|rj2z$=C__A-Gyb;YvF@%qj~CY)eKX)0FWlxg zjMwtX9jc|cIxO{s)SCDIY|u{cvGko76l>a}TZjvzDGyvD!z1yoMRSJY^ocW?4|u@< z&Lig+U7*mT*^W?*-7{T0)VQ%Ro6GIOflXr7w;X}YSmAQMWxNufhUb6eR~KNB6_2ubpQRg5=#=q?zZ_lGqMWLwpAcK4cf zuXB{dlA}%!GDZnkd#w|I1t-vr2?H|1HJLKbs)<0=N1KZHw;!pg(*IS#a5hMO?+7xf z*?HET)FEPrWIig`gcgH@tBf^-`=< z6zlgavGMp-lV~$eOajk4;71-V?enhq2@oQEx2>yMil??1K)VwL3|kn+%zbcqhQNY>S|;bGK+0f{PgjvS22G+7(5xEtPizf3?GFrB1gnm9;(@cYb% z$qKBqKn-}!Y?Wd<&NgN1838|dK;l@#=wepZ(;Fly>##`AJQZTt*&FUs4P2jq^&|~L zcU|`1ZM5nWY6#wLoWw55aA$l&6|ND;B@ckvgz@Uv`4^hhb*9)2_g`#^k)~1~I}PKG z5$<>zDv!DsHIe`W zi;gg9D0Hhymd{wx=%{MyY+t(2fHClkz=T3~m>a}=)TrNWB%0@?M3V-~1$ZAsAU0@P z3_MI@FT~uzo{=%p{YirrAP!8fNFzgwsl#)`0?C8V2#hgHq0s0P<)>o2nBl2kw+;JK2CU`qygV>87;Cu zG=d_7ef}bWMhf%C;J6Uz^nU)dk*9_=o!eJ7_)Ys^SkvHNGx9-se2YYMBD&j6ny_4Q z@QD|#I;=zzU}8;=WZ0I3(B;L%G>7>ZIL42+SrfRT3YuX-q~tfe?4SX|xf>jzaxcQ) zxJWQV0eH?IqvXgTaf|9?LN}a);4kp^2%2}?G#PAm`XRzhRt?8u9N&?E4#oGmF*_0G z!jUpqQ~-B;;<4Z73|uh0<5xSuIF|gJVe@FGr0ANQ1E&O!#c1+y(_k+=+8HXHwv&qd zMuVCwp;-5(6V?!sjP6W;4sX$mPNXJw6G+NC zE`GObKy-_tg~_4gD|(@mY$r+w79^L9tt^6Ngv$L5FG+$0+xE>N=;FsTv z4Ol`%vbr__I+mY47X3!$B@OE-aScGLm^FFSF=M)z+3S&I@#Q?_qV$LF(XONF8G#(> zC1S$+d|VtorY=8hW^}_82fXCeR8;l^o(m)bs+};3-#1FgY#u9Y6^;Y6!q{d(N@)rRJr`ZUM_#M&v z6pJIpGFFd8GMKSY?Z`StN7k>52Q()jwwrx@{JOo$@bh_GJzA0IckG4ugr8Mpd;e7r zMoW_@XO?!^;TBxy;v!*^AuA^y=~kPDPa+8+Ltic!f>pC`>6G*xverkK!PrQNUQ=+w z+bBITiB~%)jYd2YoN99wsgH(ORBXgvh)?)Dp=tuR10id@Mml73qR$o{;afy+iiaex z+8{i+H)aNVwl~DXcb(~1#6AdG6_(vWdR(rE$CIt^?KIy$#1gf|NLOuItn3#ZH@uKe zL3!$Xq_+>T77-8WxSEPsl%|NVs!5BMKEdAbjPNbWD!mv9Az8^?2u$=)1?w zJ;sn#gBS^_Pb-rBwh)IuAdO-YtaeZujd&zD)gCNDuhDC5SVs6$-`id%eTS<0R6HbE zJ<&=sL+Y0()%h#&bXu+S3-*S1gdeSWdkZOSTZo6R3*(}~VoiiYoND$hosz!e#Ck+L zlB_<_$_BZghV>no`^uQL?q+9kg-KY&Nmos}qiW1Lh?jJusv?IO_H%B$s7-}KoYfLq zr?%bC9+L3sY^6W7H^d|SH0JI8s~(K3Buc569d}OERt15#RyVFU%&3RI|Nim+1sJ7I zE8z(1*dslpC+Quksy6H)PKs*Ka=AOW*~Li`>MdQ517vLx6=4VLdBohw&I|8{tL?sj zSC*~;5}vDQe_1-iQ;KSbk>%W|fD80MWykO8KADD3A_*ZvUoIGeRol}$+MIv(e?Q4B ztLk${#zP&EL|o8V>gb_G0&`unPe(z964OIcIjK7 zV#-wbq+cbK{w+_3EE-!trpetL=~sw%1W{7>&?FKFSL5|NRzfJQ6WaSx%cCcZPhP04>{#ex<6m&z_HCv+7{SmsV=^2j@MLh3NT zEm28qnd{57hN?eH2xS?Rj#dw(C6Dt_&Xw%^%nj^98-L3DSM1DJ|7pJ+@2Cy*JOoY@ zMa~j45SS|+tShjQ0|o|4k9Y=YT@Nd#4!Tatz+ZYYTgG7w}Sb|4nmuqAaU z!7mx@*V$+MGZHIi_TaygGSxE)!^ZH)q}zX9U)#uJF^`P*8!y-pql>9c2hC zB+P|`U%x;@b#sC^ zD@s?C??j#o7~z1GJ?RJ)e(A@h0WJo(U?bsj&YmP_um9)cl;_8k?`B%)l-tq~#jcqi zxuCe9+MUUEXDZ<&?#>@x`jr<3H$Zj+xFM0( zZcB4^9g7Uo^uzQ68WLB+HYGh&dKO)pJUV%Fu9QGwwI{V1(X~o6D9~4+Pmc*1TYC~~ zrf&s89Ap|~x;(+-YfGx5(Ypp8^FH%Fr=Zw^wmDIJcCR&mdY^iqRZ_f2v^NtAb+9Kb z2Z|ghGAPC8ksaxhv4@E{@XM3^^0-mqD9(mNXigVnvFHxTKa1q^9g-s}ww&3IsvzlO zmePH=55=%30%2$VfIAt~T_2>}2WiWa9@`NR-OEyNxrf<36yw5&ncYZ*pl&ATYcIMN zeIpQ_TFKbp%VyLDq@x9>=26I_0KZU-_aZRHp2k8_T!4SJ#pk#HGhjSc+KRe}bhSt- z_+0S0cp-zH`XokYm5o`!MrU)P1?ZLm-PF8@8}_9jtKJsp4cZ*EIfpW=dhA4yKy2jJ%(%9rQHZveeZp>Iux0Ajxdz5D#vlVSQ?06ygcpYZ1!{4F# zRQ)y*rR+s`kbQ4KfVmZUZp8;Qd(nzWy-$ntEzMgR1(0_pHzUeGeZyD8+QizRbWEwR z8%gNUtC(E^qEpWUjE3z7UK;`K#&LCC`4u!8)SZ8#6YWu~3r*dhAGnG5P{WAz+zcnO7obUTn=HoBaVCKW~ly zwog?vQbBKPp19H_=m9}ykIY`&5k@-prk1mQVXD}4rs>QG9@h;C!d1Vp;>e|ud}$=X z5fmr3qYiGzAa#b)8A@lq1%R(wI3j6nGrHiVXP_~mlSwC&41UIJ%nus6B3Fb)2y!C? zuY&2T4UXtwu@R*a=!y|0Avlo{oFD>V!X^}?sw*D4$g0d)l`kcWy#sMpyJDQ48k!pN z@!oxIKWM_bqKp6hUqTp3)42CIc+?eVFpkxY)zNtW-t0Sf=(=Kv4orPAQ=bpf_Ei;3 z4>p}DGP_iSabRht!9TepbiavM|?#mZi`jEdTj5#yr z8E*_(*@*x!=tdrODCSViaX9Bn)lL+`K}V7XaRg-^LHQ0EJhj4|E(<#mb+aq!YG%He z`SJ`3JvIIyWnc2~(3?Oxlt(S+QTqjpPnAPRVi$7K*PASAk)a+c>G>b~S{2bNoxD2R z&X%)9)$Y?Iu{WVq01jS;gU9Aeap$Qk&?}=^);L>ZF&obyzUds|>y>ZNL5X=#;uj|R zDpndrJBt(_od+IxPxPN<@p(`531xg$%1D)6g%YLCqY{PgCU-Y~z|mL#3j{k`ie0rw zK6$zFUU@MsFQsiKiETYnFG6HRWPL!*uXf!*+)JE%^~f_tn2;G0@(lp_>em>;4Krow zk%&l4cx4k_8|p+Ew6%N?>rMKUo%WxF^4U)Nen!3;W%(AjCskDSNC=r4HfY%J152Jj zn}?as4k={M1N5Iw^4SA4h#bJJ15uE!LjoN)O?K1hH%nWIWm&KEAcsaLqtVe6sJ00o zPkIbRPP2#J!Q2g3o5odHKsQfmTlZ8@){6E0g)k9F_pw z0tzp682}j7naOo#EXs`)=G0}d@j=%o)AezyV6~$n zDd>)662jhc*xR4E@2Y`9Hvp%o?#K(x8OU=6%=+5SZCv_{6qH<-nd|b&1mAtq1d!*{ zQd44oR8dAnhl)-G5M8F3ZW1~TI&A~W@F04`*1n z5yI0gXSe)c7@pb`#fse2jROY#hL;{(fLwr+A0ADuPnL8XidJz8a@>OFD)H1QDTqi+ zvq;|PIQW#wPM#n4PMg@R=#Xf2Ut;-7?8%<;K+IIg6*V2QK_aq7nXD22EgFbSBVmx} zl92>uHVJPf>*X8cR7?4Qw{~dbRSlXUq14ek4Y1IQ^G*H z?t=_un1LK-0QRzmW-74AOh4Woi@`^uk4gy3 zUbxr?fJp|x>1a3Tvh0Y8C^wKQqdjnL&E3-Hf2ejaltReZ&dF6HS)?#M9_G7+|#NZuV&)FD3e zH>D3=ZfFm+hkjCF%qU2tKKN*@$xM@(_R*4e_l>J3)~BeCPAJtEl@gWGW@@V#hCH3H z@UPL6FpL5;acJToXOh(djUOFx!;{nssTJ_nQO3`zLQk|O7;`~Qduip}{YoK9A=;QM zv1Tnog-XW|#Eeu`s%nFe$y!P2b_{22x}J1B$^6)g<%L|o5XG~Jtcgr63X4?=U58Fs zBn^rM6pL06-Mcn$L^rKmrc|BqgXRP21L-6jhGbT&C5BG8MaC4?6xJ0SRDRYqC>lHA z9CeJ&7@gM7ny1=PrPexhjC4W-m6$wY@~G`Y;Hws_fHWU_p#VHr9V#V~rI0I!e%Vf594t#aS zt0I!?JGEYTcZp2`n}oK`ou_8lkW{d604(hELIg;-TCSE(gLflYIjfa4HTWg7%OE;P zE2qzNQ4kMS6+R;Q6eVfcx;D-QG;CPbL^4vGN2DRpM$4&_DUYmsX?1vs4O-AZXkhE+ z!*tR_4J%w<9ETu;MU7meW%K8$QQZElVP86T*Mw;o+r@T`i?3?CIaaa)gnWbfK`THj zv}puAwQ2eo7Q%WaL<^Tv3ooIO@$P0~Ced%6<||#Vk=ijlUY6bR>=nLVVfWoBn7hy2 z=WH93=trCQ#|92p&S7`zp)HUSzpv6aFdxUYP!N;hA>sOR_^QLuyK;4w#CS3M#9{O} zU|BS=c#lAfI82v2-$pC$(_khR1HE*8b8c%GdUu2zp`aMB%3~e~g&P_v-c@rbV=q3V z2Tj{@HDj@YG8$quioBYdTF3XPTPT>s3&gaE&)jJO06^P#`BbwEI${Wu6vS!3^ifeo zfy5_P5D6`gdGJL=*?~pbwAmLSPBuXGIt^puS z#0DHE#6#w|c(L!*hG6nlzc1j0F%FWWOAioTUwtqLEdE2R6Z?=sNsN6l67RP`LmWwn zQ%lRvk~OY&Ud21AsQ6<4tKDGbs~fPyiK~um;v6oXy4df6paD=!{R9%@2;^X3C!*Sv zh}}S6aSbjASQ5`-_#LJto?#thc|7}FA)b0n##0lBu?!%B7M%c{#8NSR@f)HN>!2?K zT4&-{rA2tX5;sB!4FCM?8!8c;l3EaC2pq@jquPeQ_&@9j|R0FRSr zzbcV0kdc@HBp57?XX*GZ8RNtRJSyr;go~K(?<+>&`Lb+#kiKFY?|z6Xj<0fgs)-3Kaufem_fF%@~d7^fnvS`g7Jo8=865M zR?7KRs=i2C^2E}#Gx4Ug3&4JWwhzcx>-r#ZI28A7Ud4TtDqy`Pj-VYd^_yQ`ma}Hh z+URqHr;Y#mcEoak+Wjl%oWn2A)n~c;{QdFlH5)$bK8N#-XWeJvJmXpJ;n^#LXRk1v z7oUA$m~+^DcZz2$&$`dA@S9&e>;Cn_vsbz}4t`%bcknD%d=`eU9G-nS&nlk1^6T}w zVHn1=owk^?K!3*nOTmJPX4x?Ds6^SI)0j&SCb-uP}!VJbQJY`UQUezox|r#^d?@GQ?vo`o~w zv%K(G{`f4%@GPGlM8B{6n&AYhdUHTAPqRUs7!VN+=>Q9Mdu$A=eUc=h#DW9NNcqaz zDE6c31X-|B=@u0E45Ht>hZzQ6-GX>iP%(BgPm6<;y^i0sR1TQ z3D(W8HuX*FLnQXAAtm*zW8#VZC;cbkDh1?%geb=It8bOq$cX*Oibw+upZ%x_8REZ5 zBvQgcL)P%wkIG>Ft7Cnao+ya_rtpv&DoQlP{uJFGKoS?oKt`Hi3!0j`t6Iedt|&+s z5YeJypK2EB37hHDka)I%$QLjeUFetqkb3 z5wQUsf>H~UF0l@t{VPGht20tX41!qfV?`J<6c>>4Lb_;V1xxtsYhj3vL5u-4gpqo9 z+++uzeJsQ+2l3yQHK~Ux)3eV-VhVvcaIcWGLSK>rSDpDL-7W3H8%M-~i zPOt??ad0v|`v&qexNs=t@#2yo`+`hv0pi3XjM7Ln70`0(82&@o zJo|=GT63at0y`7Z9(-?F{6iI={T6{7?0{bkA!kX45Lx8p|23a|Cpo?n$cY({*wP_b zZH^p`*8(j#MmsHn0J>HGD85n+iNRJD*&PJU{gCEL7ToeI=1apF?|DW!={ zD3nfSUyLaEQc5jnbN*lI*`Jel(0M;BV^#FTgI7M%B5&vaL$RLy$QWBqi@1QCBI#rb zNEq11FyPJFBN@AEVWdGQF`s?Fs12nW=Rp!mB>`P}_Sv$HLthmW8M}=LB*ih8g=f2G z-7)yeuQ2DFyMN`}efQmmIqW{?{Caie`1Q)!$!Fc?SNGv7e7(9;{0j5y)g5#H%CDTm zZ055t=iHs*?z1qz?$@2VzE{q#i6Vbr;mzH@!tt|LmXmoFX5GKS+3Q&jU-|n1lst+h z;k0hCsR!|ZxAxD{y~bPn05YCBmB7z5OOZ|w;+b|;LSL0Q)Z#%3DXOOnX#g3(QU|b{ z6hbFe;X_9!Y(V#d-;abWkQ;5tBHE5Taf$(kBpV@1GTX4Yl{`LF2Kou%2KV=BbN* z>M~+dEIzTqsVgWE*My~;u*No&#Z3VD^#zHVNFlbGNZy^=WMjpW6WNKBiB#3Ax?GY4 z);l$j-N=^N6^|<(PpwtKAVS?p1{`b#*bJ!IBgA^bh@nmirSY`nX~~EH6|8z^#(DxP zZn{mkwTuG%hI?GCgliOaL_0lTC`u?w9RsRZAHlHc7&IL{%{|SH_>ZlQeqO!M0$!V- zO>nj5kQCSnD_b-z+E9$vAdv7n;RQJp+$OlSh33ARsdiYCc+t`e(e@T`F5+wl#b0GY zSj0L7tw}Fj~dI`wTm!S{992={q5IDW?%aEqNroI*rop&$U=&(|vf$~o{ zN7^~4sy-HoQ5!uOqr6m@C3p*p+iX za}LAT%{R>5scRm)!g<5_w}1$c?b^g`1TNz-Tmcl? z`WW=?lvt6f*e#|Ki%E=f7mSuGprG0shMqb? z+LrOehh&jrH5S;ghBQ(?QieUzc==Qwr%arnFfg{3q07p`{DQ^^A`FVdR4=dyU|fE+Obpr}K7b-m ztfc}ITlYfptA1T?3d9BOEQzPQ@q_0A(O0>?t1J#A;Uu0ysm8^{^;|>h#DP?IVkm=v zlzslNc7>s@{(aUVs<;4>J#iC8J~b6UzDjrHrYZj0N)RV`l!T>2k@;2E>VReJ=eT1e zy9E~)+}2<0hapgGq?7FH%TP+IGIpO3ihVo) z%sUV?Ba)&o)%r$~}1CI)+wd5iaAD2h*T@sb})172QY z4B`PGEG$L56V`Eo1&R*mP>;Ud=iLK80zn$G$BTkV25Uv+%@HXk&_ zdI!eh40&;~@@JvYE0HL+APJz%=f@HeDTkkU&rTXskU|Fw3q&Ncc<)h%SOS_35FBub z2}B7IL%8m-%5T*giufu8J_-k72bR|IPxbvCBW9dP#^UMi$uR;oJu;l4m_)2s$3<)i z7llr`95W}zgL4@RZrC#n6pD(^_{~H^XptKl99U-q)9B?taesnqfF!Iec|LXW0FmuNlrf!&jd9n={`V&esfQ zo->DYhBJJ1pTF7t3}^Q<&v1tG70#Jw7|!mypYxlq`IlyvXy^J^6hJM#vl%qQz-od>{t1(M)Wa)q>K$9l9K+}F+-J~ zYQdrgZ1LY9m;On~k-us}-=ar_V!tyoqy~C~k;*T%SAa8#bkINpIG=xN|3%kG2iZJ6 zef|Iq*u+&cX;Q+^)+xVJwuJChNRS-kYFh`X;bl~gmkj0P65*>~-}lIBC8i4Dk}4>1 zC@O!Hm?j#KxQf|I8{mlM4{#+Z%`X1iGbfc`p;CbS0jhYT_>XK-N+E%S&o5>6hXsrD zLY%Fv4m@xYSKF6fSRs|a>W@l=B+anlukv3p(6kWmK73Ne$*0ecbs^18ytB|rHyG%{ z`utb<$yXnx9bK`sNdoBxEoPseD+Lb+an(W_DG3#Gpo(0Ghz)3PBV};|k`*9WYOw(< zJyOO4S7G_XaJ*^;W4}g)QpPb?pMOm9tuiS7YvUn($qKw%ez!18LA2rng;dfDFzS%MgLF>`*^C`Hib;FaQA79-mHhmKPOo(809VXl%!$;;)*ZS0 za$Q~_3*y7H1=7g42W0ulvJlI!K4Ksg4|au-^4RP1%I}qfnO`%U;p;VXo_Xf^dWEz5`8ov7aK2_Z^9*M=!x_%`%V;hp#}S5ka@a@4jVE+{_ebn#TzXru@$)erC#+m1JD@&6KH}$w?*xnTY@t z%bRz3oeGy7VmVzyd7<@eJ)u`1ybEVAc={LfT5982&oVa1gueWp+Xu>39;y;w$yGg_L;$jGEJFIH;g^? zJ@r8+8J{}#C}cXF9{weH8z)3Z6uXBv~4#z29%c=sw+7H5Bo zcrw-cF(j%4`orp79u_m&moB%aPFI!yh>!t9fcALtRH)0d-Km?TlrG50YzU}i*Sk~u zt4fS*X^aZ#^)M`RsLwoK^EIQP;rRQ_aL(@3zG@Ph1oTB%lZAbW>KFSdwoh-K-ar}G zp6V91gV6r0nv7MLF^UxtD{@HBju?oR9~cWT7El&$Pt_BK18ZMmBqXMSBy#N&s$$1i zxv&)XXY3ZISzsYeFHH}cg6^wQVpBr)XA&LpwS!X~Vjbe1m<|~3vz>`68CaADCJ#*E z2r2GIL0ADGX<*m0U7w0YLEwHGn>90YfX3D}XP=a0m*@8JGF;7W^Chqs^ddVQNmVt`UvL|cV15IG&{RHf>8V9x~ ziZ1J#Jxp2dN6uyS8ta`cs&tKXO@cE@G?!Je!RkkbjX(td?2pfZ2%v_AUqyk| z6~JYyOtKOR!d56*C&4RRJ}C=&OQT?M)mv4=TiSq6JjJ zOchL&z{u5On==s+KuAeQ$zmoDgVxXKD z2LwC{l-jP)0x~#N4MBm+46yz6XK>3n~jLmA4Lqo!c}b1fT9V zNHUOQ)YNTY-6k~%oJ!dS!vYKoB9yD0Hv9>SeRpAC&G^n&$|O_@ z4hqW7n1Q=(c~tqhEtWN3dnA;eL5z#Pmnm?eWF!F?%D9lqjaI~0-+g^-9Vfnj?_ z$mA_B0r}za!&4YX!EJZ8c*0*KViA@YmY7A3PnGStM~gQS0U+jQDR(39&LCG@1|}gd z2VM?fIZ5yCTx7y$un{DtK}-V_>g=gq<^~noorosz8c!(%gb)w{>SX1qoGuL2_B3#a z-b&)Lo(4otaa(4lf~JQeRTgsM=GH*p1NJo@6ID z531u@9{MsemI{=sTu_} z3RMc5z-&s>GThId2Qn=(tsaFqLhMO03~)ezd)D~Y_+HM7r(P-a{IVrk+59>bciB^EI3=y?!;| zYrogJ`Wo)_tFvBnE%$mWdi~}vuV1~j*09#F<{H-8eXU^)Ywb>HuHmcJSFK;o4Y7vg zptelfjx6Dr=9x5x$pw=OsFyA8UIt)7X+Lt5Hmwsw@~P{o3)&^YQ{|w@5bjJbQ>JeA zk1Mw;H|Uo5sY)`L{!Gt2LfAT@w~j!yFnB7OhqAlcnGv8D$Ii^^y{E$}oXCGfnD?qF4`D)|qq^Dssimg*A zAfg;P9y&s)RQPJ86PG=^5g!oKN;nXPPliuWDS*DZ#A(XrcI0Gf3Kt;ZdT>2Jqj30Y zjknR3KvN=3RXP%MB%n_Wef98!lRmriBJ= zvgoO?6O*d#sL+(DjW3-}8=y>9JXLL)WItkaGIjLOMb(*lIAe2XUTK-Kcp^q?Sabbq z{nYfIpI$#TK_?lXnpfv-JDP-NiuUD1wMbJf`cY_eXX1sKc4;_-Mdm*jdW}T}3dF@z zw`keZC_6Z|!2a`}*IQuFA68FQ1Hcm8&J;p5Weu(AxYt}S)eZnoF+0=A7-=IiN-p+a zFL}+yQsp>x3GU3!M^Zdr90UOT*J54=0Hj86YT&pZ#rTq1H3+DJ@ZSc$u7VH*E>QX; z?9VB|e=Fm)Qvx+4@P-uTC5`lB0)XVd4e=TP5-6nDQ@^li*pLKaDT%L-{%f|;9otS+ z!Tl(bPr5M^w0B;=ZJpOq0Izq;=k*)q_1ofjz1t$MUl=cHZ2s%`b>4jAA@PT}tEa4g zJ>&ILmfh0{&K-%!T&zRVnbee)nmR#7FRqL#z>@N>@m^y|p*rH$yxqCM8=lfrYqHq!u@;e?8!}n-v|@m87kS zc)^+xE$O!YHQQ^tEkUR_C0lcmh*io1r8h|bI_&iuBt59VK3j7{q}9vO1>4!bW_-YxVm7Tdmh`2AbjR_e)^`u_@izu)bCUP8$BT znb%Glpckk-)hs|3!@U_}fOW>^r3~n`mjgUe(Y`kUomn9b|CnX|vzf2WGNBZ*e3dHd ztJ2yK(004Bt+*f^k)V8rV=L}kfQ0`s0a^VVpqkdG{oN6(wM{*;?LWqTu zd{mvf&AGtL%4C5E$32dFK8g{b%~=ItjnYV^34kVIFZFP9_IO~uvZ~#mXP>-I!o};Q_Jr7+F`Bi^p?C62 zPkvGDDYMSa8L5Gl4FwfQ9%&?xpoeYVZ|j8Tmh_gxDu>XBDQonUHBiIC_bcO5Q4qJJ z0cosxs+6!)YAlsdLOk!6#-|o}Zb=GMSpS59vgNn@+NR3Ou_eKnV=eRt#ifJh(g7+M z=>6CtbZto%B3Lmc<1E{>Wg9eb@crC26_Es6k_bcVBd0vpH`@9JY5>vu(Y;iXplnH3 zxU7=Ibgt>KrU$gZ;QQ^pRT2noNuCC*mu3*g6XFR_KS%Eu_g29=_9UknoVDR*4CWY2 zxV@_=k=T<6Nb%OtqnSQ!rVr{$dcV4Dg_yV}ZP>yA_+7D-qAjJUy%lAlJ&CajhZ>S$ z3ygnu&Fd{N%3do*h4v&M7r2yWU=yu>7R>7=TF^ek_q&qgu_yho!L6!XqBzk}oInY9 zz8}}_!C+51gN0{pd4c7G#&RNzU+p=uC!uM=yPgtRBGQ(K%sAE=yC=D!axe}&+EmeP zszC85yr12RMT5=c!h-2kdTaOVA+(gz;V7om~R062GH|veP-E|1_|;F$(VSVcp1qRB+jl> z4Z-~=VEDrN!m{*Y+Oj*DIuBd`rd6d?1wC`|)F56d=Il}qXyrZwUa zh)D87^Fwn0*5bnM9Gl`4{1EMB?PcYG`C;3g0jTf`n1KWy2|Pl*yuL~xOvRVonOMgc z4>{Rh+bb}PO4PQbDS~$>bfN~L209c2q}-i$UwDYay1X{8eG|izXLmx7!%Mi9e2ILC zdSf6occ+d*p2DvXPYa$FwlOk`yYr9{XCa7%f=mUOjqg|_!EH$oB>tj+AHg+RaE;;^ zk;PqU1_U1C%?LqCEl8<2#zDrW1fq{O8rb4ewM&Ht@bXo_F&BY*k{Cf;#vdNkC+gFI zvA(G-DaRN-Bd#)mR{}4nla;T&uh^SNn=@(%e@xUAoD!UZ0xpjLrbF(_nT`ZTZwI0xoFSn`U}E z2bh@b57{3&0$_M+Ho3rel#p`fLZ7*~lJUiD%`8|v5(W@9D{NLSky(ki<_kK!hla?s zF==Bk%NXajrkgJO#}YCVo^wyNV+OcnZ)P##K?GHyAwffOHS@x^W}HfVa?KSoJ7jj$ zoPnscH8Y$vxSdWCr+>CQ`C9vNJJij?V1OJVCq(p%rY3Y>Jm_ zkP7x@R_9JW2dzQrH7G!eV0-gG%cDdP;U+-430TxN0lGJxMBr2UynriK*lbQ1Ft_66 zPg$BSOM68fo@)7))@E~NX^y2!9x9=x5kb#b2`cDjFP~ zM4L0Pk#~tfCr*rx6QdcYShhDKo#2*3sB<6YK2(x35NC5vk>FvX0>Bl5E3`w&svTUS^U+ zo|8N$4=8{2iOY_p2n;_nBamH|UDo@N@zhIxI}&0Yp5_sXO%0nGICd^B>_|8aIh(s0 zaBzmMcZZfc5{LoZO=Xj(CpUL|-VLb`7Y>I^3OPSHze0R~8xk2Cmvg1>ZcE>7y`4)> zorEXhZ9{@Ef!ATQQKzI%NhTf`8T%0qEq+Jw4qP6%JbLm_!0ktJkvxwWOKoiaS+dt{ zY}Dw*?b(@gJ~HN*Wx#kJ*W%Y)`H-j0MI;(#tp*==gO&CERm zeRiX_Odbdhq@5x=MT-awVX_;=2*C$khtN^!sD23Twxa@#{7?unwI}cC9-C1K7@U$^ zlcN|%FG?jFse~a_iW3J%Nl}ei9Y^_;a~Y3 zpIP30FaKM+z@lO|9)_$@+J3n^Ve0V5B17|i8QE2Yj05~!t-wekJ%@3`e&?{*U4&QK zu0U|OiOy0eqIt3deO;2Ic|!i(rfTq532Guvh!W~5elvsIbJYdG{8D*?ub2=N`!9$e z!M#u4PGy8i7Re|Ym>SUIW4T4yj`C@UlDVmm1%HXeCnKU-3diI5{hc?}&E#h{QkNL9 z6plJ~#Yap!N}ej9QI!g_XZTM4(@GeAOf~HtnrEvd-H87_?Dzv03Yj2}?dz}}vryj> zl-f!slw~VH@(~>LM_XBKxL5j9h5v%)pF%!awc5)X7~ZBMOUVSyebCYEWObQcMKE-x z-yGCF3k}BkO@|fGb_!C3g+#~4A_(K(-)W$5KAfr~e}8H4+qxkSEi?KcHnO6jD;D6P zz*Fnudg}3&|IT1^nz+7^C?D?IkTGLv{&4X27Z!C|KZ7^M6FXG_$u_SPH-+MfgZ7Z< z;B&KwG1J{H^C%8i!s16ZgAx%B}tkXH9 zliSfQe9e;u$f|*Zy@J&Y`zu;UV>21DqX^9J2O@_e=N?SYs20OWYY!ttC#liBXb)cX z8U>SW3et%`Y0hK(vGDZa+BBpfX`@}zWAd4QSmUrua`6tS7vNj6SLLw0KQIA=n}&Hxrm1 zzTK09nT+1?)IS`sL@ovkXwu45tNB(U+owef=*amA1fqLcwSn}hJR0{E6@$l$$3MO9 z1);TpoW0UlRkg3`>Dj|1UcU|qh2#2v+Wb}OU=jGuYC7dqwreh$^;RLN5^ulsEDHT? zJaHM098NZg+|^Thk zh4}@AMj*=!(5U`x+|I(x0nbu|;QK2^-utkODY~L~F#L8yi8dsw@3$%=Q{$P^ z!mGYBUpq*4edm>T5PpG9;H#~o5=TxEg#*tVpcjdDK2Q*OJnPlzVb)kSm`6Q`pTahZ zy*;XqwVj^7cqj3lV>mI@k03wNnQ7(CUWS*selnf9Y{rTl2Cwyt@VG^Wb-2>2sn=SD z8W-hI8Kq_QJUMU1+@fSmyTUDqKt~x#Fg&|0=+$XFDe3hSoLMpr)*(AxLj1TB(fG`%&ES2vd9q!Vl7x()8v4Q7_qDXl@@hn>7SC zt{++};c(o5k^fl1wy`1YayIz{oh4^ghEU@ZAsJK%)!rBj1g>Bv^HjKq$Z^&FNJS=XU9#)lQcCZ#b$WEKL)0A9X ztSw|v(w3aFw3-<#^DB6p%SV&PS<>x>-eyZLAWqb4R;ATsTI!dU3nT-(bSOdvaXMi8 zrJ9KvP(jkqYnH7n2i=pwuSTlt3^fk;P`VBUplXR8=^sGmqcB_}oo>~_N(Fn^FM;ql z)@2J}@_~Ewzw*oH4sxaq0ZgLU^m3z^b$XjjGzV7%xg~}@@k8~qKN3W(3VL%(EmDj- za>>vd?rSg%aqY=o+40FIDJpiDgho_`?3GLJ=4VU^?OvKI47`Fdxou{vd2_i*$EZ(f zsmDu8DB_`M>o#K^wNyBM5uV|Vacz!4ZflR?nu0)94xF|m;-zw*bTj81m0ab_71b@T z@r%u{JyUg67OCSAmk4n${H{{KMnEU!g-ah}W9`LtB1-SbzBHirCl`TXLaRoq)RidR zbr@Av`nVm%@q9=seben-!Y3JwEU%mUW2r^sFT30$UHJ!HhnqxoP`d5+3Z-KxGk-q) z!XT}|)!Vyfe!*Slr`@5BqMUOVm&>(^5tCXe@o)0+WFbrTaCxI$4lF`lOL{E#Xvh!V zO7i&>#lO3|fTulw_J>K8UG-<2Xo?w&^K_o(?vfAm3isjigo2jAk4VLKl)A;}p|JCA zX6pfehHIE6iUV%?c(rfZ@3{szWgN9tA=(a+ z@vA0mZU5u9X7GdLhZ>XhpULMPvIxQ4?@xS>reR%#-!{Tko_ytOpe1t2EXQ%1d?|LY z&j#_me%i}cRT*j9{FxX7&V*1|Lk}vpAYXK9g7P#T84(r`0Wnn8R8#2Bz$0T7{gUds z54KWr=cOE3XRnv2Yvq=Qz>_LMGZqD9y7$Y(L9{ADkpcMw1p``rRo&UXOh0BO=!u$K znq2~LUx=T;PcvdZmRfYKBE*=?1!=7iu!28clzIiaS#LOF-$CJNcewFVzT@iQX$5HoMyMz=*d;Fu z?MeD@v9L%}zhSE8*h;@sZZY;<3eiWn(Lh;rOBKbjcLI=q)%g@;NK+odW>?e zxUHPb^p$iAXVsH`wpVYfo~)x6pIx4g;I>jwv(s**>odwMhNEs=qb4BU`wri#N;|ED z$0QwM^-2z`Eb}yc+PpetRI#SG;*~U0Sxb&nSuq51i{M1l?~k$mH%-?pr_J7oEkL-MNcSg{w}xXvBQuC#)PVLj&=wa{u7g z-PZ@NVQ^)reawG>=8F;iqW;A{+ocBE&uh|wmHKXM=0XhPLxxMUC647>E23hk#w-yU za~%2R9`0OgtN-cVe<1lhEZ!1Vz}7~9{qkv}PQQHoV7rRpL9(`p6YUnuNGu-Ax@9E zG3``%`>V_p-#?mVy!%umx9MN!;UkoRYTv)`T_}QTneTTLdWVA+O0A|Yg*QWja)hqF zJg2s!Qj>SDnSlc%QHXA+?q}$Lap)=?TF~=tK;A*?C(R3=@2VlG+&PGeD)DHSZ#nbS zUCZq4cY4chHcxX++%P!L^&0J24z_ee_@f7XAW(&*dbxVUt%R$gUn{B+C$uFWr-_Ig z9LwNaJUG3;m$3LoT~kbWiBzSVyraPTp!lgaXjk3g*H!k5yRwxd?w%i~9JwV5NdM@kX*XcV13y&HMb-b5^4Q{qr|^*`qNwm+fpMyca*D=5ChX|1TP{%CvL zb#Y&^Eljnen}4~{eb+tZw~09SNnPtAI*zxI8+5KU7ZLv9)(lIid@*lqPFx`HG_MF+ zP)aH?xn`!0E;9HA=b{crLK|24%=p9mAj>}{{iB(yQmzvX?jN(Sw@NkD-HVhfP=C`1 zaCc(YI|e3(#qng^S`p?zA*eIs_+A`jFX!aLY*Qs4AEU$93+0@WC5&%_mB{tq z*4Eb|Rn*_&n2n1;ii*P~r-i>nV5o9|nn1Hu*U|SpD}+-0g#Zu6BQdXr{)iQI}zt zGI_1v+FaB!l=^PEsWVRvl)UmD^{6Ya!1pLva}-R2X(e2yLEWZ<(piitVRoC#n!ujnQXJ9Ef~Oog9Q7sV zwe@K(-kXV%#`ON-jN1ry= zs6q=5zDpW!^feIzGW+YsmvhD$w$;C%Om%KHO0Y5{onzq1>%OVtCiRG7iAv?y^xp5( zv{ZGuF*e`*K(tMdLK-F*CTMlElD5YB!u&b__&Zp+VAMHh%EO|*QsiOOl35{tv4Rbb%{OE&^W%WAuFE%} zg~|}43IVL(vMvPPFETn)DH-Byw}EkmpuX2)v0LKmAI&UnCKoeJBhK&n_c1Lr#7uuC zMmNU9bBZ)bXtaurzgtjMj-m2mb^y+;zfl;nk ze)lM^;H>_^N;jVHTSRzK(9myg^6fPVUC|rmUY)X-#Ke=x?FU*?jcvwpnJ4~)EL>3| ze<-@n$G6}rb{X>Qp-{Awr$c|t@?O6nA~`VLVl%aHr4h0dnYT^ zYYjZx{r*KX(SP2b%8yZUD9fYA}4B|z0=4>97hZ*FJ zHOdB|LCu`Qp>pD#5W`XFFHcmZbdTWUI?1i>u_?U~4h6JmVS14~jLQK&Xn3~-Zx^k; zew7rbN`zNKr7#U{TbS2XB&(^PGG)OL9dytAv;#8?b)s_GVCcsCDJBE{(ZXG%5wt6p z6h(O-(DZ!V@@?Wz%e}{ct>sqVtm6zg#;iseB>mJXCdgn?Zhs_1< z=!KTH_O+JX3$KRg%IelNyTk>@^1bVhlbnV13Xcc7U7JR(E3Z#?JJ$`#hM!t?uI>9; z*HS8J?Ekc`y((Xz7(JRFJ!-&4-Y$jg&f)bBUeC6-a+U6pTXl~qlSev}Hp0v8kHphQ zs|sG&iq}UC(U-ToQ)LS+Oc!4oGz@>uj~|g&uKPwOwE3-DEgaQ%*x$9Teal^_M7A{4 z>i2`7(%okJ+P=JXjcfPFfuwD%8T-nsfzt4oLvf}1-S00Aw1&Sv_S!T)^J)9hihP!H za{JZRy_4-D%1ZZ^UALzP*FOx~V@y3B6c!9SXym=AGS5XteX07q+Vb%Ye;uqH%}*aW z2pM&}Iy@5kxp93Yr2pqO|8=E1(d}*itIG9~-~~ti)9V;7kFor)N-6!#>#JyHkGA~S zN_SUeR=e)pOYxbJ;^)?od{x7aS1gt8wG&6N7Ypg7zXS;HZqte?(+Vo3n*ZJkXwDyv zxnAF!*2Iu{;yIfF@2$ht-~)Zrgcp3l6U5*D*034^!Bc`TmvbDN{!cc9*w5nig89&b%7pDzCG= zR@d4>u8-XFl{9be=6otI$@sARdd!L5EBN|<)=BEy4S$u%VmhpMXYw~kz9v?+q6v=XY`yLebAxnwVC-YF9fqJK|ik~PZx z#&{m1qD+F`{HXb0N38O*GbB)CNhMLsT^f8-7AiUD z?k>Y)RU0LN2F&)*3q9yFoc5cRetymt{+y<_*RR#xn&%o5jqNF%Y7yI$?+Kz4%{^3D^Nc}ve@L4b6WEhFCRbjl|?Bmn>H-CK4Er@o=dn{j;K@GdqT2ZF@fhi!c zOLJdp&iz_Mr=g33W{{&|TX9uoVJz+0& z|5x_6ys7=CPRISN^S7oDlW!(CJ@0IEd#d)hzt)8a+xKMY0J(+>d~Nb``GP#~!ahQZl^(Y1s4u)mj>_jR^mwCC$g7Vs z4@zba2}%jr&>55kTM!T^DZo46#_Nmc#F$3zrB4J(6EvEf%5k_KtD;3POqly z1`4m{c@`{n%i|pO7KW((g`@tYfE(Pr)z2kFZ#urSLVW1tA6pnddxhMozK?NRZ5jF$ zM}`t)1qx=TdH`Vxs^eo^kfu39DbocUnTJCy7S^16QDG|8jhd4I-+JA!c~w2pNL&dpciuufFB=stlF7mH(^VfIHxNdlF1;~+xb^Jbh4dFV zV@eC!5g#aOrn?m69SZp z+qXwcsWn)DhuuC#F~|Xp9vkF7O=R|OMV0UbgD9>V^-gKY?zmuJ=JMcL)s)Pst2H$^ z+FTDCP-NQrQcBYq$2Psd0Z=7%ktQk7i&Y}Rwgog!-5xhjkf@(@&4(+pI~KiNBKxc0 zrW}Xu1HOe%&FcYOwnyMB63goFTf7Pt*iuS5D3D$SlRGrEV%4S>`#k4Ja0eRrEyFKN z{u|Qia(yqx<1Yq!3*7}bP66$=%k=RP$VPo15BIwU(ZBU(f;O^K%I5S)EZdX9z@`A? zzW))k>;vzNB*PiLE4;E)4Dqz3-9!G4#OG16{-U7I{Da#d=OnR$H|#%G5B*`@oPNrj zUeDi442t8>&W0>DW+XjG|K$&ljjXYDY>-A&S#0hM1BCt3j^Zqd{KPw<6~ixskWbL! z+s?XyflfWdk^hy^vZYDQEGw zQ5_Njwg;>4gljz;XslH17E%N` z;{YKE&$q1-+pkHsFvbMgb?2RbZ%fBZdc)7K##C)3OSXXHv$$i`p?RcY7 zKsI(I8bZbPXJaQ`fp>cBT5-}Dh@(xnc^_r0E&*I6En2gwD#f{**o&hAUB;th+qcNF ztYC<8xWexgwe}=Pj*mdTO$~G2I4~Caf)#1ynzAGgQVY=(4?_MvUTU2q!8I;rn#2E3xx-(RvFIEJ5pl#*%-`3^ox8Mg-aea zoJk8H$x>+A+E!q&kuaAV6pWW{zSu8^WPXXofp$8=Yp{s*EaeDfmx0FTSsU83pt0E6 zvKL=C)JZ-M;H|4&XE13AyViCE4#b7sx;PKog4EBd&-Fz-E&7AhY4okSi{-g8&h(U@ zip^l8yQvIdR$~L{5sbQIUm2xKXr~(`N=u=tw<0<9Wus(N2%1lH#(k46a&A%PMzbh{ z0<*DTHU@hryrS{T+hk0cefO*MiGrLmg?5&hQO$pwIvLUy?N*k6TfF97!G0P~P^GLv z27*!m$kqAPw0Pn-zr_sUer&KRPUtQ^6<;t29JyzLB2q~N!EC-T<&6G< zp5p5Xxufm&SVPA_*^Q_!->B+t382~&Hqo-P_y>kq_EC3y^c8&7B4Fz6zt1tW>uI^Y zD!Gy}#ZR^SqxenSOVR)K{3+(V=NE5ZV82B}>~?Esqaos+Se$T>D6C%|qgL zfbfSY2#dN9X!wSP=*Qmi?DE;e1wyRJ40~vZ$AMpkiJ_PLKxjB+x8U}|hS=dEI zVgn~p#!)WUUz01)f3n@wjv@LYAc)`CA;+tT1=%;8XJS$Rs6iQv>-}03HV*}upXu_> z6#LKxLtLF%CfI&i1!oJ&4%5^V1$@8hw<*~~0g)4r!IQVbBuQw9STCDA=fo@oHdw#% za{jnPzS~C)FsaS-KUAIikv%R*c*Br z79nu&m^2k$dEN0I*~vn@{sl4I??|#EH+6}&KTqSN2ko59C5eu zp>*nK6h@DFn#69>6tt1SGS5B)+G88_tKu3wd@=;e#M(6-g02Qy zfr%Wf!drcIpPXAcRklm_qh$?~kz}py#vXrH*0KN&NMzbGXQK_5PA)oJ=$m6txz>*( z<{H5&mtSro{kU%cTvYf1lCl%+NjU*G9y~Tqbqa973>?0P!V)4HkMP%VLDW@z8Am=> z$7f2q3?`aDUO~sR$7+)8$l|Q%bpgV4cPRWk>-A_;A7d2yXPY*JOpd8&3I4NAy70bx z%-6n-@xft>m;#L`DD@A@(<5d4SzdyE3ydEnw3ulwwk(2INNy+{Unt`M*CV!5%vPoy zlgMmQ|96@c;H;TQwLmqbmF1{I-w|JO?6i08Bi_mwzWYI=YC!k-sW|MD zZST!ispYo2+5-jgs$vx3|2Hz*fel4ivNCj+=Sul)paF$$n{lK5mZc!JAoc$~9G_tW z7T2m#d7QsvAjcu}r3saQvigt!2`c+}6RGffEnJYytBRWKi3C^P^|hL1-v0$vzo~li z1yl6yj4nrQb&07!R7X&B8WtAV`m=kmQvpRBMoU)sDDrXx%3m>TV!-v}{Qy7xaa42c z2#$e}UIVm~<_}`@T{^FCm2HC|uhnDnPKm zo*OiVkKq%mhq!=29rcMJ31o}LhEo*OSAJ&srqE80@vH8*0Y4;0jOx^%SD=g%G@Bf6 zm^rJgLN>aU6%=ccD}lGrRm<==WE54%x@^%G0bh}WB-t_%bP2h=4dUqU*4K0e|S4jSC2EhI%Ahj ztVdTi!p|apq*d163hRp}b-{O8N9VF>ig4G47hz_W-dZdRflzc z`l@(ymV^ee1FFzVv`bmkF?I4qt6Y0Gju7eot7ePzh*3n)QOR!Q-u{xHxE2w0*i zNd!JZVh_PjEw#~m0N2UVU?e#0>@c=u9WLV@GZq?uAfMaLquqT1bHOh;<_U3_SGS{o|!OdAAY zFR~%&afpW?{KnU`o#r1w1;qccHjyV$aI;l3_j}lmkE+J{BH>#1RkXr@TCpA%uzffd z!H^0pfOb-eavVT5L}XuuG|h&8XJWe)c`q@r4070H<*CL$dYGdjT2H=~KbuIy1@>o4 zlTi5ASOq3DgzGMR;|g?I6OMEahb5M;*J4u(#(#%I{VB~t&&Q~h0CL$ZvKil^S|~x# zGTFK~9!};>Iu9zk%4lpBd$y%IRK{lpeD)hU|Xx&n`HDfSIO@mL6eKYfaCYg=py+`G975ASa%B$gbFNG#Cjw8($d8YT0(ttbRVYkf=6!4 z1XLYXGa!ldSi3*1WnBKqoRBkoQM;a*ZJmQ46hqwPQ@k~Q;{-*uJ$0ASwA1C93+t>+ zDDq$9>!AGkx3FnnWgP-v6J|(!p@>FnZLzZx@-h($f9^PWt7Jg}g)d}8jT8x0;cK%K z%_W<{CH)Eq`4)2qrfbrcP~Lu-Kkj(#k_A6>-AOFMus$PfSP%JdRnyx+$1f118XzXL zNod~-K{SZ}w^6d}`44H&r7_?bIrqX$2=9&Uf6Hd7Mtc{L{#~zrJ^^|4lx0uBY{_9o z)}Zv`ZGT$l1fDvhCl1mW+vF3NzPzXjguu}^#tx^lkt{u92~C&LnRs#z!dDEk1I%8+ zNb!BOxSZ{^2#kOWqL2*c6OSd*{cqqGHVrvH71 z25=}~#37-t0%wk~$&XX80cQ`zpIjSLR2brohQLND0+DnKCnHU~!$_Rt54B-nS$u{4 z!W46Hn|(Q6-T&o5xwqr%(I<P2UAhZrw z0n~7C6(q0BpzRDAV-Ol0M{0iT{3jn3-~SZxYSB0SF-Pzt<^u>DN&Im2H6DVP^O~vj zBX2vf#Y)9EUWGd&N%b$Uics3S2#73+C6c6Zi~+4#RHf9gsV(|)l6BP0CF3%Lc7brM zS4zcx2`_hZe{2+Ri^)oQr6~Usm%rU(du~jtt?Z^`Qo<3IAB6-*mbDjsz(fOjzdX}> zA@HYxul}nO1l)NKSm3$pW&PiiWDdIE8#MZ8V^a+gaxgp#5j>?8<{>5I5Qfx4Xj7^T zQlUsV_SbO-79@N12b}8i4K+AjP@T#|mK=W@*}UUzv4VD1x##5?T|*H)6{u-7{%0#3 zG*gOvWdqKEy9?ue6U^-pc%|eJu7kta;inZcn-kvi-e14U(hV9f&X8A6 zb{Gn0f3fLeEDqvSy&ObVzW&)`pGjv7Ce41yR^zvj)aj?*bvNIA+g65!juYUK!Z?Y$F&Ixzn#6!H(mr9 zI}@2?MX3;4p`wxg?jKOZ43k!@u0UxB5Bff&vxjoU9?v}GDL)B}i}hvIHw^crR2V|{ zeVO_1o1u^UCjB$uj9TdCjig-WL4GK7p;}y4HVA7!^eO$ zS+ooVRn15NW*=)4TT?~CCpvF7r7cARkFh0(Q%t#$*&j&% z3JgJ#>vg#+1;lTDr?vWln^YbHS?5r!bVEK;Rvdt4))#Y?&)AFwzCmIpEbx(QWv9v> z3k>ngM!`Jxl`;ij79`@|l>oINgpw$CkPWl+UuotWour_)Bwm!De@_Js4S_hb-|H_o zrb2FW0jM#zU0+W^a)MN<1M|he> zhUCy$B3yQB+0agiwullcgobiQ^|Byiz0*c{?f7tVc&HgU5V#{PQ8xC& zZNP!$-+lfuz%8fF93TC+652_Fb;ZtRedCCRkm7!_^wrwGD`wnS`0l*a9!ns9Qvk^~ z-}Z2UQgG|R5b8aTWYFDL@(d zMqnT5+l7jkW$-zHMjRp}FC1cQLjLcH*#)3i*O6aTBk9e7VpsHOhm26T%h-9&PeBEQ z8$hr6KIu=f463Nv5QO|N#LHG%hFdI9(~gfdLl1>Z^J;89=Ry4VH!TWbM-Jd8e8sDf zPOrpI+j8FqsrPf*59+Q#T!qWjWpBnH`m(FezqJc=*13>1qKpz1hk%5xo()8LGfB^2@r3oz9h^7a3QY`9zL zvv6ZcB5~U9f=3&>Lx!Sj?yUjuz~7)t}dPq z{_`G#eqs3r!j8y6th-`}vCkWBOC~ZPd701e6yO6&5_1d99me0hix5!sLF)NX*9z>H zEH)W_h%>Ow0Y~y4!auC#Y6Vs4&{5zRtkRSbNl1SY^b=hue1Cwd078pNCeu@A*;xDSAJs`WPtb$wEVs*^nO$ffLip#@s^T9d-R* zE5AYz37DY<5})kJ+YG3HOcQ>#^h9))7|7t?V>rx4VK9lEFsH==eFEvu`pbOHOMhj* zoa>z@`k*2HN~TA28FT9)QPbu!ZvOg&Yudy!q}q^vTW;C3HDy^o5o_G3&ybBW*_upS ziwbXOXOrM1V}^|!Ht6b^#I;!2e)a_lU$IZAd+G4OPC@r{sB8ZY;4O%}+TQ2P`v7j1 z?msxC_*>zCg}rn8MoSPEzNH%be4MAzF|V2uz?}4hjlM&tgjK(M+rFA@;=@?-H>n}~ z@8wgRC)*vE-V<98VL0cwgQ-Kg{;Zo9(+0-bX%A*0Ypnvsb8p|f2Ctwii=6R<3EGR%bi7#cX z5brfkvbLoW!vJs9lQU|3&iVk_bA%hih$$>nVje~~mJG=u50%~#)MX_DexWmd>1Fn4 z2o6X%j5*rEu@V1tsl-uekmsFNIzv;M_NjwuzV2#gHRaY;iKSJQKdR!4V^6L}NPx zIhm`i-#vfEkO2j>cNY5W^UJAvO-R!A+$L>Z=BL`%Xo%9Fcs5a_{pSDIA*7fo3PO7{ zijmIgqKn&nv=h`~QE8{f$I1uZh3^};PVTUYB3(?y`yf?3S?5FuZM-Xt3Jb9L_>hk_ zA-uRhPyyT1i*23;7ZyA+5WqY>`jbz)7@3hW%wlj>65P{}(rHq_=+|0@0S*S20;0sk zuIPfaEnZ}NI2p?0Xbv1}IPTKb9_xmL7k$BjD0VPuhDV&$u-q=~!~H)jxfl~saBqu3 z96Z1i8{r)8;vvWldVe&rB7;ENY5JHSovTdI5DAjHq(9XEM!^uYH>s0^Sh0yY5L#^Y zuK&ie2!*abzf{0dOX3IUclbQ-0eU8-H9K}HZAZScMJ_(Gi~Kk?$eyHxRGgTw7toTgH| zC)9xcPU{W_I89pn$A5QeFag|J;?A8-h|#}i;a{p=p>ShzIJtuZ4wh%+;CsepUQtSP zg2Jz%o2NTjq`J~BedcNr-}=JvaRK`V?c$rihv67dc-J_pJ96$)Jk{=g_iMpH6wlk@ z=Oshrl*xYDU*D`gN)zC6u#bdJ& zS}U_`i>*mH_&IX@wGc*?5{vYc$L39=M$hd^pZgo0W0Vma|EOk>=f@=F@h>@BlawiL zQ_!)F!3LSmyh%!i0U@s(OM#aj6Q9f>P`D6PMigYj?Q(w|4Uk1WVh%IUts$x83OOW_ zv;Kph#na}I{&TM#nqCGM6~AXguBx$-5mnfz^YZ8kGGNpni`i6=CO3`+1o+c*x0j9! zpFEwM+VdLOM23l}$QT&N0gsya`X%wHnj#=G`oiOzO4#k3&`wQ@!0V_8HjL?ds_Pq_ zb?cuJka=L1gjNsg9~MTr%0s=>uuZvE>l%e*)`qmI7dB>iH`xN#*dZYN)ei%Zcj0}D2D3mt2zY!WFBOUVVdO=4B9FPn(Y6?iT;*av2k8(|8 zfumlrp~g57z)4`{Rpjgl?W9cnphUqkUjs4jpuZ#&MnQTjqi)~t>_x+cfo_Tw`G5C5 zNL!P4f^lIWlX&1p^ndz*<@)T+?L>!f+PH7}e+bu)!zMKjWt&(9UYCt9Hi! zP(33CNI^`YkL0pf01arrQ`{xvv@#A7rv|^3h4}3G7q(#tGm0bMSHn+>cs}xhjMSku z2e0{aYK$m(6p~Z88EU&R82s?Mn!Tq#VU(#32N3$0y?k$J_8tu(97-UAnXV=BG?o&4 z;f<^^KKSQ>{M|uH{+t3_z*i>D-?*(@tTzWm=rWfKawh4bAqvC~el-3(j?Ia>A_4r~l12)&Hi)5j1Q_G+ckHjB zVHc5?kF446tie@CT81FIqj(F_PXsxii(h$dkaaI3r{YCpM-WzlBeKmXOp8{+<2;h; z`#9KW;g$3k7cjv({=+q=3_sVf`D9s=bCrti48rC#T_}ONn)8O<(J? zs5co9Bq}c5KN^Oh%Tf(4M@${UfUHy8_YvlVS}PqM(B;b#?yc0r9X^~B8|Q1||L$qs=Kb3m4nyz?#~E+r zEbWrKml{iQ=OOaBOAGkdDk>SoiuFY)hEKxQ$&OijTPt0evmC4SVjL#w^0H^?Reyc{ zQXV7X~E`+W7MZ&L3&{y%it!rsDs+JBa7k@bM&tUoA(a zHR`mf9hH!Ykb{vkh z(J4i9q%lSCm+@2*458sCCpU#{#tSU z&ddD=WDNeYI+_>UAG+q@g5ftT#vy$M_O~IHK>4 zf^p;$IxEid;SbwGnB@&7w9`Gi3F>Gu5DKAimTl2}4-oWfwk0~^{ zy^+b@?YN?^SY4+1yTumh3D}9e#1X!yrjEvs^4n$?ug4VFqtU>F-*lMlw@qd{@=;7) zLuLkK0MGrk2G&2|LEuC9M7xCE*WPMPc_WQbiQKBsICf?}S zC`9mbq~ivS6}ywmGdc7JO)7Oxj!=Ps2$zy-a{j~1KMEoVm*1+WK*j`HmH6u6nGp^s zl#)Z#eV<`^R?{Dzu!M|!iN1zf{Y#3F0F-S#Iu+6RtP9sA4Dy)}S_#T=q#U{%4dLr| zvxgKk?5kRIiUp{px@Rpx-ekExH!y`K)m+W{4VVXEu~mR#RlrX{b4x6Q)8c)t+-K9BrF~IRr*pjqsEHWyn)h!L_~^d)swg;I;^HA z0~tz4QQ4~a4i}NgaqUjZzTz+zI2>Rrj2DH}vu;)O(eMz?+1{u^H@GinlI_1}GK@eG z|5W2rs)w)?X0gEnmDvALbd_OkHBIz};O_43?(XhTtQ2?G7ALs76nBTdNTG!m39hBM z6^gsN6#4Q!`I&ukcQ?CtX3jY?BMye_2aka*)}ArPWUn{bigc71?u?1;P{((o_n`qu zkkb7t5uWNeY)B)me}vVB=o3BLe8#HElO@kkcxXSX1^5ok)(_ zaf|5%9YiysO<&k+d`;opMy%s?$^;()sj=h&R6yNz#0?k%DlcV31iC_?zd_WNIZa?$ zpPbH(Q|(0;%t>935c$URkC95XGoBT2&j591VWmKtal{VvCUsX3UBi^OhX>JPkhne( zEF>DfRMElruilydcF{XIs)~iXeI5|&oJOJ9ro_es;##*C*sT;r05G&7UsK+~t$*Z| znd92)hs=jk%fB$6SLuLWdG#?5T}19@hAVH1MU_f&#*w{`wckr=A`gtq86Z#}kwu)= z65x(7`6bbBp93l}{XG~uHcQCq!b$#ivxy0I5H7rAyu-w6K7F z^r+=vO&_D-T@9OLV^Y8i>-oAv0uvyKwf#uvQG~SrbRu~ye}mrffE!n%<8SREKPtV% zQ;Q_*f6ZB5%QvBTnTrFjtJp_~i}}$Nh;7nx`|SfKw;?Fn3cKyy8rg#yp#FB0O%OGn zB2Nb?pgYNZ1PjIc`4%MezoL=>W&R78(Z%S;AfZFI3Brs+dx;qMZ`6v{Sb)LiZ$`dZB=}mAI-` zKu~2ap9la?`sDTXBcQBNlxy7V^d^7wF(l)#5Wfo@@S4;EZ4|%-^c6MtqZS`ClEx!J zY)BS@yFEJAh1d&`AaRnaBHpk=7a z!#G+pe<|BSHXI^?@^ymlG=|z!G~^&xWAIhX87?{B$Yx)(_5egZ&SswsEdoKSy1t%` z5qLvyK1A8&^b!i-wSghU23G2=J3z@sN>6?kLPyMRAzZMu>Ot6k4FwP%!MXlCO9ClA zV|4z+W3-0^AbffjYnlf(vbY^3NBscq^y^&5U@+uXE8iKp-TRP4+!+{=Qr^sq{Y@Z8 zdH088VXd7S!IPAP2L_L|4kAxc0Ffu2JFWjPfPgQvxxkQbjJyvpqGf0xpD&OUaO=9fvWafa5dNXJ@(CF6?o3&-0hXjEgiz0@fN;_; zV!b{vQ5v4SeJ^N(ft-ns0t2`WPoye%h3V_|JG@Ik8$LWmo-BwEE*JGGfDfR*sojK+ z1xwEh>>81+FOdLwA{?Sk-CzjeRM_MICXn_-Du{Z;F9Ga?sB7q%fgydWC!s52SHOam zu&4kmda4E$CZ-Srbq*BabXtfOAFufN`QNABj<)&(O+RR5B!WtGty{~$(mU+Ld35NE zm`FuiONU&&K&pw>k{w*}^noPc=hW92ZJJe?->U(J3^Imc7VZGpwdC#of5{Yo5d(=! zMIRrqkRbHAEh0?`AdCK#G@b!i{LDHQO@oCFpbnFZN?0qMLlLoc86s>mxwhd33e(0Sx6#@V9q@VW9gqTyzf6xPGe!^kzS*aQ9 zNac9@w^HOgih!m0v}KgWks!=m^@YeVXH;ii$NNkSIB1p_N;E}+AWvEO6bJyF01hu4 zjKRq^Wa+OU2FMq9g7m+P{AUKE@#X#`%fbXqrp3_gj;2p#ZYh^^fv{{WeuFBv~StTCX)u zA1r->bA0^_x!8RgYU}mG(Zils!e~(Yr91PY`N3Oqq;%VZKvFL)?b83&(!nfzK{bpBvQge zFtsz$9@;2YQZ7h36i-M51L2TX@kJ6aI-iKooczX_ue_fff&q&FZ5k1qd1_$NG~$Dq zjwmbuXWS#E!Exf|N*uUZ65e~L1w;HLbcTwdQ#Q#=)T<{jbm`gS+1tY%U`ErOjwMC{ z9w+2q_bEwA1c9ZJ0%xPAc)>@Y1jXa$61Ao`3^Q zMjZQ30nQn)G*+AwrXx&!y0CH;G(*{{ntbO!f#gnBxVmwobqIJ9;W0WQhCX`UwRt8)QK)-nCa_a4G%cFjwbrV`U!P>;E1MDgpZ0lGrU=vY^A5Ev=KC;)coTW*Kh>k$<7&&(JW6CmjI znvu3wj7iAyWx0KmU1K-%4VoQLV5 zhn{^xC+>NGkxRx8n|e3Jpb|8T)DB@VbX0&U*7~U(5hCn0gP;GG3LRkeOI$!tnnn8g zx}##zK)nx8Ya*s(Hu}=X{Z{EYF@}W^DFsDsyNSzlx5qY-4nX_;Z-1`*;orc;W^7us zHw+E3tN2E}*4(2z#;8~ThG_RbdYl^%9T|R=KZwRE1VdhLOmANJDae4JKEZ;Nlwpwc zw<>!hO&c?EvCI1QXxCYao5K#eqe;VGFb_?L{~pH|?-P%ZEfO$3ame!Ixa$fKr!DwZ z^H}3L0T({G2#b`hFJNd{c6idsx%wZVgzWFqe$ z;1M_a?gLzm93`+YJeo6VoG~Wl+J*uA%~Sq#0mIWNxpkN*Yl}c7RQrb1o5ktfVCgo# zcLj&cZm8MiMKi$E=Whwi4`9fFUd-hqW}e6Mkhqs$p6W8S&8(l?wc^}Y2YugUuyp(C z$eMW4?%FT9Wxhk&%s_g4o~a$dCuz&@&z2MN83K8eG*>8zefGlf^ta5D?pTXd9~NOx zS`=1gA~^d6c8tU{jC1Te5TDwb%TskwhvR`&xVxbimbVz5*z8~Q0kH8-1}ptEc>$lT zjNx-@G6d-kt#_56jbm%v4;y@U2=!viDcr(f$QQBQm;zpGl-1Q7I(1`7cvl+@q~fFk zJid{O+Dx47a1ivDSVwPQG_Zsi1ix$kkfgx6cr`(>%n{V09D?SZIs$@**gsHgli0ss z%4NsiS%+jp5A#(rdb;@r{{Y&RNE!&4s{?b~KoG&iaVm?uWVXy;6mQRG4PQ)r64Uh! z$m^lhpdJ!LeAoj{G=BNiT5hing&7KzB;`zNQ&Pfa979QuF2tzU3W7>F8S&e8UTE?& zeT8~|-Wx5W0wiy*H)ki&hLYj+G92_9<~`Sa#p*^dfj76Dd&!SR08BlWgH%RV=*M)y z7ZnkG23#~*Zh780@D*hZ@gD%Gwwh?7mepCX3&N!*BfK$Pp75K8r~C@WKA3?c;x`WT z-ad-g*jS#q!k}c`4of#0n=yXirulN?c77yt^cwGHM#o&I1!JVOn9|fGjO835nKZ3u zx&|zM{ay-)6|AkAJY4y~%G7)S!!7M1u)FB^`|*sULDXUaOddpSvpr|Ce$gs_=*z+< zX?c(!YQ1#dZPYV_3qTtL+cH5Fe49gw@bPJ66acEUPLviqjvHb`VzfjNFYHoE6vI@( z)+;I1%=W-TtbgQD{O=$6Ff4y(Q)PjV3Y{$9YbKwC_K6$Evv$Nu{TQZMd0CiEU*0DL z8e$}v=J)ppk$^8!^!gUOZJOvn6fPqT)2E{qI+IOM$-(QN=r>xU1Dg&_w)eP#%??5H zF=m_&tJ6F@fr;{0>%v)bAxtV5RRc72$+P(PA_2Y=CqI@&KEh&KCEarxF8V$^abf9s z7(6$Eqy1nSi3zw-{3>un{|OosgJJOGMxr)HvRHr#6&?!`LwMJeGii@i#5DYxg$1j18t=1*iDw5K7gbti{J)#F=qC_(2Jr`7!9)gbxRBw zdHL#%4&YKaXTnGUYb6tU(d(vMzCHh(#kCibclbJF_6vUik2<8ocp-F>pZ8LoB&}@6 zM2<@XUE;Pb{j1)&lB zm{uL5B$n`ZzQTCu)DCFs*nXqY%KgmqZNef06TtCTe#M^b`*pp?2yVMntNtM6Bl$~% zCq>T_2_PsT7NG=IhXo!tplKOZI6uR^vS&chZ^E+A!+Zh&I{Bi9ckn231J7Fw9C*BV z=@KOUwn&Cp7(3)g1uUG?Q{0>s@DC}rR89B8XfJ!?kUM$x+Gv`k|8^7mH*3SUPfe{C z8&t)C%m>e*Egp+fi~U$ebSie2l9IUEON)YKSW4jtpfKbjA4LhUpLq;?_(&;F3Apsj zVcvvZjUDzrYlI9P`T@*pQ{+V$zryULUXoW^*w-ckaQimk*N!8r@$&e11{ht3R-L<%OT%nN!n-WoQi|6;(NA6Z6%ud-tfz;f# zMkDb0r>aL`zI;dUVHl_+K3OD;Zs-jsk;*6tc0hc!qx9SHraps8RL@{&>1v zCVIaJ|Ho5@hY47P{$>0pjew0fvCg9)jhJo5PFe61opI{RdVuv$w2@y_oPj|KDp25 zzl|tFxK} zd{{$Ab0V)o1Qa%13fhsSj;>*-&aQn>cN)1z!6rN708rAG#pteBv{)%@TQqDn1?!ro z$dndr!Mtl@pTh=YrJ4mktXF(SIl=-|$3)E|RUkP-w4w%Jxm&iR)14OBb;tX!IRibS z)fK-;9hL;uKrZx?iC8!K8t}N}v}NQL2TRK%@BfUpwtrgyxa0Txk`b365iu0|^f*5k z_V8hEvD}6J-$r`kh>-ZZjKR1GG{$XSSZKMB3dBadT3-d>R;&Ugzx%s7n&lETexK!{ zIbr~T^jb)dppCzL)tZ8m8|Z)`&&bIGFamHJXC%=ji=+erBYOuRW~H(T&JGNK=NpwB zChT2)>7+OBY;`RPM2^E--->t#I|9t$UCS$KSXQAxdy|={;w?+W3D88>kDB!eep~{u z8)ry`K$KRx`8GJdh)4yU(?|2WVwqov)1l*Oz2X}%*@fcc1R-aqScYr}&|V=tTebN7 zQ3R;SNldrh+$}78B-$>c5^PWYV*T+iS`$VvMiDTLx(F)?<))_z4&gFP*$wXk${{;p2n+N~v}(8U<_ z^LAERE)s;yx8@$8-uDA?a`bQji{C^j?iz)>Fa@xRNMz%#`admM@_0804=H?d&L3;Q z=rIhkQ!m9iaMIs|%LmABI?kb4#n;;+G8(LHOeM3zu;OVC$q9U489wX`5Y#{uumUb@ zsx^3lxjUl=QbkRmyY0qXh_Xl#qdP}r#2*X|Et2I0YL>p@{Scn&2U#%u7VwT07fQCh z_0E}Nu=t@BCgnP0X-p2BolP{V%}3Lg5S@AaMTBrt;T>2|F4aL>wrweao%2MPRjnw5 za{QXzYR;s@Jd+_&j!AXg=yR%^NM4-&s7i*;Z>Km{o5#oDhxM^3`D^}fo-QMn2;}Cv z>wl~`bX!_kt)(xFj=s+a>tbBJwPtb{;#k+4@3IR8Fg}is_tPzL zceCyG)|)4P>G`1g>bcQgsm8IYx}v+CZ*|=FgJ4(@4Le2A@tIsKfl!g5Fxce_fDnQq z8j?DXfJHqfyJIHQqggM`zcUnQ$|{dQ2c&MFvq4Z*&2#djO>Y=uAldW+DtSuIu>^#d z`BZPXNe)395k8X+(*D9W$94}wZR(RT?nn&O|8ntZ6L@3tY}zK~U>d?}Ojzau^iDGp z7Gynx)rA20>ywGP!A{j>!B=K<;HlR!e!epUR0NWCPTpArJl^b4eEOS|@?$(f49CE0 z!TcUK0xSiDkk9c1PEPJPH|<{KPb%daY`A1E%i+c)VDT48R`?WMAo#UfLlS6DyA~^m ziK|(S3ge(IP(8kMixoAUZ7b@4`T@jM*JI*76F;VIfPSaC`Ik8^kjo+1?6dq3HaB(+ z8%0Uo5i3oOkguEn#>B?x1h^?7K9~jf^P+u*~$dDjx^2>BsVi7R03D8GHA|opm7*Op^ z_xN-_s!M;zi9@t7iqr7phsMe@ejpMUete+si1YrSR1~nhG*CLhl}7dPDZ446eNV(P zOqtfZdFopCPNIPVhqVERL*3Ca!Zd@7DLWYlSB&1egS5P39xLIYOwq)od=;6GaD)`z z-kwN-JOwe2j=b+_iq4@Ij#m%gSXA>jWwhhA*}~A_z>g+Tz?s`ZTS?8S$YP&Ts7VmD zIW0}SZwMo=ukRp}g&iG{gzQs)f-8^QTo_p0r(A{OALAifzHAh$QVzbc$V;3C4}mQd zdfRlML+|0|vV@DYa&l-P1A)j&+1Mw2xTYC$Lf(!{IM&k0U@}N`@7S}Gt^mWwe!JAR z292+1plxk5yGO+vp~tv5IA$G^ggYCz?-Aei(PII?j~mpsTxj1HW+OCf)&HU>UhD1G zP*W%=X6Ygo!DK?Q7-BR-JM-q|{D0k%$VLP!CwJMl-d3j8%6A$CKyb1_T#0F2sg>I6 zJx{NNM}dX{A%@C5|DNU@L(C)A)ea0(F6sx}zb2Tkxx73Da6G&O(ml-IpLdx?yoS6x z{q)Ru71-Fb-t%c)p{K69ar&VPCYgHobN%?fvD@o8fV2l|@O=F~@ZITIJv?lH)ZV9H z#*lX(h8yeNp3PqB>^2u)o}ap+L*8C*nKtg7kVDo&!`8teUB+YQw>__m=LkJV2%C#< zd5^dM44tq3jh-L9-bb!GLmQv|_PXbobKQvbHqJJhV}#rWKi$6HyMGJ8OIyD!gALbE z4tcSAd~x12)PqSABpzP^$sTThzMY$6g}fzS)N`Tqgb2Y_;$CciANcYF?m31%;ot4} z{(afSzt4&0g$MU=Pqz$ynufQx^+*4D&$~^Zj{?`vE6jV|JwMHVPg)P=dFT>+KKeJ* zXx!opVp!-Bo=vZBN}$^|RCS0nvD4 z-Qu&^yM3{qx=EJjx{kv|PpY1KzPoy-hJo{Kg%Ap+tK<7-LfD`8Qh(m{x8UaAFTId9 z*gC&I%%_A*Kdfm;!zRjFTk+;?g%r%?EKTCFp29YXN&@g)>OVQV+U<=H34{;ac~*M8 zT3&72_!YP#<>RI5%n=<-m|;Ep_vm6o}FKdzeEMU|q3UMjkz1g|lQek{&C=ly09bExDQkHE9e~SE*V{ zrK7);j7ITIUk)wjk}2&BJ`u!jlj}16DnySz+ZOdf^~|t?0(xlqN+KH+R;8khg>l43 zi0CQA0gAKC7rSY##&7n&N>@Y!3Yrao25^q4O8> zb*AO{fDeo2=OJItzdEn`c0C=L-!>kb2e)+}i*~Jyofn$D!+*YQd^-AhUSt*=8c=;4 zP~3gK9vpfccpDsi-VOW0Ecm?Z>2KFj;O)y#-qDAbe*q!+AtCwH&w`~7^WRna&V}$^ zUO#Zb&Ut;~di{C8efRN8ePj2NS+My!+JT{Y@a>1j?zi1f=D~~gw=co>^RTPd+L0>K3C!S^}~7BKeOP)z{YOF^~L-D3}HV8dvNf%ZSvL6zxISz|2~im+_$AAQgbqCBbXE>tSywTIIPO$EB3RLy>#J9 z*&f1vnTdBq_GX#6_3!(b2!mI@SUuQI+aFe?Dv_w+(Wst~P=P^+~OEa#CN>bcz`#ZOx`?9~CI^!j$u5mkV z3jOEV%4+;6-QPFX>gN)4TdPBOoCoADcyUaNj5h<}iqK(=05|m7TIbf z-LNWStol?-LWNS}&y@rw1|$syOS?*~CM}QA-58}}hu@+%72T$~V)A<(v_1;TZ1Axu zHab;i(k?dt0YOTuPs&PkbJY}L_lvh?!Ea}QjYHe_`4%B>f=G`q_4|wKjplDx$IIvD zopStdJvx*s&%R%OV+w?7WkjH03JVM7G1ubkHQhvWJ;EVpiO&5!b#0+PGuR&lo z^}(}>U6#$<=O4Vvq>P@Ua(>7<(I z8{t(vNygD@$`4o-XJzM{5e=hVOl@~o=6|qbKo70cP4$V$4c|I&yM=P$D*^vLmP)X8 zR)&NxS(fa&F+jP%9IG~7FBJtWx-#`9^nTuWoyKsOUd%LhEJMZ5d^8_S2dmnu+P)23 z>`$5NTHKVeC#5kyT|u2=7L#`o6fp-|DoS@1B)U~7cz9)?=pa5dv`|b5I*S(_PxQ)k zO8;7v4ky{EJU!AE`eiUnH9Bpl13Qb6M-9REt?R3D?MxM#@e&eLK76a`6gK7uJtIvO zt+cp`*lpG=?tcvfXL>rNt)Ed?7l96ENNg~s{+3=CPIEpNce}sOnmDcX#ceHf^YSB4 zZ+_#yy@ZposN`xNBC%t2iA|gyrIrLLPhGhk2KtaDWL8$IU$hU0$8uD0R832u6!W@Hb7sIpNBl@gPRRjs zTCGF5@sHZJ>dw7ubrbH+n=;vuo@(-PA$UF-*Z)=p4bcx87(KeL>f9R@;(?eh4G408B2=w3K(n(B>1%qt2qrb?cwfxp8rjz3wQ8BL_?!CL%?+pW! zeZU5SY-!^(h+|_{6i!?47<=GC zkHP5PGL2H}2h)crwv?1Hr4)<-tb|@MWJVpVgNt1)nfViHcPqrgh)GzcqWIrTRBEzm zjl3vKLeWm*i?YXzi0t>s8w|ag;f3MoO{rUIhcbWU_G-w3JT5Hdv4B69>BcvlWo?#l zyYORuBeZefa0uN?bW3!zA12BnaKa4!y2b-1bSa#0x9~^Cf`@+k&LB$7~PS2YCck`7be4=QAM(lqUPvlrf^ds6sAd*SvC|c0$mfeSv zsR-FB_{^v4)cP_B1s)C4_on$vfS4quZn}$MS}UKo&7Q;2VDyQEN^^5_JGQglR25|0 z`NwT)Y0>*}B4#9Aq!}(*rN0@NLiuVqO08071|LGEUmuutF%@oi4^L<|A*a6R7zGps z6d*X&8A@Kv{Fhypp@u|b5=De>etuf)+$Y>w{FSz)ycWQ(KvBKcVRfsb(ZW#?`bt>n zTN^Z@%J4CFSk2k!jFj+vps(Bj$>E@jNnDZ)rnbeUCnnFa^BYkldat2BN4i5=!36EU zDdPOH;>s9H;_PF7bSbvbu88pYjb3pyx*UQ>lB=dT#6=-`gOO7J_ZCND!4__Hu`v}P zXXzAiu7dX0OBUoQ=OgE%8bfItU08cvbQ9ttnhC$52`wB!EzZ`k94!8aRUdP{>tig-!jt{R?PljqD+?-W4;rwpzplhm=^gLc$Z+E zG99DOsa#yI#4!zm`K9eEMc1s)s2ABup%13>QDurQ3m3bt^d) zKA_s`_m`WX(W3R}k-`V2E`^u`n0P7Fi3fa>4u`A{SWQz7R88dN3h-qT&oN&YidHPe zP`_)lbsX;-MIZOm{G4|pZ;7zV-fj%xpH15psfGJihJbx^w)#WN?I2UBaROfCL4$eY zM4=XOz07@D3&TL#Pa|Xcl(`SJ4R8YC_G(|`cW0j{{ZeDvoKZ0nZGztt$R+-h);=|r zRzkOlm1ps&o*wm$9xwE)W!3glN=ZEL_kZQTBfr~dg_klW%aTWF*?Z1VW>B|&K+#V6 z#&tHXAM&SLxXd!|S>5mx86F+EW1M4L2(x&VL&X=zj8y)D=&r#nZ3#!DyJrSD+K<&x z8n+a;lA*DC!6WfNuMN}$&oF93vxZJqPP{dd&Z6#f_ zSrs6Ov?zF}E!`p{kp0qI{%2#FL<`O5CMB5qzJ}Rg1lFR{ql{%z%FXw5L>ltoV=75u z;VjDdOqYrKCli$l*^Rm#B<%E*-3PQva-<<{RfQSxeWxBaM_rxforexJoaJQoY;J3l z`8YDZ;gYs>86;U*5v^hr$h}(%!@bR6#rXCpAGu50W6OxW2JZg+ZGKF`s7F=5a1DrB zDxyA{AWXi8H=fjaDvC&|n42ZAS#~g`f2OvQinR}9ht5)LxVEv;11eV1>DXo;6e1^Q z(F&H`ggrdtiK^Ys4G4?=5;zSr7)W|~f4A~^9{J+-FMhOo_`|7abuJxgvcrhu=Xi;i zWtUFJIK}iN`fao0%R}6&Z{9IAidi1zsvc?-{#sh4ZKc{my1cr)YREv4R&R@XY_*s;$qu6|D~wpn-qK!=IsEqfmir)9J(Ee10|h#mQh zVHi z5F#^~l0tN+tCIZsMyS`Q*HWuiGK$G}nxc?}(=An^8Pot5an_2CjiJxsGCD$D`(|Ul~uWmSi*+w^Z_cl{b$%Iq;`+qMe+6KQO^x#&%eLHE~9E>K-y(gsaFqo9VFW0um_|Mqa-W(;=St~ZfPm|L|g|6|2? zRI|3b%jX5nEX^!wTXGydsOjIG2!|L@#s*NM;z1A!UuV4Li{!qI)%Mo*Qsq)+fm16V zvMzQs?4!1d@_){g4+pxz%tm>Tkw1NLdKlkNO`51Kra-MSye2P~$HNriKbWY3c!C}8 zr#;0jXAv9f>E8|%#=0{Yx8NXXfaVg(O{%u>iXSvv8qCAb9guZ+M#E6kdd;i~iHL|G z+fXBZORFP#A8_>Rzqp_CG{YY?3NE9SNNSW`rYIB+m#8V^A zK;|ZvmQS9KCP)Nl0!YJD>A!m}hjYwk1mqvWqJ(T9q;o@#|Ipv%+Ll zL4qlbb-f@Yed z+FZ871R40h&pyVjNK=1n(wyk&h51K9D>lm%QkB%#GJmlAhY%@e+|HygsY_&*57kQB zFWRRZ=Jh?`KdU>Y+T3*Nr9AZKEml_&!KHtl#9!)1gPLWQ+H{T25!`OT}w*r+4B@y`%r;7xWM91Hf54jOM zNObCy8#RVD%JosHd-g(uYjttfKUo~opR<-VgsmQul4G4t zPl)~5ZVZs8weTRH^hY)F?d7Mi1fJ!7;Wdhz4DC?l*3Q-eMJAf#{;Bij7@V!8MbqzN z4Zgr3vMtSkqMuSq<68;f^F0JMoF1^aF*%-7vQ7rfNl>qFB9_O;4gGmXk@HN zcX9Vrna>6mcpe-vxI8?tq|u&Q)M$}N_KRV$$7CPwY;j=BILeU!aH4M_MOnldE_F&Y z2}Oy4)#k>NY==;pO7qHb%-a9hsniIwh_3+B;e1UM19vzI9sl4**Aa@I8Q79LmzFF2 z^L?Sm_g=xb$x-=lgWmA$fcQOJ5WN=X3ci!%CYeT1bgt66>46yI<4o1VS0Q2qEJaW` zcuXj7d%9|h9&U&n-8#8n`PzZZb(`dn`5OzljE2gF8^iErCVY??^zu;w-**K+J$FvM zE?0g);TO>g=`86MixBPx8On}Jd5si zht4H_khqC#@8|XFG?RYZNP?pFzKqd`=EYf-dfR5!F=BpkM2#tYxKzoxaspTUk6L(Q z8Yk^2D3}s@3}Mk&zH(}6wxC#A5~5MftyH<@h6n9hk3fZ_Fnmf~zsPATqQ|nP5A}m0 z?~HY92Z=amb`xd-roEP1N015WjB*+(T7zoHQcbvIm~TO!>gkhjLG-4Cnf@B#cCMf8 zN{OR*P}k&;V(N^hoBT9TPFl?rFH_SMR0&xljf+R|&Dju-?{dNe)jhK}p42FoK2D%@ z9}G^vC#+IGsV*dKwT@LTnTnHT`VH+X$AmikxK54rXtRd;9U4XYvR^?pD1F08DmExC z{T+veX(u1}xAowW@D#@QOl0Sh(<-b65x-gWzX^)qqvhbtNZITtso6nt=wcp5N(&kA zJjBTsWRB_4sNp}ZenF?8YqGW(g-E6B>6I*~@x#Z#0TPh0?UcxN`8L_6oY1A`aL)`Pljm_G?R6zlhz;s- zcpgQpq?BlUqs-|E4C_CAW+Hi}*CVDMLKDeaL_`5kAtHV6y{smV$$LCs%G|)o4PK#^ z!^Ys;>Q#UVB1$hVF7KpDa7_lrnNHPKJ~vWQ^LJv3Cz{Cf$2rE#UPbFo*595i?M<1MBV$`Rm5yys4Vd+ zBBMH3`!>>!_&qI1SYyQd`_ZPKY|-y|&l)t0$Z>a%)@p{bhu9PZssvfAnSq-y^O9cO6o4+M4|>1sgf#bsZX2|F$WKgTgG3 z!VCA9?3S)BWa9d&UNk-=e|3{O;8x+olwZaew-@nuT-q(YiahOc} z#ds)gBK5)?PrgmP;Gi_jlD|pY|Fa~JoA?bSjm*t`=~QDpL0a^}_)_tBpbrpYTly8B zLE%-iKt`go^Vy=*gsB`^hQOjGyi=CG&W09`10T^FSJVQ9!c?Z5E}On45AM-~Iq-+@ zMg+yhWfR`3PpiGW?_Ayr^KUgK#Mw9z2t6KpmiDe0TV*ce1X_&02pp*jwPTI0a@Goq zoA?;@1~0OYld{g5_S2lErM~{B=;O&w2v$tUV>*%@EAhKP6IQE%%)wq;&jWlO{b~!G zX#*7J85y**l0gK#Hoi7x6fM^*r6@RhCCs1|4QUNEN<*Bo06Y8#)S6V~DGUfnHu&|p_sxsd5#Z(?OL3Oh6aA#>AroxUqEPVs? zMSv$P13-ggKOs;Pwf|oC0;a94lv5kNZQa$PbEH>hA+MczJzZi241u$cyxUrOKCS$b zbeVTriV>2YPXiqVsD3_id)`W_bV;sn+6OR1Ny9~4RmoepB7-se(CJOTmwC#l2jyv*8cEt1&0P75x6RgIe?kl$l77X{WW_*`E)E(lVZ}eA z{VDOb3_=kWb3QQRZ!$DT2odJ z#{(t;I+LR382pNq(ZoYtXw*s8K17d?87Hb^-Pk1@h!mBK zJe*hSPPo1o=A$8nf-|4Go?hqGI780orJm1xpH589k84T)Lo!V;Lb63|uH+Rl<_tx) z{EZJ-%2?KT-*yKEv}Mho(vB2=@Ct1JlO&#$4zZ6jz;i2}l#vS^do5R^kr`U2zH3#C z<2r7)u^>v5Z6-5p`pwU0q?FaXJeLyL5UE$Qq$G4;(V#MgN4PQ{Vfzdh9pziP>QO_Q zA!*RZNZUY*r!E}Qbu3!Cqs@i@8L=qmMm3g;vK^PvuoC81pNpgwV+PeWa!FR@HNdMR z7|EE#0wDXHXUl)Wr5506WEqrEh8UA{%cw$+=)tFDUJxieP(E6zk^Z(hFeuIFa4YZM zUkSi`R}#EEBk+IjE8v~& zW72fW-OBE|c=T8>j8GYvn|w_#-qm_VI9y4{5Umb8)iSjB$d+tDLH`Pb*jrPHKVlSO z2!3PxOxhG+BHm;}>{C*n@s%3!*c8oXP@Zpf?G!~P#+Nm~q8Hz4e!eZG=-oQIWmlrp zjg+b$DznXHlWMb(W;3OGHnw6I-Lnd|c$@VWIW0-J%}69OvZ^@!2RB*phj21^N;`wB#SZmkp%6Jb>Do9-7S&5sxRgdiW9 z{xEI7_Xk_XxwsRFPN5}@R<&e+<53DlzRf1m6jdHKc7RRXlNpcLI%I4k*8qaHFv zHB#n3>cSb3a>eEm^{WxrR|vok8DXL4*!oQ0*^_uLR4UK<9Osi}G9co;vby@k&(F`S^7*D9*(YZjwi3`e%t-?eilTpTDNR3yJIm%pF1Re!N_2v;-%!T!3 zUxzg65=+ILU&z&HWt|1Zmih7+H1KxOP#@x~?4n=V6$UCyq%N*!2JYft+Z_GVukF=V zRv3&qn@L7p6kF{cmLGU&HB1;)1y^bk1;_A{a0(>RwOsQ7-E<3DHEzpNM4KqC4Qb6| zc=%K?(Z}S;+(OykQ9ClnFio78;&O7@G6ru*D1mBAaG7mf9)*C|8N-(UETOov?5fVCQ%?bx^S z)KlijOGs-8sc3(BVYm=|wLi+d+;rHF??GaNTGvx-Q~@%-AEYz%))RH7bW6V~E*!Yc z_wpHUH_AxW&^-eVT-ui~VETJs!-#U_6T#E8Sq;A(kS7i#1=2jtrkN1gHNNyaujuax z5~$7{!RwCCc{p)Z5R@&CI0(9QlGbnuNRzj9K_ie>>|4$8_4O~M+Z@|AO7Y|-tYW*P z4z$?f{)^64o_5lixB5cj%;D4PSd}g>7OTh#(Iu3woruTNq_?U!!G|vklHoN^YDnB3B<>k8<5^yY$_4N=TkyY7&r6KfefwFiLo8&dBV7yF|aLX(xVEQ_9n%KTwo%$CcKK={ggqsY@Eiu_7f{cLnT% ze$3HE@HZo*+%+bUXohF3s-uhN4aXrJrt_(<^zb7SLW}?_9l2xlR0Ghvl9#XLteWZt zInIZ_N_AV7QUg>57hfsLMpA^O%v2lXdi0g5B$W$;4ZbnJd2j>hz^^b4-DV)-E?*`{ zm^YctQOxIEv4yQl8}g|uYw(FnoNL_LuxN@`$I;Gs(%kW>yA~+I$^UyNc=eJ|0Xxn^ zEmst(3Qs?;Bsw_Df!Hv>hGdB8R!$T81BaV>H*-fmq4(=5>+eeiPYQ{bYCP)7w6tW7 z=z1p2jDa|xEDQw>M4qqgf(i3RiYIo~tuJzeg8BUU{BUjn@<}W4f0&_bY-xveN(a?{ zzM@$thHev_cacX#yN0LrU$I<%3yx!srm(0@XV8xuvPoA)gEau%S%~-zCwMkMgBFLL zoHak+a-h|MX0s&HtKG1jBej%!uUal;JD1DSlAsN75G-ua8QkJgfpS{?v?G6RbN6O8` z+WsS%H6sznqht!rwc)?ZMlCMnYn#w8=c{C_Z~t9_6RI@+{MoKhk!ZHwpDSs$xv5i5 z1;!2;jVuRaQ+tO$3E-xv{)c0A%DvNUiGCTYC?`^*%)KP45_m!cf~W_HUpka%OBnrB z#$CD{Czj|^btZOZ%86Pl8`U>6V>qo&ZtL&wwL8?^J3BjS-({fd7};aF(cmakH_K574~hvxNX_w+cod!Un!^!KOpSKDAh=epin}<@C$;A}0?bCa#6EBa4 zA9zBZ9|*lVFRh=6#aiQ?J4+-K^#Re=3(e(T&*vk} zZm@M(!_LroiD>+5i{HR zWOTAP!C;CB`q$o;1y*8EczJ&EXkB?Qeq0# z5a7q?BU9N>y2H;@e=UG97kFw}6dHM=15aUhiH!O?2%A7@7^7sS2<~cbVK6OFgJH|H zJPkd~yL}4fiq%wu9|mrEUP=O>d()kg!B4q~hWJ}h+jSiBM#7As+4h;+i_h;KT<+aH zqW8Q!6|$)gPWb$8t-9g)z<0ln-*vyC|BlFdk2+pH9}r#^o;UU}lJtOZ(V9?rx}AJ~;1B=xccMiPlsw(fJQGC+0vnJ9l0|C_>q$*S zgMsDf0lk`ap^Oast#bSEjdkR@5cJu)BG98|0u5%hPlA3lAniM$H!%FOzuasi#UtIS zcn3}Dy@>{q4PJ>HZi&Uok_W36_DP7<6velP{~zy(=|2NV&VNb8$zNYkk{lPWG1w>l zmstJ(k7se);vHmhlTDTs8Vu}5Hdsh=$b{=9`x6eb40_j3$_%hT2VI6nhRfPC4IrVZ z^Z&C{96qGCPih+YZkG96@V`){S(hhb@VXV4Sq10EAmacs(?RW6NBfb18+gHB3AfA| z)4DR+tXj`_X_9U4+KSxFc4iN2P_9@*NRHvSJ$%~4H&Jhfn=maszgLP(;te?# zYLiF!o<^{cxeB&wb^JAa;aqA#N7g|4idVe%_mTuP|Lu4ZjVE`&%R~UIFe( z8)GU?vthvaoxWkl5y|%czh6Z)vI@a4Drm|p1M6XvdH*Kz3`RX zqkaU>{e;v=*R%eCMO3HMV6s-+?N?MaoO_wUjqBaXmD#53Y$oX8)Nmo?$JdGbSZ(gv z+_NAH4VC_6g_^#5XQg0J0isTapVOfbo$lTKC^sdQ8a;H?o8ea-itv>7vBYyfjgcCs#)pJ2S(^xz@rEXLw`DQo9b*;AnH@@{ zhR!e%(OX629UT=J1f=SAs&>4A*=QWsCkKR4aiM}7CAF@)!Z7BYmU33D@tY1(&YGwr z6kVlY7RKQf0!uX^2Iol0X0y2S3pGQ)Q;9j=iT|Yv@0HjPBJS?9af(k5Ai=Y-ObMKs z9{zTgR=k&oBQQ{Ef?5EL-Y|GfHcFYuJ-^HFId=U*kW{Jmh)~?Hy`_=A)qnN%%gQH; zDRrcVM|F9iqJUr1_HD0w0M)E<-k41m&9j8vLq4jhLeg$;_LcNVqLgq$0lMza>&l77 zCce1q9~%=|9q;p}EVoC7npFg9{?{INhvQ&OV(^Iq{a4c*#t$QU1FyNwoHhJ3X4A~| z%#QzF7dK(mso_>5%`U5`9lg<6S6iC&DT-=>JU|$eygG)IBhsK zgH3GOqmwx8n;#owZ65@9I7etwtMhJ8n3<|_lPbkabZ=ICzYl|3RW7-4V+k?im6j!kH`&pW! zHiyppDSVfMADbVlq8nuK{@*VpG5CU(iHo0qH_#2g6R@9`@d)OBE>{J=Cw@Ps)C;KUuHFh6%~Ljp5IICoWDV+9pocNsHvM zx_u{j2A#tK->Z62x=EJLhnLLvj|vps3i?kb{wq2eJlYE?x}S`f^wgd#@|RjU7th6y z_4rPlKaRq47JXtXdK`x@kVH*3^!n0F{`1G+vGV^NUp~ZyToqIn{nZ}x<(>TT9~tJK z88P&m730QV)Ubt76q>AiZ!F&+jk#}hczFwR|L0+WSy&c+p+PU@{oinexaP9z#E4Swep zzkeC>LlQ4PJ|o#6hwt3N+&c*mo(ocZ`%9uh?uUS-&ExTzf}a!LXHK9|J9*XU@0hRW z3t6A7PDa^KmCs`VK}M`X1v! z(=8L0_LF%#{k}H76EmI05r>22R>2|dg)z45GCwbDN6`4f7B|bA$c1dZznPmZCB{Go#eIUXO@pGX*-K<5aZ#Q6D+n@x+kDqe9MZQyHj zG9e4S3SH7YGep$C<>p17+RPMHT=lK%OFmU0?{|&$i63Xfl()39WKF1U-godGwzSSp zs8=~bwrpT(b?Z!(yag#zz2DGoE`VG<6eXo2^EV|$S~DX56yiIt3|&h%Ox4W2ZZoSJ zjz?`q6qdPHll0rwKI_A271BLgsoktqi*pLS`;Iq8$dy&wu55JMr#6O=hoJu1jr!7} z_6&Rd$sN8tjg;VRxzd<+yE)=AioWA9vXt-=_-!rT)Db>n_JzD*^Qw`!=$>a{E$Xj~ zUHhJ;-saK~6GzDo(4~={rYBO@8eWvyNOrp3=vR`>)ZwN~x;@OFTE}a$vv}`|CQ5HN zRfneStnDoOJ~vxkxyMC%{*@`G|0zSYu)fZ13~G_cX-CKMlcAf4x3}bu>qn>yUga&V zOVIEDYuzfGejKH9OOd*^>$a9^#05;Z$VxQ$b#u#X`bhD^-Lka>4cXmNsI{E`?Z4fi zdo2@kc$Wza3|_JUjX1tUyA}Fh6$IFv;3q$9?FEgyO`zRAkYKDn1^(UKR9IkKuLzP- zveo{YFMtuRd@t#I=5DwfM{fYwWKu#-L4h+)qFuE-_|dW4mG0g|Z(KCr^xe@##+NebNuK zQ0F@PJKwSG9y<4l)vZNZfqCV|-sVW&l)GIbE8Qud!qS>uCd=USwB-P z+4;!7;?mFYm7|cb>1X^(*&<1+^(phVxX5RRQoM_;SRZl5m_tM+k#Xpl-i{@1HuENb zyf;BDq~lI2rR>_B9p6}a-8On_En_-k`WU@q^ybwMN4x^6^c081Nt zZBImJWuJy|StGc#oyRb&%*zY3x*Qhem?{Q^oCk%4>)1T#C{+zN-iN>6>l- zI@<_-+W4bmKm?2+!A-oNyzGPYNpYNY1)JR%BRM^}>FRYy_Mc1Fjhy7H=z*^FfT(-| z0Ru0I!3)MazYO$cf6Dy4@y#{R^k9~=9&65B$>z~x!KL()+7`ZrlmI&6bt>gwS zk|tsNX*C1yOT7nUDn!%_O2`xA*!exVo`?aY74*`XXe0Jr({}+>=cssdQHsd%q3 zPS{|-qt))X&*l58iy=a|c`Sk5aVnl_HKQXUfJRkV(7B|$i zsq{bM=ktw)U6s2}rqWhM$*?qbyHkkGZ0u4hir7S=O@gb=~%zs6K0^?@EH0s_2$j?&8CvZCUWJj6+{$A zDy24tkE-TTb_>GoX^_WJmt9@41J>K^h;Ds<=nIXwp7ZNV)6DpB$2)ZNgof0Dbme0@ zJti&zu=M{(%BZ6Q9;_OPvE=;Ib>=5BW@CDg@?I;pk+>_ zm9?NSv+^`QpRw9++eoBIfx|^Pv2frC3!2>r6Z-LEIi;yVO*5uKDXhu-l7fN@O~yNWQkB==eSPBPd1(~mM&ExdeQo&&-Npb*=+oJJxVg=IJ8Ry@qfh$R!k0v)uV}7zE3V_c z=u=x}fm!KD!+`QURt!s^gmp@V291Rt?9Tv4&aZMa|G)s|9|yQk#jqcmv9WB!W9tng zA9i6gMoV&avseS>7iuz3y|(rkQ&@1qAsR%y(tL_~yGt9g(pKYW?)TH_H~yq&QDae9 z6wzWi9BQqqK>(*s^JuJ{b$@Q)2eGv#91=`lW22_io9_;xAkJY`Z@Bs&2*-FhFIivN+lF{ z+&frWPOTZNcJs!*vE|bMv*UYNiDrV6!go97t2$LXe0^eJ?q?wb*F2@_^!#)8^^DAI z7R2*5=d;o*I?awHo}#q1g!PV@bH4>A-IP}h4sUK<>I~zSQ`Bp5FGYymNKk*@ zERx;#`_hGst>#mM3VtZ_@2w)1s|htDI7uKc+@IqJ?An0|+~-4^>cDQ-3yoicOH z7t~BmEQynXOw~j>r<)YV6h0Kh;>DT+$>z#2ik|$-x)scSR)0S=erdkf&1^&7iHsh= zqz0*25wt#dpt+s3lf``9q~+)-Yh0@%dF$~qoep`?3`Y-7r6VWlcXV!XMl$)f$oox}RlTq)6N#d}t6SGz#JjRNi->mE$l}nc4A&*D~>*Cmy{7tzzYyRya!693T)bnjHY$uV5zhW zsboVVe_jL*5ZJujHYi1O8;o5*`Zhc9mk=`!g+A4| z>Bd5p#wp8(We03Tdaq-^*h|i~$PxqI5l;sKHMhW$_?(6tTTfHx4ik?Oi?{Bo5YRr1 z+hl#LVJNCQb|M(Jb$6|YdaBw;u2L;(t` zZyy7X|AE!8bPN1UkV1|T<96xhO8bDvWy~Gk9HkLCFt^8Wz}5we*!NE=&x|#m-c?al zh}AsiqE&2;f zF%Bant#|jvvoc$BpU*bWb(z*NT6Ief%kG9v?ik04am{=>&mN8O3}eFSm)JHOi4$jJ zoZ+3;unJ@pqi(GlbrL5_xw|nz-(Nv(S2^{5Zj#}zBk5RWP3jxya|3nWSN67s1V*Du zvjtInDGKF=rHA>vx%)_#vJ1wI_e2zBIE$apsNb~Or)LJW@FEI6)X?wHoA6&JR#9&3 zoE1j`Y9K>SXon(w!0WUJr^mr`o%)OcAt_jKG(pnS;<{Hj(Ez7~1Ar6dD34h0lYY*l zVgBfm_`w^QKS3NXi^)B}A+japjUvdraykm{d#8Uo>g+LiM2wRf2SQA?GQ>^`YoSQ$ z+sn}I?XK}8WMPr!O^^lVJ@Vaa1)@s^5zxtjXXFCy=tlck_owQEY=;~DwOEog?@dd@@;{dFx$&?*__~Qu!dO_S zhB#?;oSc%t*VgaAFpNd~)IKDo+J$&4??>L@CFetFn*rCe`pjZCA2sRPl+TY!64LHL zET%GxHqGZ;+g6+>xvYTEWBEY)(h;wsjmECH2k8-=-!1YFKR%pb+Xm|KS;(T{q z?X21W3=#P?_DD%G*_00Knh+C*@h0T;^I7o6L1IMu9Ppx-@s)0vtewM+EEvr{Z#L-b z)laSYEl(Rhj*-HlcRc-UY;1q=TKQS8 z(gpW^{#Sil$_{a7nfqMIHn(~A`qjXI!B+9y}Ss1UHgXWb#uT@1S26^P~L`ID_(7#V%j5l*l(?d7rpBKz&x*YIRMXx?{6~ zF!5H%#K=>)(#~OjEmj``hKRQ{x(i?>evL;{!*;Z82;3c(7WU~~Vw~niF=g}#WTtAW zlCiyiGU!JM_=Dvi+#_81UIJx5?-ON@%uv)D5Hsy5(nk}rViGSutmxwRsdis;d7$@E$5<^ObuGVTzcl0o#iZV?!}%Y4Z;-&PV8xWGFi9gO%JYdU`*eRY-qj zf#wh6UAY)*2%APuwSHe@Zw zix;Iif5}a{IM0f^4N?R;)uf*VZ764|INZ_`_jUE-wzn#JZ-C!*z9&K?*esSBfB>~F zQ2i5wjmngDq+c%U@kg2ed9pqWVdJiEB?M}ZpQQrD`O7E#n8Y3d%Ak(|aZj<=NXM!d z@(mv8lisyt{#Vfx+S~M}Jb|<#74Y$E-9dB+mTcSAt>zm zrM4R;+rq)n5*t&t8*)cot;(d06=+WpjvYMW_}R7OAGgvt==<8@!tO7)D?u$F&PMbL z(Ux2u;j+rZRa@Dmb)P=r9p+6>GdF@#1Qd**JokC{TkcUnmUpUF#9(5Je)4+w2Io0j zS{BDBueT&9n5D7g01o|j5e3B-9U6Uv?Hv|_`94=e3!jBp_l$NP3(j==CsSQ%_cth# ziHCx?=AxcySV^$=EH5Q+J!;!bEs(0-@OzGd>$~>t+h0MT4B=7D<=_aqHrA(Urr?*4~;B)Px9R z5ng{u*yZG1P~!@538mh@<-S2DK&(Q3;xw=m#Aq+E4w>_#tqV1rUQs)#YO!OBRKe${ zS0lu@;?w^(JO#dgo>}@^4s2}Z>V>FI)L#u#EzOU!*=<{l+09zX!=-kC6E&fyz+~ML z3J|7gIQ=94N=pS!0x8QPsrC-wO4*@O5Wg^5B-=0TufZ5Z^K;i#sHNSOc$)Lt&Ey3k z2S}9Agv=X>4ybL2L3MsZ^yJ53f%h<65=PRU1yuM7IK!bSEh3V+f7f#bqz<#&$A)69 zBDVV5NFe}SQ9W|$5`CANVbW3oV8mB3G@P%AClki`256ucG+nNQxK}nZG=p}y1|W~N z$rYgE`AT{7h5k7#`3t%-FkAxNJlI;p4``-` z_ta+P|A<)l0^|acZihMfISltwzAUumN3y7DYQWflyBDYamyM0!+V&`tA>xSAZ*Zi8S!& ztj_O4`1&<|r)k*AMO9oTyz4S?MID0_-0~03&f##B>KlAaRKi*>x=@#r+QAHZ*jRVJ z2s9V?KeEmB{L6)=1muQTsd(<(58mH{>as-vy&HY`Xt;KIH~&^<1!c_4>!+`Yq6m)PmVGv zntL+vS_?;W0K+}?6)PPCf*5)`b}=e2jZ8B${<6iJ13`Y6)!|)ICO+&%OhFgp0+9wL ziEBj$Rk;#?x~VA~vZUtt?!H~3*cTW39;H`;0^Ilw4HCy2DKv)Rhp|5-&M5>RdHAlU zQ4y){Hg`3;m_7@kbW8DOGpx@&bO>lpQ|~CcK1RNRplL(ke7}|=vDoXhiod&u?k^?A zU;(E?vf&X(or)+Cr&{fOC>%3*Nb^(N{zQTRvf(bN6#BOR?GNMQHUCj>Eh8TOyoE0N z91cK_oG~s(Zo`$f8_;!&{G_FjJ+JwGcm{{FMEk3Mt2L+A@0~o`!;8*1lS^;Jyo0Qw z7pHhKp|uOQ5#we=c$|@g1O(H`1)VN`w*~u1fANz`f4eHVB_!>=A>EcyS}@Ug!2BPI zWkT^2)ibg)E5!-3C4L+b$?3-0(#ishbdQV(WV@4ie^O+@y~c4Llc&9H4ii_kBf89S z{k^$R*Vdwv5YHMRR7sTqmsndz&P{2|M=5qHH(tr%QmQ+Ncdi`kubC7`E0P0z$c<8= z;!Y=G;Q4b&d8PO$L4w=Uya)L};-X1(({4~O#&%*+WLT{5g|nKZIKkRsN~*(%g|6@O z?oTDH*$juweS(oPYdg47*1;9z@A%%F{wPu#PH)ZDy{_Znto3gXlQ#^$W%?Nshz$M+ zPIk}t1r|pS4X>p};MwKJT^M}GMZ|*5>2w5yUryHQKnChF~aDQ7gIX+N=cUddXFqy#(<2xlK+(u_p`~TjO4$+HZLh7(6F?opMt{e* znuD-nxBjR{9?X*E2C4SJIFI-;d{+>-N;ghkl8O6^@_-yJvA2f@B8_JIK_8$ektY(TH=O4W z&W7-9+GAR4z))SJXJr*|CM)nfw-Ep23~D&%b|i9h*Y;su)grH2++?0YfC=6do}i>k zA7IhUAv;FOvV`qs24#^8(N53QrGA#8epnO6_wFOXu>)P^ft~gXxYBHxMc47=XG$R2 zt771=_W={xLICfp4$kpUW1L<~(-#8MkwP0(f!R-5OD7DuCH4OWIKn;G*G5 z4ic7eFXC(N#7tGoueL%gzuywNJ?G}zjdfCV-oPS|l0Ucvp}p0!WjLfW{S>?x9f^FWG)lX+9_pRKhzY8bChdgL=Kc zGO2(WbDW1~zt;;YaN9{nN``W)?)cg`KkXYh&V?n7Vj+h&3&yG5IZ9l5E9n}+7cnTD zx=MD))zSt!tZ&&uA*c2O+I8(fX0V}-xu>DGYVcfREKv^LMcOV~&N~B<_p)brDbnWm zuFF*;QoiE(^_dr8UCX+7UQ&gkGjTxw*k_i9j1oNcd39|(AUb^LJ*pFGT8+Y=9wRvd zhg3jWJ)o9U22=zQ!h0v}mu*2@{BB?h-DtmC*;kmY)8XZUy%)qWWc*w+1Y7Z8?v{&~ zMSYSHp#;6yvfp1?de}hG3xaT^Jm}1Mo+i0L?#b80-mgCZ7M)~xWD?}QUM3g&LIP;o zuNHVDl2#PLy6lbyENdWh?BK4{j4}%FNi~F}Uk+Rf5z8X>f#IPyo{_!Z>lVCAo8|o< zomHF^t*krX&4|V%tB^gtvbT9L&@Ovt{c_(qX)3K;eidgBPSRF2WtQ#CIU&d3m*a4D24RQQ` zRD^H|&}PN|4x2NFCA!7Izyf*lkF}InUqEiO|DEwL!6A{T|5QV_}e30Ggi== z6s*XnLIvytI}eDy+JBM+reBoj{8Q1Y^{|!N?V}~3>Fy~MhyhC>XImWLWJNkV;I)ksd-zP|1j=j9DXP(_@U9Wv$-gJR;^L`5z+_#6C7mMJaFbjdqjm?J zFHeqg=MR9FL#hmmI#w_57ZiZ0vEp5v?L}U)fNaNm6OZl?r!ffA;KKOE~hymV3b*tBUsP8OLQhCHn#<*Ve z;sVMg7MbrCS7*IO3=4$EKnAKAKY(61Kq^ks)2@@8fz$z#5GS+G8?L;&FqG{++cDam z?QTFQKYb470Y%>$;Jm&5-mfLe6HtNI!uKR5 zA#iSTnVdhL>gC01|2IKxuFj@J8Po&3l2{pD@@ibBQ}Fpf4&t_3tmP&W#j*L%mi$Cg z&RU$pMh!#>#lQ_QJrX+zKgALml459REM7lOR69<1Ql6tor&>4kL{R=7&A$>}#qQ z=AITiFwS2DJopQlb9pENI=BRP%l|s*Z)@d=A(5c1aa}It3)6_w3}X~&b%*aU47VRK z%=}b8;P9SrI2|3yY5+l**duSGLbJRPgQt1)r>Mq}9aZ%CffY+GWp#h^NH3-5)F6TyX;nKtz~C9tJt~ z{(Cp-^o1EzFfA_Oo|G&54@8w~_xAJyk?-xCOCqc@A-7H7PsG7&&ylUe$3sZmL5cMK z(_F`QE*#me)w{5;fvPqJmpV^iNWmX!sDr@C`n-}hgurqiSl>%H{-Xp~U;Y&qO*Wt} z%awSK^rQf%;nYColTJFh*9W|O=z0$mz$bq>*u!}6FKB_gRMB5 z^QVjjwz>GRmtS`xQI}ya`%Ce*nW@q2!6_wNiG!S6%%fLJG9wP54Y4Z>><6kea!&%h zrL)k|!1}`d?M~6I`&#THe14Nfi7e4{?g`DOOvLQw6=qt9xtv#aG*_fC{LtI6LgWv= zFLd5*;u91Fa#eFm>eXMjx%{W{H22~f&4uqEm>Q+jlD^2J&8_Vtr<#sb?!WP)q)_}x z=|lE2H8}3ecaita(|tAE6z(@qnhRciKXBncRnYoxs`Srqszr4Dpz7l#G0NVU$A{ML zaGYPiz0^=GF?c;>XP3@$t-ZuUD$w`6^j55#Uf>D7}`>W*J&%#4={2mhH9P(P8i)w8tIt#7*TlR*4Pdxz* zE~!5f2`7Lol4MTO0ICg{-lH)iq3yq<4$d|RIlQz5ZFJaUdn!6dS~yN#Z&~;HxbxpH zqFn?n-gQFT;80&hd>%ADWMO0y0sSXi;2g0nf4AJ$Jzt>8JoQ+@}?EcfE^6Q12}U z1h^x$ekUT>ni*0Dor=^uF}}_Ii|0nuWY|oyyoJ6eGmpoZeKiB!$;Dz-V^r?5AoA56 z`XO2wsCipQ_^KHsD}>FUPDKFw)?gov#OLt;tz$yCo7Z+? z2n%M}aGnBc_ajas3hjGeN_*qv4GNjYw||=wz*JL~jNwYha*8d#Z$9ve z5u^MsrexF~%%ii__r)D;Vz*>6{m6nqT&{Cua5A`pb#>>D#HDm+i-5#w`-;*gVq7EZ zmW1>VV7R5N{`i#5A3=<>%#j`2t%-Pl@r9X73h-(-88A{tK>*#UD4i-k$3da7CSOd? zAn_}htwjOpzB02ndQ(Ss@M}2_u^QCvIh+=N^b!Yq`?LmZ4_SfJtwqJ`Fi9|!hYp+B z8UdWX*jXXL$F!S5lc;NJo=z>OJGr#Ke4|te*a6G0&{lF_xtQ^o=Q*6Q(c+&9dtKET znykNvH>|epoT4H^)a(ce2scRwtHv)ywIGwHLau)2z)@%Qwt?_56ay8&Z$!t#mHLa+ z4uNe4zG_}sL?urij7^|Px;n&D+{M5UWcBlK3=Y~?bqAnS0~NUsSIYA;%-Xk*GcIX> zFiezX@`K}{5Cl4(YVa?OB!au8v6ys&9Psa?6(WJAHW8fp-w(QW8PGR^FE3I{j=bQe zLq3a!Z7l3cg3z==JvM6j1O`a^_%umLHdLIR*c;b<{>1OdRtEx3XDG_ErO-r~$V1E5 zYP-1^mg?CzX2LBP=ir72<0rVM>7RQay<@I_SSfsxO1{Ov{_dBb-L+_0kC}ElqH(^% zlO>UV!Or82>##1a;-?~3tI7f*R6!;0?wBRkMiBp*xSCvgE>90%cD!SD!`_#wj>OxS zoYPe{v(5Y6N%SIjPS3uhCb6uhE%yl&RF+0!Hpy zAQ|jbRxp2$Zprd{z-Z7${&d&3oGWVTYSg!>GddlI0RtGVZBfbWU$sHiJD2vc||2PA_=vN=H&ViEnN#k|URmH)Uf6p$N|1Mm~)L#Mka#9%lfvvPo zC@X-CG!-9aK={SzhZIJSc^pu$CfzDERqX9{ChG0+1ZSH6v(CiGyJZ~rB5JIbd zU{609ro{3!IfGkFi0A^WtW%M_hpqfm&*MPgK93ufQ-XT&=x8$5vb$}6Q0-sZS~HUO zK#A4NbP79Atu9<7)En@JSWUHV|~X&`H)P(9Vd`#K876G zvOCM90M|vHFHr%Zhy17^kotkE&O1v8Nxn=HWoG@)z!9R$&TPHnLan?~a`|YRsPh)a z`8EmR#0+#jXmk4ExYO0Zk_lyR^2es$#6fJJ*~75b!c8XL)BGwl>i30D?O{gu!HHz+ z^+V9ufaQKkgj3X#nVY!T%-&ZK`88$o=0(p$y*MCD%2geNtthUC3&NFZ^u(gszHwZ} zwnW5wKz$4CbmLl~M6r}~ zXHnThLX?Wjl}?%NyHv$g`B)C`SCq&Muze@QXK8keZn!E^fx;OM8uAPzoWYcCy-NcG zP$4D1$FoZ4`H4ca&fpBIPcILg=UlX*%4IXxmydumV}KUs;kv*NuAJ^?HlElz1-N}CN znr(Da6!Q-CHrQxBc{TFn-J0fkYULqcRCS$;oA~S_r|OS)^8K6&I4tv3D}IkBMzOQM zM=ynVAz28$Z$&^5*s0L^?52I&*%8)230(G1Qi~TFveAswgmjT`Ts7-lli_&$miGQw z(g8?CnegktmE!nabcn&i4*jI|ve@8t8Wm{ys?7FjU)Q^GF24O^%(riFCBm^* z5^+vwg?{C4+_gGw$Oah4R(MnY#klxXOHfjQh>P{A8$7O3!2Jbroi~L2$ThHha5c5Y z$Ujh!{>Wzr-ip9oCNqCrpscW(jeFdkEg5+mr;y35))92+KKa@xbYGroeEg{Vg{ zq_?BTy9nQWR-B6FjGl@UM`nA-`)S#!dBi0gs-#b$(IIW%rn#L0C_%FZ|sDi)0irj&@PH z_OfyylOlVNOjpyW5`ynF(5dYT=o<=%`j-WJI%&X5mkUZh2~A`eA#gqS5Cz1%aA3yE zQWo%`PzTb9*;p3~7F<~Xue$u4hPwtlwdFekr~jGP(W@^Jy8V%;3?gUgFk(;@^@)M5 zuLx#KI!3DZi*GO^GE}Tr97GpS^#%_jkZ)heCYjMvfTOEVyP4c@rEo@{PpVnag7{#p zJQ7yI_w1Pds^H}eZ1*#)#Fp>17s)#CD*drSVFcw?O}XCdu#Qpz1a4C%&8NUv+I@y~ zk1PA?uz(xO2GnN?qI+tFSK&DG@VBBJ4Sp5@%phE)BjBnG#;K+EtEvHNmw@>`KaF@+ z4=d4~{b4|;=I*=o03xGJZaBEZl`5N<3PpVwx8@b5EstQfdw*WqnNxyiqhML9mbY1& zv!d|zvLShzM;_4vXFQ3A>AM9}~iFjC#@ek&5Lq+2;|5C)08W;YaZ-GExfE|Xu)ON#2eZ!P1lrtgvol#la)~g`7upTp4%*|% zMLpg`$LwA8|5@>j`GUi9iQjfc2=J+nM&Yp-Fufr*M(5NTKRX61YPBUD!ogOuF|7C? zxYDJRkXef45;4w3#Uv#~0{BTdw8=}`znLh_aifh(?#%fRK`|o*OxYfZ5A5#{l{Q(i z0eUi*V1B|LcSxV-yzr)sQ2;{$@Q#AWEp}6_H|+OOfEI6!-t7X7j95QSsevAdC>Puf zk2jNn^uaqL9uH(I*e2E;8sS~OMOrY0La5iLk5zqRqJZV=E}#xDFfL|yd61(w}zfI)}?QGqT98E@87&gHeFr7A;f1Pd=RR}mH;yY<|Iy$}Ub zbE?a)&C3)0V6^QOMc{JACsOhusX;+U`6RKAZ_#IE8LC2Lyh}gQlKcQu$~oAMb%*d- zAqyPPJ{kSPZ)@ch9ES`ZKaKMyJ1%OeH*Ls$5AQNq+Z?>52&`S=9XJx0Th->VE}D{Zh!ll`!QQ!rsu z6T0jNYV6X~UDUfVsVF2Eb0gdH4gexGh>tu;Ei%m5zhU_b>YAr^@e^C*4DhaRwGcz{ zCBx67fohnN{DMVr&!ovMU-OXaUHRFsZG%#_B<_qYXkob0H5u`i>Mi6Y5IHzs9>FW& zJivDR4>?tJCa0C=wIg}$)u^;on600pBh|!0!p+4UsgHr=U^M%@C<+A_SC7_8CUR|| z4wAfc(x?-e@3}#gSG^^MlHwMY5$6yyY^uP?%~~}-f{+~CQ#eajekhztaawz5m-9Ns zN&xCSA-$E2x>NwOoZ_3fhgdl`AS3$jQsBdwIJztuuC$b+MSN`3I}QwYu#f$d5$bRy zo@Y!>uB0uVQ&coadEVqB3A!#xT4(WhJFY%DxV!PxU^7s>Tt zuCEB*#sA&Mm!WS(7@BA1Amf2h1n7m-$fx5?TF|r}lsKCZ}D~01| zxv>c`4mDOP@_Pz_TYcJt%69=%TK2L|1V+R-ajz{Y@%CYoOJK`1ymbe8u7biYNfa$! zKT)Ox!x=L=`w?m}2p~ncy{IufDC36!o>LB`i_W~hKy#o%q&y!HuIv`g6$V6dCHXEk zZot*$F}y2Fgk9eiTzDn-RM^1TjVv+Z?}F`=yceP#I0aC%a>O>Udn9yUH9ca$0g#Ng zyp5+RpUr{U65Ug2DOAG*85cWqih{<(s}>i9`;x6jE~!}|c*2rtkoqL)r#^kMEF6K; z`Z@+16-7PF@(d!zaj>~|{|o7DHTq)Up<+q7CeFy^Vk|0#r}!B`5Tg$(cHTW z0XuV;iEUCuQ6PI%FV3w?t=x0stK$8+q{sW@P5ydKVtIT_q{rmkl$BG;kYIipQ$EmV ze*;oVR2B%4K$~oxxbRd%Vw~7Pj!m!Shrjpa%V%#Z)U(#)7i4~(SqaLrHhO?KoSA6IzMS(#D47V?gi9S8tlJy zPr%oOsNvG|`^mo{_%@ZSROog!(E$Qy8HOS%4 z*U1&w5x5R!)>9zy&KKSlYe4iTYA_Ze5~gBPs=vKxs`9dJjLMsT$M?n2udng(Vl=t$ z4xqNnGi*M&CIL`cM#?q0f5d-5=-KywR4lI>T$3Wv8gs!Ie=t#wBa-rE_A#Y?%QmR7 z6BeW6j4Al9syLcoPm_5s4BBzNLN!?RNF#NPCQ6XZn6W%sFaS6e2hWI1{-33}Ds;&C zDx~?tyWESGquw!RvI62`iYIF6AecAZ5g5Ku_xq|R$22b#aF=N@6eCH1z)72z3sHe* zyw4JC;ax4`#7`yZX40U^JihWVe_n+O2eZBJG0(+`!1XIOkqQkaeSmj;UsyRxH#wyM zHqii~#MNZptNhn06H zm+L0Gh2;S6;B6BqO7Ynxnbr%w1tFR{C41-8By$^N6@5I^i`c=fI!PU#-xiOeU*{&U zQ9R^Ly#E)j7*|7u(9)J8+Pw0zfs*R@YCb+x1 zdvUko?pCb0yOg(Bad&rjcS>;y?(Xgs3c(%zeE-QwcC%+@c4ubCo_puHmG(wtynu|O z+zFW$=zwWrEjsX%$cW1g^~<`J23G+WSbe8b;L{)XkL9#zFL0M;CHXr?3WgPsEKsR9 zo{}88PrfKWJjxC`2{N%|R1FhOh+@|saa@H4%w)@GEDh$usOuzJ!AZdfED+(|@MsVp z$qild0aVZd@>3Uk8HJ2g+W|5OUC~$8Vts|C$#gVJIbfw9)~9e2>y(ZVHBEOV^f9kb zdDO74Z(a5-GHL8u<1_wM&_6?f6v(K&*bt7-XYx;kt>G$aIDnD=1A7V~-gAj;#2;ue z*K8qv)&toVxB!OnLKAvg*9)b|p@gU7gn;o58MbeH3g$M>c_;&Kkp6!-75#w2v@#DV zOX|b1JF1W^^JOJ#|atQ)n1n|B*j%pWdH_RpKE@YR3HTP%$%WN24zDbe} zcU#`RH5=20f7dmeb?n*;_Hy8Jh9r`ecv^gOt3M`w zF~<-k&}!1sSzbq*lsh?-;o#cLVT{0Tgg?LD=q(ehMCb7o9v~{B&@rgK(?oz(GrSdI zKpeO+yNvvJNP(?;O6(LBP|opW_x+=vD3}!~`B~-PT1kwoqHw1q0R{p&00o;*gi4;B zgqdZ2)A0pQKr1Y)mAK5>H%OWt8%8yVTI$~BQF=1NOO<+oC*yK}buB>yVmy0FdJgmX zyZ71Po@W%}(*qpUq^vHqdl(MHKYm`V=nazo*n-Si$QE(hpA1-(Z_N2j_3KXYy>n}bTZAKyp$hpRZIBhN@ zq}`aQA?i3`&hFra2S~o;Ae?y*kTyA8v-1OJA|l=B;t9WRBcmp=YVx|=_HcXVES0+n zu9RP?1kE+7XyOv={ex1{WQO$tHSTFURq%R8?f2YPM?$E`!mHQOR!QofvgzGufHu@4 zIx+a&PY_rl(o4|8;6HQnV9GFi!J&)jb@HjRaP1Yyn1XsIgP_oAmgypRp)~tkQs;q6 zlb9yV%yB~}>24MqCpc?zeD@9ScSLuNjj1T76Y!z?2-|1wHW^gNmban%Y=7`jEy)~R1I7MEw-CDNULG>1ky3;DmQEBFo-+$I z-~iTax9XXK4eW3MBg&Y(*v?O~{QfF=v2)oo*!SAvbX=NBb^x2U=IF*U#sm^TS){rp z5`@af8vnGbG>+Z3!LZ;;%|Re+tol|YRpjtUVdw?O@V*S+Q;+QxCF4u1>fs|IM*)nu z_|^vv{(%f3WSTxw7{D5?b3zh86@$;8NIsSP6zgA-xx+VW0I-|V1J&DD`Wb=<$rC}& ziqO*3CHYlGFr`a^dk)aVF;A@=Js1En7m$@HGVmH_cX`sZIzDX@gO9lHQ!co%AcPh& zU&so7&sCiqFzsRe3Ej60A(lgY`%H-+f8_uSp7qb607Cv@U_ii*A0H_Y!t*4`528QK zV?Qpxaal|Q#GJ?t`?l#9{+1A`us{d=^o6LF5Y#2Gx2bi0+*B|^49>YH+@)OdlOXY) zzzNM#L4ej;FwKQ|T=sB#N!Rf18GvG1U_%5Tw-#Oqp>BJq5kUj2iLlDZ00T44ufJV% zt;qpJ6!D|MRz}wF0R);WU{`C?ZU2CYW=#&U!x}NUul^-Rn+Oc65&T9Y%wM%;_6V{Q zQKbs9@@nyr!|BX11Swj*-lu^e>PTja~zOb%#QwfF}^FQt`sp{eTu$hb3B zBKjMG?MT0P*79zX`Y!->{m`j=xrblj0F^(>?BD=yu6SlPrbG}XLeQ1CgQsjX-i99Z zF@#qp03rO)%RrMC#4Ncb|7TYT9}wXYGka$abLVdJKu0RoQoi9*!GP3+@cg!Lg9^mt1m0E~{@b&Q1iYQ>$_#_4w#(nfh zUeSo8*TgU7P-@8R(Rb0KIRA%j`o==+zw*s6X1$f?U8)@n2@o8B**67qbq6dbeP6-P zLx6Au+}->N^FVYmB4gPVle$(i(q{|Mos`wi?86p4?-sM zzNnK=EB?DE#VN^F5H`(m5W+FlWt~Ip3Lq#AmRO!*oY5b0%wiSiM01*M>Kkr91P1sD zkUs#_a;aZJgQuIKqAgcTeXTY1SC+goS#CZNluAnuMmrIqsmK5lC(dZqrxhEPyMC%Q zx9S8d62%rfmVkmHdD~rpODphfdVq2Op+tLy5q>3LZ}APn`0i+w9XM6jJ*rx+7}0g$ z_qw*Rw4g!(VEz|(1F?My>&<7p78=aixN3GGIi@v#_esvVR{sEq&l>SbMnWJR@cTaNOkb$H|sf2{Dr#;j59P|@rn8i#-kC!d*@%!%U^EuorTyy(f zx1)bBb_vA*NP)D-pdGNTv3oKd+giK4_m4zt*bOZw{@dDS$91k_Y~IZKRGf^B3%Gy) z%K=t&0dpA0ENK>ny{<;a^AIK2e76!Fph>tX4PTB`?D~-lq=@4%arZ$h3@+Rq8DrEi zWnmRms}KS>=*ZH)wVSfztWst>?nA8JAklh^{t)DQ;BGkazN_f@0=W{^?`wEUZdG3b zltqe0Zr@wr;I%wqcb&;WB*IEZJ5l^yve3;gky284g}g#leHXKDY< zGp4%Fp|V0qXIRbr_#}T23wAu}+#W&r>;RH*%>t$H&M{0o&t2@fbc8wz1gwVj2bEE>SWp8r8DqH{_A&Q0;u-Up&A zbK%4<*bO21<wIsoV{lRtGX0%(Ea zB~#kXCzxee;dV8H7BeI6r!*Lx2-S`iS4KLEgZ>p;Wl@L~{kTWY+po%xzm=wy;V#(`M# zg&M~@v0yp+qII(gLifGYKA2p}-?j$q&do;FykL>lIOGokcYCyBCm#Upsjp8iOTPe5 zUwW)EAZtl(;<(Y>DAW*=_2B6nYy3W2wHBZW{>7AfLnzRs`d{k7ES&au|F6eM!0zkl z7WSD!`Zsrr`d0Bg%@-&(4hHsuIq#&C=1;)xD$V2_DiVFc_vH}*OD}-Sd&?Y$!&eBd zVU10n;|CW2>?b%0SqGYwZ0aVPFquIpjd9r<66^4JkgoM2ryTU3xP+${)1?N3t{M+Av-$#%$2R)!Rfqff&C3444 zOXvVoqGjR}cL2gNA&)g5pe2%PJ{V7H{zHx#x=%_5Yy}|hcL@Od zteb~8^8gG-kIF~{70HpW=roo)%LQ9Qo?$lr}9^H(ZN^byCsGsnkz`6k#bieP82|}6) zW}mnYI-u@9XhP(BfZFe=Sr~{BIE3LI?(L4|5*AaV4uILBTT0Fr87WQU-RA?Ge&t|4X{lac_k|eG zoE75UbNYc|4=HCNFf}z!WPA{{)dzmZ3qOG8Fz4dQr1DLLzzull$!f)J<3ulj-sKn! z*{4%Ukd29f0>Gjj;mtAu$mO_?obq5FEr2-D0LauF0`U-tY#~eaoL}AsV%NJ^cYuN* zA>-mx+Z=KvRjvWN6SL5LD28S5tY`qjjISb(?`7y;C5F%Vg4al5fRO!Osnaoxul&U* zfcHRIa*scx0IMRzg>~W4F`x;NHYt1)IbeTJt?nTdr0@!GX?Fc~piBaI;Hr^4`R9uM zAHb#z=~4FvF@=4Et;mk&NK+7y;$Xld#R;=L4l|0$gAwtC6aY{wP!JQxg3`BJM&A|3 zr#PbO!vRF7l_g}A*}f=@rY){2Q$-SHOuO%XV|l^m}I5yx1vwWnffI1o4oEGQ}l@W25RIK zYIs&B{p_CoH)yb9U~CLq{TLBv77s4F2n9=I=qq>Ys3=E1GW=602ao=Ktav0VM{wxk zmCir4w+OB=(9PETR(a0EWzpWV$fffRM!^yy`j1<%ORGvm1QbGnMnVd4o~u&Uy@0yV zW2%-aiu#Ml$S~Hk_g3{WAovSd6W)&%3i#_QqBJ@tF5lK)9T2plfuR^0NjoML^PQ+J3j0cs0$M&b7 zLgXDba^??)D`G(?*)Xz}{)1jIlm2@xpa+SFr*MET01**1wdeKQaK=?m9y)J1 z0RBiZg|4^xq*7?TJFJtLP2l-%8!LG#X3|Dm z@RW|DL8?rE;>G`iEKY*Owt(mn3!$YN5oQG1i$a$`18sORxo4_NJq;c}EE_`g=|&+# zk7^bSOtAulhLnOpi15%b4KwOcL3r?A;uaz}-V~|YjZRpER!#(Un2|rR($*A5PD=#l zY*j=x`|WdKY~0x5eFVt{MI0QnY8{y{fR6*c9eI-9Ck0#e=#kT-qyD=WrC6P(S)hSN zsp^*wMK}#uUsaPe=3_TUT^y1`M*JIuSK7kX!qy~*2%fDmptpdXi(?VuH2w081c!GH z6}4y}Q_P*trMS9l;#+bY18m4NbjPh2$ycQwOXMZofBZ?VEfyp!E|w%K0|(N^G=~5} z8jFOt!~3l=d=?j$!vBurqjM6aXGU+5?2VY$aDt4-Zp`w``fK`Xj1uY%AhP zUW+v+9g-jM&UEah=_gVr0$w8`^|-rc?bocDP$y71&fs0}ZjbAbeflPwZ;s+=%9lUCwGQ`xiW9zAfrYbNm7|c{fjul=oL^EYZwjH zzxH=1g$zjqrJq;=?=xm){+(dMbVT-08god(0)JD@C8}DwA39C_6{lPl3xTMLmtl6=#6K zA%z-WNX4S{Riq$oHbCO5Vbay^3At~!(f0}G$Df04Hq1|}W$(rv?j)$#D+IVq{#47- zJ!_o5jf56egnft-PHA!9K2LY|aQ7fD@_!f&|I&0&WW#SQRz~maxBVuJP%{IEnJg9( zOr_2Z@5X4SZL8fKJ~N|{yT-q7daOT*#nLSIm~i=wW>o)$W^zEb(pNNO14BA2jtYdb zf$qkxgD@3N5Az(bC`!@JJ~kjtymJgbfp)8cR#OO991sn1Jp z7nrnG!9|=uuwlQ}XY(I|E299(WO%VniVHVwPCEFg)yhPkqwcJ5Am%jBR&v47hCJ}H zRc2NYV>bi))M~I-oL`7&8m$-)cVyEphY!UnchiQRZ=FV3TGn?q7M1G=x{Ac)Q)_xf zpzjz5WV7erkYP2B|Gg>H0{JVMuVFk!Tc%rh5?9ho%whPqH|C>KsJO`#sqv?V3H#mf ziN*o5G8||*anjm?>DSZ`A1kO;n2#|!CnuVm{=VeElSj*^nt7P&36UYe$yk1!(;BE= z>yw=?SgSu&U{yHy#_O;$vDVIM738SL`5rB%zWHLdk8H&=wclD<*ab8if(bjUH&P`3y#A0p;q zxQmE~HPU)JL}C#W=qQ(^c!g6GYXQFCipevf;mO*D z5ymT_cuGSXJy&AXmnt*@LVjI~Isblr>)VSw1|Jq5ckwDJA`tH@`u;|>Cr2Rm}=j|3_S=CRTtG%#AO1JmXU~+ zkt+^{#4USQWO~XV`lTq#5LMRInapKAqrpm1PNmo2xl1QV5Dzv9ysvP69F?eG=0# zvIz0C6~AG4AY0wy?h~fH7Gcl?`A^kgr4CzVtk(~}XeV=RClUHY*7!tL`8{;~hXLeq z%csNPG9u@-LY~CadX?*WKm`!xDHz>Z33D#UizfR@b(N<=mYmu9mJ|_z6xt$b`R^S! z-dUKl90#}qLxRhMzwtb$LxDNhqQ8_p^vXM=usb~^Y+Nm-H%yj2byqwmO&f=}2J!l0 z$}47%vL<+T}DbJ9KXetr+TO-_`lj;P9+n)z!QePQa=LC@`j zD~_oSC;}KlCx91Gb|F(i0&Tnd*qY*@a_fLE2;axd4LxQ?;_|ZD3k2emVz5%Nw3n1W z(`AmbPA2sPPnPr!bFP{x z8Wpbr7mCf1s}pPToIWN~5z{A=dtsS4KU$$5^0|?*NN?WlnAl$cELMl7Wd=h_qiG)9 zytC8Nxn;7FR4sj&Mso(saXc$g6Rp%jF}|^^n-m)U7s-&mhtx!ih03ECut}ksuLM^u z9|)yAidxSTMM+l-q)Hq~pyix5e_aQdQjfcM*4%#%iiQQaD zu$cwS%V9pVMB(TiyCX!`ZYMNbS1R%fDN-tq1<-wQl+Qylv1n$9y|Dt2li?k8;bHd# zx1~<8UmZ?5lw}c}U=q3o(+hLr2>qO&7u*x@GCSlXHRMcYWoNQX@LZ6Sqzi5zm0M4+ zY4sg2v~27lze!}!{it8U2itG8w3{%-H;z#&DpbDl@HpB(@E$c{>bN=ByxQ8CaF{pg zBC|GOu&uYY&hvrNrO>$g=|1^2LSw&R7|QgK2*jXPq7c3uKQy2r-at*kamJ5}J34CP zh;GM@58c5gg5nY^NX>Sen?<2#M@`*inu@8YxmzTKo-EkECUT<9W90C8S&bpn;U5t* zyQQRRWB=+(gSbVXFqW@yiF?2=Y$=s+ zeLr+8pVlj39M)*wP+h&gH)=n@piU9osz=!yF%l5CW(&G0;#_^D?Og4X3WSq`Pjqv8x=2RWxGSiu)u>{OmiE1_dJC=f5LP_%oyN?X>FK@&)!1VVrzDuF<_zBrF zOMS=%g=>HD_NhCXf$4cc+W;J67d=li`s&CG`WYDgCUAy_B)85u|-+o^!XSi(_ zTieLfXCRPN(B^hN(%|u;<22QS`#Y^W)!^B^8WJcG+=Yuwjp<#DYx}C>)au4;cv$P| zAOhB@=K1Z-e4^tNTHDZSAb{n6{tdqyS0)K^jpX;RPMpQB|2>n=Iegc`LSN2qBxd`U zHgdT5711=zdNZ?8U!L%Oo~wNQ-A5iaR4Qf_ShO60>So~9UT1`{Vt3Y_CIBY0RcCN- z-ih@eXlaJ^vhh&s2hG&?|HAcIzAO_7yXK6i{CtYIo*p1B+^w)rub@U0OmXKJvgr=q7~TPkISNwnTg(oV*`Kw2d}Cr_Pb zj5KGQl(Lzq7?EG7P^X7hG!%Y$@&P9j7swp6$$C3jc}KN9218P@SI1Llu! zU9rH7@-$*FQm}+WQ{b>Qs#Y_pNh(tXQ$@{Uz#Nb0Ct~*faEaBG(#MG&?A2E$K?q(9ZrfXh;P~!Rp*LM=D146p-xsK1?#Z3A+ z$2%Qv`U1uMp^E;FHhcOX41Gn`u3p)nO!U~h1nOti*q1M|oNCEDPNgB+z6`;M#lMd# z5Swle`d(MFpIx+|3C2)mu+W8Xnfxy&!@WaXo}1g>7`1N(OC8*%B9_DqcpuU><^vS# z+5zLfqi3Vs&SlTkv)$P`!ABXYxwm`d(xos4jQ#jUUVQfWMIl0jcP27}-kB_X$wyzG zeA>RJ2eUojaZ#tKw;`+>z?=nQ5;(fie0`dX@HUFg{?w}2iOs$e1GlPZ|FZsLiFcGE zu9$J~-zGW1Nprq04*zNHpS8s5zhZ}?7|+8_|H=PJ>l|w5Knfp(`_uyVViVW61Y@`} zn<`AkDG^}#%7c$yIo~F{C}6J^NSY76(HO5BpAP`>A_o~=Z8A+sgVD!7D&=~~Q-&B; z|CErV5njIWQwX6&AhIK?d*)~fg1l0GcR6^eLxGB}DRND%@D*3f=F8^mH*T-O*;vhm z-J%`CUP+3^H?+J|PoA9NecHif;r`9YL`7bB|K-6FA=9~BlbXpk7hyDZi}|o&n4>|& zkZ{d)ShTF5*7E{n+9|lvN5t>>dowT!97mis3h|Fy!!~pQg1xIU1A6bO^8O$_M+A@( z$W}Om9b^KOm}RorqI}>UH8))4ijnd_Sa?%~ey3Bqm8?mKVTN1JFsER`M4=|I3eUi2 zoTfrvBq>&9Fgp0enR4!u8EWK%=M$HiV4)t$;)WNR0&c`}oE=d1*i3Wc06T=5ucrIV zgDb$O@ou*4{sDdjR9#b~=3y*2OpCfy_J5l^!nalFim&5OL$e-eAxET6UZmuXQCj9tNn^AwPHkv-An2Ka?LfA!fep zm;C)_M_%dHV;#6x>*V1Jn)jYWgicMW}<|+->zt*Gs_XF_(0jFgRK6z~G=r?fifdSJhY!U-@X3O&uUG2?* zI5sun`@}Ba?N%bzqsfTKQ`uLIWs|lHn<;-roTDa(b{17QxxOHg*<)^E?;upcf8yoF zH@9otxPV3-)|_V33mzZ{sO(q?y(HwVvr@w46(R2Y0)ZVF?e}MrpRjA9j9s zRRz#gS#^mD+;D%!et6^^7pPJ!@t!R0{bIlCPsnb?22eos3PQGW%B^hDADm0>)>%<8 z9B6M#&NYvPVIb7?=?jIK(jM|#^?(xKRY03Q()}9GDkVC@k@ym)h*G|UqL!l8lAwwo z%AY;?RE>*5EhMxiDSagrMjw)v(LYYe&C$1#gkt83`81|H}e9jYLYtc z+siabuw}vyd6vKRC%t46yoE|YSLcr&C?iD28+DzRjhBsa9xH;&3RT&O;Mc~>te-T> z8NS9T4!9wKD5FZ1<1~PpJ@%t&K}riDW7nm>$trZtlnoT=yil`WYjw;-;*DFBw(T~O zPP+6G?x;Pa5|rImG1Q*pvRPzgmZ;4>|0-ipM@z3#thhD;5N)H8zPjn7&eHZNvv<-34F^_p0j!4{3=$v7~sshTXP;*I!fx;`r0_`x^^N-96 zLSlGhDVpc7_Q|V}O2DlqS|Oal9o#n+rs+_yOLE-xzj}YjlurVHsDvmMI4Y3IxO9#? zRoaLVoEyHl;lO~dFh0K<0Tvx+8e5*deJY*VC^8b;sCy9e6>}&Oo&bvq_FD5;*e5Ro zM=1;Mspe&u+oAW7Rx(f>FIqz`uW3M4*)b9dmNS^w`X+^2^Gc}7G7@E3Rw~Tx*K7E} zEzONZJ$6Hv1=i5{%s^t2dy}fwWt$T<&3AJ+l=%487y)9*Uo~J0LmqAF-_*f ztb_V|_ckG|>^qE)5oJpE;M-W7BJbc`bY-CSsF4;uXPgADiAk|bhQu0kP1xk<({Hg) zC~o5+`G||quo#D6MR-EJ0=7Yk2Pi4Yx;>LS@=4`r>85W{x_PbzSNA@Nfwd8%#tDi| zF|62yF2v<~I&hsYl%Ug_lvJKJ+wDvWe-5(*T=F3;ZE@QPm=>y^wb}`K1-0txL+LF0 zqXxlm7TD1e$rigwN#k?JFiK%H+F^kgfrI*5DX4#HBLgJ$s?}ANDG)#l$;fp6X-ztr zN`#W%Qza#vP8!3YPtn=iwEI#S4YfHr_89g0rH}P7CGy1jhS*A5X$6Xs+xRCF$xFuO z8z3+G)mM>zE1HK3dY4_7&UtD~ zr%ZLvbdro&XZR*6`o&e&5Ycc5AExW-I*g+3=Kd= z?o+kC^|jvh?Z?X$_4oEpt=EFfI`u0=jML|`8h835IrThmS$sXm`u24~D~9&diD<_5 zy6&X*WRJS%b$ib`Bif?%UMt!{^`>OBt6SH2karh#8tSI%`oM1A(F`iGG5vu!2I;L*sw zwq54sTHpS?19G;!RJGC&OUse>xWcJ{b-ym;LY%BrowH1H`ceQLCO{KEa}bX;Z)H28 zXtcZi=Y^-kc7De~N>2(A3Z-dvf7>I$d8Stb;1z3c%movLU``VWQ^(DjbT1qhN-(5k z^2Z+aXRS}UdGOTjKi|%^f1f+nw0Tz_)I!R)`_>V~zSd{OyShzqd${B0 z)32?z4M@Dl+PK!RR=>=SWqo`8<;HC%kDAwsl6##IqGf|x^YLE zD%zBk0jlBG-^)U9W{q%x#^^iA!+SqQ zTtElEI}k2UmJ8MAK$(3^G6`Nli9P#d%>-UZ@;qp%wPS&r?t?aD?LO=vbEg5EiC>9P z>0$?iVFe>09KTMJHb{|We=qnYXy15xV*bSQ&uPC^K#|MhQ3dkDO+S2Gc|VrJo3N;;x$a2E`RB^_UM0ch@1U1oQ#>d7)N|c&iTg$%BE|vxElOh$r14FAqY8P}wnI7; zIs5v=J|jrO>$Z|ubvL3OIJ$kh|HK(sg&f>Hxjx9hdvz0jLC3xEZQYLnJ8*>~1I~Kq zl2-t78(oS;tzgG=b4HBW2=6W`hU3eYOnYLQD^1+tetdSAbu%#vP zsEU>N^+=Wbzcm-Gz+GvV7N1bge4CP+vRVIm)Vd^2*3gA`c`8ckYC1EMm(Q4QNP!bk zzf_p($Gzti*H02$4_`8RSlcmIY|t63*UEQa=M(sc7p9ELCm%E#n>{;C!x=8m- zf_4y!1zeWhqY*JB>s_q3M1ipfAfSjyO)DsuK%XszOz3;H*3I-#E^-Hs#x}h)vfxUl zLQToA0#ii}STr>7)Yx_SZmdH!j6Iy=RWO$Hft?}R&&?0}&|%vPUXJ3Qg#rPlsnlxX zn7kytfiNg!AcPaf*}uROx&pi~mBv8V{y_}%%ss5K9$O&}Fh8};IU9|jrY^IJFk*(M zo9Y3UvH#!|%@ErDugfb?z7S@IR@6n1%?u77@;Mm}3&QU!`E6Zm^M|>MIVUdC^GhM# z88Stbls`2ph_CdyjI)Kq!YB3b=8tx2f0s;`Ob;c7E_p7%ogj+Q;kr#b@2_4NoY#(& zP{HkyDX+|1tS9@5zuTlI;w#0Rr@Uob!-tEy2-i-~z)vg*2}%%ll3JR8svimkbwG?= zPNRRm%b&Xx8VbRaHL)5rot@E{Hnc9`%#g$>uOH@$Zi;TMC3z=SbgMbmk_*cIJrH*y z4l_io5H9pKNth4H`O{R(LAtFo!S+V?mQa~SHUB6)e~LGq2xmZLU732mfYKq9mA7!E zXqlE@ve{q%UKK+`y{b_b`1MH>IE5tt>SL6s#PYt9F+Y+RjY=iG?|J$I64RhXZrI!G zNbXn#y1tSK2l@p$%F8fS?98a*gtNn~IrX4tXWZQMYM2&?@`WW+YnIgCouBHG!$Dd} zimhcNq;n<<^!ARXDd)nOjlKG{gD)#sDv>-*_#x?vgrtxz)0Fg(jg#(B5P=s#km^BC zpcCt61%aC|&X|~?q6(8G|4Xnnlf5JUnAICfLXx+8X;S-P%$U`2j$A^4Ldk@E?e^W6 z$q)TQE^m^L?kdSkBWB7;0*R&|lK7_6NG2w24EB0yOx_7YPhEkQsFeb7YIN?*2^6YK z#q{(RZmJ^LP)>X(svBHkY8-JV&wn%wVH}+JC|EZq$p*aHM{T1f)XteAi9%mCy^Phy z1e)+{(k2BgIar<09K;oui}oq3ne?IHt;?e^^r9z~dvKtyAFO0Kl$MKXsDuQtN2X#` z$-fGmPsh{@%A@h|iOZ^5u&AOpSt5LKa?&0~yW%~fk~v~-!Mwo~kr8PBTa@z?;=qd6 zI${jbFd{Kp>C2p5wF@=zvt3lI5{&)czo}Tp91@xEgoH#yMfCa_ht(yFO>t}vc6Ls(S5g;hJxLf$ zLS!#o8_r<}Rz>eA*a1gl{Nrj*9HMgb7R-a>oaW1OtGYa}xhrUpkOtzdiS`iW3J(rp zx+t}J96ZW3zu(Xxk!t)js{PaIkbndY=v~j~hMR-Vi2~F!FfiZ!J&tQ}Sp7~9%23E~ zdVV)_W|%@df{8MHZ)bXui|4N8rz^Lum+10p2fK0D-0b}RVtX1$>rfB5wX?D*VHJwm^9KYnyS9%o<0hWdZS+wS%M{CF4h>s`O<=JUPxxtiL^ zHfZy^8sG5&UF{HWXFIdR3_iX-I+OPLJNNMIfVW+{_8=+jygq)0Rb%_z+C}d4K3;Wy zPWXLG^?U60clJv;S^@) zQS{}u6OyVgT*&TiIA7P^m(Q@rV`9;pTVhfDJK}BPvK!*KcnN>DkGS}r*> za>|#fpN6>~$HdDozod@&c78!JARF#C@)|$>w%zG{pY7~v{^e!7`{WoJbuvi^0i?-$A-3f zOy{P}AnbL{)uuR}gO zNV?7}4wf&mp^qQ``)=`lLhicL%NKu>%^bPT{&r_!A%Cw=7D#c(+Sy;dy@-K1PK7{M zpEm*yYuk8Y3i~$t~lEp7(`3qWx-uj**3L-b)UR%<^2#V?fa=S?AoGpz3)$qnm%Bu!oO>XC*M zycnxj8e@Nrd=~L$gzL~0+6_1UVyyk@EpuD@rK3h{kt(>-2_0FdXTf>Vz4!hR_H##Q zd&BeUe^1x_x_zlU_1b-a>YQQs=yL_RdVqT6*-QTTq2uUzg?81wMZEpX+0)q*QY7y~ z&QBF*kPIZI@A#6Jz8`0kk3Bi=K9VcHZ*Pu{K*9)Z`+#11L2EKDWeV$W$X+JHmkskL zm6(18g=O;WzB;xcMBB#`qz-Wu4!pZ?QVW5H(Vd7iAl8p5c|a0Cc$MnNu}-XSprB!x zDBMl~*7@Lgs4ih3Qp)-?sm+xdo>)RQf_lCZQ|$9W1-eTao9)6h8@0S@A&sc8Yl;e* zrI1SXFDYWQOgk}}-l_=UMA*;#G+>jN{t2Il{ma4WPF1&v@3|RfHSGiceU}nwON$c% z;h2`GwwCF~^WVAfr9wBzhH;i@FI1hy0cRBXys}m3Rzx-HvO!vHmDGX$Ls*Dl9x*h- z+Cza&l1riOY!ETroe-oi$ad~|bmrSZzuNKr{Fs7N?ZFnLx%z`%`97&9&Yea%PNF_-9!*s| zqASET%r#AX;rYp`kkp$=Lrl7#Oq2az=gk_gzZK?jG%l1%IqTXY)jX9T^nZ^=u#M+( zW-w1kVBEem!i63kM1IMBC;gow2%6{AfJegSz z947b;cR37Yx)6}!?|>S~ZgOsN$bZC)pv8OEEI!z)BaG{tTv_ZCk@;vD%02 zLk80!z1*q5(w*t?EZ$ANsban_a|PoI+8w15y`~36dC7=^cT(6)$#6^n3tFb#bMo{4q<`2#?D`^w z)CE0AacXUB1ds>HbJ!rp-XQ&775^s!yWDwi7@Hn+35gJXu*RNr=n zRAnA**e4kjE(LCEu<^~j1Kc|xk^6l8sMPCy^iwNHIzYu3koLDXAfed|4q?u-=}`=| z^3K7+_tWsxAgN(!^#oqAhkRY6zk{tsOFEOJVi)zPKit^TZz*dr6>@+&x*vsKz za|@K$_jBmy(&B#ZI>)TI5(ntp$TxV9hQ*y%xxTTt*=p7fK`#e~$ux)m6-e(ttT5cC zrl%&HSHqssv*v7ebGQcRhqD0s`tO@K2V>ap6bqVvJOZepWJ3nNlgrY;u6`r%a>sAI zDpH#C8bI~(I|Bn+_)6c)a7+{9@fBMBvXb~#bT*u`#yl~E#el_~wmZxsSp+BjnF4oy zWJK-^z~Ea1D*Iz85%u=8x`)*rFd9{R(OdJtL^ z2iy2yINDA5eu*EU9C;_OU9h%T#UOZsCLlP$n?(0QQa(N zi<7=K*l#<8vmx05-K{!y=>`iu?4ps^%Q7ih*OYJs4#Hr`m?%w6-&Uxf@mI`KP(EV} zUPrW^2`IcF&>++JkmU3!qMJSpEC0<)7h&T$#kkgBO8SuNYiw}C*Fo}{Y~$XX#@1WG z1c|{+Af&0vP1&1IN~N##v#)t6<&MqLOOh)of#g--A^wX%g9V6DbPO+I?(DU9U;$nE z4s;qvDENPU;zF$V2z#z?7if&K$>2or^@uSR)sbsj*Gv3Y+Ay+@O2b}M5sLP=gRwmn zip4;izZ596DDS{tQ;o$??kQAb-_!Z4kO(G>W1 ztfw5$6U1+}oh~k;r4dqjVQ_^ijJzQ5#8=#_wmp1W|L2F{xyiu$ zaq$u4!cQ%@UN(rHua(nNCM;ZsWsLUTc|OrY=Ri8GlE?5AN#v>AY>X3oZzx`uF0z1_R+>pIVK-_N_ZclCPIj(^B!3g>A$#o*+v9S=neGrh#EBSz#$ z<)6?WeSO#KZ&-s)C7t|8Nx5sA7PxaTy!&zNWQlEp@D8oYg; zl01iT%CC8l;dOW`_=cCPg~mYYKx#&f`B^;v(HyI{+yX<(qRs1Izwe*4?IgZUB>CrY zddv#V3OzEvWl!XODubD*)23JgK%^K??>%ZoLfi`geXhSlKTo4>50 z>#rLGQfSI^m@s-sc}d;Ci^d1LHZa{*V|~W_ESpmO{2@H)fiIFlSa53G3M~3hLd8cu$+WEZk-$ z=Q@4(lps~eDCcL1>y;cRPCM0??mZ6gwqU8@0UOjg09-xbDd_d7)k+5@sKMcWfHE;ryS39KfXW!CoFtcQw`Hv()G_n_xA&Tm7@c z)yHZ!!fLO%da%Bgv}JF`+iI^v+jiNz>1Xp^zR%SUtJfUFzA}iw*DVJrF+Nrd+Q-!k zORa-TbKcfGtozMN-YT%`gWo?D9LzS!bF#Lb9cDIH|7@;ZuwruYK4OYRAwfByh z-~?R>)Z|TV1lMdx@YY6-HR|e9g^pwl^}La%=sa{=HoN%Y{9AGnTqEOMH!M4rR?a&=-CEhRghfx%^^?ea5c z?tAtL3xg4F#*at2QKlhRX4L!8d^W5y@C>3Cj$$bc#S+XA_5^=-sQuJEu=3PhRn^{?jgS1 z5xCY*no{0oqh55(ZH%2JZIx=f?Uz>Py&ZL|gnLoQfj*q?o$GZ(+CSZrGJZeyo>%`z z>2@hGNg(OT_=_r}UO<8ORaa+9gDRZHDkRmUS*BS=iqA;9m2edmz;N9NcO5H4l!K-| zHjH8WfmS@C>g0?}In=9#ZQDrGpK_nF55CbmNccVQgN)$ArTWr;V?=j+7u$@Z_58!A z13VshJb30a)M_M^;m;hY@dC3mG$kPJc_4p{X=)x{HpNw;!TIaSlPj zkuKHU0}>p(%z-$UYms|V@KBd%(KlyGY}@sY;~4P-@l3+`0g6p;a!pURudcsN?azS8 zO;K8cazh_B>IQ}end=K5BHrm!t&ywt%UK_Ab{c_X@kUR}%7~#OpHP$rMX#h#_tUF*T~u2(RTf3aSYi{8rw+6M|KN@8 z5*>a=TKYR&)-ybYh;|$s7wvTT(7R?UQuz1B6DKtjyU=)sZ89^Jlhi@lbiOSO{ixx% z^&7>uHLFY)^!>lwk%;H_o~wSt`)G@VT81OB&aFCe(f(1=i60hKymyx#xIQezJzeAf z^SET9l43QQRF2Yvos+7P<$(E9`>Cc5h(2j?qU$|l6bHk&@dLs>oZ(>gvDB^9IG_Dv0hrre}XB)lad%> zPbQyuRfy?G>2$;TOkjqN)#_1)Zzz|jm#Cw!teYZ9xxFWC`_ztvj)fkP@vMcc;+n}c z{^R*tOF^gk<}WwYl+9UtPKWNlM#K4n_bCBN-k zz@Yzol`oDcjhZ(`Q-r_s0(ar9&7&4!Nzz0Z!Zq9FuH-Ix!a_Q6xv0W`%O z`-1g^Baw(uipcp~Da@CSq+`kJd@N+@J^s=9(Jg-^ipM|uN+LK$dg`?)i?2me+^wa7 zV27?Zq!!#5*Y?HG|I$`I2~9N~Nco5;P!B2wJl|VhSpCN2KwCMVP_Y6F#9hhrJ^n-V z$8D@{whL=TLzD);Ns+GF;jCJDa+_Z*?9m?n{(6LNekAdt0I>>jsinFAryJZPQM)7j zaaM5>g1s8So~xW|`I2{VHzPxfU-#0K<69|b%z6M6z00J;C!Z>DH_GYCtlNijvaqYk z17*3@OvBfwP9DL$s_Y&(b;srj&FaXLu>TM$h}hy0%{1Z;2BIhNQ-|%{U1m%D9|PR* z?I8|~67`qi&1>*wWUGqj$=9qN?n^C7%c#4bX4D3DIj6V3Qx&{qS{q!+>=b`pQ<2|f zoA{EOq&YfN1=ICZT;mh#mPvZ3U9?13jvHk^FGZ{-H0i={sd}aAjZ+eoI1ou-{$0kC zOKile=qDQS`02bxKi?(_N{LNGiT_2PTN5wZ-fwU4O&%}zeLs$HhEvumwRYhj5-oTM z-nefbz4|cv$jY)cBPNL^vN1zItgT+#^da^mm}Abhyc15>_N0hwO|{4R>78%~If9DO z;#(h?E#F7>9&LW2sY$tC%zEebCs+G}xP+tx%Dt!*zFw56LWTfGPhC&lrNr1%YA_zd z8pk3NM7oqJGn&&&N!k^~lz3C!X1`-|;_t-Y5enfWp(k_!jtq&Pz}`dbC(dk*^H)j| zOM?&M8-$-*5Jk9&;ocK9@1P0yzuJ84#;bo-br*V2E5b{9=Qa8@-YlB>rF~po+F`ue z!rNy|RJTn-AGJ6C#Nc<{Sh!6J|K$ytec2w;{+^C#Vth_qG5{6uoB!V9I8MW$;C(G)WcLy>8R$pzsK(w|Q z?bE^~wOciOcl>@Aj=X7fxcjCiL%6DukA^R3G)nWC`{HNWhq_~49!5g^%>^9%bK3LT zwCFT9+4k>Bp%DLPU)CdKdYty)=IroyX$9Fn)bVOPptF-q2+h$hs*HKf&`QEM{Z|liesHM%wUr*6 z-M-6PeiSvNX!Q~HDE8KOOfCrxdHp!aY`5z<$HFX4qure~Ga~&F9rTZysG2Bf*Zs7d zU80Xb+1{v-X}goM3NQ%ap`G_GEv;gpUS!q09)kF+`xiL;$|tWT644Q9@m!(tiDs*# z`tr)%jFX`~qDC=lYwi)3$fgdtFnm@T3H9%ZgHoUHXioKmt56crbh zdXI9~dv0svdtbVaujksP4&7yrH0OFyHCCRcKGM`ZppVDqe?+-H%#tyuIb6xTztE(5 zoy?tLQ5LB{%J?na7AyF z%J9D)#KnHF49y4aH{&B!$zS@}yRkp&dOU44tz@pTN!GjLh01A~@B1##vR z;%HDaz`BI$<#5x@JJ44Q#(b8N`@r^Cf##>!Z+v!0n#Ah~yQ&a{8CC5~;_4=Kj~Vt* z&(oeZ|4wP5l;_%xB&nL2iGn--K4fzgPV?NT&DpOxL%B6EH6}ZPm2BOrA)i?F z`uMtd_OMr@FK^>;oRF_fC8uj=-!X2*F-*yNSm`6t-G+vBdUpv)sb7EaO_PQvK^Gc-%Z zuUU-8xzYd3O(`IKS#AB!@{KHNiyNs~9{u{n>R+ckZalc*pm{@aF}aybOOqhh^y~n| zbGMSj*0Xa?S!|?5POTQ7>MDupV)Nerl8bCTaBcowPLlNDF-8*=iC4t=`TjTIBgAPB z7}Te2w1u_S?*E5~pPKCr25%)CI1j(~QqsctyH;LZ`!Xbgn35AeGx9#zi>VbbO7u;m zjP)evx+fUjd|lIrlV7YS%gisA$x4V??6T5tKNjo0PU|Ij$op(ct~cS0#K&O4g%$_L zCk>mQEYL{1a~cYZdRu7(x|lTc+$-_J^%5+3L)D460*ShsMo1)g-1|wA*R{<!dP5C+tyfZFYW4nkZBhA9s%XXPG}!{saEww@uv1w ztW?|g6`_|P)2g+IV7&9%_}T|O<%RA6c2{+$M>qb z=)3vBvJn)MO_8rlWvhb)zqPnqMjAVvhutdib2seHD-YOo3RP#lY>4oDoKDcw^Wxc# z^ljgRR<+)F#QmZQ3D+s6-SA2cX^~AW`30hbF6n>eu5E+T_ID&Z>J86_;?X%T!Yo_V zIEua8KC|DzXe+RY__~Yw#;6z{Aq-y~ClCc$N)6PBiuJ)LfBwyWF8mhnu>IRYP@BJ_ ziPTmrm){`SoPj?ddv_>zZA*vEk}a2OYuO*y8YzvaWzZl+8HMg`1@A0;2z2|(&)b$ zOfTK@>g*++tv@bld{z>KEri-}7q0q@R5_f|->_M;F9hsy%+_`}hgCJ_9|y}}_vSj) zAD(3$GT)gOYjj~m)yk6G9P;H^oci799Q1kl?Er1n-$=*h!!X3zPFS64cR7x#hV0M9lSBf zMA^Z^TprhR0q9h6x-^P~9CN?L%h;spzq!MElt$Q=GsjXk`tAmtz%$>JMd-ooz^PTw ztn#I|=7V%OBeHmvroWX>Gg}|qTMhZ5;=QcBTExD1_dX4BtaMaJ)y_X28|e~jwqazz zIivD9hy5ZRR;4%HANcI=@Yl~~BmjRcj({O9RDq*zngQmS4@WsX*qfg=;(PK4`BH*$ zkb5I^$d7h$SLNP;Rn=}-H}IeqdW=!z5ang!wIC2lzKK!PV*8l*P)Av**-nLaYse3- z+kT0=vV~?FD{)L>1@m?`Yyz+q+BN_8xtDILE`|*!QeT zO}?1GRN?u4t1~+&coll2dIs01qEttPR|3B&L3IKqA;XUw{C^zjeVQqR*5rQgoFVC$o~I+{Ok_oXF6 z-;2zftRRe;PSfIwf$Q@FwC;?9@WS0Z&>(_3K|V#ua`YN?|v|y0bl<|Kw#Y(x7~}n)1#FWHZq`oy4R* z;6RY}^Y8-F^NpXxTUepw+NTQ@)^P#(M>!Ng_noPj-rP{!Q&98C$*mKH7Gx#B035~0 z()^z=iT;&7zHtJSf-4*w6y$740p4MmRHqef$!rW9QQzcVJtI;W$ngH>_!13 zi7wiIB-(0;}{xCax%gr*9e%ZI}apUMlTz?E1vvrV6W(()~=$yE5;gCLl zvOi31q;7knpahZ$CQ0E4YzaBnkM{zQPLGt+fUB}%VhW*1$R#(*1{fBQoj_!Emp}G$ zZ!BC?%oir}Va3#_5;0iItC_>M7OcqS1{x)zOB%x@EU&{=-!Mh}7H3Ku@!(+0sX+Lz@eVg0f-jo4@T_PUpZiy@i($|m{?B}?Ph(cMnVF$pjh zgA7>&cwltNOJ&MJ36@SO~Y9Z|}pnGZLmiU)|r=oWbyAFyXyS+as~6z-6( z6v1mq#;UMW;w&I>%GsK2 z=dJATki(ClQMj6qm6B?!0j^^A#BS45=>3H zFh?jG@%zJ7RqtnWO<-Y#WkvmWTP_A)rS($&t2=V%X><5*3Acj8-k&td8j&<-V|xK) zC;QiJTT{&&+C8eL{9Hugs`M+2p8qIJ{1|O1!AgZiKl1w<#iJ}(VM*$OLEPV~1PaUPAFCO>aF%n9$1KDPAJ1|>U9kA#Bx{sn@AG-fKuHX{e;`Xz z#j{~s2_KQLO_dCO_9OhW;=Dd{tCVL2iK~%SV-26v@M z$+8;Xhq_-VIrb9iIHKcZ&uJv0I$nN7n)@YUg}u)xOF&;v6{Y6;jtr2zRa&?97A;}| zBg#&T=K-{Y>E1~>aBTiN>L40xpF&T)7 zTTQ3gi;`gmsn4Ru=tvM)(rkh3&@qraGc^}AK@QR4V>FKPOw0&uCt<}4b%<9w{ZpY$ z4dr-;F-UE%1FDa9k$dOvgV_)9&r47nQ%Hos?%jQZfYh_ZUXQ?a4wf@W zOMn7&drUG{{SjD2Zsm6q8_ucXIh7U-CjA%~7e4 zb9^UU^_(^K`fg>R$%zw>*cuGC@cjyin;tD><=>aT7UEjk%2Hhh8Kigpyp=+_vGf~ zb;Y5slirKac(-{-ddXNzi$pX07-{SW#`#}o=)zgR0FO@~T?aT=usEI(*!s8+@O)CD zu#`iyJy3I`XquJ?Epwk0)+vb5}=wX*lHn6mAX9H_`D zeX`40?gWLNGF+4AHQxt-+xVRv*7fs60nr~(y0`6Ql{R%f*0Yh&p?sW}vzCmJ1{FO=J${(Oyc$2KHC|lroJlX$e=S3kM(~P>cW{MOL_8L2{ zJ>v;OVh3b>8ozyAiQSAJ?MkN9^#{@BwP~J!5~$=CeK)}zsTph>fU$k4_e@>v;J(~E zqlU}W>3}3vFqrcrSz2cU6R4WqIedv^v5{_m`aGx+qIzI>2$d^!wm?l)!z{bdE~rt+ zDsRjG5Cy}(*k>*Xh2vspuIIUvfoZAKEka0ZzE_@kr?-L(q=nuTzcXdvDM4anG*BV3 zhK6ot3i%}CDg-u0%K!3vUX2~_9X86)7Qd*^OFfJp1ce&7c0+42u=d|;DdQd(x}0fR zaI8asyc!7>{Q8F^GgD!zY*D$GBo^Q$zj-0O)ku;p#D`4HhXQnS=ot|2P(Xi=mYH2C zHvx;p{aWkIj zrB<%nT8McLGv(vC_^u-$*7oOZ7L?z~j(1M*L}G);zNbpF02;TR>p6Qdk+8z>jfeFG z`ud6>-Z$C%VR!2H`D+!44T-0~_-TUD#m*rdx102KgdxW*mJ&3}nHjD8fa4mG zrVC%DYC>uIWxpY@XFhw+J+xoJR6}}=mwGV5)3Cy&5OO0( z0U4N(WNlwz1T;Ub9kGxGC{=!2KuPMcF2ep4j&nm*YjY#8Azh8+WvsxgtHNrF4Y>;e z;~63o_3BV2zsW@y?++MWM5Fb!m_(*KsA$u$3wz6UteiW;k>W=Qz42ouH7I0Oe;_e8 z4>{oeW+yt?y%Kf_+!VWI6WlREsoE)h`(2LN&c9OTkh)Vkt!ZR0TTAy$7+lcew^|^Y{>Kq#ioO+S-I@1KL_N36&pBf z$h8v<<0JWORO`aLt$*yV3Fl)Ug{z9NQzDSL_VY+AMhNo{N+zk`P)W@+SNX&S zhQ7`LzTjiTLU|^9f$y@2RE;gr{cvvW>Tw<$lC`a9b^ow)B zyt==v?W9sb@l=2s>vv$apJa;uYSf_dzQt;jGZ<8)vHeTPf4*l6t#XT1&rf>YC6VAW zfkv#LnCGp*_z0ph`KiT*w^uq5vOer$hbW2nnRFcUhw-yJ*EaN}I~~sbdrVia<+h*Y z^M*0xJHCm2go-|Poop~NcE#u@{qY>pG7N#C@AX=muBkl>x>MLI7bP{q9Sz4-!Ac;! z&z+Vai-hxq{B;GU`+caUXI1G9J^YcOlRp_z8S=alu)-Ga1|#v-v}pk9`b{b=NIC+E+Tn#bhd#8nPBGS{*<-Fq#s3P*ZtPjJT^aEa@WjNo5SMcHKt zw_(_B#v63!CdJagcjR%m|J@`q#9=q2koow^`nw$(!u1qp6+Yw*kOOB!9y_-)fTlCY zzMupoy05FtL`VLLPEQ`o-jR1gaMUuuahLkc0!mf#vwS820sR}X6kw8>lA=n7qP>px zGOVKyaJzpw}03j~e~^&?BUq(@?HmKB6M-1XGBTTBjyT<76pm-9pt2&|ICfdhe( zhBpO}YwMLl3VL`KU?E@i3#FXBJm?zLwEngGcOn4jO7UrW<}X1PEo4Bdwa=UW49rz& zLdq9n$Gc-XIadeWa&x0I1>m9jtd<3T)kN7gH%BcjvGj2t1QU0NUm5?MCq@Hh^e5 zGD>TI#4Uw_a(H<&a22@iTLk^Hj^#TkEiE90NaI_Q&n>mr=-@a;*Kvg}e2|JjEJs|` zaeFo%hR3>#lO6t{0HUvQWJ7OJ0^7x{;q_OvZvsK1`DlDRlLXMb$qTra3$De$afidc zkw|(1&2UyvS4VJufhLYo?paQzxMn?M1qpy!f2ahnN&KmR62aqdCvixaN<=njYwq(8 zQHj8DCOJxnV(bm>PX3>x+`kq=Q1O-JZ7(VXUj?sfNvw{Q&4*#T;lMGDMd)pXO&hk5 zvw**2WatjSX}3%nyr;aN#{mdjGa|O1KfA}`5#pIN??R+ZX;`U#ap|Cft9?b;O=PdK zW)R9DZS%jY*# zB6OmMTosDL@uR+7+J6AvvP9hwtDrcSlvd_95qy;RL6ld7Jnd7;2(0C^@aPJI)es)Xowy`=!#58ZFEzZJ6f&38)}7vu^fbt7Bj1z8eGkzK zrC~amkQ<6B9aiXYQF<=9Roys<1m-1LqyohhqE~KY45LIk6FXKsU=E$rp zn3JTAP%}G;&>;ugsD!a`ysfo<*^HtlOJ5KG1F0AulNQ*j>_Lsw-a zRz>lM??J&JA0Mr4ky;j0xExt7)jO`3BpG_U5itz(;2yhvI+tBS5={x7d+<8EvHjOU z4(2~k%A#Cc&pvFD!I%I>+6~MF?(=kdl8Kc@UMPH!A7XGcazYN6Q9v`LXaY+*TeOO7 zUUuSaxWl29x$A*;60H*L-y^Ku{i`sz>J7H`T~IiwG9m>ur_gH&{PGfJ?+f44bqIVs z8vtXX$uXK9FMPcx!{JLmfhtpiM(+2DMn?Z3!=Anfg9?Gz(a8WMq5wBQ&sxVbcenC; zyh0HzrloDj68+}M--USPlI3`(S9gBqa^G|>4Dn62{KsVQ(K>ch>#FD<4@(oh4trf{ zPDn7Pm>7AON1`r!qV2(>n_oDjS6*%1=ZF}8{+iN;fMWQgGj*Y)(e?;9co*_$w`qC0 zzDi%2BXNypeMl==lv#V*g+Ef1F1~F3BT$umdl#EdEiHot17al$%D`?#z@SKFPZWwv zU*o+@^m#()_a*9Ir$y)yNWKU6Pa+h%58PTLEG%dy>Ebd#bCzsucTjctC3S$_xSLW%!fcT4 zOF`iYI1|bo!YM#+c2X9Uq>bi8pn-3D#lycDQe<2gvhFnPv~}9@Q!qSL37=cO&h2W$ z>@<2gH3VCBTLhQKwV#ZwJs%*mG5x<(SYJ`1{~yCqGe+PV+bcV0w`b~LWnVGmzm@A? z&9o7ymBA8YEyM<127MIOQlvm)mtK4=f#H&JN99SyV)H806clUQ$xSQaxQ;~1kOfcF z6C_rDjV$7awW|OekpAPhgLaX*Ui>x~NkU(Wqu(wl?}~8|j2ms6J3f>Q6bDg^!|+ej zdQ8ZGSzlTl#cO9VbyAx0nIRe0W5zph)w0|Pi8}&1m2&{Eo)iI%1KP@Ejl!{s-gAFR z#W;V8gQ-HxUBU!nk1t?t`U*em;JDfVWe6uv2}-U#v0J}~ z^OLtB2kZC}h@90wkEwwb9#QU=tl+bZMt%Rm=+vWpSs8&?f=y9`>5-jL@?ddJSW~~4$bftd)aWDgp_}C#J6ko>chT3Y&oU0i zB16`>^(o;h2?-&6$>E#B{Rk{tjOp=uLlkj(^gN134w%K##v;d9=EVu7RLshhO)Na? z;{_=}@nr8WNilh)KQWrjWLyxuZn6nKa6&nR1b{iN(Yxzdr+Hb>7pp_8>n`$&=iSo| zs#}Q`tyU2izvTZwgc;KzodyzHvKI3~9Mus6<2+K))JPTgvipF^m5EK)RZK-qq zU2fM0W1<3T+G!&oX0)My`5qiAsW?kKal~)T8hL(s?!A z7N2}kzNgEg2(Wz`@{kd9Rb2q=th?^V2jZiq{Q}y}g{xq<8CL$mR1AkTDHJa#!4oA@3%Kf2d8Sd2-01#9F`8n%(2wd19Lti; z?eZ%dsmX}KvPHmgcdfl*HUXZDR*sVqgnx$d8q2fJKt~eG-jDc^oqhb;%ZZQt)Xa_0 zz9Rms3FMzmg>w$UIQu!%9YM{4*NGdDl{TF6&AflT_ZCJtJMV4h9jmQBoY%esxS?2g(0MX_s$^?P2hBjb;ZcEqdJT2 z?~1CPH||KRDpIN^_{~u1ed4M%-3c&u)^Ei1k0%C?!4Re|pz)ot!#x?72`U+^rA$)n zhit%qf{UEl&U9-;R)=bu+}Y3#a7J$Pr2HCii!5OW^wn$c(-q?b_kikhJkyK7JW@08 z%CvJRDM>m`L?_AHprZ)>4~do4FW!r%NC`dDq)^Snf3GJ--$@$mY}yv}&}QWt`cu)s z{eCISg&NNw@{--k1;{3(_}Bbvk4|sf>-#i@VSlgPl}|gJOAbV2iYS!q9S{$R$iUp8 zfXMS*yCM|IWt1Zsi4{60YDrI2AmvZ=8Xk59y0S_2*Q|}`iCNV>hZ`&8bo%CY%8W!( zn50{<2~SGA{^)a&A1|0{5YM61pKWs(ddnm~XQS7INm7jLkR*^b*9#4|K8LEHSF+bn&EdFZO!m-`9$T2C3J}igG1$ta2>vS~Cocwx zem*<#R^QD2ONOZ?x`yyI>S+v&FiEBz7T|Vm|CYxVZty`hMETtA1E?FLg35c<@@O^X zKgbw149k?i@xu2d7lcas-T&Quaiq-S^Gy5Dz%ohD4P>_0w{EPRyc))|mL&P0`J}XRf&XSA&X;6a@k|6I7}k&Bvc07Ub(8fz4sJ&Q}>% zBmHneQS~46JX(BS@pPeyV$HDM^OK)vJf*R|t&#t=b@Z16=vMS}yH!^@EA(J#^}O_}Y#_38napxcN3y@1>`HT1oszyPdC0;Ih|A-MAaq zcg!rS7t9VW7Z3UQeU+z$&pQ{)L(f=eceD6e=k+ClLg}=sebnBs}C1t~Kid zI)GIh6>}*~*5qGv*>qtAf?*F_?VvJv?P|Y=3u`IMiyrbXvVd^Yq>WY#=sR}f`v+j0 zOj?$`N#tO1+b!#9(7Bl;SR_t(-JT1>|1#kVt;`i8R{!EG%>}tMWbC&8D8Z^scq<0| z@~basT;?hY$$b0N9T2e@+7v=%NUrf=9Zpw31Yhvdgr4fE=G=(?Ab#0j9+mx4?+pj| zVMG0$>N)}hunWkYLgz~Q{i*Xk7$-&+lG06t3MhcB|2Y?lwn7gj_z>jU=zKN|6~1=g zPkvzs%~K!S^#U1fKlrT6sUF@}+IRxhtnEIV>SxifD}daU%Fr0Of@iK^jzd!tV~r8g zLM^-Cy;fq+Jb8XWr9l*Q?#cTtXKqK)1*wSSVeuiJZIwrw=fK#)$OSQ$`u*b3%w>~0NKp(T7?ArLR4gy zSO*vjH9{q4$I=D?BKc*DRebU{1oq8L8f7XuQ20tVf61=}SG_5*#v?~D55_)^h{q+G zFS3D=FD5&t>nvoRR8ViAid9q!XCi?7{l)@{I#=|)8pK9;8j#@#@FBAr~yj^uV2@Zz!<$2^YZ+bu-vV|=B-FbmQ z7=B5HCg%YXE9^4t2Ou4Gw$s(2lml!iTKuT?*vWva!d@uxDw2Vm_#`6xv-c-F#rAMq z5dMvLY_^%U(gj4fN+_CShkkimVf4}s@WbLRh$mp21cER4grL4m*xX16BS;*-Q2Hg{ zunWg7g-8w`u>g6cN!IuzetFQkjo{;b^^w0`!U5t<9Sh(h98(Iw>+8j?cYG3O7y?T` zK`Y&c@wTH0AB||Ez@+a69o3tXvuGRMi~a^-udj;n3I63e-!e2 zJ8egw&Ek@_{11cNwR_TO%#-XGC&stG|Jo{k$&ts^T4j~p&Z=a* z=Lp7D=9ku9?r zBNvnnG#>Z~P=HShO7R;K!0y5@P`eDa^3nlRBo<-fUn$HU`g_RI-Mj(Ee)PG!zZI8e z9a;UnmCW56+IH?&a+6?&IY3!3pE!$SFvuKc@3Tq2%KS75LuatK4>FpSLS@R;d{Uln zlGeN3)FpP$ZJ7k^^%}92F|^0_5)$rPF!U^ZSCL+UNyi>8KQWF{yQXh2@fiWwi2S~5 zs^z+Q75>$H{dLeE>Vh<5v=7DN?V)vqO2ZTP4OY13!Y56uDRYSUSKH$vT_x(`8AQbS zX9mL8fAD38ncj!y8P3EG84G96V78DaH@(DYW32ZKS|GLw8I_IEFi2c`Pn+~#31qP~ zXrqR9R{@&(txZ%MCAb$o-lN%`C1l4FY&UQM4%nD`s+z7qjiQ3Jgaic!52yl8h>wSX zaVhD3RuI%K`4&sj09VB^QuUtvOHGHm&jX%yy#=tsq>zR5nvMI{D8XWNe8wv|w$iZ8 z8~+&`oJn-Owcu~Nt9Ss={IF#T8b_}g80S438rk)ATC0!_k1>MYIvS~9$$?@E`|wiI zKMoV6ltZE&5+L^qK^9lwLO76NrpOfh%b@v~W@>~8Y5_qj1T-OV>yq*c|Xh5X&aL~;)}i$2sw)pIZu|I+DG6*BZZOUz?fI0o9M z8uInCVxVJd`S}eGtsW8We9kmR%{-W@>z}UlN2&>V|K1}3U%un}A%RG?u8%9{FiwN4 zdh z`*lTW2f$6H9@+L8bNn8Lzqb2^MnmO^(H%b>-b9im2msr4b1oYrzmf?br8c{Qq1&il z==tl5m%2$uarV}#wdqdQv&d5q(k9C@DkcjL{~=tnj=V}gf7zv-!FiFQBnijm z(W>+Pa(XBSK7PKKgFlok^@q|Xl z&;LAs9mbh3a(&(uzrK*_uDp$JVn4tS+6{`6MqJcE>Vi}?jo@xIuQ~y zETaek2OwKFLRH$<iWXHEiKcjExx)*eW<>Op*x-kWo;iVKu zrshU3PS(M^Bn*ouH~8|Jbc(;oMMD{gl!&Q^x3H^G8*As?9C|X{NvG7XN zjpb9u3I5a1v}-P*0CiisRF-Qb)f&_Pu`zoMF>)TGKgNgBVX3&%Ja+y@J<96pRk5PB zA}Avc)7a=?2jT1^?`CA}0q5c$Co#IK$tgavOIgo<(rOPD1Vossp?51Lmuhh9DvY4P zB}0Iy#6O2$8lVYjTWdlReLb7*3gz}nxB;pY))m~rSg8q+@WJ_*Kurv%*IuVs`RtRhbCpb^3x}z zK`w3L(=J^VHfzluW2m3)mJ8D;zOvT;0>e@=J7`^6Y4819B znlZA)j$te}@tW;2cJHdCyES?FMh_#=(>&h-H4S9}Pr{2IqzlAcKZ^cK7EYYatUQtR z>y@xA?(4sO{;X3&{j}&?D^0%S+eRkD478&8(Al6eC2Na#Y)KrsKRea_ds=Y2JM1&@ z;dg*G;=L4BGeKEw0V-2om{gc_n#Gep6@p00H8372C3RxP9J%#Gb2LXUT%`6|(uBa+W_BmmW~%`LOjt^Iuw zBvv(ukvOWy{x1SsuKg^ubKoS2E{d!#@w~~BohY9~7UXWjEO?x?rBzjHY6EhLO8q29 zGzUqe=ToAEH9pfCcx`fVtg%yr49ZEP2|9j)j?x~cQ*H#q?&g zL>;O3#GT7!+U+(1yIHWG7P}IRl0yQKBhxA}P=Ql~Mh&oGo}oEcSIQLdexw~84IJAS z`7#s^LS$&{HUKthh>S2cBh%o@ufALYtUDUIxxcHEgy`iRom;7OTe7{moJ;Z^y)l`A z>IdO>eWO=JXdn_bH$r^PB)@B_K9JYj6R9q)bBvVZJR9eH%ujrMQY_I6_;+tEi$NFduf3Bxgy|!>*FWwRsJf`0>+7NHU zh`bh;R`=@ z&Djk)?y0Ok3S->7cXMMsQzg6(Xz32m8~C?fQsN;;EDu*Nxy#wP8l-C6%6o5EN^*%LdirY{>-_w zR%T!*!W*N^%1O%P#R6=u(0q*GV0E*}7%|&b4xN_Q*U6sH%ZbOrRmV=_zf%hjbx>Y7 zZ@k|96<-RKug%#t*MlC;ksM?Nb*Ayqy925&Bh80h*{`ALsdZ8`PfQ$Py(~;J&1nlA zy8?%5+69rTnGEG?YFxW1E(&j4?)v`N2`u=M9gMlz6d?Z=+A(Z4MkascedpU9utWZh zi0_Xjni=p*IH>) zHjy@(_BmFE_dwd30=&{ufAa9H25;l=JcaadJGv{B(ku4UMV_D-sr57KcraVuf zP=#0;c{|iVaNy&AGfUt2$Pz)uSyKnH_+hv=!(e-f;ovR><*(BqG3#fH@0HMniI5j+ zQFm2VwtZ&ko^L-gYA@IUp-h!H8;GA!IYrZ`R4lzYXDwn;GVX;9ATKV&hd^PC&}1CI z^iTLi0U^fZZ=D&zX54$TH|KA!2%7KI1Lqw3kZ0c9tm1`uZ!LaE4^w=`olji~u= zTZ;IOd1$dINbwyIIl6EM2SD8vegRB_ZSuK!zcnPY(Fzq?Kf z9oF}Zh)~m;(15IKh9Q9i@1tp3#yt@LrnwZJNk?Yd98fJ>HvS6m4GmEz8V)bvqXM&` z$w`j8*+tC|arlEEMJEewuyDl%-Z&1e5dSePfc|Wh}AU1=WxQ+(^!-X#*KH8p(8e$&3 zL(GFb(cT}YB_?hMxd3As<(U&@l7M9)AYyw?5G4R-YZYL#&)Iha*x>tvAJHH+gX!@0 zN{7Y5rh&`4Li_R;Kl&LjZC_bW@Pc|XB1DuQGq!WdS7`^UV@?81(i*M1_Uz#tw7MI%`13sY+9ii+0oz zz%bsHsTYN_UIZ=?Ii)p46{CXS0LIxHlR;o}tTi`c4Nin~6elmM6G|F7ck^Btc(L3@ zU+)#VoCk?rE518keBcH$XeK6tKlYft?Z5zrInK4H0L<0P#3-^ov{>-gRtpGxE%H$f z9*q6{P5TT4q!I@1d+yuszNKUdOgUE15=Bl0daQx@vv~nzCRbzj)krr;9Es5Y3c;SV+!2I5 zYR7FD2u;jRS8&>oR$-DiBrHuV`z0b0#^-ifo2vi!e5xKfTTmMu8f~0MX^Jj!hmYM3$~{TTP*vYo1qv;Y~TA33-plCd;LJ!UK*{TAG$(sq_=nq3sRncUc1Hjn| z+w%b$7$q{aFjeU%gp>#%u_9qdX5#|Qxzm;sg^&T4-r>o{?E;)2b6_)ULKzKGo7LRP zWwgZP@9c=1VMyi5Q_-y>J%UpvvFU|0;-`ld9*g-pdcfP;9-J96*C*1#HPT zB|9fRxZBKd@5;y?`hdlH0M$WM!?cb?6Nx>Lu_@I~3Um)iA?XQd)6vp&I)<+?iR1h| zcX4`1W@?kGK8YVHXEf#M%AE{5sF~Y|t_7dKN3%TPr&tdd^4f(fndv7VH_oJN+*MNg znZx=xrIwvJ`3Oj`@6i4uQ}`6lZ|3CHzgvm=I676n*5@vwmg9*pfw1t%Zq z{rO_l=F?-)NT!7Wa+DyY=!Vs%Kn0O&1aqYC494z2&nDel_SU=1H%nlIDw5_xFAGEi z2fmW(7{g$09LR%S`gEV@&ZA(z@nsU*Y=qv;;5|Db@1_@0^NKZ`ExoDpPc&OSPB`$=e>h*xO4(C?tN}X{Y3)&J4_k?d8%z$^g+DV zPw7SbC-C4pJ5@=R<&pI36ogG{*xdbOiY@X zvL`W*N(fqv=|on)Acllg!U z!0V2|*9F@mf&;KmPns(u1O7G8Wa-lL0tUh6OVBgifUm3EIr#>JQS#}Q&V~o4WuFe* zwU97g0%DAZ1SOZ85SA~>asQqe$k9y>7_$a0_cM#D$0-N_&olq^-OKBbu4Jyj$ZFF= zApi$f32Vh5-On2X8DoBYN3T(U19v|UTV6s!44_nGKHNTU*BL_;x#SC^WGhR+JDnkN zSP-(DS%b^%0_O9;bPW|>`RBUfZ ztkfirz4QSA(yKK5tcL3((~iy%aOq+bYh%dZ)GbV=I7*~u*z>`N5ZLE6<0mo$mYOtw zkF%Azk;8*;Gg~x{kYiwFY}=-|;lLGwNBrSSkYJ(CGPwsx;L#z&w-{}uKk#HgSb~)Q zG%p}^3!ugx4jVvV@Shdo(>qwg_sho1z~yDjiPG-ycj}26o{u-#mpXz%Jbe-&O410% zoPKHT*br;kXZzbt9f-AMeJ*o$lnp|hVMcrn`-}$E;tO(vwm`<$VRu={S*=j$K<2Y= zj4hs=KR|H*K^V0h6aEe-DH0?9M{DKekLh-{*i&mb@KM&}${MkBajfxAUSOB0-$AMc zT5Q{t^qcK!4n)RXNX?nJThg9@$T7yT)Ct%YjEQ^w``NcI^B0b|`qTkLe`yM8k#Q(B zI0T4RpF~RJ?yshy%aR7R)FlJtn+Pv5Ar7X0C=?v**8ZQOun!|49(zx_Aawe9kyjpI zAV^VA%aY^~a9&LIQpv{8?6B!Ntxg#V#)1R1WK&}|2}^ZjdR-uv?9jE?tQj^y%33+^ z=@>}pB{lt#M+OKWn|hWw0|>dLNqf-PBEa~Ak*l+n2oPcK9qYwaKZSz_E#}G@m7i-2 z@U1R6{OfIk7Xh-#Q1Prv>-4gA+EIY0KJ(KTc{AXVEoP@qkb7Ynl&6liJw_#h`2->- z^Z89d3PGt4$5%-M9x;*ep(Vlg!oC#H;MJ}hXHst~LS};v^I(-&xK$8|$QgV3Em*Mq zRV5Sh66kox%LXB{j(Es|>zUwX(11T&&toJaX7l^UZlCeItrF?J-!R+sxgj8s4iIEW zX`usAUSNV-h-N+9KVR6f01afys01tx(!kj{OYp7_0b(wGpYg^cAWr;)ho?S7#Sl`r zI{C(Em2fqM_fc-2YkGp~Jb19e!xp>7q!s(LdxGU+8 zO%$@0+kj>eD_Xo!-qZWdrE+2$@t{OJobLU(Vd$AQU1?1(>&V;>@1pD*UXFCvT zPz4n*EZBYJQ!d8D%S7Qo1-M;@KQ3_q^fXE3<^{nNXvSH_1zXYYd_JR!U0!&gSdiGJ z9bwg1-QK)Z5cyb%FUId3;~yG?@0Vo1wr>~sEtriDP!4%q#$jOIQMl*PkK$1E=jIr8 z?8dpTduI9IV0hN@E(S#m{ifOaymV11APh**Luxqf>2O#G-H$nRN0%VKu<4sQq9uq! ztYMR_>?$%$eZz&&p~6Yr9gxjZuIvd$RMym!ggyuLgqZ(D&vk5q1cBhiJ`bS+L3wel zCHl$`C^_Fm`SlxpPbVmkzCSbK&l9FBZ~e0=8DU?gN5a`lcfu?4Vs zIANpiX>wZtkFS^H3~+p5L?;73mc;x&HU@^YwE4it+@T;ZOwn%{v)wY2yeFo%3Eih} zV;l&X#y-?Aj0#pm_tXT7|19vi_c_gm-4NwSKm&s5%&s)xv!2N-;eaQO*zOu2_E!eCx|3PggDVa?*|nkPQ}ndY>`Ayb4{nrfdl;O;`)s{Mhsy#(iP_nD%E zz_}JRpFG&t!Z^wz5wscC-r4^HRot+lHCF<!GA!TL)oRDTb6?X4W7eV)D;*xnh3lapPQKaMja;r zVQ}C`HewspL*X_rE?H)R0|^~=#tVNe&yzW0`xaybwd|?{3Cp0Qqq1~wh5$80aB1b* zYJrn>Mk88y*KTFCpr4RHP`B`pQZMRHAu@hwaJ^n3LzkGk)#F;3fVak zTemTr0d+9walAm)8W}Ls-na=Zc0~}f@GTp&W&x-Rk;&o$>WnKggF$(RevYnq106PS zVDDU=Ju!vZ0yuzboKaYt@7oeiTFY=Sb?0xCN@qjw)SOois!dPt!V>g@U--^DVO*Yg z?yL}#5qFKu*PT#+O|vkzltx zXQU~>;<_PQeglPI|H3oNB=z`a26OZE0ghqK#Z08$3%X28fdW^l*9WcG=d%? zm<>6zhOg*fIWxl87oONu_iF{v(vf7`975$+H3S49UiE&aNr*_kfv_Qcc@69wMBlK) zx06+V74xG^>D1*5KS^NW|Md0F2CZX0MuNzqgzM$81IF3#N32xRI2R6+!j`#QLnK~C zKwvABz0lht68=b{LyTEoFD&mK03$eXD+L60Jf56XucC)BTpDP_w_ z;jMoh|G7X|EzKnIc>m9p4oq;^Cs7LX-!y8(vXLj;N|eXsv#7h6DzZq|n6{X@SW4Qa zs)T*`Hi`2tdDUlfcR5W%!)2!J1vRq5@X#>mghJMrF^*4_6exlWab;7|i=fsi`fRmB zb+>+0MABmc3@=TU!(&7Q%ao1;)RC@WH9{E6GB{zZnQv0aF{m?Ul4Toxc9F9$95F{x;E#CDESplCn&Nw4a`(e%!|e6r!&U+JY^=IlGW|5Wzy+vY zZp49igJlu~3mRk_8ND)>&4Byc+o8VCwX>or{dS%5{9V&mB0rpeCJf#JNU-y1V6&)oSyI>m76{$k%Zza~2xDWnwtJX(ZV8alK(R zj*9a2YUcMazBp_So1x6LWPGXNy!c5TCl($54`lMMNu07QaA&5|myGL|eEV8)XpwZo z(=*d9aZz>3B6JmhQrXn?@~c!S)Tw`7@iEN)OTzu8wb=*7nk&grsBqjIcZt5fLoU~a~ioH{-_ZP^RF1^IW0UeP2z0-rqW@U@!Nc5d35a^T- zM5hP zeFD=p9@bj*tZt^MpWlv=xY6%h^?BtI8scOC-Xh!I7SPsf33U zMDdi@kt*o5LUi_9*lFsj?*cK!G(~uQ()das+|tVY+A`SV>;iw`5V$rw6O$K}$M|PE zlxoWV!|d>jZtzDa?SVjKEIaQ7y@k=GqbkbJm=6Z{0etPJfwH_*P-rX&!2#mtShYo_ zi!4`r0}jQSi^RthT#*^iIQ0- z!%waO@=YCDpF{uM>(!do8mu+x$d^ay)@!5VKay#*>W@IjsF{|LWdZSLWas9MmHC9)h-0H1|K)MyM*OD>cYn_2Vh) zE?LA)UfwkLEDK?X2cSX+_f{J~O{%jcd}ja4xF1mTXh%?}iQgLJW#OofvA?bOiw_w} zWlIa;M`r1lk{X&CnhpcUmJT)ds;B7{XReR*kY9H==oDIA1fG z`CA@axEPN(mExS~Rb{auj#PmMa8&8n_}s{O&Rf06 zubYFKJHHE04>`Rk8cbTsCh*GHFeOI{-90nlrPTWIBv7@8<7XcxO(GZDtoDCkqlF^u z8f+>fqUZZWh|>~Y%8m85g9~ZR6}XnMWC5YJWC8xhz}i8=aBeiC&=*L4FaO7)AB3E` zIoU*byi94VpVSgml>IX%W;c8K{#9ZX#fkDWaebr@)cRcfyJB5xNgtQNOzobpcCc6-06>x&YmZ8A=GL|xrVx1>J`opTns_fY=;s8h4=Z+U%y&Zrq z?-hH46$ro$vHBUN(N35FO+_&Qa6p$~pC<*$uuof~jc3m@Jffps|J1l-uouDmyC!HQ z*oLjd?3S0s;Naxnw_#SjIZl_>5lL0Vud2B8g~m@ivhr7^5QesdtPPrP@UC6dTNYH$ z1VwyxoLe5r06i_q;4LPUaM~EEwzCYBi<@yFmx5=G(`7yLc5BcH*sgZ;nG|xsQ~nlhBxhOx7)BpVAx?i{TtOclo8JCxV zyAGKY6Qw#*L%_c6>-0Hdx#Ceu(aw=txT?W2u-ldfw~VboXl6mbVxorjAj4f;CZai2 z?yT=|z06ybm;*v>UyIdf38o`XGtpqH@;|$1)Y1GGIZg$nk$K6oJB+vAyS3rYpUJrl zIOOJ-KJZd1C|ivR8~v-?W(sEwoVJqdL^M_=96zd52{FI&mUB#-ci#CJ`6hqudS(;= z47pILObU>PpTy6#`OomZ~mNB75> zeY%|xlIH5In9RIR zu(;3nJo|mzbvhSFD+<`~KDR-oCX~@o&qj26>)cv>Y9V)RO#8k9!*&humyAdGle1aR zqGa9X@uoR{R37!2`Gmt8?b$dmbJbh;(|J5P+Ea(f#*XTA_Ah}~(x$Pcy&5mC>=bGP zQNv(sjy~;Vg3Tqn5#kk${ z4wTN$~(e#pKK(5UI)X{A1qC=rs|u6B7=aCjTHTyAPuN zcY>eZNp=o?{2fz@a3~}GtoB$&5D@BGuCOH{UdFkD7-+^>x;*}hT`-*08+B_u>Emmj zOiQaIqo38~ z4aUWM8ZkfhXUhb}7DAsLZWfBY2}nrijPZr7WaHn>sfu>rzN`BziUl&H3KO8;G8yRr z1Er)B zF1w7{8;6P0{kCnesnI7oA1Pej@Sq(931df+65BV?KI>sQJNX$C-WYSGm{O7 zw3#iIii!MYMMm(Rx}j1THmZNK zrH}|y*ak$-1t#?#c9=^`=13J@E)su05d%FX1lL(}xuBxpG!UdH@~eILFxOgJoUG&I zpfDF9&EpI&#}g86>{MQq-ugE-Fd4lnGXYVlPVy5?EXubNa~R@l0veu7PIj5kX(^*G z3*!pHuDk4X;A>JA|9@%Jk9Lc}|pP&Qa^ zi}8awTj@)!s~Pjq=~NCvE93 zA?D`t;w>$4IHJzGtTc+|AA>rE{0{?XY1Ho}dL`3lL;C`_Z}PKA61%&~5(;-wUDrhG zR?X4zuNMV>C#w?f?iPv2zoep{7EK4g;D#v0-9nkg%H$44{Ef}nq*1A+bSZi zBrPU|ag1K{j#;=wZ)

lTOhD5u== znbzt*kly#WzAHbSSB4x`=(-)Y(pooKAUpjU&+#eWDYrHIzl)qH1o*C8LnOgUi^FgI z^=lr(OXk%UXXI8AM_4~(FMpy|NFL!1hm|(S_e{ELZ)ViV=l2SaqYuhMhQ_~^v+5T} z^A3lt7MWgOqKB3}HGz$mI?a<6yf_JeqO%(ZqSR{#A{nx7zwS`|^&tmh*joq4DW4&C z(E<@Xj@^428!ZO5ulM@vE!q7JD}5b%%n+Fr3Xx4a_mJPW7S31FTG3k-IlCNIob>S> zA-8EnE3v^r@$dd*rP+zqcctr*hvmv)Wqxy|C3K}_^mnT}q-mN;?#55&Vy9A_jknw0 zR;9!)hXB3%GnMDASK3b-Ee_8PEB+emkol-yxn2le`EN`u|6Mt)RUwYLJ!cp)0pHJ5 z(o^_+PBPNBrma8AmHBzJwRWCeJ%4vuY5DH3I&`VNvIDtMf^+uOd(u{Ch&UZ~`L*2Q zg6!1dvZBuy_l>xfjkuMWxV6*4=fyui#t^3!O zvyQX>)|-rL-W#1ayRj&}Nm%(?Ja#O;p?4hBlr9H$SP{DZY`FH$wKg4ts3SCL$g*4v zXU*>kk0%?WfHlS@xa29YiG?SJS*$u;vGN&0WH%o>ub^7fOvvucz#E}O0iw*z@}zQ7 z2)9@xQQlMIQ>|Rh+oSQFb6QBFOybOE^IJ=cfWb`!^bPU@`!pP>h>1zd#zL>VZBsPM zan2GWbF1&COp3_0J)QS?6P!r5Y9^225>HB@Tf_4&3VeI!0Ln!4XRYS`YR?F|C~W#? zcGj>Ft~zrf9f>{ZC{YjB49s-Ca{c;~&pNWeVC{W&#s@QA)()eL#zcOgPVOciHc)3c z*5d`_K?1(SbFzDz*>gF!o%+J>&7IPANrQcj64Hxhm+Da6w+KXTip&~vCUE&{jei+b zG`k^O6$|7!Ah&T_(fU?{W`En#micLQT-^4MfXxg&!-TxXeMewHEgQs3Ad=Targ}UZ z9Q~Vs(jLxNUv+y-1%>%x2u}!lEV9k6!!8Bitc_{iegaHS3tl2OoEDopo% zk6%nH@H-1C67&Mx)^&iM5mbWm;-%%XuiJ1LA6TbxNjcu~GrE079?Ofm<(;7m|8a}i z4MCe7!B`3gKGBjp6$`jg$g~b~OXA<8K|QE%mRRcIGg*Wy!z%oNQ!R(&luT9BwT@45 z(o8+8XfXQMZMxN@xiCFNx-1csvXkaYUCAP`rNv*86iyns>n!GhUiQY?GD^-$@hj_Y zj9!;Uk7oM^1$DTe+6>dVB{?k*PX+ad=9H)+}D3+*2?`10+yxAhznje-=nv2HaN#Re{p)r_0j3KD-CGG?~a z6!gD01abUT8}w`3@g^iEO1uG6yzH@>AD=Lyl2HX+BpT~)F6f@fMGtf==XieXe;J;V zH*pk-&-FJnFC~^1%b#7I?iPJ!r&J*E^AwOBy|M8L3+Zsn8#xvaz1@E{#>3sC=Qh`+ zE4N&c?_~RWf)f|XG}uPF0t1>|{|x=bCCY&NPo_l!a9Ff4ikZpTkV;_46O|_l-T6jS zjz{@Uo3PTi14a=GEkI!a1zT%oSsWSlEf&Zq|I&0LVM1O|aT z_4axZwb*G4RHc}7w#2TH2j=29twp7F@Ez4nb2sO8|2wWYX8u+g(}-}bHJ4>iJL}L3 zb(91X@a2U@9ej<8$9-L#g~X~28)ncXF|_@iTA$-uwc0X};skB4sx6QhPK=7_^F%uP zn4Y2LE}9GDR>OA%qcwq{Cl=vaSOo`q_+jiAU2j3_*Kw zJs!KvRSeg-xF2h@#hiA*h*fC3LlnHkS%t{T0QmZ^Z z%jK9CJc-i1<{#8gIfj6%a0h~!5PQcwNP68I8}PvF9LWrf{OwYgVQ>w2u=F3WP!>hA zDF|fKgngoq6yn~;RZ0E)+eXQq$`cdG!h2lS7p1f#k?PxDqk>wMh!cNS(>-dW>OeKTGDp~T`UCMq^Uo+rj>BHd3U zyf#P_Q)WwJ72z?>dCDwTAweMQID?S6Zp)qmXBoUA_N`*W3Rub=82;!O3jRxYt8d}W zpqN#V^Z1;`mxTS3OjMo9*Xn*i;PP`yDKCK;pOx^SSM#V#L%vDLC>z* z2j{qXpKxKLc|XgnD~|i+U$dvls}i>1`$?{ZQGmvQok;Q5*MDC`YmK_!Y=Rb9s@52m z?P~6LNxrUe?L*q4+ST|}b@!YsI;9|=5M12Zy#j`I_zPJKH_Z!KoWfm@Yv|aKDC~7C z2rL-5RX6*e*^2CPFA^@)`A?Wsb3;yObh5}+)qXCpe_>a{k8)DOUBz9s)3E3qwOAMC zG2XmjU`HBi1nu+s)n?M8%8Uf{O3!Yhki!t(hs18JJ&t=ZYnX6hreo2-4fUif7t*IW zj=Rd_RnM*3=XUx;=b>~_sg`@uAOL69uEu17d$A5OK0Uivz3!&LlXSwa{lB5j3wqrP zwL3o-A?+aDe)(6MRA=rk)|yoRH#2&zo&MF$3;%}gs=pg_ywOfB>ekj8zT;dpFZll4 zygOk4@dd1h{x=Zr;zbrAi*FN$+_|&f!fTgb1-9=_XeDbsG`8)kzv$R?dYV+SPNUbn|(gf1z=; z+1eICT=PO*n8}(ML6`?|KS`V!)~U@Ks}2+3-fmI&-l;jJwLC%46{hWa@Q> z^}MPfr--{NfviCN!goT*C4c!BavkWM=y~9P1f`8j{Lb}4nXS&3q%DctJ zm}kCmGDX254h9R z)P(WO{~!|^YF4tYzX*y?ovx1!OLL3g^=16DC5~ppv`CwR%cZdcthv-hG~ZVb%Lzx$<%%VDEz1b1l$XN)Kw0kk{+Q8w@Y3)iIYG0l7OXNuwc}g( zVz+i?*q^211DT-A+UET){h|Mm*A9xQc&mzrNpK3;kR)YHgb=yAnlGR6obmKC+Wbvr zL|;qudy`))7((uDXvi)q_J5J;ayZlIx45jAkyQ|0#}HZ(r^8#(vE57X~zcjs}Tr&!SI zkphdg70tAOOPTOtRb#W789^~8ga6?8L*e~&<)T9Jl+dS^G{&03QEadHa5h!UDW!S{ zS6zIxBfP-U_6;o_0qb@^a(81X_87Sbcj~t7E5WGyqFz7bAOGLN_h;EvLw4d~-2&l% z`)LBQn<4wBIyWgN$!~oXzfx*O>} zRJKyI+wn%(`D*2)diutGCJIY0_hD((Svp4mrI)J-w zCUz^`#JNKAutaE!ZoI@N(8yN4LO@Sg2t7A^id6qyI<>}&O}1wDw=S~L(Pe6=<}PV?uZWRQ1F z=EEZNF9$Cw`c2^?2_+!*QYrmNxaf(rwT`kvB709_!#E0^3`~B-|HzdxggqD(Aj*1f z(EvRTg|N9NCm|{)q_pdeMwNf5Tn#hk&#*KjR-4P&@z~*jHO}-mkdDOfn!5h677iAo zaFVAp1@m}yryMlTAMy8{8ll$2a+$N@BGN1)G2>1QZ1H>Fv*w52zf#VmcGi2y$~*}{ zC%$|KL#6P>M15!4nI$PzKAV4n3A|@bt--EcN~^0fLte{zS35naZfEe^CbK;*R@e53#qm+df1yv&$r`sB=`ZqxF74 z<6^u!BhLKGyZB5>)^%q11M7Eu4yhAV=#*1c;?ra56KYHZxw*?At@AsE61o7;8|uQ`yw-VV5aq%$t=yH_<}kS=`UqOBRz3kqoS1fSIh=;0FdVse zbsg3>Ehfxb7(>L4)(Rkgk|CBo!VII;3}aovgQv`BkgInts1&v@4%c+O9Bt!+?@>j6 zWxv~{ykErp5!}^fyNw7hngA(+4~Y2Q;}_>ZXK81t6;x@r>4-__W$Y|P(N6DCe;!IO z$09L0QirM~<8zL$%wj~%3`v}DChL5PdOXH&iXJKOlN5NnIu47I$){W^wUZSoj1Aj_ zHV6K1>(3`5eO^P<0;zPdi9pW?&quDF?OJv2UHsZPs@%8md;dWHKsJMZw5uNtPbWAF zDQBt6A8)8Pw`&^n5td;Pbr;c|2LZ#av@M|)P9;41CB&F^|9p#`Tv;oeQ_B?!K2 z(0{_=`EP@XNvv@wt~hkd=S2~IUXx#l8^RL?MzdmP7~^+9ABYDAl*KKgBf00*J;=B6 z-C=5iyi&YUC~}5Y?owOMjzYAnMu1 zG;zVhUC$v08t3j8kM%!nal6l&^JwsdhZ$11{&a&R2onZtY*AyP2ydSf`f>`rq8;?+) zN~c*uKla%V?&tHHXR?1D`#u}ry1O5qT{@oEFV}sCpLsXB z1KvHiCU`fl-p=}-zi)3`g+2JU9puZ!r;-+UlwqkEm+Encwmv=Yrh9V|C`j3A`f;$5AJGcK66cuG<6P|5Q+C3m+=m(s;nz_GdY+egv0lZ{aU-rHmeOp zJT#BOI|ol$5fM-pv!RIzQ4F-WuBhLF5)kn@1vx)}s~XxZulqlyDbS%dOKQNkABRZ; zafjUdvmf+lY^}M4Zg70;@&%K|ok`Fb+rAbxWvNdblq~6QRpdw`wZ$Lh{0?tG)u7Wj zz(!9Lq2_ohm`CwK-zURPJNQxGO8Xfa(Ud=dAG9hWd@Kts2Q7T2iR{3}QiSxORRvd< zaYq-vI*PuB$goHmc~K=Jg*n#ORH92OUQo|_O@uXPrurbexm8_@C==vLtr~hxN~-bg zqO*OcllNgg+sXTaWbpFWMJI2^^Bby@_xt)KBm&QC(?8OUkpC0WI3cmZ zTV%-lV|pg-TWA!Hw!L_XdwgKMXbre^v;-IPI0(x3LvRqc*u?wa4r?WYI?kmPM2y$W-s+q{G5iIO0`AU(;XU z6tpu@S7+@e0$}s4JT{NB@nW&!^A-QXW}Km?_!2qEA-{$tuEGi3F1@ z(=^^VoTgDE(xI+aaE!v+U(7j0a@n1sBEh0Up~ac4_6b?o!@biZ;G0&+Z)D0!$K-vc zXEw{gAj?osSETGpZXy54p4+SPiHrK1UG|6l z{isjY*Bb7@Y;_@qgUG(% zvG(%pDIAe?LJGL$XLBlv8tOqr+@IysbWyNQN|nAFw7i-P8YX@@;JUN4`J0+s1SO;ho(Dy(8HAx z!Q!}zGM-u>zJs=db|BI_TBk*{Q$zeEQ_oHR4OLHp-WJWX$zs3=rXY_X4;wDtsUeJl zm*0^(k|9h7f*AuJ(^qtB%p;{>3#Z`LUhJrD`3s$;)OCTN0x!u6xWADVn|7ND@&N)a zKY}^nEuyC6>>QrG<>;~(xphSbj^k=X;{Equo&atL{Biv>dI`l2(Q*#$$%72*tCfln`?(zVY z!FBdcRmzzAEnmSu_>Xs1s(0lc85IrdJS8-V1YQ^j677{^q3CqR8YuSnxoE5YCNR>S zVk%UK&$x@NPJ2=A$X8uYVj4kw$0zUgWJi*3XWQWy)1g1q`K6ri>>PmH$@=LcMXmbz z6QlPX!7ho*Ezi;D3AU^|Z~XTIxt>9zY99)SCM3Sje43YK$FWE)k)yx*+x@WI%6MDH zp|VBm3Wnb&Qvlu7HdSuu#%V-ABu~yJ5&~x}jME`k z1pSCcQt7~8Fz?swS@(19Ro?_>x-Ay?7cud18Xuig9yFS^Z_W?k!7bA8-2p|F7c5WO zmLG_?9**uWiAeG!;Ts#Px$UAvIg^SMUbiVrZkO~;^S93xLw)m9R#JX3x1>~gkNXq67vO_` z_E!>Vlt0m-4A$*CmW|mKHAmooV4jL#41zy`w72Qh(%Z--_5EC?k5KzXxh5`nQOFP{ zjSgVAM@~iayyhEhb(`BUY=u+{3(<KwV2TolFRPrHfs(Zea z+P9RRd>i;1+VR72gInL^%Z_A9jM>0FKp&Cqtv0r6i?guS%y}8PomnAVt#H}p1iMo} ziZRFueLY9&7p%ceXj&X~7=ci2?19Hl1#aL@rrBwIY8hE6S!u#lw)wGK6n&r=Y~WZp zt^{qXS+7+1>FnU}=4=)A1gsNtUkr3OkIcE{4k5Cm^RqlqyQ7zqD z=+z@J3WGqhJYfxS4YBl#Ai>a{Ci2|~9|!vGi{$qgfAVlR`_$PhzCcTNI=w$#$LEz$ z?JGMjII8!fbT=U-h)U9%n%ExnDn#lLvJ~6n-7e~Lg*`S|#y9AYT$?c8Td`kH2-;$) z=)Xk84iJP1hHcG7UM(sv{oPDz#57-qRNBG8l#?nNj(O8E><^QuH4TM>qb5k;*f{2n ztOO6q!}I3B^}brhagBO(%mfx z($XE$@SmTHi@}QRoU^yjbMxcE1(`a3q++UuGT=%#W+()>A6s-VU!`^nNf$rvDZE#! z>wNS=$W7z{yQdxUE4Lz%9ZS-(Sr6Cglf-4~Xvp@2zn|eL3n*)s8IT>=Hj7zFjY4`C zAzyfYU)p@LeEzxkTQD9StHz5DD>aNl(uXb#s&FW{V zgx)k4NtI#;YY*l;sh9^!egqk3*Usd4+YfSBT4vg5Y-O`9!kaE=p?SWdW{*IlM@4D< zwiZUn@NARs=7W)X6|Ea)PZq3EU->C6o|iNW1b(?ccktBYu}C@>7ugdI!=?vp0piP@ zFI1i$f9#9v{|6J>{f zUzYTM$CVGhb^S+Ux)I1qILw}{;r;u?M4(>l+&H4B!AW4nE^IARW>tspU^DEu19O@( zN-3st*NKyNKU~+TrN6zu|G+Ow(}@$!^W&p{8bg~@$PGJLgxY3a(bKUV3=^y1N_fX^ zyH4JTfVT5w~yC-aKd%KZd+9vE%n+|$)ahLCDOy}mMDTm*{)jnRvb8{1&<_VQ{2oF7w@>g~USAj99x;`q zXWf$8$8|ai6bvC0zGJG+4zpom!4N94s=cV%(lPk`IgJ2irb^e!dGmAlES)o(o&oP9 zXSg9r1G8q3Qd7NoUg3pOm;d;cieRYcPCE;NCcS`ar@Gj0irK2;q4*Qw6pK_BsX{v( zst-=RxN&5%X2rOuv#HvJ-?ymSZG=z@la5o`XF4VC;1j~rqbvNe_OWJ(Kcp#co}}!w z)#SPN?caIK(kigO#&|Qf)b9IjLwD`tK<9(@`X}>+vC-)-TIS%ygmR7Vd+#FzoLHkx z*MO7Tr!bF6qM`>DnnFqIaShuzoLtJpIZok%Wo;IEv>**i|1kt%lFoNhwkCaZ{@dkW*y~)ssjWAH$eJou!u1 zJqb%ek0_IbKcp+Q2HA7z=TlGf5*RRd+lAeGER_h9Djf)@b$xMghr9xp3!m29g#IfD zeotHxa%88Q)P~SC3Dy}o5PKQ1_}n)-ZPuk_WV!m=d82V!9aV(sveH9oD?uTB9ZC}blj%P4=5(OTU5v~VlqTR@UH5$FKg1u(EqWbV~V8;ZG zjUQqv;HjHw=_KwEI5;#aawLjVm*gRHM<er)MVpQi{&|&jgs)eD7+#Rj*CGmk zXjQR$z{5co#_?VQr_TF+iuB$;$>RLBf+FtDldSK{(GBvB{&z)+1gXGI~?zzHWuFL-xyar-{~t9yhq z*I4iS4JspNETfv>&!DG2Znbf%=fTs}NTRAK!tx7dh)da0Ed~d&KYI=0Uod|Zr5?@B zeS0#Ux^B?&hRkNDz40iU)*1dZH^MCi!@3hy9KQ4>3o7-?P z6VcB~N&Y(;_ov~NK4&y0!QBd+o3QxMU0`phD?IZdWo_88jvjQ9yV!Y9L$mSi?BuJ- zRqQs(dn_3&dWzGsb!Q`fuTq+^o?P!sTs&LUA=21%B>6CXu0C_#Q)RQNLv_C4EYPTU zph?!G&7UMowPRe0t`>RqDdEm)Bu_$l=TGE~1U|iSHBNJWh7=^0S%q6)h(Q z4^b+La-&}rPjSxl?!Bw8Cu3H)%pMNfmJQuDyBE*WS9Cq&u0rO2d6}bh`o+3JJ66#! zDp_N=ApD;De+o0nfltMrFTPnAk9cV${a-~|=20tJR(Nj9uk@rL={Db0&7Dm=?KwI` zO-8^&^0oK`vW$}RK*z!68t1;aIFVUIeMx=Ex1apC<~`86CpF1WK_}-`I*$Wwe;TU4k zpN3<`>RS-mEn}3vm>3s$Gn#@UBJJ~vqRUB%p8Pv=6WJVY$}0ocpiqV9En{pJE@cn% zbcwG&<~JugpBk(rn7gbgG{*mvSMdqP1(7|O5uPZ?+`@(`C$lOR20WtqkTS>DTWyK6 zQYo`UYaR`itIFlLYwS6nv7YG!d($S~em8oG7v-yDHphiOhDCruP7pMuw@TC0@x{p@ zXglOy3q~4}sAl>3%fj@MKpP#P10Z_Q*@*>1f8am2X8nl=Z>7>0WN5zf^p$7Q2-P@Q zFWF#Tyd8k!sF6HtZLC)8v)BA9K<|9{i<88}lSFLnqF0T1Yo54VKSP@ChbNz`SMSH~ ze%vut=PQR;ZItK-ViiH3OGV;4kqXOeJz_f2eN?&p60!3vv-zC~g`3=ZlkABti5V>< z7P#i%>Ulcdi#o9cvSF>ZZlAguVaB(4vifp#L&U*&nJpcXT@Ql}H7I zp>xrnIq**3duFM9H#|_1pDqo%UI*EtZHa)$N znyvRebv}a)U2r+wfvouAx?)QF+B2-vG-8@d&34W9OCp^4%g|7Y*Obe_;#fANwXJ)!ky zwK0s}F*I5|P!zW^lXgd*)9@ef#LsUysYHUNt=-Ccrtcrp=@9SlMqeu$tVEv`*Q;uN zNpj5J|1Z(vu~4E_P6J^A7C+8}Smf=6{e4FqE>>we$F@vRd;al&*TTG}*U{>k(F0fy^=e=}KlW8?RmZj(s zqhLXVUJ!QraFP|1DYS+}G^b7Q5q=VHU4g|RY-oD)%9nTvKbk~VNt2re$KtSc$OsdD zH?VEhetiChHSSwRJDQO0`B1RQy^$96(H5~c-gan*<<5s^L_O=$m3xfvNm(qO=?*>C z?#eTMLhl9xb!+Kg8L4+h)dRuCB&(d)vfVkHdP_^iVjh6)#5qE?G&mD6b95jCtzem* zH0v~O6O5WQLJS7mzZ2v0w!W_AjN|ud<0i_2;V2v%N6Pt-mimz9sI-{jpO7RMX0Z*W zBgo4-I`g_ZJ^7!+-FuX872mAVkcFr=K%U>`BXauC`%psrE2;kEk`xX;!rwSjQ_ono zz3;uDdzAO#fIB4fMmP&Wn;=~o7G*q0S|9Qn>A;{-M(T6$ajOf- zw&++U#GkW^KXyjE_O|TplbR@t5^VXW_euHjoO=l03tj1gmV{3&hj@(_k7knbvh30E z2AVt~Y{f&7#E(;#ERB8-B&vJo3f!sRPDt42B+9UR1-9W;GiON<6uE2=mijzeq8AWM zAR|0RV)#x9PHa&Ve@LpJXIERJG&G#J+h4x=AZY-#e&Qsh2=>UCuq~xX+%GRrPMzU0 zk3zTQq^6Lb-(*c9pPO*F;`h1pty*D);N8P33kuD3U;Z-O#Pq~WcMQp{V5=o%uLWk# zdW>XoUh&e$(ga7TBgnlTQN-!Ah(`yaj=w2|1Y(fGLrXADrg5F#q%%9rKV;@z$Pqio zA@?U?4~xP>bXly)hJ@_lr0n&=6;GxasV>K^o$%BdvY2*ITq>NYYYLs`k7zuHZCm`l z`tYs$F^oRM`1TBw-SdmMwu)OwYt~sU!!QQ_Fa`^6+g5ssplgnC39j@fTxrgQ&*M5h z(p|#C2z*vHZC190z4JQWcncls`OQ$OYmIbu&r(Z9Qkhu!#!0rPOP=_eKe3u|dfrff zPWEQ*6;CG?|B57G7u04Kd|Gs(2ZFSW&Z)FM?7&CmHtr(V%Tz+hKI<)i?<*#VG)nAF z=Ri$xpfc?Y%(-b_TQ;$$-D#gpu;SmDAAR}7P-rLgEQhfLr=SZ3^)NEDi4y}v3$)`$ z2fR@0lt(K?n?;dECm&g#VpN+9`H-=tC~_v8#3u#_Ug%rqYfBAzoB3(d6bc!K{p<-# zw@5q1%^#A=p5e)VG>e6ub6T-z&jI6L+&Ugig=hl!Qe!u>>mwZVZ!5I>O)G;}4u^;H zKKrG#`#0)rqbw=z%Fz~OF52qsiNfzvtScl|pT{9`)XLID<3nsy(sW8^SfoUK+NrK=lJ6#c?$46otMSklKuM&5^Sl~>qATdSZokK1$2a`oY43Dh&a(ipHH z8uBnLk5$%De^#D2rBw4WMNXaqBFCo{?38m4)yNZ)hbKb;}hCNHw*h>1pQo~v_@e}vFD{{5>Nh4e!!EmWcGm2CztX6nH%@{d{vZ- z#5mO}!I$f5BTYI^v$?%}x=GsC?9bnZPOd5@>P@F$@9sJXvU_Gx&Y|Bm=XdphnddnN zqaLg;X=Wnfz(u3jKP0|iZt3_Rs|C-aCU^7C_G}-V-{1BtC45n&h<|!9BqkGev7Kf` zJYx&Prc~C&Mi7X&i}eCbbnmMc)WG0H>*BN2k7SBdZMyD@XyWJi|YT!&)UI_v8rcnRUe)cmvqT~IKBQ;t?3~jhbW!b?eANDjyQ>c z$8Kr&f_DUU@|&v0{PEO*dmo=S9fm~iEXU!>Kcjp<0=&BZX8!ls22U6d3;QBo&M>b{ zV%Ut6^TF52Og(6wstCUh`zsE1=3HZK(?Q3yP8`MIrP8=IsmTdNta4t{?hwv4(HYrw z%I736=xq1p3!=&ERHe@NrgO+D}H=y{CqIK;OsIN-J+=xBO48+vwI{J8l{4gbc!M7a4C zdGp<@aPynoo7;san{j#-(0_4rl`C>{ z)BHH#Zog#V)y~=e+1Y;g!rg4&{-4FOyXGgySNtgpcbC2U3*S5UfARS{Vm9BMEenOn{LN>xn;p?6$yXnY5zb|^B1=_K7Gq)Y9-BQxeJo~S2yE3`wLIR?63Zu z{5|t9ng4fvHnDxScJzZ|cz<)+Q9|b+^X}x}U2e|BjjD2U0M&9`(#cuQ{KYO5)z_S@ zdJS!Xfdy4be^H#<9E{6d5$XTmlF-~hy6*VEVbZ@jY1MJMchK>H$I|^HLC1n?k0pVl zGmoXI{{1`M{dbSn>pnlY_BegN@P$9%PT^bD!rkwI{a=#)Z>5{xJi5KL|94gQ$I*WM z?5^VK=ZEXt1?%R!cd-k1qDNJG+JU{^in{7XEed9(0HW``>91 z21vS(?FW3iy$$#d9Y<=~{&)L#Z1_5=J* zubS=yM8aE0nI+IG=-(J-Gz!2!W{jSHoJ&gPvK4#M6w3OBCMI(oJrLca$*N9+zdDysrCb+=eXyV!K)3Wagtq!=DeZfr|u06 zUR?KgN{BxrD`jrUrDo^4E!}Y(^Ed@GB3P;xa>4h1YgIaxk*Eis0v^q6@!!5`yY=TS z{e4=c9$-Al`E&vshOvRc9|xUs?a8J4Im9|*_kq?4X>ELIA3mdDA}e;bPbsDn`M9|Z zQa;iAOaos101yEu3ZjTMDFP;GwrxNLFjPHC4Kc&80mI({1~L+W0dwXCdD3VGW9#T} z0-BWt$botxujJJ}d?zP|Zbr4P&O77r+NLs{)e6yrg!+ zt-D+2f`UN1pl;cBVY>SdlxaV4YZp}u%X>xaqgDom2a8l6EuDCBgX;~lL$?uF-iLwr zoVl!@Kf$Hyto3;7907`}^rLVUYtlz`YtRk!oKz+&oOCx%_3o=a z(8NO1RWxP6DZw5p-fr1W9L!}L!;8rpBtqOhUvUF%OUgQqQE4sQ8{%DKTPel#H@eJK#=NdZ1>^e(=}U^|C`8k?iyvPk-YvCQ3TUISX7 zJITMDCTI-QFAV(P2AEw`&mZ*wRMnxYmLwMNvXB$)2k1ASarvhW^=|`E_Vik0T`@}JLSc{h=w_rdh{s2>^*h!m?JiY4As9>LEf!kX^7$4C z|E^uj#70gSN|uiF_V{5ut15&1r5ig}5@q=l-cYbrd@l(Jo(}P@AF_sB^J2)bJ9Jz^ zp=@apb6WnN5g>+JI;*`Ligg`P3cH2k*kFIk3M1`Fzowq| zmb(uS3KyGGs(K4H+MieY(pa+{Dul^EYQVo}4DNeBf{A+Nn!B!#Z$P{Xat3FB7t_{0 z+UsM|jZzGP6*hhvC2h?E3KYum*htq@c`e%NI%j)24NwJ^XkE$(qkVhSOfOiuU;YL2(UjD^M{^n zD6WWHZn^L5|0hl zFnnS|C{Gl1nA-*G1tV)(9p46Q{_Fwu_{+<9NMZZC1|JRYC+>zM3+^BMt4Mu5B#5-K zC3z=xqPRK$rRRrl7xwH>H()v}7@Qgt<#U7N579!z6^m=shl5CPlTKgTArySQT(05W z?g~OJBSVyKgg<{3)0~~ZVw-$kR?WIx4cIt#NL?fQr-z$WdMis}`tQ-QZQKPc8dSH?o~`O7{jKblYYfI~WB z2eSn6n|%}McK$%WC0)b0mYLW5sOkh?>lhLiT<|sS8D4hr&%qdej$q{E50q>MLr6`z zn8#fV>(M)aeNT6iCMN)F{6EK?ke1>4Q0TFe7$_wa8ZyGGR3o7A|D!TfZ}-c<*&bkf zuO!L8T7NPm;eBiseB~amnhz%|3gmeK4?thpfO8^~k^f6_Dayq5MPC%N{I|2N^+vR) z7Oa;$-GzLkC7aA=9@U!`y&7_JQchsWbet;dEquB4fWliL?rwRqVbdf_$1*wu3vp-t2kN81Mn{B8dY@3nWNUVn#1r$%lFf zsCMUDt<|4-)%%@=6^l2vnSA)YbB2n`k@4T#*omo|SdAyJ+hBSjH#LZRfr~&>>u+%; zVX9Cjw&Z6rocFCZc_Cfvjb?VuOQ*kw{-*loYdx5`m*#abhJ{WOTiPZEgy&P}x zYe#!S14}DQk+|HmyvI4kK?@*0FYHe|yoHOmW)3Fxc3|d-+<^jT@}#v>`2Fs=TfYk5 zu+}4SazYjgU}^un7Afoebu2NQQ0oUQkB-TuuI7!3+O_n1e3gduPfL%(;Zl#St%{%$ zqRlBfBCt<6g^6aZhFut?M2N%r&2jiyZr}8)G$U5FM$ad0DO!Jp+~Rgcgjfp1`SpmX#$kRN~n9 z0yvqCe8+o4Q10q^yQWvJX#J^-Iiyd@0|`VnMj>!*eMv|eR}h2@I*Yo2^S5xdfgN95 zD9D?u9mlnDbfheDB(3b%d&079di&zuF|N%>QV!W1268MkCWT8#Q0L2gT0^8mZ?G#_ zr*?avGPll`jfNJ@KHq{%#hN90U4sK3LbkaPKF5WFB&g#emC)C-GHC*W)({d6G9@CeY;%+Awpb$jjgSn`YeIGE znD#MEEu27!Zw?DJ9-qQ6=?)YpZLS;V!;;AG&-xo-X z3mZd9Z|FKaD!si|Hx44_W>if3o=~Y>Gmt90EhO)btK`a8x?7OEQ?b4>*`LO;&n5Sm zt80&-q60E34;_a{SXw5S5sD{0lrirA<`_8fB9`%v-%51iDK2LD+_-u*as_w${?#e$ z390{&DSx{2V+4(TYEMRSXMZAOa*YcOOE1(kaKzCadesGD)UE49uuiw|#n5s^4XIm3 zmflRu-`G5^y|CQyo?&x0Bm^&iVZXKn6rJq3m~z51Ed+o{5GD#GhxQQd^lZXvEZ!7v zi#(BEp}+Fw7GWgE2Y2`~?WdAdqhFag3MLdFd>F}gj2ElvgM~JyOsZjC2^<*(sF&e# z=R&f*{{VxRCBXU?$p4ut+T=d|{&`FnmfI;s zi!Y<#J z7HjW{hxkutyeY$;aE#1qBi3d4r(ae-7>wV>PRl#6`O+`O;fsat7)m=1Y$bbJAM<&! zU+m&R|5&=8XVaBQqU@`8q*PXq+r2Ks)QWScz{mLZ)0=H(@m{HnIEK;4X{Ezjp1y0dv#*P!C1aAI0Lc!@i3zZl6y0O0H8IN} zH$q@$jGc^!1$_BgZKlAY^&KXcpOE+PHl}{?5bIW^*jHMRXl9)dqUo_^^4`k+3io!E zbpnAtsGuyqge{VXQQ}Gm32b;g{G16ZXH} zCpKY6vbHUIlk=7h!_hyuvpRZZoDgUVYMB~wfXWUGX+;3y75#O_r09QEZ#CBj;ZhwG z%zNxrvcoV2Pts9gB$%IM|JMG!_;PL;YTWAvfP>l`B0?aX-zx{@3=fYAbvV@F*dySe z4mGU30^;S1-#=ra_t$?16!SIIm&gWWFd(S3t)`{mqiK_NFbc|&&#EH%DXzGe2d4_* zr&oS>L+QKsEj}_b`BsZU z-py0)boC=xUMWf=ei*bw4Qv2~<0{x9{^8_$m+N5nue4+ty^RqBd#xnje~8$q zvG%fBNDn9=l`pO)i(|eb5|$4o#}xk2oAJokD!Wqs-m7vV0K}&T1r0fdTJ%|k4aovnme@i> zxtV4ZL)fpsKjdD>?E zO$-Y?CO%V-lojg}0g9RyYv){|0S@Zv&1+C=Kf5@nZ;*CCT6D=PET_*%c2$fw#m8=t z@g^&&HhyR6#bR#sc>M=1#C>UgU`NJkCsjVmYJmM9=a;-kVYZ&;{f{x}4_CRk=^jp} zQf}yH+?52<@ZaJi)*l@Nqzb+gNX0>Sdj~kZox1m@vRM~vmj?XA+G0o8z<1IaN+uYN zZ$=Qs@JmGkh$rRRpye%hOv&PFC_GL}n&3$kLkt{}trU4;4n|Y!jEm zrU3P04c12pRudxs^)2;vXO$3rq}9uTgNnclQBD@s)oTPA9{nlO`15h+C|19U>KC{j zAR5F{zrY=cW)_`L{qY3}9$BhS{)o;N|IU3|B7Cdlv5FmQf4b>OTCQS37-DeD?x^M+&smNxT*a7!#F1__6%;wL$Yf8t2-o$ohlQ<`TjL$PD_2~v7iPOnnyf{|(VMvw(({3@ zph4>}#GoDxX0a>ZPa!QSW=!WMNmGU))T3yWfl5TTLlON47UXx82e1KjTUL$uFkGkxn2dh>2nDth39@K-i~3YacaI1VQwZ*E zz#{Iq4|5YI?`7vLkt0v0IpavY|2%g`CT2Z7WM>@Tg5 zR#4N8I>2V-0_z}Gwj0DR4pfF2!qNCeLHM|XFym;lT@7X`5dB3-O(-nECDz&wf%bdp zhOY^cdJo=Ca2zlR-N5oPDoTd8u+R$$U+_crLA>;3L;frsVBI;HiLRTJ9fBHxnAX!v zc$)W}hs42ZR`rC_OLz-0_sWbEy^&yP5e4Y_W)j3`FHwqxuJ#O~e;NmC8HsuuV*%NZ zL9RVJ)i3j4#!TCcY@xLsC&p{)qgZGH!Kwo442}{}z%?ql5a|xAr!4hltwE_G(hAdV zoWv9hO<9O7cq>q5u^-=Ic)@veHxg*jrYVglT#xr+Eu`8}9hig4qVtX8V+a!r zcwNC)_ZM==GkM+;eT6N0az_WZEoIfi7zp--)gW}^-vcv(dvu_yu4313gbI=9rtY z&{!=SN1sVW9>QBlvutNh%`F`c&;7|{A+0$dXOoNxVNdp-QNa_Z?8*{4jz$Ff)$}Z- z7u0O9F77BBUP*zrZ|-^=F$wf&g+xg!1p4{ywO=oaz@sSFfBp;PBVI8i7l#{tQ zWR@d{D7YDEodu|p)Gs`4u5_UB(i3eEC3k>Mxh|5<@e)@lt3mtokzO5(E(s9int>C zUqN`~lcR#p4mjE$=|q#{1R|z~g_45cEd$C+$W-W4$av#MQx0PYi{=((7Yf){pwlgg z7Xz3z5_V+Bf zg}M4vWWgUqWKt`{Q8~HQj+?)+D2K20g2k_T*K10UZs*jztu|&`Vr|+feai>T*QP10 z#X!w_k9mvq3&?vD6?+(mmaLEJS!6c5vQ zNzkp?LJ#CNdxz_mNfVD@3^^hvd0!YA-6TMveR-r6{F#{92v9I(?=o3YB|hWsV~kF~gN^2pO*i%N?T^mVsPKOuof%qVJHZ%gh&=vK9`E!(l_ zwyW87odn>kyTvrKH7%24Vn2(tPloSkWjMYL-aRw;);1`GNFx1cz$;ZLi7Ql$gUdRu z89`0N(&a2nyVg$|eANeIND5$w*~`3wY@6ZJ@vJ6luMch51pH}uEn;TNZcN*>aee6x zi*8n$%Fo4KnOPy$ejOmtJ>R;!2`S}1jtJ3>xLHOPWjE5t48ZaXdrr$V^1@1La&&rE*(E^Qj!MFhp~ew-%R~i0FCNn z{MXU~pz%F4@#B+mze3GSoYy}twHn^y#X&;aFaXp$eExa~sSWrL@PI}U%hmhUjKy#% zbCD-WIU80YJ$k2-P@>}>K6wqS>Z1yLAWYeLpl!=nEg#B zt%r2@!yz0^Uf|}&*5@3S-3^S(#F_R?cJ4PO(%9NV9gup`LnLJ}5K~%5l|T}maDeU_ zkKhWVUv6_(*SnD< zigy=ozXd|fdfuol$Tk0-!ssFbC27ucCU_T=fi{bOhSEBWUaDd=!=#ATA&Y7;Kt@YF zF{!#u3A4+6U$x1!iN;YNR-gbw3;JI)BBxivAU~PWL^}tbk#3@DJjJ@|h6v1-Ktj#( zOsuf?4mF2g`YqF35ijXFITn!sIbNK~O)v}9^<9Uj=GTz@MRVA7vRSk244gi1E}M~6 zy%K_D{cLe=%cDU(U$p+{BxD*p{2$S0FATWh3y-gaT02@BgM}szj6?Un<&Ve5OT(b? zfav{x1~I5`5wFVF^Z^@NRr#L>>}DZC(y=F7*j_)30Qhx_#p!Ih9@+xWFTEyS+pE9m zfdht&U$g}!$l3mGL+1nZ|7-jspF>MGMe57ard2E3E?Zi$>B9ra-iBaDTa3IS0L^^Q z8lT(gN?svofy*DN8OG*VOan?a8YMXTO@GsIgHHT%U=zVayY?D2B+zYG4*-jMqQ2dz z)o2gp^pR9Es8p9gpr>*AyX+Kgwy>p;pt_|_U>=s&u>I_};?cF0om-<05~L7ti%;(1 zg94^Js)}ALbmY&Rwe>X!Rk7Jhd7CigApn=E=0jlFLLBBgcgR;f zH^kvyv3o;*eNgQ>Bq8xs8S}hGf)-f1I$wzcZ4ZgTHzk&T!N|`j=WDf$Cfb1;xRhrG zJ$5V?DpL;$aDQ{B>fUmV=)zk*io~#eI#XUMr%;E4@qI>$GkyjyPxC%Hvv0~C6fU(( z;D9-9g{_0UoC>Mw3KX-~Oqc|A1tU4ske>XGmbQFuF|uFJH+L=nsnHB3T%dq9R(PmQ z$LQ$KD>7=b2vci0G%cv9TY9cMJS31$2$t-k6O5iNK-D<$-+w@=ZHCa5wXDaaY1A;8 zf51KP_wj+YluC-%FTh}uE`m{&?t`!{MDAa93A}&BUTZ`r@%bbQxIw2A%N2P2)fl9^ zvE4REjPk;h%MORhM|;J*4qdFF&uMeBC;IaD3z)j(I6FXmf>KM;3Lhms zoPhXEF=}|S-9#A<<)_^k7B9hDRF!w12?H|w;?U0)_!N;wM`gOz*5~wKto8cwXpis` ze)Ez^ORYqwUzO*>{isF8rG>MnaBzW-?9ZoCr9hw?cw7pzcIz37gh8WnfdDfiiYc83 zfhH2bQk3IY%gk-D`|-H{bTxDp9bi<)i=ti^2*&WR8DcyN4mY&{Z5!zdY4MjlYc*9uyg(u`ntnc8`Q z(y3^~@@aQLY;5oX+k(g&Wsb+UsRlioKMsmX5}2yPx!^7K;v55cTLc2U)8eJm@79rG z#a9;GyTKAvvJ^vX#b%2PNlm}r@!{msj_WURhkUup)pl- zA)peRa02x+m8y?5MFD2h()@z;jbv8tM0*QWWW?EZ3@^N8*ZJW+5Z<3d4B>i#@kpaE-f5+$03d(^gI23c7Du;Vn2w4Icl%zv-$?H#^;rP^~n8eB!l-lcss^&( zIy-~^!nlSsRWo0}@Q9(C>h(F^f=D`QfJJjABnIN@#_|^a!qFV*M`qAGahHV?b1vm@$00ZA^op z!{DXi@%694U|6dbLPFEEo6V|Pj-cM374ume9d(u$hz9HS(q5dij2wgQS4lh4Io-+AMjhBdzt1e3H2&!V)3NnqK$RJG>@R{p{m)Utfs8j8KVA_yf@ZA2YI zctP4yy5juG@?u6j1rx-uO}MMBV@x5pFxs(ABoug$*3Mjutii^d*5%o|L9!_FKW(Kf zSin6EwiYQROV$n;M4X8%9Pca(Z8R_nc-T=Nb7df^da769b%i&(@t#77+3lr4x0Qxu zs4UdUSN_v}R{?9GqP@uEgh({9A!ceYBv3D!3u}=Rol7&4+{$8XZ}lO-Dfe{?qdN+_ zU^XKg7AFKIORL1@F|on*c0%-X`88GJw;-Oxwf25ZD}yVemvZT)f!OP4tT^0niV4D; z#XUH|p<_pFhnGtaf^KD^y9Pkzd}Zv&{>T@1;OVaB?kVCJ-NrGcJ~C`^-n+8blZGT9 zv&gxp$mZ{X(aq%`wOsR->J{oIoPc+BhrWY|!k&$fr4*HF`Suz`5gvX}5XtB3W%cr( zwT3b)zt5=KeY>ZF%$xKft+gs1C&!h%!vCB{gZB^y65dgV`7{8xd5GiT;f-Zh8!Rt^ zICl@*I{BdfBqEdj?f&AGxughvEEZrJoPxUf9ekEZ_qXzY9`xD0!9uT5WAheuq^S@B zmoKEPVj#`Nn8FX#2dB6ekx|7mHGTbIWl`z3!MQ6+E#|kMMyW+uL`Y|X58CD^l2pA} zv&-hh_e2Rn;FRpF$+os9egN~_{r9M9k(2ebdvLUc{}cGFV`}5zC99sb>u>D0A&Whu z(}MgHxPmW+TjK+fMzL{UK zCg_1@S40}-sUbl)l9$E&-DQaF??i#P=RP_FhNSN;IsZI~6MrKm(I4VJj~X?l?b1*~ z%)1TfV0uU?bl<8}sA34=Upb3SZQFpJ)Zr&^bY51zu~j!kkPx7$_M)2M>^kv6ptEi$ zH0_{jbAYKCB3f&2KFf(8N~@O6xgif%Qa-zY?t7dSi?R zYhIfp-rhM&cK;iu=!c4$onZT?;%`#T+D5}B;ZipL{Ufj+vhjGy`uW{!2PE)%&HH^U z5EF?#SYfw->=V3;u=oHY=np_)+g5HNd0x>?RZ(IV#I0yg8U}lT4R1CFhVDB+RE}-R zFx7wGO5xe%tep&gx|UIgSg`aLdY2eG^w*6Eou=j!Ih}eV3!stk@((n6JHV7XNlsZp zbphf*S2Q6OT8T!i)3H51=a;kUXMSVJ!!%SUEz585U&6g{MW%88$UWZ?og726RH8MO zH-E64qiARaok>rm496^G+HhTykm0DSF9q^7T=raljq_MIEDu+Sgu^mB8N z?c4<7L#6wJvO|!-?TRlyyj1nCQ?7he{(FN%8NU6eY$d@S_Aj0Ue)5Fk4U0@CHlneW zcE$P4f8A)u*k&Ys<|ubI-;-qb8`@Yd@pi{Nx$5zBeL8DS6ck+``SMW9Rfz^#@zk?X z#RN6?OVq!P-4o05{{@MhHY$Z-jqBsek1xj`^Bml)J@HyYET?Yu}4z zG(J%N`NU{YRXLqghgIkmG*~~|q_oK@LO<^@Bm_y$ZQSh*d?xOjn34t*yD zpc|CKqnyg4^o2{Mh<{OYROJ@h-Y(Yod0A5ykNZX-Jz3ksk3QrW2`nhYg_E7_T6rMa zmpR5~>>a9MroiF|eG4kJ$mTq%?AC$4N=9>a()a%cS;svO^J`?f!%1&;2PoAL<6ybD{xE7J3CE30s znmR8*;T*?){b7n;)v+W10Ph6PPv)ecf-Uy=5!QE;e_H6?l=wijFd;*Z{Dl`N$T;RK zjxYvU5tUGnuybDbe~bV%nMyWqb2AlPG!rl7B;L$PudyGH5!W)zmVoA(j~ z#f6~pjekdcwneCI`&?t!PpWcF#CuF=wm2c66|PkTfj9w2w6ubVMID`x|2S?!f3N&p zj`@u5WPx6ym-SzNX?YjO4i61eouVM@KznNq_=N$;#pNaxZ*@S;`>s7$* zM~@L4A}24m>_7h|_9xU-?Lh$GuL7}Wp`3WN`Ihoq(}}YVSZ_?O0I-wS*2mptDoIqZ zazKhD5-wvxHV`Pck;9d>lUlE5k{@FV@$RVR_Y#_?QfmXUY~Us?zZrqP_`qSIeQ21W zDpxQA1F%{8UZ{K+v?Ykh_e#Tn@aw7QK;Hv3_er$Gs!HfLX#6xlhib%oC1kbtU&n_r z`KU0O<9(q}d_dpH0}A&7j!<6gI0mD_rf-xO?Ce)CAb;}*WC#g>)U@9+dq{`Xhs&vL zA54f|76u2|he5a<03&ujE+EGP{>=4#e540N-%7pYi18IoK&oX!um8>_nDiA%H~=+@ z_hWC6NEf)z6urC^$ftiFkuD-pr!JAe;O7QV8G`eQqeX-pUewz#BrO4+Toqd@XHs4k zj91i7jKFU{bYXM3r;L7}W)B7Ie=QsfnTm7#6SZk1u@b0Td(i_b)DhmYlnuZX`?4 zW)VY}xD1^4(_YZwgaWRkj$ft;e%me!?WV83fl8-1?h^n`;|~n&dRUEg@O4n>SFI6; z;0WYHrTHDfjw`Ij34SO62vf?1<1)?-<+>v4qz?||r@zkvZG|J8P?s?wy)%r1lSsai z87tQL5k3mc)JYLcgV~~EWoWB?7y*o%U~g9l%pS0f=oTmm{HFs;PI(KA)e+eU2NPZ& z(ml!5CDQ`JQ?Z$w75So4fMZ$Mda5Vq?7V(42E_cNljhMIz`{(CZ>k{ z0hPX;)QEgwT4`h0m;r%{b5v7}904LF^Mx=$3}B3akbxPtWfpTIV39d+NeTq}%dljMA|0 z95^2<8HioKhW%~bxR?joCfw+zjy8lU$#x`KB>2$-nmca_7w#@OW-n2?ho22!nLxo(>aBk^5A0G076l9>rv<@)yeMjHn+BL-y4mif!& zwFD41ih>BA2Qi_Q;QDxalcRzYb+u$X!tXCG1dx()$;5L>cK*ZvN~)(b)i^U86R7L} zH50)_W6^()$P-V5>QGDfJ{7}NQSvLdLe(~s*XzqGqt78q2|B-Og%Ri+4FN5R7k|v& zivYgKtETK3)qmarU0i!fE|vFkkdZe)O?RPW^JixABn%b2K8!77) zjt?3pZC9rPN(3M@aIGoU@_0jTXO&!W0FR53gEqHjWy!NY-Mn;^(j>h8rsf}x7QAR7tK@ADrQQV)C7fWJwmSaP4h^yqST zAurL*7+In%z==)6QamsM+WP$0EI9T;=k#o#o{{Lv==aKo1A$I~Y~<~M1*<_~zU;8W z+C%^qqepkuShM6Spk-kQPhyiDEXZ^}R&e=^ZgV4u7!d9v~{bn15OH2$UY@ zY1)K%B99>rO&efYtvgLgo)w zocoS|7E#&TG8+Ui^K^pP9!VCcKnl+dbz;Rva79KV6o*?!k>FXt>vz9}FAy-PoA<*x z9?8C6$Y#oIaTcwp3-_S^bJ=mkrpycU`%l&p&Eox>?4l<|l^L?Kz(u>Yh6g6Jo05<4@h9={Fk&XD)?y60|_u z#M|p!)pgYLZ%LN6m;WxG#;=8db6E$Q1D|Q4iusA|La3MzIr0WRUwEhV14>n`bqlu( zNGI1mV?}*40h~``n{}(_;R7iQRgN3zR>{njpR09Jr z;5eH%=%KW!hkUy-eH}`}M}=`46<{LHct_bR{FsqC&|2qPJC%*CS=5rK_Si_8a^e_H zRB+Ty5_1wQP-oTNH;5B~Ku2HzFJAJ6jBBfLSUCe`Ex^kC>*F7~W=L^VLyJhtrTUj{ z^LM~lio~{rZIlhtOJ=Tha5BDz7IyP7jZ+RVx^+1}S{+Tr`H{T8>ui{d3HVWwH~G=% zTBE`UgB@L2wo(0%t|N<(Psb2Uh|`VzpaLB*xtPXJ)WF;p>9>Ol^T8vr57fe629=`3 zCi9!&VIfaMxPjztMwkD zHjL7Zz4Jw?c+Ov2P+?jSLTO<Wd5C=H69-=c!CrYb%O6I2nkV?yX`K?_vwt3_!FJYT+JOCm#3wz1Od z$6$9fNDTsRc3=Qi9_G=d$hbV_6cI_;MmXFN7%OEiD`Y^zSaqoYinfu@)4(|wf6Z}M zWZg1|DVWJ{Ey)8-42>Xp#bq7{-Ru{xAXqNmTkj9F6b*#goExJaGiu zfCLcrU(S8jTH~evrgJZH0%Jv?Zz5@6TM9=kMfr|4Um1-TW#H9vTLeJ43+p!;Q2xCh z5)w^fxDW#4#q{a?<#i1pNR)Tfh-`q(Y)j)dcpU^$-+Di)W=Q~ii|ji9(#EvV6La7g z>9UJcEq>h=sWs&UfG*C6E`tKz`-jY!+dMA|OcZ^j7XQfh0M#~Cf~T)!hs2~^FB<8b zzTK;zFQCfYKACuV0B@eF-!EMiOIK9zgETrS_sFGk)Sz&X-RZKrTN+jS%yTUjdW$#~>z|LhgmNFK74; zQZ(e=ugk(7#vXH-aQ(gr#q@G9p0hrXvH zax)A78}2L5LdnAbu$CW62rXanaC|S~C6stPk`KQ1W9a5DR*|MWIcKT>!8Qb<Ll} z*aLNvsT<0r*`czP{&oEfoc@X|&Jz78XV$z0&SxaP`^6}S1-zOTAxbNe^lKj>mbYBG%JreEdB9KP|u40Fr!`{qKR z>(W34*x$q!`7O^<*WzpM*&D!_npQKQkYxACqL@XrB1jpHnPOQ%#LG0K-u!3bjkaCi z;#ds-j_{jV3}D%6V8Pq&Ky%-y+HHfqYl!$j|J5TGyU`UgdQg85rjhnfTk>qF-Y&=z z71o9Sv#^VW{TLe+9P-~Y3wgVjOyTA5e_@l>|^T8yAAEh<9CcL0%U(pl+yuJhXxkgc_xR7*uB6~*D; zVL*#o(ma0^pKK>0w1kK7h{;p5f;OJ4pz|^&tg9Nw-(Xbv<0OxWDf-M+xKuAb;0rH> zh5-Tdf%IGp7s5G6sV19YC=qSl_Dg?2%`O8&$p|X1!1o{adwl{x-_~Keg90|wRx3ty@`-`2iAtipC)wYav-v|Ig4?TUP#7z0XBUb?HE**z< zP;M&d>lLjlz@JVqU_#-+6@d@*2pF*rsS&23l79#M#^@CS=?q50p?jk)fjf8ARZWFz zQq0>J5LF*6`nRbfx2FU^I!5GcOrTB}McEV38}qoT8z0;z0NyMs-ckdWnL?&Cz>1&| z4>n0v`SV~E;RmIHep#FgG58`iAjlF()v?JD&den69`7smwnVa*R7GP)UjQrPb#D8N z5CpY8qsVW|8{}&Bv_g8ZcDHt)RyUhRTVR#uRh+~PB z&t;xb0XhQwTBSVNsa}$R(I{gbkMuoky8_{uSl!=nbO&rT`DZBni84bx^yz0vhrR%NUUcyt zSyd6cpI5x}3v(Sz_Nz)JqTQ2nIG|wYb48fZ$ok{CoEp*;PbM$;sFB*<)J~PHyuc1r z+5|kEU(VIb)qq>`SsaikQlD-dM_rU*D2}S2Qv-b$Wynb+xK85g z&@ohlFB=%I@d3@U-9YiU!*NvDhi#&5B<+3fYla8N{SjcM26P5qzLo^Ys)=iae^3=| zp+dS3lJgUscV>`E-iKICya(G)BV2g9uPpF@j_L6$3`mPyH@VXYEs3uz8Ue#_1&IU^ zkwG;oGguC7r5^kqE8`L4bK+ml%OAG$HeuxV^`$9saaf5Rymw}II631l`lami$!e+D z<<@jS<|N{zm?Xwt*BG=PyFC_y?GI`bWK-BR61&d`p8d2r9(NgXN?=AWQ)qgKTm&*z zDh(3sh9r`eq|IyJy=z)k`mJjT$%i7vd zJtma}w%Za{mJJLUtnEB^v6IL!Vn!=-8pH{0MTp}tR(%^GW#kxET*yxlF|D{CPa^t5 z8!a#V5O*`Lc&27Pu9jG>b9p|hQ{F_Awz{=ij-W0ak&fByEAR3KP=U9iOf@yE8uG|CtcI*`iVO zS~9WnKpkGQ5pf|4I!Tgv-D;fBzM`Ch{cIxm31a49&kg%xHe89r&mi;Py3_zg+`Zd` z22ULN;9)bDSLI7}EffF+(dhhm+@092Vd+3}X7>ard?Kl%8A7|>i#Kek&G>6bM21@8 zLz)iXE)psVobHv__6Zd~jBUpaLnhf+bXlejdpa9{Q$wM`4=262&kGQ^2{i2c_#vDR zukvBC%oWNwd{}f9mNno)nIaqFe}#Zfxlm5)OF>uRD8Nyk@FaKzQ73o0})R8Eyz zPIWgpMdhB3?XgHA*yJkkS39nvoMwo>9U;^!_zBtR@VSI;C8EemTvsOa3F>x%xO!e# zI4r83lhu8B|Jc0Ujw^>BQuXlod@C_^cs8&aU0fi0*!l1{{e<{gyVs6;`1nWbBHxA5 z9|bX4%q9Du&{;dKlw9!BWBy_8z{Tnm!e?4SmHy#uK*B}*s%8L=@Ok3=28Y_r9TxdK z6!-`nqKjyH#R4*|o zVWaYZ{OYbqX#GkPeb7bL)Caz3`Rkvc>Bqh(pWodD+eO>bCKz&mkmhVM`3aXc4Q#SL zctkcF)Rw)1=O0cW`}aJco0h&p?qQ!M=aRq}AHQEd{~OIm{1ETZa;h8WaNS45=AdNT zatgVC+eJR`?gF0}pXq$A|4px6&*Oh9dsU+X4Lm>y!QUbKkaH;sY;q>Z-a&qZY&}aCsO7kYJw(}}i;DQsnc6`-?)j^3jx#iV)<0Y9o+1_XiJDa_%5IF5J3LyR*68F?W zu?Ks0)RIKg-54^pm!}=0!oV3k)P*IHRxpXJJgFalh8XBVZIUF6uZ)x&xn6^X*e$^A zD9@OAe{{Y3D{BL&uMcBPT zlgfI_$kIrmkW3*lh%_$VA#&^HA~!km2lceBpX`<8l;=VB1M3lTM+YAK|Gs>+gFG+r zM<4&~#|$#Lr09<0TInsy_+NW=ng0FI`{$$(B}a4|{6-1^jy1uQV@A*U|IQtj5||aL zwKcDWkFJ_!n1{;#8I-s2Y664bAK<=^9!^f6qw57~HOq!A$&;3@_%SS4P{hqA%%nXO znkI*7sJog9c>}eXP!LnU#h{<>$u`bTCK_%cF_D~-coHNDcPxU2hnI(UNiV$m5)$pY zeQr*Xn@XWsPh%otrl3_KS)m_|LXhg}b7huxK^BbD7D@K#Yf!kSM$h5FGB7p&Y3}&Q z*YdKOMC^Lk=gQK}C!#g?Z59ex+&)v_jwE$;{_M@jll~vZIOm5BLYWw7WD^_(hKEExw!nDp&8yOG8jsz3t;%=mSO~6^kkg z_8?owb}~U60nvhUH)`ywH-aN@0pbX^xsN0+Td&xC5#0yzZd>;1P{1nbNqj3+psu>$Xn& z^fcb@QlrsO$I!Fosy{Lq_CWJ9z}k@|^nj=p8}o$}!Q+`!_c3 zGFcFJfy?U8Wc%}U8HI{79mGxh(}u5W;;rXN+=oVVw^?~EHNG@_(Xlp~py+;hLkCs0 zRrpoD%lXn<$+xi18amR=Ec{*F3D+$^VDRYX8b*@wPpb0&Mebw0ryOMwC&nq9Ci?Jt*6fo zJy3Zp3kwc4F7}9W<2fz>Ut+_d>_i^0aV^TqD2pwbGT7Yrae;VfzPwt2V?uf0lC%Rp z?s~e^o`+GKI_^rEN~vm%FJX77kUE(+r4uhS zn?JTm^|#hk>Ww?D>0apY3!27R8DGKyicE66b>ga5UPR!x9oilEniXf&WTE|J&1kB8 zDUPK=m0I!H%zM>zzl+Yb30Q|*ZCiM6;b(t)ht-sTM#(V$GU<Am9_)54Nwla90X|aKP|Xw?ER=@KV1(yxwh5s8%irelH9rcP zpPt@CcWDdCg2&SKC!hPy_S9^)(l~|)S!`^Ne+VEd;?=)SFps$5mTs5A`aop%O-fh! z+&SlHvqdH?2qKMr&(D8kaA*txDNw?kr%{A(CtE&1Lf0nH5P|(C48D8~o#=tMmu6pk zq5gt8*6&f!XnK|tl`9o06(9}g@I**dPD&|0*2`M|@V&A-$j`hcCW8bVAM%68k%PUv zdinNOkc=G%*eEby#vHjkaiQvRu7sPz%AgJ!1`KVg_|{gMK1s+XVs<;!GyQI zp<&z~dwRRCb>1dN^KaLpkzDJa6KtKHHsLDEAal#^kVt=S1~8^%c?ZLDb1vrA@zghY z#ks8BL6Y&jsHX|c?s_eVH-n0W2%r>DlTFZZjuLuJmt{N0h(o%8brdChjN^vlCF_q5dbFt7`Fa2I+pc0d>sg>A{MbH*TaH;Yu zuNZ|8+d0@d*Z`$;;ll}uS*K_Dv2V#6{JIO-4<08-xx+e2H*Wy%f6UGE8AqvT=eivX)DgJwrxwiBco977|9Rwo0DAi=Xl zP7=Ku#j@!aTG>$vQkV6oO1>)>Lc1;pL{;9eJXPM%-Mmne1t{)aov@IE@FW}J=0X~g9iK6beWciqU{PqR zWFclQ_7vSt8td~OAbfj)=p-_woG81KBH>bd7Wky#$!4#AT*JCmL_eCig$SS4Sg; zq!k&mp3=I%A9fj6Hg`753B{xCi}9`K1!HU(6J;p~)kX~?w_0}m*4K~IDGMN;7*UB# zC)>Wa;>X53wZK2YQk!&%IK3WnDNH;Lr$P0^asw^Pl&m}MgfT53g9A_}$vz$271uk< zG-$@x5fu-NYR9Z`2rMeAP+yweyZotzb!PsFiy1{8&3-&NV&ij*@xl4T_8rHXJjVNZ zk6CVWBVv2ixMHB3S>s$JrR#x$&$cuGva`Lrb>D^G?%GWwA0Cu@utdW4to0eLne|Ak z#dsIxH8TI{&Fr_y8~Mw;bm=}?I!4OH3Mnox;T+KQD^W_Z4E0wgnRmjG zKOTk3pxdJ1Tdv0GsoN1VjqasbwNXY2v!j%~E(&waQ^U-?r1i3}%<7K){n)=p^Tma;7c{nUBY17b2w0>aIIgqB*7sk8P*#P z8!UfK?-m+mZGdlhBRf3=;c?AcSJ}}Q<1B}~iW8?MleK7kBwgYw4s4;6Mhd6qlzca5 z*JlIQV_hFdI|6PRPc}{VOju*2Y%;CUjTu8=CE%!9m`Cpm`{&GX3a0ANk7?^DC9j-FG)GHiDsi`)u4~k$7Qn!DyBiPMze|y zGB*csk~xJ7+N1kFLh#k(3d5DpW47>Vyr+-uH0855OR;jTsJ`;McnPX~voZ_z{URjd z`_A_r?p8~BFurh`znGG+H_e`K)VFRWgtxZ+!p}O#v%QsZyPu*m#=r2Hb|-Gg%R7V4 zAIA}yk)4z`Bx6cd|LL1tQ=bSKEjz2m=Yki^Q$Kn)I_QnsPHE(Q01MU6(+~6~?kHEf z?>Gi@O8&d~iswd}t>!N2 z$K40%HiMMMb4qw+Jb;&SZyzI+PC_yq=e#DpUfPzCx7fDhJo@SNCxw}y1OPW`)@pl;?An$R&`j{_ViCb`560+-p-Iym;v6uJkl?e^d7w- zv{NWw1+Z(T_oRt)e^sFPK4GZtd$C40pH3e#mclQi+*my(TP!0o;?t@$@G8*hC_X#B z=4H)G_9Np9jl`uUS5=a-u-g%LDFx0iy=DCzA_fbT_~_+dE54GpRt{uomV--4XFJq5 zZs81p+8noqbGJ&~YUWQp>6{>+(1XWdz^_Eoz6C2x$!dmrDn-d`o_Hz9XhdQ=S-A0r z<>meLm!r3{W&5+VE}!z2Xp^-{XOzoMd23~iiI=<;NHDi9f2k4u2lER*^%gq>nUe9& zW@Z7T@+2>^z$rB;*-f7`DC3qx5qjv{)lyU3I7s6@47e9 z#ehmGxtB^xV*4UM((gFkQ|nCrt~A!>{9MX_{Uf=8$m6Hyv-suEIVPcnF8Vi_@0|3> z&nY=Le7F2IHFEK`Kw1QOMTe+23(huld>m?yxmg3Q2)WbSo_#7a6lntT6j^hM1JR9x z!|NeV(Y@8G@rp#T^$A~5vls%ve($P z^ab<<0vTD3d&?lJhtJyc%;AbCNvzEHUfg92A(I%)U`|k(lDYw0O zPA(OWn2QY$KFRsQOsAECE`!fRF~UIgab4?4|MC?B(~XFoGc%u}rGbujm-j38V#z{W zO|vmO=Fv&k9ID>LpUIofdO7&%qDmQp)bYTfJ#*WCW#_vJs3B$jrZFhr+Kbw^#=Cx z1ydY^3CoEdlVO}){%7V(L%K?F4OWZq%Ax^#rWSuPYw#8R!VA8|%orfpVuedrp3~Ai z+Rx#A-`=&i+%Cq+S>(^Ws>f?me_q9JUjF2WYH+S9B0zNk$LVwC-#M8O&z9s4 zV#ZCQGkP{BZl*U{{6aRcWyr*Q9E&yC+B(jQMLSTP6@H@xR~HA1!*sn7MsFxJNP-hK zOl|g1X5P7p4S%+-GUZbqGI}2+V*mF0S{AC!8d+f0M9mXtOkN?r7_B2Ol`@0$cghX6 zvX#q&Tlm1YP@lgq0O`;2vVno-XP)gFH)dHr_1Y79zOb5_t~6t4fwvv~^Rq_!~r?Wpv-xQgQ6`Qy17tNc)X z@ZSd}h96@c>BOEQ>BIGm3|9H?oBdUPP2ZLc({JoqK_e^?kLQS1SY(QnMOl|Z<+iWN z(<)K(agx>#LEEe%Y>q~E!^xOVv1gvu(@rrf=V{j+tr`9vjnB51FfBQL+?n$H2#HKl zEz{~!G56Rdf{?PSTDT~Fgha49e#dDR4SFZZGf&j=JGRi42ctR@?PT62rSxs`0;d-I zCj9Aq2%c%g(40tT9P5uOI7T1)k1L7NM_=8Es#%N@$JkxtD_6(d0lwkgyr8;*BAJq* z_ruxWhw)ENJ4ZT+5k_1pSajH{3!mLob1ibi{72kB^V#fDbAYHfkA_BWaCp^eb4B^& z93~8HaOmC#&v_3QL`H|47ffo?NG8-h#<6jl@@~XvrD!#2(Z5O-ub{f?sRZj}vIkPU z^&IJ`IR)y@y0)On!N0m}aV=xNN54{$@Y$%N3&%tujG}~MvUCw^I5%N@V)A^`C2T#M zS1tPYd~8>lXS_QoL|Lc0$`BfHR;xSAR5X@%e4|OkMae;V$I+y_Q7C&p%q_Hx>gIsA z9(Ma%1hrF8uV{>t_XoFG7PfoJ(`wVQdrBW#K6HfGw>E>DAl-VffgulF5cisu?y