i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 16:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

rtabmap_sync tests and doc (#1454)

Browse Source 
* rtabmap_sync tests and doc

* Added some diagrams

* cleanup some diagrams

* fixing running tests in parallels
This commit is contained in:
matlabbe
2026-09-13 11:25:32 -07:00
committed by GitHub
parent 61edb4ee85
commit 5062bf0614
45 changed files with 4481 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_sync/test/common_data_subscriber_fixture.hpp
+205
View File
@@ -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 <rtabmap_sync/CommonDataSubscriber.h>
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
#include <memory>
#include <string>
#include <vector>
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<Record> & 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<cv_bridge::CvImageConstPtr> & imageMsgs,
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
const sensor_msgs::msg::LaserScan & scanMsg,
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr & odomInfoMsg,
const std::vector<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs,
const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > &,
const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > &,
const std::vector<cv::Mat> &) 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<Record> 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<RecordingSubscriber> start(
const std::vector<rclcpp::Parameter> & params = {}, bool gui = false)
{
sub_ = addNode(std::make_shared<RecordingSubscriber>(
rclcpp::NodeOptions().parameter_overrides(params), gui));
return sub_;
}
/// Creates a publisher on @p topic and waits for the subscriber to discover it.
template <typename MsgT>
typename rclcpp::Publisher<MsgT>::SharedPtr advertise(const std::string & topic)
{
typename rclcpp::Publisher<MsgT>::SharedPtr publisher =
helper()->create_publisher<MsgT>(topic, 10);
EXPECT_TRUE(waitForSubscriber(publisher)) << "nobody subscribed to " << topic;
return publisher;
}
std::shared_ptr<RecordingSubscriber> sub_;
};
} // namespace rtabmap_sync_test
#endif /* RTABMAP_SYNC_COMMON_DATA_SUBSCRIBER_FIXTURE_HPP_ */
rtabmap_sync/test/msg_builders.hpp
+284
View File
@@ -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 <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rtabmap_msgs/msg/odom_info.hpp>
#include <rtabmap_msgs/msg/rgbd_image.hpp>
#include <rtabmap_msgs/msg/scan_descriptor.hpp>
#include <rtabmap_msgs/msg/sensor_data.hpp>
#include <rtabmap_msgs/msg/user_data.hpp>
#include <opencv2/core/core.hpp>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
#include <string>
#include <vector>
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<cv::Point3f> & 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<points.size(); ++i)
{
float * p = reinterpret_cast<float *>(&cloud.data[i * cloud.point_step]);
p[0] = points[i].x;
p[1] = points[i].y;
p[2] = points[i].z;
}
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<cv::Point3f> points;
points.reserve(count);
for(size_t i=0; i<count; ++i)
{
points.push_back(cv::Point3f(1.0f + float(i), 0.0f, 0.0f));
}
return makeXYZCloud(frameId, stamp, points);
}
/// A ScanDescriptor carrying a 2D scan, a 3D scan, or both, and optionally a descriptor.
inline rtabmap_msgs::msg::ScanDescriptor makeScanDescriptor(
const std::string & frameId, double stamp,
bool with2d = true, bool with3d = false, bool withGlobalDescriptor = false)
{
rtabmap_msgs::msg::ScanDescriptor msg;
msg.header.frame_id = frameId;
msg.header.stamp = stampOf(stamp);
if(with2d)
{
msg.scan = makeLaserScan(frameId, stamp);
}
if(with3d)
{
msg.scan_cloud = makeScanCloud(frameId, stamp);
}
if(withGlobalDescriptor)
{
// Only "not empty" matters here: consumers pass the payload straight to
// RTAB-Map, which is what knows how to decode it.
msg.global_descriptor.header = msg.header;
msg.global_descriptor.data = {1, 2, 3, 4};
}
return msg;
}
/// An identity-pose odometry message at @p x meters along the x axis.
inline nav_msgs::msg::Odometry makeOdometry(
const std::string & frameId, double stamp, double x = 0.0,
const std::string & childFrameId = "base_link")
{
nav_msgs::msg::Odometry msg;
msg.header.frame_id = frameId;
msg.header.stamp = stampOf(stamp);
msg.child_frame_id = childFrameId;
msg.pose.pose.position.x = x;
msg.pose.pose.orientation.w = 1.0;
return msg;
}
inline rtabmap_msgs::msg::OdomInfo makeOdomInfo(
const std::string & frameId, double stamp, int inliers = 50)
{
rtabmap_msgs::msg::OdomInfo msg;
msg.header.frame_id = frameId;
msg.header.stamp = stampOf(stamp);
msg.inliers = inliers;
msg.matches = inliers;
return msg;
}
/// A SensorData carrying one RGB-D camera, as rtabmap_odom republishes it.
inline rtabmap_msgs::msg::SensorData makeSensorData(
const std::string & frameId, double stamp, int width = 8, int height = 8)
{
rtabmap_msgs::msg::SensorData msg;
msg.header.frame_id = frameId;
msg.header.stamp = stampOf(stamp);
msg.left = makeRgbImage(frameId, stamp, width, height);
msg.right = makeDepthImage(frameId, stamp, width, height);
msg.left_camera_info.push_back(makeCameraInfo(frameId, stamp, width, height));
msg.right_camera_info.push_back(makeCameraInfo(frameId, stamp, width, height));
geometry_msgs::msg::Transform localTransform;
localTransform.rotation.w = 1.0;
msg.local_transform.push_back(localTransform);
return msg;
}
/// An uncompressed user data matrix (several rows, so it is not taken as compressed).
inline rtabmap_msgs::msg::UserData makeUserData(
const std::string & frameId, double stamp)
{
rtabmap_msgs::msg::UserData msg;
msg.header.frame_id = frameId;
msg.header.stamp = stampOf(stamp);
msg.rows = 2;
msg.cols = 2;
msg.type = CV_8UC1;
msg.data = {1, 2, 3, 4};
return msg;
}
/**
* @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.
*/
inline cv::Point3f readXYZ(const sensor_msgs::msg::PointCloud2 & cloud, size_t index)
{
uint32_t xOffset = 0, yOffset = 4, zOffset = 8;
for(size_t i=0; i<cloud.fields.size(); ++i)
{
if(cloud.fields[i].name == "x") { xOffset = cloud.fields[i].offset; }
else if(cloud.fields[i].name == "y") { yOffset = cloud.fields[i].offset; }
else if(cloud.fields[i].name == "z") { zOffset = cloud.fields[i].offset; }
}
const unsigned char * base = &cloud.data[index * cloud.point_step];
return cv::Point3f(
*reinterpret_cast<const float *>(base + xOffset),
*reinterpret_cast<const float *>(base + yOffset),
*reinterpret_cast<const float *>(base + zOffset));
}
} // namespace rtabmap_sync_test
#endif /* RTABMAP_SYNC_MSG_BUILDERS_HPP_ */
rtabmap_sync/test/node_test_utils.hpp
+208
View File
@@ -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 <gtest/gtest.h>
#include <rclcpp/rclcpp.hpp>
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <vector>
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<rclcpp::executors::SingleThreadedExecutor>();
helper_ = std::make_shared<rclcpp::Node>("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 <typename NodeT>
std::shared_ptr<NodeT> addNode(const std::shared_ptr<NodeT> & node)
{
executor_->add_node(node);
nodes_.push_back(node);
return node;
}
/// The helper node, used to publish inputs and subscribe to outputs.
rclcpp::Node::SharedPtr helper() { return helper_; }
/**
* @brief Spins until @p done returns true, or the timeout elapses.
* @return true if @p done became true
*/
bool spinUntil(
const std::function<bool()> & done,
std::chrono::milliseconds timeout = std::chrono::milliseconds(5000))
{
const std::chrono::steady_clock::time_point deadline =
std::chrono::steady_clock::now() + timeout;
while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline)
{
if(done())
{
return true;
}
executor_->spin_once(std::chrono::milliseconds(10));
}
return done();
}
/// Spins for a fixed duration, for the "nothing should happen" assertions.
void spinFor(std::chrono::milliseconds duration)
{
const std::chrono::steady_clock::time_point deadline =
std::chrono::steady_clock::now() + duration;
while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline)
{
executor_->spin_once(std::chrono::milliseconds(10));
}
}
/**
* @brief Waits until @p publisher has at least @p count matched subscriptions.
*
* Publishing before the node under test has discovered the topic silently drops the
* message, which is the most common cause of a flaky in-process node test.
*/
template <typename PublisherT>
bool waitForSubscriber(const PublisherT & publisher, size_t count = 1)
{
return spinUntil([&]() { return publisher->get_subscription_count() >= count; });
}
/**
* @brief Waits until @p subscription sees at least one publisher.
*
* Every node here publishes only when it has subscribers, so the test's subscription
* has to be discovered before the input is sent.
*/
template <typename SubscriptionT>
bool waitForPublisher(const SubscriptionT & subscription, size_t count = 1)
{
return spinUntil([&]() { return subscription->get_publisher_count() >= count; });
}
/// Collects every message received on @p topic, for later assertions.
template <typename MsgT>
struct Collector
{
typename rclcpp::Subscription<MsgT>::SharedPtr subscription;
std::vector<typename MsgT::ConstSharedPtr> messages;
size_t size() const { return messages.size(); }
bool empty() const { return messages.empty(); }
const MsgT & back() const { return *messages.back(); }
const MsgT & front() const { return *messages.front(); }
};
/// Subscribes the helper node to @p topic and records everything it receives.
template <typename MsgT>
std::shared_ptr<Collector<MsgT>> collect(
const std::string & topic, const rclcpp::QoS & qos = rclcpp::QoS(10))
{
std::shared_ptr<Collector<MsgT>> collector = std::make_shared<Collector<MsgT>>();
collector->subscription = helper_->create_subscription<MsgT>(
topic, qos,
[collector](const typename MsgT::ConstSharedPtr msg) {
collector->messages.push_back(msg);
});
return collector;
}
private:
rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
rclcpp::Node::SharedPtr helper_;
std::vector<rclcpp::Node::SharedPtr> nodes_;
};
} // namespace rtabmap_sync_test
#endif /* RTABMAP_SYNC_NODE_TEST_UTILS_HPP_ */
rtabmap_sync/test/test_common_data_subscriber.cpp
+315
View File
@@ -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<RecordingSubscriber> 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<RecordingSubscriber> 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<RecordingSubscriber> 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<RecordingSubscriber> 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<RecordingSubscriber> 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<RecordingSubscriber> 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<RecordingSubscriber> 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<RecordingSubscriber> 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<RecordingSubscriber> 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<RecordingSubscriber> sub = start({
rclcpp::Parameter("subscribe_scan", true),
rclcpp::Parameter("subscribe_scan_descriptor", true)});
EXPECT_FALSE(sub->isSubscribedToScan2d());
std::shared_ptr<RecordingSubscriber> other = addNode(
std::make_shared<RecordingSubscriber>(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<RecordingSubscriber> 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<RecordingSubscriber> 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<RecordingSubscriber> 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<RecordingSubscriber> 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<RecordingSubscriber> stereo = addNode(
std::make_shared<RecordingSubscriber>(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<RecordingSubscriber> 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<RecordingSubscriber> 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<RecordingSubscriber> 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<RecordingSubscriber> 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<RecordingSubscriber> 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<RecordingSubscriber> 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<RecordingSubscriber> 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<RecordingSubscriber> 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
rtabmap_sync/test/test_common_data_subscriber_sync.cpp
+540
View File
@@ -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 <rtabmap_msgs/msg/rgbd_images.hpp>
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<sensor_msgs::msg::Image>::SharedPtr rgb =
advertise<sensor_msgs::msg::Image>("rgb/image");
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
advertise<sensor_msgs::msg::Image>("depth/image");
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
advertise<sensor_msgs::msg::CameraInfo>("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<sensor_msgs::msg::Image>::SharedPtr rgb =
advertise<sensor_msgs::msg::Image>("rgb/image");
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
advertise<sensor_msgs::msg::Image>("depth/image");
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom =
advertise<nav_msgs::msg::Odometry>("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<sensor_msgs::msg::Image>::SharedPtr rgb =
advertise<sensor_msgs::msg::Image>("rgb/image");
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
advertise<sensor_msgs::msg::Image>("depth/image");
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
rclcpp::Publisher<rtabmap_msgs::msg::OdomInfo>::SharedPtr odomInfo =
advertise<rtabmap_msgs::msg::OdomInfo>("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<sensor_msgs::msg::Image>::SharedPtr rgb =
advertise<sensor_msgs::msg::Image>("rgb/image");
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
advertise<sensor_msgs::msg::Image>("depth/image");
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan =
advertise<sensor_msgs::msg::LaserScan>("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<sensor_msgs::msg::Image>::SharedPtr rgb =
advertise<sensor_msgs::msg::Image>("rgb/image");
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
advertise<sensor_msgs::msg::Image>("depth/image");
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud =
advertise<sensor_msgs::msg::PointCloud2>("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<sensor_msgs::msg::Image>::SharedPtr rgb =
advertise<sensor_msgs::msg::Image>("rgb/image");
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
advertise<sensor_msgs::msg::Image>("depth/image");
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
rclcpp::Publisher<rtabmap_msgs::msg::ScanDescriptor>::SharedPtr descriptor =
advertise<rtabmap_msgs::msg::ScanDescriptor>("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<sensor_msgs::msg::Image>::SharedPtr rgb =
advertise<sensor_msgs::msg::Image>("rgb/image");
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
advertise<sensor_msgs::msg::Image>("depth/image");
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
rclcpp::Publisher<rtabmap_msgs::msg::ScanDescriptor>::SharedPtr descriptor =
advertise<rtabmap_msgs::msg::ScanDescriptor>("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<sensor_msgs::msg::Image>::SharedPtr rgb =
advertise<sensor_msgs::msg::Image>("rgb/image");
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
advertise<sensor_msgs::msg::CameraInfo>("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<sensor_msgs::msg::Image>::SharedPtr left =
advertise<sensor_msgs::msg::Image>("left/image_rect");
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr right =
advertise<sensor_msgs::msg::Image>("right/image_rect");
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr leftInfo =
advertise<sensor_msgs::msg::CameraInfo>("left/camera_info");
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rightInfo =
advertise<sensor_msgs::msg::CameraInfo>("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<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbd =
advertise<rtabmap_msgs::msg::RGBDImage>("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<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbd =
advertise<rtabmap_msgs::msg::RGBDImage>("rgbd_image");
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud =
advertise<sensor_msgs::msg::PointCloud2>("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<rtabmap_msgs::msg::RGBDImages>::SharedPtr rgbdx =
advertise<rtabmap_msgs::msg::RGBDImages>("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<sensor_msgs::msg::LaserScan>::SharedPtr scan =
advertise<sensor_msgs::msg::LaserScan>("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<sensor_msgs::msg::PointCloud2>::SharedPtr cloud =
advertise<sensor_msgs::msg::PointCloud2>("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<sensor_msgs::msg::PointCloud2>::SharedPtr cloud =
advertise<sensor_msgs::msg::PointCloud2>("scan_cloud");
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom =
advertise<nav_msgs::msg::Odometry>("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<rtabmap_msgs::msg::SensorData>::SharedPtr data =
advertise<rtabmap_msgs::msg::SensorData>("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<rtabmap_msgs::msg::SensorData>::SharedPtr data =
advertise<rtabmap_msgs::msg::SensorData>("sensor_data");
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom =
advertise<nav_msgs::msg::Odometry>("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<nav_msgs::msg::Odometry>::SharedPtr odom =
advertise<nav_msgs::msg::Odometry>("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<nav_msgs::msg::Odometry>::SharedPtr odom =
advertise<nav_msgs::msg::Odometry>("odom");
rclcpp::Publisher<rtabmap_msgs::msg::OdomInfo>::SharedPtr odomInfo =
advertise<rtabmap_msgs::msg::OdomInfo>("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<sensor_msgs::msg::Image>::SharedPtr rgb =
advertise<sensor_msgs::msg::Image>("rgb/image");
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
advertise<sensor_msgs::msg::Image>("depth/image");
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
advertise<sensor_msgs::msg::CameraInfo>("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; i<sub_->size(); ++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<sensor_msgs::msg::Image>::SharedPtr rgb =
advertise<sensor_msgs::msg::Image>("rgb/image");
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
advertise<sensor_msgs::msg::Image>("depth/image");
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
advertise<sensor_msgs::msg::CameraInfo>("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<Collector<diagnostic_msgs::msg::DiagnosticArray>> diagnostics =
collect<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics");
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
advertise<sensor_msgs::msg::Image>("rgb/image");
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
advertise<sensor_msgs::msg::Image>("depth/image");
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
advertise<sensor_msgs::msg::CameraInfo>("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";
}
rtabmap_sync/test/test_rgb_sync.cpp
+258
View File
@@ -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 <rtabmap_sync/rgb_sync.hpp>
#include <rtabmap/core/Compression.h>
#include <string>
#include <vector>
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<rclcpp::Parameter> & params = {})
{
node_ = addNode(std::make_shared<rtabmap_sync::RGBSync>(
rclcpp::NodeOptions().parameter_overrides(params)));
out_ = collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image");
rgbPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>(
"rgb/camera_info", 10);
ASSERT_TRUE(waitForSubscriber(rgbPub_));
ASSERT_TRUE(waitForSubscriber(infoPub_));
ASSERT_TRUE(waitForSubscribedFromNode("rgbd_image"));
}
void collectCompressed()
{
compressed_ = collect<rtabmap_msgs::msg::RGBDImage>("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<rtabmap_sync::RGBSync> node_;
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out_;
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> compressed_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgbPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::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; i<got.depth.data.size(); ++i)
{
ASSERT_EQ(got.depth.data[i], 0u) << "byte " << i << " is not zero";
}
EXPECT_EQ(got.depth_camera_info.width, 8u)
<< "the fake depth is registered to the color camera, so it shares its calibration";
}
TEST_F(RGBSyncTest, DefaultsToExactSync)
{
// A camera publisher sends the image and its camera_info together with the same
// stamp, so there is nothing to approximate.
start();
rgbPub_->publish(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<rtabmap_sync::RGBSync>(rclcpp::NodeOptions()));
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgbPub =
helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub =
helper()->create_publisher<sensor_msgs::msg::CameraInfo>("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<Collector<rtabmap_msgs::msg::RGBDImage>> late =
collect<rtabmap_msgs::msg::RGBDImage>("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<rtabmap_sync::RGBSync>(rclcpp::NodeOptions()
.parameter_overrides({rclcpp::Parameter("qos", 2)})));
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr bestEffort =
helper()->create_publisher<sensor_msgs::msg::Image>(
"rgb/image", rclcpp::QoS(10).best_effort());
EXPECT_TRUE(waitForSubscriber(bestEffort));
}
rtabmap_sync/test/test_rgbd_sync.cpp
+530
View File
@@ -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 <rtabmap_sync/rgbd_sync.hpp>
#include <rtabmap/core/Compression.h>
#include <cmath>
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
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<rclcpp::Parameter> & params = {})
{
node_ = addNode(std::make_shared<rtabmap_sync::RGBDSync>(
rclcpp::NodeOptions().parameter_overrides(params)));
out_ = collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image");
rgbPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
depthPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>(
"rgb/camera_info", 10);
ASSERT_TRUE(waitForSubscriber(rgbPub_));
ASSERT_TRUE(waitForSubscriber(depthPub_));
ASSERT_TRUE(waitForSubscriber(infoPub_));
ASSERT_TRUE(waitForSubscribedFromNode("rgbd_image"));
}
/// Also subscribes to the compressed output. Call right after start().
void collectCompressed()
{
compressed_ = collect<rtabmap_msgs::msg::RGBDImage>("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; i<count; ++i)
{
const double stamp = start + 0.1*double(i);
rgbStamps_.push_back(stamp);
depthStamps_.push_back(stamp + depthOffset);
rgbPub_->publish(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<double> & stamps, double stamp)
{
for(double candidate : stamps)
{
if(std::fabs(candidate - stamp) < 1e-6)
{
return true;
}
}
return false;
}
std::shared_ptr<rtabmap_sync::RGBDSync> node_;
std::vector<double> rgbStamps_;
std::vector<double> depthStamps_;
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out_;
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> compressed_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgbPub_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depthPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::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<const uint16_t *>(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<uint16_t>(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<Collector<rtabmap_msgs::msg::RGBDImage>> late =
collect<rtabmap_msgs::msg::RGBDImage>("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<rtabmap_sync::RGBDSync>(rclcpp::NodeOptions()));
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgbPub =
helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depthPub =
helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub =
helper()->create_publisher<sensor_msgs::msg::CameraInfo>("rgb/camera_info", 10);
ASSERT_TRUE(waitForSubscriber(rgbPub));
ASSERT_TRUE(waitForSubscriber(depthPub));
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<Collector<rtabmap_msgs::msg::RGBDImage>> late =
collect<rtabmap_msgs::msg::RGBDImage>("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; i<out_->size(); ++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<Collector<diagnostic_msgs::msg::DiagnosticArray>> diagnostics =
collect<diagnostic_msgs::msg::DiagnosticArray>("/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<rclcpp::Parameter> & params)
{
addNode(std::make_shared<rtabmap_sync::RGBDSync>(
rclcpp::NodeOptions().parameter_overrides(params)));
}
template <typename MsgT>
typename rclcpp::Publisher<MsgT>::SharedPtr input(
const std::string & topic, Reliability reliability)
{
rclcpp::QoS qos(10);
reliability == kBestEffort ? qos.best_effort() : qos.reliable();
return helper()->create_publisher<MsgT>(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<sensor_msgs::msg::Image>("rgb/image", kBestEffort)));
EXPECT_TRUE(waitForSubscriber(
input<sensor_msgs::msg::CameraInfo>("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<sensor_msgs::msg::CameraInfo>::SharedPtr bestEffortInfo =
input<sensor_msgs::msg::CameraInfo>("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<sensor_msgs::msg::Image>("rgb/image", kBestEffort)))
<< "the image side must have kept qos";
}
TEST_F(RGBDSyncQosTest, PublishesWithTheConfiguredReliability)
{
startSync({rclcpp::Parameter("qos", int(kBestEffort))});
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> reliable =
collect<rtabmap_msgs::msg::RGBDImage>(
"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<Collector<rtabmap_msgs::msg::RGBDImage>> bestEffort =
collect<rtabmap_msgs::msg::RGBDImage>(
"rgbd_image", rclcpp::QoS(10).best_effort());
EXPECT_TRUE(waitForPublisher(bestEffort->subscription));
}
rtabmap_sync/test/test_rgbdx_sync.cpp
+263
View File
@@ -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 <rtabmap_sync/rgbdx_sync.hpp>
#include <rtabmap/utilite/UException.h>
#include <rtabmap_msgs/msg/rgbd_images.hpp>
#include <string>
#include <vector>
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<rclcpp::Parameter> & extra = {})
{
std::vector<rclcpp::Parameter> params = extra;
params.push_back(rclcpp::Parameter("rgbd_cameras", cameras));
addNode(std::make_shared<rtabmap_sync::RGBDXSync>(
rclcpp::NodeOptions().parameter_overrides(params)));
out_ = collect<rtabmap_msgs::msg::RGBDImages>("rgbd_images");
for(int i=0; i<cameras; ++i)
{
pubs_.push_back(helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>(
"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; i<pubs_.size(); ++i)
{
pubs_[i]->publish(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<Collector<rtabmap_msgs::msg::RGBDImages>> out_;
std::vector<rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::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<rtabmap_sync::RGBDXSync>(rclcpp::NodeOptions()
.parameter_overrides({rclcpp::Parameter("rgbd_cameras", 1)}))),
UException);
}
TEST_F(RGBDXSyncTest, RejectsACameraCountAboveEight)
{
EXPECT_THROW(
addNode(std::make_shared<rtabmap_sync::RGBDXSync>(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; i<out_->size(); ++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<rtabmap_sync::RGBDXSync>(rclcpp::NodeOptions()
.parameter_overrides({rclcpp::Parameter("qos", 2)})));
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr bestEffort =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>(
"rgbd_image0", rclcpp::QoS(10).best_effort());
EXPECT_TRUE(waitForSubscriber(bestEffort));
}
rtabmap_sync/test/test_stereo_sync.cpp
+323
View File
@@ -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 <rtabmap_sync/stereo_sync.hpp>
#include <string>
#include <vector>
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<rclcpp::Parameter> & params = {})
{
node_ = addNode(std::make_shared<rtabmap_sync::StereoSync>(
rclcpp::NodeOptions().parameter_overrides(params)));
out_ = collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image");
leftPub_ = helper()->create_publisher<sensor_msgs::msg::Image>(
"left/image_rect", 10);
rightPub_ = helper()->create_publisher<sensor_msgs::msg::Image>(
"right/image_rect", 10);
leftInfoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>(
"left/camera_info", 10);
rightInfoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>(
"right/camera_info", 10);
ASSERT_TRUE(waitForSubscriber(leftPub_));
ASSERT_TRUE(waitForSubscriber(rightPub_));
ASSERT_TRUE(waitForSubscriber(leftInfoPub_));
ASSERT_TRUE(waitForSubscriber(rightInfoPub_));
ASSERT_TRUE(waitForSubscribedFromNode("rgbd_image"));
}
void collectCompressed()
{
compressed_ = collect<rtabmap_msgs::msg::RGBDImage>("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<rtabmap_sync::StereoSync> node_;
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out_;
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> compressed_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr leftPub_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rightPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr leftInfoPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rightInfoPub_;
};
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<double> 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<rtabmap_sync::StereoSync>(rclcpp::NodeOptions()));
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr leftPub =
helper()->create_publisher<sensor_msgs::msg::Image>("left/image_rect", 10);
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rightPub =
helper()->create_publisher<sensor_msgs::msg::Image>("right/image_rect", 10);
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr leftInfo =
helper()->create_publisher<sensor_msgs::msg::CameraInfo>("left/camera_info", 10);
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rightInfo =
helper()->create_publisher<sensor_msgs::msg::CameraInfo>("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<Collector<rtabmap_msgs::msg::RGBDImage>> late =
collect<rtabmap_msgs::msg::RGBDImage>("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; i<out_->size(); ++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<rtabmap_sync::StereoSync>(rclcpp::NodeOptions()
.parameter_overrides({rclcpp::Parameter("qos", 2)})));
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr bestEffort =
helper()->create_publisher<sensor_msgs::msg::Image>(
"left/image_rect", rclcpp::QoS(10).best_effort());
EXPECT_TRUE(waitForSubscriber(bestEffort));
}
rtabmap_sync/test/test_sync_diagnostic.cpp
+301
View File
@@ -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 <rtabmap_sync/SyncDiagnostic.h>
#include <rtabmap/utilite/UException.h>
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
#include <memory>
#include <string>
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<diagnostic_updater::DiagnosticTask*> otherTasks = {})
{
node_ = std::make_shared<rclcpp::Node>("sync_diagnostic_test_node");
diagnostic_ = std::make_unique<rtabmap_sync::SyncDiagnostic>(
node_.get(), tolerance, windowSize);
diagnostic_->init(topic, "nothing received", otherTasks);
addNode(node_);
out_ = collect<diagnostic_msgs::msg::DiagnosticArray>("/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; i<count; ++i)
{
diagnostic_->tickInput(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<rtabmap_sync::SyncDiagnostic> diagnostic_;
std::shared_ptr<Collector<diagnostic_msgs::msg::DiagnosticArray>> 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<rclcpp::Node>("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<rclcpp::Node>("sync_diagnostic_small_window"));
EXPECT_NO_THROW(rtabmap_sync::SyncDiagnostic(node.get(), 0.2, /*windowSize=*/1));
}