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

rtabmap_util tests and doc (#1450)

Browse Source 
* Initial tests

* more tests

* More in-depth deskew() testing

* slightly less verbose clamping corruption warning

* added tf buffer related tests

* added remaining tests

* Added rosdoc2, improve tests when we require sync of odom stamp and sensor stamp

* cleanup doc

* fixing ci

* rtabmap_util tests and doc

* Added db_player tests

* Added MapsManager tests

* Added map_assembler tests

* Documenting node first draft

* relative links

* Fixed british->usa english style. Reviewed all md files.

* added link to install ros1

* updated badges

* added Iron

* added ubuntu

* added codecov

* updated coverage ci

* fixing rosdep

* updated ci cov job

* ci bump

* fixing cov ci

* small doc cleanup
This commit is contained in:
matlabbe
2026-09-07 21:23:22 -07:00
committed by GitHub
parent f77dda2b58
commit 61edb4ee85
60 changed files with 9443 additions and 192 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_util/test/db_builders.hpp
+366
View File
@@ -0,0 +1,366 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#ifndef RTABMAP_UTIL_DB_BUILDERS_HPP_
#define RTABMAP_UTIL_DB_BUILDERS_HPP_
/**
* @file
* @brief Synthetic RTAB-Map databases for the db_player tests.
*
* db_player replays whatever a database happens to contain, and which topics it even
* creates depends on the payloads it finds. Rather than ship a recorded database, each
* scenario is written here with DBDriver so the expected values are visible right next
* to the assertions.
*
* @note Only the *compressed* buffers of a SensorData are persisted, so every payload is
* compressed before being handed to the driver. Saving raw-only data silently
* writes empty blobs.
*/
#include <rtabmap/core/Compression.h>
#include <rtabmap/core/DBDriver.h>
#include <rtabmap/core/EnvSensor.h>
#include <rtabmap/core/GPS.h>
#include <rtabmap/core/Link.h>
#include <rtabmap/core/Signature.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UFile.h>
#include <gtest/gtest.h>
#include <opencv2/core/core.hpp>
#include <functional>
#include <string>
#include <vector>
namespace rtabmap_util_test {
//============================================================================
// What every synthetic database contains, and what the tests assert against
//============================================================================
constexpr int kDbFrames = 3; ///< nodes in each database
constexpr double kFirstStamp = 1000.0; ///< stamp of node 1, seconds
constexpr double kStampStep = 0.05; ///< seconds between consecutive nodes
constexpr float kPoseStep = 0.5f; ///< meters along x between odometry poses
constexpr double kOdomVariance = 0.25; ///< diagonal of the odometry covariance
constexpr int kImageWidth = 80;
constexpr int kImageHeight = 60;
constexpr double kFx = 100.0;
constexpr double kFy = 100.0;
constexpr double kCx = 40.0;
constexpr double kCy = 30.0;
constexpr double kBaseline = 0.12;
constexpr uint16_t kDepthMillimeters = 1500;
constexpr double kGpsLongitude = -71.9;
constexpr double kGpsLatitude = 45.4;
constexpr double kGpsAltitude = 123.0;
constexpr double kGpsError = 2.5;
constexpr double kEnvSensorValue = 21.5;
/// The odometry pose of node @p id, one step further along x than the previous one.
inline rtabmap::Transform poseOf(int id)
{
return rtabmap::Transform(kPoseStep * float(id - 1), 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
}
/// The stamp of node @p id.
inline double stampOfNode(int id)
{
return kFirstStamp + kStampStep * double(id - 1);
}
/// Where the camera sits on the robot: 10 cm forward, 20 cm up, looking forward.
inline rtabmap::Transform cameraLocalTransform()
{
return rtabmap::Transform(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f) *
rtabmap::CameraModel::opticalRotation();
}
/// Where the lidar sits on the robot.
inline rtabmap::Transform scanLocalTransform()
{
return rtabmap::Transform(0.05f, 0.0f, 0.3f, 0.0f, 0.0f, 0.0f);
}
/// The ground truth pose of node @p id, offset from the odometry pose so they differ.
inline rtabmap::Transform groundTruthOf(int id)
{
return rtabmap::Transform(kPoseStep * float(id - 1), 1.0f, 0.0f, 0.0f, 0.0f, 0.0f);
}
/// The prior (global) pose of node @p id.
inline rtabmap::Transform globalPoseOf(int id)
{
return rtabmap::Transform(kPoseStep * float(id - 1), 2.0f, 0.0f, 0.0f, 0.0f, 0.0f);
}
//============================================================================
// A database file that cleans itself up
//============================================================================
/// A uniquely named database path under the test temp directory, erased on destruction.
class TempDatabase
{
public:
explicit TempDatabase(const std::string & tag)
{
static int counter = 0;
path_ = std::string(::testing::TempDir()) +
uFormat("rtabmap_util_db_player_%s_%d_%d.db", tag.c_str(), (int)getpid(), ++counter);
UFile::erase(path_.c_str());
}
~TempDatabase() { UFile::erase(path_.c_str()); }
TempDatabase(const TempDatabase &) = delete;
TempDatabase & operator=(const TempDatabase &) = delete;
const std::string & path() const { return path_; }
private:
std::string path_;
};
//============================================================================
// Writing the databases
//============================================================================
/// Builds the sensor payload of node @p id; see the writeXxxDatabase() functions.
typedef std::function<rtabmap::SensorData(int id, double stamp)> DataBuilder;
/// Adds anything beyond the payload: links, ground truth, and so on.
typedef std::function<void(int id, rtabmap::Signature & signature)> NodeDecorator;
/**
* @brief Writes @p frames consecutive nodes sharing the plumbing every replay needs.
*
* Nodes are numbered from 1, stamped kStampStep apart (db_player replays at the database
* stamps, so a node without one aborts the read), posed kPoseStep apart along x, and
* joined by the neighbor links that carry the odometry covariance.
*/
inline void writeDatabase(
const std::string & path, int frames,
const DataBuilder & makeData,
const NodeDecorator & decorate = NodeDecorator())
{
rtabmap::DBDriver * driver = rtabmap::DBDriver::create();
ASSERT_NE(driver, nullptr);
ASSERT_TRUE(driver->openConnection(path, /*overwritten=*/true)) << "cannot create " << path;
for(int id=1; id<=frames; ++id)
{
const double stamp = stampOfNode(id);
rtabmap::Signature * s = new rtabmap::Signature(
id, /*mapId=*/0, /*weight=*/1, stamp, /*label=*/"",
poseOf(id), rtabmap::Transform(), makeData(id, stamp));
if(id > 1)
{
// The backward neighbor link is where DBReader reads the odometry
// covariance from: it publishes the inverse of this information matrix.
const rtabmap::Transform motion(kPoseStep, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
s->addLink(rtabmap::Link(id, id-1, rtabmap::Link::kNeighbor, motion.inverse(),
cv::Mat::eye(6, 6, CV_64FC1) / kOdomVariance));
}
if(decorate)
{
decorate(id, *s);
}
driver->asyncSave(s); // the driver takes ownership
driver->emptyTrashes(false);
}
driver->closeConnection(true);
delete driver;
}
/// A color image whose pixels identify the node, so a test can tell frames apart.
inline cv::Mat makeRgb(int id)
{
return cv::Mat(kImageHeight, kImageWidth, CV_8UC3, cv::Scalar(id, 2*id, 3*id));
}
inline cv::Mat makeDepth()
{
return cv::Mat(kImageHeight, kImageWidth, CV_16UC1, cv::Scalar(kDepthMillimeters));
}
inline rtabmap::CameraModel rgbdCameraModel()
{
return rtabmap::CameraModel(kFx, kFy, kCx, kCy, cameraLocalTransform(), 0.0,
cv::Size(kImageWidth, kImageHeight));
}
inline rtabmap::StereoCameraModel stereoCameraModel()
{
return rtabmap::StereoCameraModel(kFx, kFy, kCx, kCy, kBaseline, cameraLocalTransform(),
cv::Size(kImageWidth, kImageHeight));
}
/// RGB + registered depth from a single camera.
inline void writeRgbdDatabase(const std::string & path, int frames = kDbFrames)
{
writeDatabase(path, frames, [](int id, double stamp) {
rtabmap::SensorData data;
data.setId(id);
data.setStamp(stamp);
data.setRGBDImage(rtabmap::compressImage2(makeRgb(id), ".png"),
rtabmap::compressImage2(makeDepth(), ".png"), rgbdCameraModel());
return data;
});
}
/// A rectified mono stereo pair.
inline void writeStereoDatabase(const std::string & path, int frames = kDbFrames)
{
writeDatabase(path, frames, [](int id, double stamp) {
const cv::Mat left(kImageHeight, kImageWidth, CV_8UC1, cv::Scalar(id));
const cv::Mat right(kImageHeight, kImageWidth, CV_8UC1, cv::Scalar(2*id));
rtabmap::SensorData data;
data.setId(id);
data.setStamp(stamp);
data.setStereoImage(rtabmap::compressImage2(left, ".png"),
rtabmap::compressImage2(right, ".png"), stereoCameraModel());
return data;
});
}
/// A color image with no calibration at all, which db_player replays on "image".
inline void writeImageOnlyDatabase(const std::string & path, int frames = kDbFrames)
{
writeDatabase(path, frames, [](int id, double stamp) {
rtabmap::SensorData data;
data.setId(id);
data.setStamp(stamp);
data.setRGBDImage(rtabmap::compressImage2(makeRgb(id), ".png"), cv::Mat(),
std::vector<rtabmap::CameraModel>());
return data;
});
}
//============================================================================
// Laser scans
//============================================================================
constexpr int kScanBins = 20;
constexpr float kScanAngleMin = -1.0f;
constexpr float kScanAngleMax = 1.0f;
constexpr float kScanAngleIncrement = 0.1f; // (max-min)/kScanBins
constexpr float kScanRangeMin = 0.1f;
constexpr float kScanRangeMax = 10.0f;
/// The range measured in bin @p bin of the 2D scan.
inline float scanRangeOf(int bin) { return 1.0f + 0.1f * float(bin); }
/**
* @brief A 2D scan with one point at the center of every bin.
*
* db_player re-bins the cartesian points back into a LaserScan message, so putting each
* point at a bin center makes the expected index exact rather than a rounding coin flip.
*/
inline rtabmap::LaserScan makeScan2d()
{
cv::Mat points(1, kScanBins, CV_32FC2);
for(int bin=0; bin<kScanBins; ++bin)
{
const float angle = kScanAngleMin + (float(bin) + 0.5f) * kScanAngleIncrement;
const float range = scanRangeOf(bin);
points.at<cv::Vec2f>(0, bin) = cv::Vec2f(range * std::cos(angle), range * std::sin(angle));
}
return rtabmap::LaserScan(rtabmap::compressData2(points), rtabmap::LaserScan::kXY,
kScanRangeMin, kScanRangeMax, kScanAngleMin, kScanAngleMax, kScanAngleIncrement,
scanLocalTransform());
}
constexpr int kScanCloudPoints = 50;
inline rtabmap::LaserScan makeScan3d()
{
cv::Mat points(1, kScanCloudPoints, CV_32FC3);
for(int i=0; i<kScanCloudPoints; ++i)
{
points.at<cv::Vec3f>(0, i) = cv::Vec3f(1.0f + 0.01f*float(i), 0.02f*float(i), 0.5f);
}
return rtabmap::LaserScan(rtabmap::compressData2(points), /*maxPoints=*/0, /*maxRange=*/0.0f,
rtabmap::LaserScan::kXYZ, scanLocalTransform());
}
/// A 2D lidar only, no camera.
inline void writeScan2dDatabase(const std::string & path, int frames = kDbFrames)
{
writeDatabase(path, frames, [](int id, double stamp) {
rtabmap::SensorData data;
data.setId(id);
data.setStamp(stamp);
data.setLaserScan(makeScan2d());
return data;
});
}
/// A 3D lidar only, no camera.
inline void writeScan3dDatabase(const std::string & path, int frames = kDbFrames)
{
writeDatabase(path, frames, [](int id, double stamp) {
rtabmap::SensorData data;
data.setId(id);
data.setStamp(stamp);
data.setLaserScan(makeScan3d());
return data;
});
}
//============================================================================
// Everything else db_player can replay
//============================================================================
/// The gravity orientation stored as a link, which is how DBReader rebuilds an IMU.
inline rtabmap::Transform gravityTransform()
{
return rtabmap::Transform(0.0f, 0.0f, 0.0f, 0.1f, 0.2f, 0.0f);
}
/**
* @brief RGB-D plus the optional channels: ground truth, prior pose, GPS, gravity and an
* environmental sensor.
*
* @note The prior's information matrix must not leave a huge rotational variance, or
* DBReader drops the global pose on the assumption GPS already provided the prior.
*/
inline void writeRichDatabase(const std::string & path, int frames = kDbFrames)
{
writeDatabase(path, frames,
[](int id, double stamp) {
rtabmap::SensorData data;
data.setId(id);
data.setStamp(stamp);
data.setRGBDImage(rtabmap::compressImage2(makeRgb(id), ".png"),
rtabmap::compressImage2(makeDepth(), ".png"), rgbdCameraModel());
data.setGPS(rtabmap::GPS(stamp, kGpsLongitude, kGpsLatitude, kGpsAltitude,
kGpsError, /*bearing=*/0.0));
rtabmap::EnvSensors sensors;
sensors.insert(std::make_pair(rtabmap::EnvSensor::kAmbientTemperature,
rtabmap::EnvSensor(rtabmap::EnvSensor::kAmbientTemperature,
kEnvSensorValue, stamp)));
data.setEnvSensors(sensors);
return data;
},
[](int id, rtabmap::Signature & s) {
s.setGroundTruthPose(groundTruthOf(id));
s.addLink(rtabmap::Link(id, id, rtabmap::Link::kPosePrior, globalPoseOf(id),
cv::Mat::eye(6, 6, CV_64FC1) * 100.0));
s.addLink(rtabmap::Link(id, id, rtabmap::Link::kGravity, gravityTransform(),
cv::Mat::eye(6, 6, CV_64FC1)));
});
}
} // namespace rtabmap_util_test
#endif /* RTABMAP_UTIL_DB_BUILDERS_HPP_ */
rtabmap_util/test/msg_builders.hpp
+217
View File
@@ -0,0 +1,217 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#ifndef RTABMAP_UTIL_MSG_BUILDERS_HPP_
#define RTABMAP_UTIL_MSG_BUILDERS_HPP_
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <rtabmap_msgs/msg/rgbd_image.hpp>
#include <opencv2/core/core.hpp>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
#include <functional>
#include <string>
#include <vector>
namespace rtabmap_util_test {
inline rclcpp::Time stampOf(double seconds)
{
return rclcpp::Time(
int32_t(seconds), uint32_t((seconds - int32_t(seconds)) * 1e9), RCL_ROS_TIME);
}
/// A rectified pinhole CameraInfo; @p tx is P(0,3), non-zero for a stereo right camera.
inline sensor_msgs::msg::CameraInfo makeCameraInfo(
const std::string & frameId, double stamp, int width, int height,
double tx = 0.0, double fx = 100.0)
{
sensor_msgs::msg::CameraInfo info;
info.header.frame_id = frameId;
info.header.stamp = stampOf(stamp);
info.width = width;
info.height = height;
info.distortion_model = "plumb_bob";
info.d = {0.0, 0.0, 0.0, 0.0, 0.0};
info.k = {fx, 0.0, width/2.0, 0.0, fx, height/2.0, 0.0, 0.0, 1.0};
info.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
info.p = {fx, 0.0, width/2.0, tx, 0.0, fx, height/2.0, 0.0, 0.0, 0.0, 1.0, 0.0};
return info;
}
inline sensor_msgs::msg::Image makeImage(
const std::string & frameId, double stamp,
const cv::Mat & image, const std::string & encoding)
{
std_msgs::msg::Header header;
header.frame_id = frameId;
header.stamp = stampOf(stamp);
sensor_msgs::msg::Image msg;
cv_bridge::CvImage(header, encoding, image).toImageMsg(msg);
return msg;
}
/// An RGB-D message with raw bgr8 color and 16UC1 depth.
inline rtabmap_msgs::msg::RGBDImage makeRGBDImage(
const std::string & frameId, double stamp, int width = 8, int height = 8,
const cv::Scalar & rgbColor = cv::Scalar(10, 20, 30), uint16_t depthValue = 1500)
{
rtabmap_msgs::msg::RGBDImage msg;
msg.header.frame_id = frameId;
msg.header.stamp = stampOf(stamp);
msg.rgb = makeImage(frameId, stamp, cv::Mat(height, width, CV_8UC3, rgbColor), "bgr8");
msg.depth = makeImage(frameId, stamp,
cv::Mat(height, width, CV_16UC1, cv::Scalar(depthValue)), "16UC1");
msg.rgb_camera_info = makeCameraInfo(frameId, stamp, width, height);
msg.depth_camera_info = makeCameraInfo(frameId, stamp, width, height);
return msg;
}
/**
* @brief An RGB-D message carrying a stereo pair instead of depth.
*
* The "depth" slot holds the mono8 right image and the second camera info carries the
* baseline in P(0,3), which is what makes consumers treat the pair as stereo rather
* than as color plus depth.
*/
inline rtabmap_msgs::msg::RGBDImage makeStereoRGBDImage(
const std::string & frameId, double stamp, int width = 8, int height = 8,
double baseline = 0.12, double fx = 100.0)
{
rtabmap_msgs::msg::RGBDImage msg;
msg.header.frame_id = frameId;
msg.header.stamp = stampOf(stamp);
msg.rgb = makeImage(frameId, stamp,
cv::Mat(height, width, CV_8UC3, cv::Scalar(10, 20, 30)), "bgr8");
msg.depth = makeImage(frameId, stamp,
cv::Mat(height, width, CV_8UC1, cv::Scalar(60)), "mono8"); // right image
msg.rgb_camera_info = makeCameraInfo(frameId, stamp, width, height, 0.0, fx);
msg.depth_camera_info =
makeCameraInfo(frameId, stamp, width, height, -fx*baseline, fx);
return msg;
}
/**
* @brief A dense unorganized XYZ float cloud.
*
* @note This writes the points exactly as given: it does not model sensor motion. To
* build a cloud that deskewing can actually correct, use makeSkewedWallScan(),
* which derives the distortion from the same trajectory the TF describes.
*
* @param withTimeChannel add a FLOAT32 "t" channel of per-point offsets, as a spinning
* lidar publishes, so the cloud can be deskewed.
*/
inline sensor_msgs::msg::PointCloud2 makeXYZCloud(
const std::string & frameId, double stamp,
const std::vector<cv::Point3f> & points,
bool withTimeChannel = false, double sweepDuration = 0.099)
{
sensor_msgs::msg::PointCloud2 cloud;
cloud.header.frame_id = frameId;
cloud.header.stamp = stampOf(stamp);
cloud.height = 1;
cloud.width = points.size();
cloud.is_bigendian = false;
cloud.is_dense = true;
const int fieldCount = withTimeChannel ? 4 : 3;
cloud.fields.resize(fieldCount);
const char * names[4] = {"x", "y", "z", "t"};
for(int i=0; i<fieldCount; ++i)
{
cloud.fields[i].name = names[i];
cloud.fields[i].offset = 4 * i;
cloud.fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32;
cloud.fields[i].count = 1;
}
cloud.point_step = 4 * fieldCount;
cloud.row_step = cloud.point_step * cloud.width;
cloud.data.resize(cloud.row_step * cloud.height);
for(size_t i=0; i<points.size(); ++i)
{
float * p = reinterpret_cast<float *>(&cloud.data[i * cloud.point_step]);
p[0] = points[i].x;
p[1] = points[i].y;
p[2] = points[i].z;
if(withTimeChannel)
{
p[3] = points.size() > 1
? float(sweepDuration * double(i) / double(points.size() - 1))
: 0.0f;
}
}
return cloud;
}
/**
* @brief The raw scan of a flat wall captured while the sensor moves straight at it.
*
* Sample @p i is taken at `i * step` seconds into the sweep, by which time the sensor
* has closed in by `displacement(elapsed)`. Expressed in the sensor frame at capture
* time the wall therefore appears to slide closer: a straight wall is recorded bent.
* Deskewing with the same motion must flatten it back to @p wallDistance.
*
* @param frameId sensor frame
* @param stamp stamp of the first sample, which is also the message stamp
* @param sampleCount number of samples along the wall
* @param sweepDuration seconds from the first sample to the last
* @param wallDistance distance to the wall at the first sample, in meters
* @param displacement distance travelled as a function of seconds since the first
* sample; must match the motion published to TF
*/
inline sensor_msgs::msg::PointCloud2 makeSkewedWallScan(
const std::string & frameId, double stamp,
size_t sampleCount, double sweepDuration, float wallDistance,
const std::function<double(double)> & displacement)
{
std::vector<cv::Point3f> points;
points.reserve(sampleCount);
for(size_t i=0; i<sampleCount; ++i)
{
const double elapsed =
sampleCount > 1 ? sweepDuration * double(i) / double(sampleCount - 1) : 0.0;
points.push_back(cv::Point3f(
wallDistance - float(displacement(elapsed)), // the skew
-1.0f + 2.0f * float(i) / float(sampleCount > 1 ? sampleCount - 1 : 1),
0.0f));
}
return makeXYZCloud(frameId, stamp, points, /*withTimeChannel=*/true, sweepDuration);
}
/**
* @brief Reads the x/y/z of a point from any FLOAT32 xyz cloud.
*
* Looks the offsets up in the field list rather than assuming they are 0/4/8, so it also
* works on clouds produced by laser_geometry, which lay their fields out differently.
*/
inline cv::Point3f readXYZ(const sensor_msgs::msg::PointCloud2 & cloud, size_t index)
{
uint32_t xOffset = 0, yOffset = 4, zOffset = 8;
for(size_t i=0; i<cloud.fields.size(); ++i)
{
if(cloud.fields[i].name == "x") { xOffset = cloud.fields[i].offset; }
else if(cloud.fields[i].name == "y") { yOffset = cloud.fields[i].offset; }
else if(cloud.fields[i].name == "z") { zOffset = cloud.fields[i].offset; }
}
const unsigned char * base = &cloud.data[index * cloud.point_step];
return cv::Point3f(
*reinterpret_cast<const float *>(base + xOffset),
*reinterpret_cast<const float *>(base + yOffset),
*reinterpret_cast<const float *>(base + zOffset));
}
} // namespace rtabmap_util_test
#endif /* RTABMAP_UTIL_MSG_BUILDERS_HPP_ */
rtabmap_util/test/node_test_utils.hpp
+320
View File
@@ -0,0 +1,320 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef RTABMAP_UTIL_NODE_TEST_UTILS_HPP_
#define RTABMAP_UTIL_NODE_TEST_UTILS_HPP_
#include <gtest/gtest.h>
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2/LinearMath/Quaternion.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_msgs/msg/tf_message.hpp>
#include <atomic>
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <thread>
#include <vector>
namespace rtabmap_util_test {
/**
* @brief Brings rclcpp up once for the whole test binary.
*
* Registered as a gtest global environment so it runs before the first test and shuts
* down after the last one, which keeps gtest_main usable.
*/
class RclcppEnvironment : public ::testing::Environment
{
public:
void SetUp() override
{
if(!rclcpp::ok())
{
rclcpp::init(0, nullptr);
}
}
void TearDown() override
{
if(rclcpp::ok())
{
rclcpp::shutdown();
}
}
};
/// Registers RclcppEnvironment. Call once at file scope in each test binary.
inline ::testing::Environment * registerRclcppEnvironment()
{
static ::testing::Environment * const env =
::testing::AddGlobalTestEnvironment(new RclcppEnvironment);
return env;
}
/**
* @brief Base fixture for driving a node under test over real ROS topics.
*
* The node under test and a helper node share one single-threaded executor, so
* publishing, the node's callback and the assertion all happen on the same thread and
* the tests stay deterministic. No launch files and no separate processes are involved:
* everything runs in the gtest binary.
*/
class NodeTest : public ::testing::Test
{
protected:
void SetUp() override
{
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
helper_ = std::make_shared<rclcpp::Node>("rtabmap_util_test_helper");
executor_->add_node(helper_);
}
void TearDown() override
{
for(const rclcpp::Node::SharedPtr & node : nodes_)
{
executor_->remove_node(node);
}
nodes_.clear();
executor_->remove_node(helper_);
helper_.reset();
executor_.reset();
}
/// Adds a node under test to the shared executor and keeps it alive for the test.
template <typename NodeT>
std::shared_ptr<NodeT> addNode(const std::shared_ptr<NodeT> & node)
{
executor_->add_node(node);
nodes_.push_back(node);
return node;
}
/// The helper node, used to publish inputs and subscribe to outputs.
rclcpp::Node::SharedPtr helper() { return helper_; }
/**
* @brief Spins until @p done returns true, or the timeout elapses.
* @return true if @p done became true
*/
bool spinUntil(
const std::function<bool()> & done,
std::chrono::milliseconds timeout = std::chrono::milliseconds(5000))
{
const std::chrono::steady_clock::time_point deadline =
std::chrono::steady_clock::now() + timeout;
while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline)
{
if(done())
{
return true;
}
executor_->spin_once(std::chrono::milliseconds(10));
}
return done();
}
/**
* @brief Runs every node of the fixture on a multi-threaded executor until @p done.
*
* A node whose callback waits on another of its own callbacks -- a service call made
* from a timer, say -- makes no progress under spinUntil(), because the second
* callback cannot run while the first is still on the stack. Such nodes put the two
* callbacks in different callback groups precisely so a multi-threaded executor can
* overlap them; this hands them the threads to do it, then puts the nodes back on the
* usual single-threaded executor.
*
* @warning Callbacks run on executor threads for the duration, so do not have any
* Collector subscribed while this runs: the test thread would read its
* messages while another thread appends to them. Use it to get a node
* through its start-up handshake, before subscribing to anything.
*/
bool spinMultiThreadedUntil(
const std::function<bool()> & done,
std::chrono::milliseconds timeout = std::chrono::milliseconds(15000))
{
rclcpp::executors::MultiThreadedExecutor booting(rclcpp::ExecutorOptions(), 4);
for(const rclcpp::Node::SharedPtr & node : nodes_)
{
executor_->remove_node(node);
booting.add_node(node);
}
executor_->remove_node(helper_);
booting.add_node(helper_);
std::thread spinner([&booting]() { booting.spin(); });
const std::chrono::steady_clock::time_point deadline =
std::chrono::steady_clock::now() + timeout;
while(rclcpp::ok() && !done() && std::chrono::steady_clock::now() < deadline)
{
std::this_thread::sleep_for(std::chrono::milliseconds(5));
}
const bool result = done();
booting.cancel();
spinner.join();
booting.remove_node(helper_);
executor_->add_node(helper_);
for(const rclcpp::Node::SharedPtr & node : nodes_)
{
booting.remove_node(node);
executor_->add_node(node);
}
return result;
}
/// Spins for a fixed duration, for the "nothing should happen" assertions.
void spinFor(std::chrono::milliseconds duration)
{
const std::chrono::steady_clock::time_point deadline =
std::chrono::steady_clock::now() + duration;
while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline)
{
executor_->spin_once(std::chrono::milliseconds(10));
}
}
/**
* @brief Waits until @p publisher has at least @p count matched subscriptions.
*
* Publishing before the node under test has discovered the topic silently drops the
* message, which is the most common cause of a flaky in-process node test.
*/
template <typename PublisherT>
bool waitForSubscriber(const PublisherT & publisher, size_t count = 1)
{
return spinUntil([&]() { return publisher->get_subscription_count() >= count; });
}
/**
* @brief Waits until @p subscription sees at least one publisher.
*
* Several nodes only publish when they have subscribers, so the test's subscription
* has to be discovered before the input is sent.
*/
template <typename SubscriptionT>
bool waitForPublisher(const SubscriptionT & subscription, size_t count = 1)
{
return spinUntil([&]() { return subscription->get_publisher_count() >= count; });
}
/**
* @brief Publishes a static transform on /tf_static.
*
* /tf_static is transient-local, so a listener that subscribes later still receives
* it. That makes static frames far less timing-sensitive in tests than /tf.
*/
void publishStaticTf(
const std::string & parent, const std::string & child,
double x = 0.0, double y = 0.0, double z = 0.0)
{
if(!staticTfPublisher_)
{
staticTfPublisher_ = helper_->create_publisher<tf2_msgs::msg::TFMessage>(
"/tf_static", rclcpp::QoS(100).transient_local());
}
geometry_msgs::msg::TransformStamped t;
t.header.stamp = helper_->now();
t.header.frame_id = parent;
t.child_frame_id = child;
t.transform.translation.x = x;
t.transform.translation.y = y;
t.transform.translation.z = z;
t.transform.rotation.w = 1.0;
tf2_msgs::msg::TFMessage msg;
msg.transforms.push_back(t);
staticTfPublisher_->publish(msg);
spinFor(std::chrono::milliseconds(100));
}
/// Publishes a static transform with a rotation, given as roll/pitch/yaw.
void publishStaticTfRPY(
const std::string & parent, const std::string & child,
double roll, double pitch, double yaw,
double x = 0.0, double y = 0.0, double z = 0.0)
{
if(!staticTfPublisher_)
{
staticTfPublisher_ = helper_->create_publisher<tf2_msgs::msg::TFMessage>(
"/tf_static", rclcpp::QoS(100).transient_local());
}
tf2::Quaternion q;
q.setRPY(roll, pitch, yaw);
geometry_msgs::msg::TransformStamped t;
t.header.stamp = helper_->now();
t.header.frame_id = parent;
t.child_frame_id = child;
t.transform.translation.x = x;
t.transform.translation.y = y;
t.transform.translation.z = z;
t.transform.rotation = tf2::toMsg(q);
tf2_msgs::msg::TFMessage msg;
msg.transforms.push_back(t);
staticTfPublisher_->publish(msg);
spinFor(std::chrono::milliseconds(100));
}
/// Collects every message received on @p topic, for later assertions.
template <typename MsgT>
struct Collector
{
typename rclcpp::Subscription<MsgT>::SharedPtr subscription;
std::vector<typename MsgT::ConstSharedPtr> messages;
size_t size() const { return messages.size(); }
bool empty() const { return messages.empty(); }
const MsgT & back() const { return *messages.back(); }
const MsgT & front() const { return *messages.front(); }
};
/// Subscribes the helper node to @p topic and records everything it receives.
template <typename MsgT>
std::shared_ptr<Collector<MsgT>> collect(
const std::string & topic, const rclcpp::QoS & qos = rclcpp::QoS(10))
{
std::shared_ptr<Collector<MsgT>> collector = std::make_shared<Collector<MsgT>>();
collector->subscription = helper_->create_subscription<MsgT>(
topic, qos,
[collector](const typename MsgT::ConstSharedPtr msg) {
collector->messages.push_back(msg);
});
return collector;
}
private:
rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
rclcpp::Node::SharedPtr helper_;
std::vector<rclcpp::Node::SharedPtr> nodes_;
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr staticTfPublisher_;
};
} // namespace rtabmap_util_test
#endif /* RTABMAP_UTIL_NODE_TEST_UTILS_HPP_ */
rtabmap_util/test/test_db_player.cpp
+809
View File
@@ -0,0 +1,809 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#include "node_test_utils.hpp"
#include "db_builders.hpp"
#include <rtabmap_util/db_player.hpp>
#include <rtabmap_conversions/MsgConversion.h>
#include <rosgraph_msgs/msg/clock.hpp>
#include <tf2_msgs/msg/tf_message.hpp>
#include <cmath>
using namespace rtabmap_util_test;
namespace {
::testing::Environment * const kEnv = registerRclcppEnvironment();
/// Replay at 1000x the recorded stamps: db_player sleeps between frames otherwise.
constexpr double kReplayRate = 1000.0;
bool hasParameter(const std::vector<rclcpp::Parameter> & overrides, const std::string & name)
{
for(size_t i=0; i<overrides.size(); ++i)
{
if(overrides[i].get_name() == name) { return true; }
}
return false;
}
} // namespace
/**
* db_player is driven by its own loop in DbPlayerNode, so the tests call
* publishNextFrame() directly instead of waiting on a timer.
*
* Two things shape every test below. The publishers do not exist until the first frame
* has been read -- db_player decides which topics to create from the payloads it finds in
* the database -- and everything except /tf is only published when someone is subscribed.
* So the sequence is always: replay one frame to create the publishers, subscribe, then
* replay again to get the data.
*/
class DbPlayerTest : public NodeTest
{
protected:
void start(const std::string & databasePath, std::vector<rclcpp::Parameter> overrides = {})
{
if(!hasParameter(overrides, "database"))
{
overrides.push_back(rclcpp::Parameter("database", databasePath));
}
if(!hasParameter(overrides, "rate"))
{
overrides.push_back(rclcpp::Parameter("rate", kReplayRate));
}
player_ = addNode(std::make_shared<rtabmap_util::DbPlayer>(
rclcpp::NodeOptions().parameter_overrides(overrides)));
}
/// Reads one frame, which is what creates the publishers.
void primePublishers()
{
ASSERT_TRUE(player_->publishNextFrame()) << "the database has no readable frame";
}
/// Replays frames until @p done, or the database runs out.
bool replayUntil(const std::function<bool()> & done)
{
for(int i=0; i<kDbFrames && !done(); ++i)
{
if(!player_->publishNextFrame()) { break; }
spinFor(std::chrono::milliseconds(30));
}
return done();
}
/**
* @brief The database node a replayed message came from, recovered from its stamp.
*
* How many frames a test ends up replaying depends on discovery, so the expected
* pose is derived from the stamp the message itself carries rather than assumed.
* That also checks the stamp really comes from the database.
*/
static int nodeIdOf(const builtin_interfaces::msg::Time & stamp)
{
const double seconds = rtabmap_conversions::timestampFromROS(stamp);
return int(std::round((seconds - kFirstStamp) / kStampStep)) + 1;
}
/// The most recent transform published for @p parent -> @p child.
static bool findTransform(
const Collector<tf2_msgs::msg::TFMessage> & tf,
const std::string & parent, const std::string & child,
geometry_msgs::msg::TransformStamped & out)
{
bool found = false;
for(size_t i=0; i<tf.messages.size(); ++i)
{
for(size_t j=0; j<tf.messages[i]->transforms.size(); ++j)
{
const geometry_msgs::msg::TransformStamped & t = tf.messages[i]->transforms[j];
if(t.header.frame_id == parent && t.child_frame_id == child)
{
out = t;
found = true;
}
}
}
return found;
}
static rtabmap::Transform toRtabmap(const geometry_msgs::msg::TransformStamped & t)
{
return rtabmap_conversions::transformFromGeometryMsg(t.transform);
}
/// Subscribes to /tf and waits for db_player's broadcaster to be discovered.
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> collectTf()
{
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
EXPECT_TRUE(waitForPublisher(tf->subscription));
return tf;
}
std::shared_ptr<rtabmap_util::DbPlayer> player_;
};
//============================================================================
// RGB-D
//============================================================================
TEST_F(DbPlayerTest, ReplaysRgbAndDepthImages)
{
TempDatabase db("rgbd");
writeRgbdDatabase(db.path());
start(db.path());
primePublishers();
std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
collect<sensor_msgs::msg::Image>("rgb/image");
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
collect<sensor_msgs::msg::Image>("depth/image");
ASSERT_TRUE(waitForPublisher(rgb->subscription));
ASSERT_TRUE(waitForPublisher(depth->subscription));
ASSERT_TRUE(replayUntil([&]() { return !rgb->empty() && !depth->empty(); }))
<< "no image replayed";
EXPECT_EQ(rgb->back().encoding, sensor_msgs::image_encodings::BGR8);
EXPECT_EQ(rgb->back().width, uint32_t(kImageWidth));
EXPECT_EQ(rgb->back().height, uint32_t(kImageHeight));
EXPECT_EQ(rgb->back().header.frame_id, "camera_optical_link");
EXPECT_EQ(depth->back().encoding, sensor_msgs::image_encodings::TYPE_16UC1);
EXPECT_EQ(depth->back().header.frame_id, "camera_optical_link")
<< "depth is registered with the color camera, so it shares its frame";
EXPECT_EQ(*reinterpret_cast<const uint16_t *>(depth->back().data.data()), kDepthMillimeters);
}
TEST_F(DbPlayerTest, StampsImagesWithTheDatabaseStamps)
{
TempDatabase db("stamps");
writeRgbdDatabase(db.path());
start(db.path());
primePublishers();
std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
collect<sensor_msgs::msg::Image>("rgb/image");
ASSERT_TRUE(waitForPublisher(rgb->subscription));
ASSERT_TRUE(replayUntil([&]() { return !rgb->empty(); }));
const int id = nodeIdOf(rgb->front().header.stamp);
EXPECT_GE(id, 2) << "the first frame only creates the publishers";
EXPECT_LE(id, kDbFrames);
EXPECT_NEAR(rtabmap_conversions::timestampFromROS(rgb->front().header.stamp),
stampOfNode(id), 1e-6);
}
TEST_F(DbPlayerTest, ReplaysCameraCalibration)
{
TempDatabase db("caminfo");
writeRgbdDatabase(db.path());
start(db.path());
primePublishers();
std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
collect<sensor_msgs::msg::Image>("rgb/image");
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> rgbInfo =
collect<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> depthInfo =
collect<sensor_msgs::msg::CameraInfo>("depth/camera_info");
ASSERT_TRUE(waitForPublisher(rgb->subscription));
ASSERT_TRUE(waitForPublisher(rgbInfo->subscription));
ASSERT_TRUE(replayUntil([&]() { return !rgbInfo->empty() && !depthInfo->empty(); }));
EXPECT_EQ(rgbInfo->back().width, uint32_t(kImageWidth));
EXPECT_EQ(rgbInfo->back().height, uint32_t(kImageHeight));
EXPECT_NEAR(rgbInfo->back().k[0], kFx, 1e-6);
EXPECT_NEAR(rgbInfo->back().k[2], kCx, 1e-6);
EXPECT_NEAR(rgbInfo->back().k[4], kFy, 1e-6);
EXPECT_NEAR(rgbInfo->back().k[5], kCy, 1e-6);
EXPECT_EQ(rgbInfo->back().header.frame_id, "camera_optical_link");
EXPECT_NEAR(depthInfo->back().k[0], kFx, 1e-6)
<< "the depth camera info repeats the color calibration";
}
TEST_F(DbPlayerTest, ReplaysImageWithoutCalibrationOnImageTopic)
{
// A database with no calibration at all is still replayable, on "image".
TempDatabase db("imageonly");
writeImageOnlyDatabase(db.path());
start(db.path());
primePublishers();
std::shared_ptr<Collector<sensor_msgs::msg::Image>> image =
collect<sensor_msgs::msg::Image>("image");
ASSERT_TRUE(waitForPublisher(image->subscription));
ASSERT_TRUE(replayUntil([&]() { return !image->empty(); }));
EXPECT_EQ(image->back().encoding, sensor_msgs::image_encodings::BGR8);
EXPECT_EQ(image->back().width, uint32_t(kImageWidth));
}
//============================================================================
// Stereo
//============================================================================
TEST_F(DbPlayerTest, ReplaysStereoPairAndCalibration)
{
TempDatabase db("stereo");
writeStereoDatabase(db.path());
start(db.path());
primePublishers();
std::shared_ptr<Collector<sensor_msgs::msg::Image>> left =
collect<sensor_msgs::msg::Image>("left/image");
std::shared_ptr<Collector<sensor_msgs::msg::Image>> right =
collect<sensor_msgs::msg::Image>("right/image");
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> leftInfo =
collect<sensor_msgs::msg::CameraInfo>("left/camera_info");
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> rightInfo =
collect<sensor_msgs::msg::CameraInfo>("right/camera_info");
ASSERT_TRUE(waitForPublisher(left->subscription));
ASSERT_TRUE(waitForPublisher(right->subscription));
ASSERT_TRUE(replayUntil([&]() {
return !left->empty() && !right->empty() &&
!leftInfo->empty() && !rightInfo->empty(); }));
EXPECT_EQ(left->back().encoding, sensor_msgs::image_encodings::MONO8);
EXPECT_EQ(left->back().header.frame_id, "left_camera_optical_link");
EXPECT_EQ(right->back().encoding, sensor_msgs::image_encodings::MONO8);
EXPECT_EQ(right->back().header.frame_id, "right_camera_optical_link");
// Both cameras share the intrinsics of a rectified pair and are stamped with the
// frame of the image they belong to.
EXPECT_EQ(leftInfo->back().width, uint32_t(kImageWidth));
EXPECT_EQ(leftInfo->back().height, uint32_t(kImageHeight));
EXPECT_NEAR(leftInfo->back().k[0], kFx, 1e-6);
EXPECT_NEAR(leftInfo->back().k[2], kCx, 1e-6);
EXPECT_NEAR(leftInfo->back().k[4], kFy, 1e-6);
EXPECT_NEAR(leftInfo->back().k[5], kCy, 1e-6);
EXPECT_EQ(leftInfo->back().header.frame_id, "left_camera_optical_link");
EXPECT_NEAR(rightInfo->back().k[0], kFx, 1e-6);
EXPECT_EQ(rightInfo->back().header.frame_id, "right_camera_optical_link");
// Only the right camera carries the baseline: it is P(0,3) = -fx*baseline, and the
// left camera of a rectified pair sits at the origin of the stereo frame.
EXPECT_NEAR(leftInfo->back().p[3], 0.0, 1e-6);
EXPECT_NEAR(rightInfo->back().p[3], -kFx*kBaseline, 1e-6);
}
//============================================================================
// Laser scans
//============================================================================
TEST_F(DbPlayerTest, Replays2dLaserScan)
{
TempDatabase db("scan2d");
writeScan2dDatabase(db.path());
start(db.path());
primePublishers();
std::shared_ptr<Collector<sensor_msgs::msg::LaserScan>> scan =
collect<sensor_msgs::msg::LaserScan>("scan");
ASSERT_TRUE(waitForPublisher(scan->subscription));
ASSERT_TRUE(replayUntil([&]() { return !scan->empty(); })) << "no scan replayed";
const sensor_msgs::msg::LaserScan & msg = scan->back();
EXPECT_EQ(msg.header.frame_id, "base_laser_link");
// The scan carries its own angles, so the scan_angle_* parameters are not used.
EXPECT_NEAR(msg.angle_min, kScanAngleMin, 1e-6);
EXPECT_NEAR(msg.angle_max, kScanAngleMax, 1e-6);
EXPECT_NEAR(msg.angle_increment, kScanAngleIncrement, 1e-6);
EXPECT_NEAR(msg.range_min, kScanRangeMin, 1e-6);
EXPECT_NEAR(msg.range_max, kScanRangeMax, 1e-6);
// db_player re-bins the cartesian points, so every bin must come back at its range.
ASSERT_EQ(msg.ranges.size(), size_t(kScanBins));
for(int bin=0; bin<kScanBins; ++bin)
{
EXPECT_NEAR(msg.ranges[bin], scanRangeOf(bin), 1e-3) << "bin " << bin;
}
}
TEST_F(DbPlayerTest, A2dScanNeverAdvertisesScanCloud)
{
// initializePublishers() runs on every frame, so the publisher it creates has to
// match the scan being replayed. It used to create whichever one did not exist yet,
// which advertised an empty "scan_cloud" from the second frame of a 2D database on.
TempDatabase db("scan2donly");
writeScan2dDatabase(db.path());
start(db.path());
std::shared_ptr<Collector<sensor_msgs::msg::LaserScan>> scan =
collect<sensor_msgs::msg::LaserScan>("scan");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
collect<sensor_msgs::msg::PointCloud2>("scan_cloud");
while(player_->publishNextFrame()) { spinFor(std::chrono::milliseconds(30)); }
EXPECT_FALSE(scan->empty()) << "the 2D scan must still be replayed";
EXPECT_EQ(cloud->subscription->get_publisher_count(), 0u)
<< "a 2D database must not advertise scan_cloud";
EXPECT_TRUE(cloud->empty());
}
TEST_F(DbPlayerTest, A3dScanNeverAdvertisesScan)
{
TempDatabase db("scan3donly");
writeScan3dDatabase(db.path());
start(db.path());
std::shared_ptr<Collector<sensor_msgs::msg::LaserScan>> scan =
collect<sensor_msgs::msg::LaserScan>("scan");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
collect<sensor_msgs::msg::PointCloud2>("scan_cloud");
while(player_->publishNextFrame()) { spinFor(std::chrono::milliseconds(30)); }
EXPECT_FALSE(cloud->empty()) << "the 3D scan must still be replayed";
EXPECT_EQ(scan->subscription->get_publisher_count(), 0u)
<< "a 3D database must not advertise scan";
}
TEST_F(DbPlayerTest, UsesScanParametersWhenTheScanHasNoAngles)
{
// A scan saved without angle metadata falls back to the scan_angle_*/scan_range_*
// parameters, which is how a database recorded from a 3D lidar can be replayed as 2D.
const double angleMin = -0.5;
const double angleIncrement = 0.05;
const int targetBin = 10;
// The center of the target bin: db_player truncates (angle-angle_min)/increment, so a
// bearing on a bin boundary would land on either side depending on the rounding.
const float bearing = float(angleMin + (double(targetBin) + 0.5) * angleIncrement);
const float nearest = 1.0f;
TempDatabase db("scan2dnoangles");
writeDatabase(db.path(), kDbFrames, [bearing, nearest](int id, double stamp) {
cv::Mat points(1, kScanBins, CV_32FC2);
for(int bin=0; bin<kScanBins; ++bin)
{
// All at the same bearing, at increasing ranges: db_player keeps the nearest.
const float range = nearest + 0.1f * float(bin);
points.at<cv::Vec2f>(0, bin) =
cv::Vec2f(range * std::cos(bearing), range * std::sin(bearing));
}
rtabmap::SensorData data;
data.setId(id);
data.setStamp(stamp);
data.setLaserScan(rtabmap::LaserScan(rtabmap::compressData2(points),
/*maxPoints=*/0, /*maxRange=*/0.0f, rtabmap::LaserScan::kXY,
scanLocalTransform()));
return data;
});
start(db.path(), {rclcpp::Parameter("scan_angle_min", angleMin),
rclcpp::Parameter("scan_angle_max", 0.5),
rclcpp::Parameter("scan_angle_increment", angleIncrement),
rclcpp::Parameter("scan_range_min", 0.2),
rclcpp::Parameter("scan_range_max", 20.0)});
primePublishers();
std::shared_ptr<Collector<sensor_msgs::msg::LaserScan>> scan =
collect<sensor_msgs::msg::LaserScan>("scan");
ASSERT_TRUE(waitForPublisher(scan->subscription));
ASSERT_TRUE(replayUntil([&]() { return !scan->empty(); }));
const sensor_msgs::msg::LaserScan & msg = scan->back();
EXPECT_NEAR(msg.angle_min, angleMin, 1e-6);
EXPECT_NEAR(msg.angle_max, 0.5, 1e-6);
EXPECT_NEAR(msg.angle_increment, angleIncrement, 1e-6);
EXPECT_NEAR(msg.range_min, 0.2, 1e-6);
EXPECT_NEAR(msg.range_max, 20.0, 1e-6);
ASSERT_EQ(msg.ranges.size(), 20u) << "ceil((0.5 - -0.5)/0.05)";
EXPECT_NEAR(msg.ranges[targetBin], nearest, 1e-3)
<< "every point shares a bearing, so only its bin is filled, at the nearest range";
for(size_t bin=0; bin<msg.ranges.size(); ++bin)
{
if(int(bin) != targetBin)
{
EXPECT_FLOAT_EQ(msg.ranges[bin], 0.0f) << "bin " << bin << " should be empty";
}
}
}
TEST_F(DbPlayerTest, Replays3dScanAsPointCloud)
{
TempDatabase db("scan3d");
writeScan3dDatabase(db.path());
start(db.path());
primePublishers();
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
collect<sensor_msgs::msg::PointCloud2>("scan_cloud");
ASSERT_TRUE(waitForPublisher(cloud->subscription));
ASSERT_TRUE(replayUntil([&]() { return !cloud->empty(); })) << "no cloud replayed";
EXPECT_EQ(cloud->back().header.frame_id, "base_laser_link");
EXPECT_EQ(cloud->back().width * cloud->back().height, uint32_t(kScanCloudPoints));
}
//============================================================================
// Odometry
//============================================================================
TEST_F(DbPlayerTest, ReplaysOdometryWithItsCovariance)
{
TempDatabase db("odom");
writeRgbdDatabase(db.path());
start(db.path());
primePublishers();
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
collect<nav_msgs::msg::Odometry>("odom");
ASSERT_TRUE(waitForPublisher(odom->subscription));
ASSERT_TRUE(replayUntil([&]() { return !odom->empty(); })) << "no odometry replayed";
const nav_msgs::msg::Odometry & msg = odom->back();
EXPECT_EQ(msg.header.frame_id, "odom");
EXPECT_EQ(msg.child_frame_id, "base_link");
const int id = nodeIdOf(msg.header.stamp);
ASSERT_GE(id, 1);
ASSERT_LE(id, kDbFrames);
EXPECT_NEAR(msg.pose.pose.position.x, poseOf(id).x(), 1e-5)
<< "the pose must be the one recorded for node " << id;
EXPECT_NEAR(msg.pose.pose.position.y, 0.0, 1e-5);
// The covariance is the inverse of the neighbor link's information matrix.
EXPECT_NEAR(msg.pose.covariance[0], kOdomVariance, 1e-6);
EXPECT_NEAR(msg.pose.covariance[35], kOdomVariance, 1e-6);
}
TEST_F(DbPlayerTest, IgnoreOdomDropsTheOdometry)
{
TempDatabase db("ignoreodom");
writeRgbdDatabase(db.path());
start(db.path(), {rclcpp::Parameter("ignore_odom", true)});
primePublishers();
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
collect<nav_msgs::msg::Odometry>("odom");
std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
collect<sensor_msgs::msg::Image>("rgb/image");
ASSERT_TRUE(waitForPublisher(rgb->subscription));
ASSERT_TRUE(replayUntil([&]() { return !rgb->empty(); }))
<< "the images must still be replayed";
EXPECT_EQ(odom->subscription->get_publisher_count(), 0u)
<< "with no odometry in the stream the topic is never even created";
EXPECT_TRUE(odom->empty());
}
//============================================================================
// Transforms
//============================================================================
TEST_F(DbPlayerTest, BroadcastsOdometryAndCameraTransforms)
{
TempDatabase db("tf");
writeRgbdDatabase(db.path());
start(db.path());
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf = collectTf();
// TF is not gated on subscribers, so the very first frame already broadcasts.
ASSERT_TRUE(replayUntil([&]() { return !tf->empty(); })) << "nothing broadcast on /tf";
geometry_msgs::msg::TransformStamped odomToBase;
ASSERT_TRUE(findTransform(*tf, "odom", "base_link", odomToBase));
const int id = nodeIdOf(odomToBase.header.stamp);
ASSERT_GE(id, 1);
ASSERT_LE(id, kDbFrames);
EXPECT_NEAR(odomToBase.transform.translation.x, poseOf(id).x(), 1e-5);
geometry_msgs::msg::TransformStamped baseToCamera;
ASSERT_TRUE(findTransform(*tf, "base_link", "camera_optical_link", baseToCamera));
EXPECT_LT(toRtabmap(baseToCamera).getDistance(cameraLocalTransform()), 1e-4f)
<< "the camera transform is the model's local transform: "
<< toRtabmap(baseToCamera).prettyPrint();
}
TEST_F(DbPlayerTest, BroadcastsStereoTransformsShiftedByTheBaseline)
{
TempDatabase db("stereotf");
writeStereoDatabase(db.path());
start(db.path());
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf = collectTf();
ASSERT_TRUE(replayUntil([&]() { return !tf->empty(); }));
geometry_msgs::msg::TransformStamped baseToLeft, baseToRight;
ASSERT_TRUE(findTransform(*tf, "base_link", "left_camera_optical_link", baseToLeft));
ASSERT_TRUE(findTransform(*tf, "base_link", "right_camera_optical_link", baseToRight));
EXPECT_LT(toRtabmap(baseToLeft).getDistance(cameraLocalTransform()), 1e-4f);
// The right camera carries the baseline in Tx, which db_player turns back into a
// translation along the optical x axis so the frame sits next to the left one.
const rtabmap::Transform expectedRight =
cameraLocalTransform() * rtabmap::Transform(kBaseline, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
EXPECT_LT(toRtabmap(baseToRight).getDistance(expectedRight), 1e-4f)
<< toRtabmap(baseToRight).prettyPrint();
}
TEST_F(DbPlayerTest, BroadcastsTheLaserTransform)
{
TempDatabase db("scantf");
writeScan3dDatabase(db.path());
start(db.path());
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf = collectTf();
ASSERT_TRUE(replayUntil([&]() { return !tf->empty(); }));
geometry_msgs::msg::TransformStamped baseToLaser;
ASSERT_TRUE(findTransform(*tf, "base_link", "base_laser_link", baseToLaser));
EXPECT_LT(toRtabmap(baseToLaser).getDistance(scanLocalTransform()), 1e-4f)
<< toRtabmap(baseToLaser).prettyPrint();
}
TEST_F(DbPlayerTest, BroadcastsGroundTruthAndImuTransforms)
{
TempDatabase db("richtf");
writeRichDatabase(db.path());
start(db.path());
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf = collectTf();
ASSERT_TRUE(replayUntil([&]() { return !tf->empty(); }));
geometry_msgs::msg::TransformStamped worldToGt;
ASSERT_TRUE(findTransform(*tf, "world", "base_link_gt", worldToGt));
const int id = nodeIdOf(worldToGt.header.stamp);
ASSERT_GE(id, 1);
ASSERT_LE(id, kDbFrames);
EXPECT_LT(toRtabmap(worldToGt).getDistance(groundTruthOf(id)), 1e-4f)
<< "the ground truth is published apart from the odometry";
geometry_msgs::msg::TransformStamped baseToImu;
ASSERT_TRUE(findTransform(*tf, "base_link", "imu_link", baseToImu));
EXPECT_TRUE(toRtabmap(baseToImu).isIdentity())
<< "a gravity link is already expressed in the base frame";
}
TEST_F(DbPlayerTest, RenamesFramesFromParameters)
{
TempDatabase db("frames");
writeRgbdDatabase(db.path());
start(db.path(), {rclcpp::Parameter("frame_id", std::string("robot")),
rclcpp::Parameter("odom_frame_id", std::string("world_odom")),
rclcpp::Parameter("camera_frame_id", std::string("optical"))});
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf = collectTf();
ASSERT_TRUE(replayUntil([&]() { return !tf->empty(); }));
geometry_msgs::msg::TransformStamped t;
EXPECT_TRUE(findTransform(*tf, "world_odom", "robot", t));
EXPECT_TRUE(findTransform(*tf, "robot", "optical", t));
EXPECT_FALSE(findTransform(*tf, "odom", "base_link", t)) << "the defaults must be gone";
}
TEST_F(DbPlayerTest, PublishTfFalseBroadcastsNothing)
{
TempDatabase db("notf");
writeRgbdDatabase(db.path());
start(db.path(), {rclcpp::Parameter("publish_tf", false)});
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
ASSERT_TRUE(player_->publishNextFrame());
ASSERT_TRUE(player_->publishNextFrame());
spinFor(std::chrono::milliseconds(300));
EXPECT_TRUE(tf->empty()) << "publish_tf:=false must not create the broadcaster";
}
//============================================================================
// The optional channels
//============================================================================
TEST_F(DbPlayerTest, ReplaysGlobalPose)
{
TempDatabase db("globalpose");
writeRichDatabase(db.path());
start(db.path());
primePublishers();
std::shared_ptr<Collector<geometry_msgs::msg::PoseWithCovarianceStamped>> pose =
collect<geometry_msgs::msg::PoseWithCovarianceStamped>("global_pose");
ASSERT_TRUE(waitForPublisher(pose->subscription));
ASSERT_TRUE(replayUntil([&]() { return !pose->empty(); })) << "no global pose replayed";
const int id = nodeIdOf(pose->back().header.stamp);
ASSERT_GE(id, 1);
ASSERT_LE(id, kDbFrames);
EXPECT_EQ(pose->back().header.frame_id, "base_link");
EXPECT_NEAR(pose->back().pose.pose.position.y, globalPoseOf(id).y(), 1e-5)
<< "the prior pose is offset in y, unlike the odometry";
// The prior was saved with an information matrix of 100*I.
EXPECT_NEAR(pose->back().pose.covariance[0], 0.01, 1e-6);
}
TEST_F(DbPlayerTest, ReplaysGpsFix)
{
TempDatabase db("gps");
writeRichDatabase(db.path());
start(db.path());
primePublishers();
std::shared_ptr<Collector<sensor_msgs::msg::NavSatFix>> gps =
collect<sensor_msgs::msg::NavSatFix>("gps/fix");
ASSERT_TRUE(waitForPublisher(gps->subscription));
ASSERT_TRUE(replayUntil([&]() { return !gps->empty(); })) << "no GPS replayed";
const sensor_msgs::msg::NavSatFix & msg = gps->back();
EXPECT_NEAR(msg.longitude, kGpsLongitude, 1e-9);
EXPECT_NEAR(msg.latitude, kGpsLatitude, 1e-9);
EXPECT_NEAR(msg.altitude, kGpsAltitude, 1e-9);
EXPECT_EQ(msg.position_covariance_type,
uint8_t(sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN));
EXPECT_NEAR(msg.position_covariance[0], kGpsError*kGpsError, 1e-9)
<< "the reported error is squared into a variance";
EXPECT_NEAR(msg.position_covariance[4], kGpsError*kGpsError, 1e-9);
EXPECT_NEAR(msg.position_covariance[8], kGpsError*kGpsError, 1e-9);
}
TEST_F(DbPlayerTest, ReplaysImu)
{
TempDatabase db("imu");
writeRichDatabase(db.path());
start(db.path());
primePublishers();
std::shared_ptr<Collector<sensor_msgs::msg::Imu>> imu =
collect<sensor_msgs::msg::Imu>("imu");
ASSERT_TRUE(waitForPublisher(imu->subscription));
ASSERT_TRUE(replayUntil([&]() { return !imu->empty(); })) << "no IMU replayed";
EXPECT_EQ(imu->back().header.frame_id, "imu_link");
// DBReader rebuilds the IMU from the gravity link, so only the orientation survives.
const Eigen::Quaterniond expected = gravityTransform().getQuaterniond();
EXPECT_NEAR(std::abs(imu->back().orientation.w), std::abs(expected.w()), 1e-5);
EXPECT_NEAR(std::abs(imu->back().orientation.x), std::abs(expected.x()), 1e-5);
EXPECT_NEAR(std::abs(imu->back().orientation.y), std::abs(expected.y()), 1e-5);
EXPECT_NEAR(std::abs(imu->back().orientation.z), std::abs(expected.z()), 1e-5);
}
TEST_F(DbPlayerTest, ReplaysEnvSensor)
{
TempDatabase db("envsensor");
writeRichDatabase(db.path());
start(db.path());
primePublishers();
std::shared_ptr<Collector<rtabmap_msgs::msg::EnvSensor>> env =
collect<rtabmap_msgs::msg::EnvSensor>("env_sensor");
ASSERT_TRUE(waitForPublisher(env->subscription));
ASSERT_TRUE(replayUntil([&]() { return !env->empty(); })) << "no env sensor replayed";
EXPECT_EQ(env->back().type, int(rtabmap::EnvSensor::kAmbientTemperature));
EXPECT_NEAR(env->back().value, kEnvSensorValue, 1e-9);
EXPECT_EQ(env->back().header.frame_id, "base_link");
}
TEST_F(DbPlayerTest, PublishesClockWhenAsked)
{
TempDatabase db("clock");
writeRgbdDatabase(db.path());
start(db.path(), {rclcpp::Parameter("publish_clock", true)});
std::shared_ptr<Collector<rosgraph_msgs::msg::Clock>> clock =
collect<rosgraph_msgs::msg::Clock>("/clock");
ASSERT_TRUE(waitForPublisher(clock->subscription));
// The clock is not gated on subscribers either.
ASSERT_TRUE(replayUntil([&]() { return !clock->empty(); })) << "no clock published";
const int id = nodeIdOf(clock->back().clock);
ASSERT_GE(id, 1);
ASSERT_LE(id, kDbFrames);
EXPECT_NEAR(rtabmap_conversions::timestampFromROS(clock->back().clock),
stampOfNode(id), 1e-6) << "the clock follows the database stamps";
}
TEST_F(DbPlayerTest, NoClockByDefault)
{
TempDatabase db("noclock");
writeRgbdDatabase(db.path());
start(db.path());
std::shared_ptr<Collector<rosgraph_msgs::msg::Clock>> clock =
collect<rosgraph_msgs::msg::Clock>("/clock");
ASSERT_TRUE(player_->publishNextFrame());
ASSERT_TRUE(player_->publishNextFrame());
spinFor(std::chrono::milliseconds(300));
EXPECT_TRUE(clock->empty());
}
//============================================================================
// Reading the database
//============================================================================
TEST_F(DbPlayerTest, StopsAtTheEndOfTheDatabase)
{
TempDatabase db("end");
writeRgbdDatabase(db.path(), 4);
start(db.path());
int frames = 0;
while(player_->publishNextFrame())
{
++frames;
ASSERT_LE(frames, 10) << "publishNextFrame() never reported the end";
}
EXPECT_EQ(frames, 4) << "every node must be replayed exactly once";
}
TEST_F(DbPlayerTest, StartIdSkipsTheEarlierNodes)
{
TempDatabase db("startid");
writeRgbdDatabase(db.path(), 4);
start(db.path(), {rclcpp::Parameter("start_id", 3)});
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf = collectTf();
int frames = 0;
while(player_->publishNextFrame()) { ++frames; }
spinFor(std::chrono::milliseconds(200));
EXPECT_EQ(frames, 2) << "nodes 3 and 4 only";
geometry_msgs::msg::TransformStamped t;
ASSERT_TRUE(findTransform(*tf, "odom", "base_link", t));
EXPECT_EQ(nodeIdOf(tf->front().transforms[0].header.stamp), 3)
<< "the replay must start at node 3";
}
//============================================================================
// Pause / resume
//============================================================================
TEST_F(DbPlayerTest, StartsRunning)
{
TempDatabase db("pause");
writeRgbdDatabase(db.path());
start(db.path());
EXPECT_FALSE(player_->isPaused());
}
TEST_F(DbPlayerTest, PauseAndResumeServicesTogglePlayback)
{
TempDatabase db("pausesrv");
writeRgbdDatabase(db.path());
start(db.path());
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr pause =
helper()->create_client<std_srvs::srv::Empty>("db_player/pause");
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr resume =
helper()->create_client<std_srvs::srv::Empty>("db_player/resume");
ASSERT_TRUE(spinUntil([&]() { return pause->service_is_ready() && resume->service_is_ready(); }))
<< "the pause/resume services were never advertised";
pause->async_send_request(std::make_shared<std_srvs::srv::Empty::Request>());
ASSERT_TRUE(spinUntil([&]() { return player_->isPaused(); })) << "pause had no effect";
resume->async_send_request(std::make_shared<std_srvs::srv::Empty::Request>());
ASSERT_TRUE(spinUntil([&]() { return !player_->isPaused(); })) << "resume had no effect";
}
//============================================================================
// Opening the database
//============================================================================
TEST_F(DbPlayerTest, ThrowsWithoutADatabaseParameter)
{
// The node used to exit(-1) here, which took down every other node sharing its
// component container. Throwing lets the caller decide.
EXPECT_THROW(
std::make_shared<rtabmap_util::DbPlayer>(rclcpp::NodeOptions()),
std::invalid_argument);
}
TEST_F(DbPlayerTest, ThrowsWhenTheDatabaseCannotBeOpened)
{
TempDatabase db("missing"); // the path is never written
EXPECT_THROW(
std::make_shared<rtabmap_util::DbPlayer>(rclcpp::NodeOptions().parameter_overrides(
{rclcpp::Parameter("database", db.path())})),
std::runtime_error);
}
rtabmap_util/test/test_disparity_to_depth.cpp
+248
View File
@@ -0,0 +1,248 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#include "node_test_utils.hpp"
#include <rtabmap_util/disparity_to_depth.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <stereo_msgs/msg/disparity_image.hpp>
#include <rtabmap/utilite/UException.h>
using namespace rtabmap_util_test;
namespace {
::testing::Environment * const kEnv = registerRclcppEnvironment();
constexpr float kBaseline = 0.1f; // t, meters
constexpr float kFocal = 500.0f; // f, pixels
constexpr int kWidth = 4;
constexpr int kHeight = 4;
/// A 4x4 32FC1 disparity image, every pixel set to @p disparity.
stereo_msgs::msg::DisparityImage makeDisparity(
float disparity,
const std::string & encoding = sensor_msgs::image_encodings::TYPE_32FC1)
{
stereo_msgs::msg::DisparityImage msg;
msg.header.frame_id = "camera_link";
msg.header.stamp = rclcpp::Time(1000, 0, RCL_ROS_TIME);
msg.t = kBaseline;
msg.f = kFocal;
msg.min_disparity = 1.0f;
msg.max_disparity = 100.0f;
msg.image.header = msg.header;
msg.image.encoding = encoding;
msg.image.height = kHeight;
msg.image.width = kWidth;
msg.image.step = kWidth * sizeof(float);
msg.image.data.resize(msg.image.step * kHeight);
float * p = reinterpret_cast<float *>(msg.image.data.data());
for(int i=0; i<kWidth*kHeight; ++i)
{
p[i] = disparity;
}
return msg;
}
float pixel32f(const sensor_msgs::msg::Image & img, int row, int col)
{
return *reinterpret_cast<const float *>(&img.data[row * img.step + col * sizeof(float)]);
}
uint16_t pixel16u(const sensor_msgs::msg::Image & img, int row, int col)
{
return *reinterpret_cast<const uint16_t *>(&img.data[row * img.step + col * sizeof(uint16_t)]);
}
} // namespace
class DisparityToDepthTest : public NodeTest {};
TEST_F(DisparityToDepthTest, ConvertsDisparityToMetricDepth)
{
addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()));
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
collect<sensor_msgs::msg::Image>("depth");
rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(depth->subscription)) << "the node never advertised depth";
// depth = baseline * focal / disparity = 0.1 * 500 / 10 = 5 m
pub->publish(makeDisparity(10.0f));
ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); }));
const sensor_msgs::msg::Image & img = depth->back();
EXPECT_EQ(img.encoding, sensor_msgs::image_encodings::TYPE_32FC1);
EXPECT_EQ(img.width, uint32_t(kWidth));
EXPECT_EQ(img.height, uint32_t(kHeight));
EXPECT_EQ(img.header.frame_id, "camera_link") << "the input header must be preserved";
EXPECT_NEAR(pixel32f(img, 0, 0), 5.0f, 1e-4);
EXPECT_NEAR(pixel32f(img, kHeight-1, kWidth-1), 5.0f, 1e-4);
}
TEST_F(DisparityToDepthTest, PublishesMillimetersOnDepthRaw)
{
addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()));
std::shared_ptr<Collector<sensor_msgs::msg::Image>> raw =
collect<sensor_msgs::msg::Image>("depth_raw");
rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(raw->subscription));
pub->publish(makeDisparity(10.0f));
ASSERT_TRUE(spinUntil([&]() { return !raw->empty(); }));
const sensor_msgs::msg::Image & img = raw->back();
EXPECT_EQ(img.encoding, sensor_msgs::image_encodings::TYPE_16UC1);
EXPECT_EQ(pixel16u(img, 0, 0), 5000) << "5 m expressed in millimeters";
}
TEST_F(DisparityToDepthTest, PublishesBothUnitsConsistentlyFromOneInput)
{
// With both topics subscribed the node fills the 32FC1 and 16UC1 images in the same
// pass. The two must describe the same depth, one in meters and one in millimeters.
addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()));
std::shared_ptr<Collector<sensor_msgs::msg::Image>> meters =
collect<sensor_msgs::msg::Image>("depth");
std::shared_ptr<Collector<sensor_msgs::msg::Image>> millimeters =
collect<sensor_msgs::msg::Image>("depth_raw");
rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(meters->subscription));
ASSERT_TRUE(waitForPublisher(millimeters->subscription));
// A disparity of 25 gives 0.1 * 500 / 25 = 2 m.
pub->publish(makeDisparity(25.0f));
ASSERT_TRUE(spinUntil([&]() { return !meters->empty() && !millimeters->empty(); }))
<< "both outputs must be produced from a single input";
EXPECT_EQ(meters->back().encoding, sensor_msgs::image_encodings::TYPE_32FC1);
EXPECT_EQ(millimeters->back().encoding, sensor_msgs::image_encodings::TYPE_16UC1);
for(int row=0; row<kHeight; ++row)
{
for(int col=0; col<kWidth; ++col)
{
const float m = pixel32f(meters->back(), row, col);
const uint16_t mm = pixel16u(millimeters->back(), row, col);
EXPECT_NEAR(m, 2.0f, 1e-4) << "at " << row << "," << col;
EXPECT_EQ(mm, 2000) << "at " << row << "," << col;
EXPECT_EQ(mm, uint16_t(m * 1000.0f)) << "the two units must agree at " << row << "," << col;
}
}
}
TEST_F(DisparityToDepthTest, LeavesOutOfRangeDisparityAtZero)
{
addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()));
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
collect<sensor_msgs::msg::Image>("depth");
rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(depth->subscription));
// Above max_disparity (100), so no depth can be computed.
pub->publish(makeDisparity(500.0f));
ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); }));
EXPECT_FLOAT_EQ(pixel32f(depth->back(), 0, 0), 0.0f);
}
TEST_F(DisparityToDepthTest, RejectsNon32FC1Input)
{
addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()));
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
collect<sensor_msgs::msg::Image>("depth");
rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(depth->subscription));
pub->publish(makeDisparity(10.0f, sensor_msgs::image_encodings::TYPE_16UC1));
spinFor(std::chrono::milliseconds(400));
EXPECT_TRUE(depth->empty()) << "only 32FC1 disparity is supported";
}
TEST_F(DisparityToDepthTest, HonorsTheConfiguredQueueDepths)
{
// Queue depth is not observable from outside, so this pins down that the parameters
// are accepted and the node still converts with them set.
addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()
.parameter_overrides({rclcpp::Parameter("queue_sub", 20),
rclcpp::Parameter("queue_pub", 10)})));
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
collect<sensor_msgs::msg::Image>("depth");
rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(depth->subscription));
pub->publish(makeDisparity(1.0f));
ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); }));
EXPECT_EQ(depth->back().encoding, sensor_msgs::image_encodings::TYPE_32FC1);
}
TEST_F(DisparityToDepthTest, RejectsAZeroQueueDepth)
{
EXPECT_THROW(
addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()
.parameter_overrides({rclcpp::Parameter("queue_pub", 0)}))),
UException);
}
TEST_F(DisparityToDepthTest, BridgesABestEffortSourceToAReliableConsumer)
{
// A reliable subscription refuses to match a best-effort publisher, so setting the
// two sides apart is what lets the conversion cross that gap.
addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()
.parameter_overrides({rclcpp::Parameter("qos_sub", 2),
rclcpp::Parameter("qos_pub", 1)})));
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
collect<sensor_msgs::msg::Image>("depth", rclcpp::QoS(10).reliable());
rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
helper()->create_publisher<stereo_msgs::msg::DisparityImage>(
"disparity", rclcpp::QoS(10).best_effort());
ASSERT_TRUE(waitForSubscriber(pub)) << "a best-effort source must reach the node";
ASSERT_TRUE(waitForPublisher(depth->subscription))
<< "a reliable consumer must be able to subscribe to the depth output";
pub->publish(makeDisparity(1.0f));
ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); }));
EXPECT_EQ(depth->back().encoding, sensor_msgs::image_encodings::TYPE_32FC1);
}
TEST_F(DisparityToDepthTest, TheTwoQosSidesFallBackToQos)
{
// Only qos is given, so both sides must be best effort: a reliable consumer matches
// neither the publishers nor, from the other end, the subscription.
addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()
.parameter_overrides({rclcpp::Parameter("qos", 2)})));
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
collect<sensor_msgs::msg::Image>("depth", rclcpp::QoS(10).reliable());
rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
helper()->create_publisher<stereo_msgs::msg::DisparityImage>(
"disparity", rclcpp::QoS(10).best_effort());
EXPECT_TRUE(waitForSubscriber(pub)) << "the subscription must have followed qos";
spinFor(std::chrono::milliseconds(500));
EXPECT_EQ(depth->subscription->get_publisher_count(), 0u)
<< "the publishers must have followed qos too: best effort, so a reliable "
"consumer cannot match them";
}
rtabmap_util/test/test_imu_to_tf.cpp
+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.)
*/
#include "node_test_utils.hpp"
#include <rtabmap_util/imu_to_tf.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <tf2_msgs/msg/tf_message.hpp>
#include <tf2/LinearMath/Quaternion.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2/utils.hpp>
using namespace rtabmap_util_test;
namespace {
::testing::Environment * const kEnv = registerRclcppEnvironment();
/// An Imu message whose orientation is a pure rotation of @p yaw about z.
sensor_msgs::msg::Imu makeImu(const std::string & frameId, double stamp, double yaw = 0.0)
{
tf2::Quaternion q;
q.setRPY(0.0, 0.0, yaw);
sensor_msgs::msg::Imu msg;
msg.header.frame_id = frameId;
msg.header.stamp = rclcpp::Time(int32_t(stamp), uint32_t((stamp - int32_t(stamp)) * 1e9), RCL_ROS_TIME);
msg.orientation = tf2::toMsg(q);
return msg;
}
} // namespace
class ImuToTFTest : public NodeTest
{
protected:
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr staticTfKeepAlive_;
};
TEST_F(ImuToTFTest, BroadcastsOrientationAsTf)
{
addNode(std::make_shared<rtabmap_util::ImuToTF>(rclcpp::NodeOptions()
.parameter_overrides({rclcpp::Parameter("fixed_frame_id", "odom")})));
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
collect<tf2_msgs::msg::TFMessage>("/tf");
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
ASSERT_TRUE(waitForSubscriber(pub)) << "the node never subscribed to imu/data";
pub->publish(makeImu("imu_link", 1000.0, /*yaw=*/M_PI/2.0));
ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); })) << "no transform was broadcast";
ASSERT_EQ(tf->back().transforms.size(), 1u);
const geometry_msgs::msg::TransformStamped & t = tf->back().transforms[0];
EXPECT_EQ(t.header.frame_id, "odom");
EXPECT_EQ(t.child_frame_id, "imu_link") << "with no base_frame_id the imu frame is used";
// The broadcast rotation must be the IMU's orientation.
tf2::Quaternion q;
tf2::fromMsg(t.transform.rotation, q);
EXPECT_NEAR(tf2::getYaw(q), M_PI/2.0, 1e-6);
// It is an orientation only: no translation.
EXPECT_NEAR(t.transform.translation.x, 0.0, 1e-9);
EXPECT_NEAR(t.transform.translation.y, 0.0, 1e-9);
EXPECT_NEAR(t.transform.translation.z, 0.0, 1e-9);
}
TEST_F(ImuToTFTest, UsesTheConfiguredFixedFrame)
{
addNode(std::make_shared<rtabmap_util::ImuToTF>(rclcpp::NodeOptions()
.parameter_overrides({rclcpp::Parameter("fixed_frame_id", "my_odom")})));
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
collect<tf2_msgs::msg::TFMessage>("/tf");
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
ASSERT_TRUE(waitForSubscriber(pub));
pub->publish(makeImu("imu_link", 1000.0));
ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); }));
EXPECT_EQ(tf->back().transforms[0].header.frame_id, "my_odom");
}
TEST_F(ImuToTFTest, PreservesTheImuStamp)
{
addNode(std::make_shared<rtabmap_util::ImuToTF>(rclcpp::NodeOptions()));
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
collect<tf2_msgs::msg::TFMessage>("/tf");
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
ASSERT_TRUE(waitForSubscriber(pub));
const sensor_msgs::msg::Imu imu = makeImu("imu_link", 1234.5);
pub->publish(imu);
ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); }));
EXPECT_EQ(tf->back().transforms[0].header.stamp.sec, imu.header.stamp.sec);
EXPECT_EQ(tf->back().transforms[0].header.stamp.nanosec, imu.header.stamp.nanosec);
}
TEST_F(ImuToTFTest, ReportsTheOrientationInTheBaseFrame)
{
// With base_frame_id set and the mounting transform available, the node re-expresses
// the IMU orientation in the base frame and broadcasts that frame instead.
addNode(std::make_shared<rtabmap_util::ImuToTF>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("fixed_frame_id", "odom"),
rclcpp::Parameter("base_frame_id", "base_link"),
rclcpp::Parameter("wait_for_transform_duration", 0.5)})));
publishStaticTf("base_link", "imu_link", 0.1, 0.0, 0.2); // translation only
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
collect<tf2_msgs::msg::TFMessage>("/tf");
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
ASSERT_TRUE(waitForSubscriber(pub));
pub->publish(makeImu("imu_link", 1000.0, /*yaw=*/M_PI/2.0));
ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); }))
<< "with the mounting transform available a transform must be broadcast";
const geometry_msgs::msg::TransformStamped & t = tf->back().transforms[0];
EXPECT_EQ(t.header.frame_id, "odom");
EXPECT_EQ(t.child_frame_id, "base_link")
<< "the base frame is broadcast, not the imu frame";
// The mounting has no rotation, so the orientation is unchanged.
tf2::Quaternion q;
tf2::fromMsg(t.transform.rotation, q);
EXPECT_NEAR(tf2::getYaw(q), M_PI/2.0, 1e-6);
}
TEST_F(ImuToTFTest, IgnoresAYawOnlyMountingOffset)
{
// The node strips the yaw of the mounting transform (it uses only getYaw to build
// the correction), so a purely yaw-rotated mount leaves the reported orientation
// alone: the IMU's absolute yaw is what matters, not how it is bolted on.
addNode(std::make_shared<rtabmap_util::ImuToTF>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("fixed_frame_id", "odom"),
rclcpp::Parameter("base_frame_id", "base_link"),
rclcpp::Parameter("wait_for_transform_duration", 0.5)})));
// base_link -> imu_link rotated 90 degrees about z.
{
tf2::Quaternion mount;
mount.setRPY(0.0, 0.0, M_PI/2.0);
geometry_msgs::msg::TransformStamped m;
m.header.stamp = helper()->now();
m.header.frame_id = "base_link";
m.child_frame_id = "imu_link";
m.transform.rotation = tf2::toMsg(mount);
tf2_msgs::msg::TFMessage msg;
msg.transforms.push_back(m);
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr staticPub =
helper()->create_publisher<tf2_msgs::msg::TFMessage>(
"/tf_static", rclcpp::QoS(100).transient_local());
staticPub->publish(msg);
spinFor(std::chrono::milliseconds(150));
staticTfKeepAlive_ = staticPub;
}
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
collect<tf2_msgs::msg::TFMessage>("/tf");
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
ASSERT_TRUE(waitForSubscriber(pub));
pub->publish(makeImu("imu_link", 1000.0, /*yaw=*/M_PI/4.0));
ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); }));
const geometry_msgs::msg::TransformStamped & t = tf->back().transforms[0];
EXPECT_EQ(t.child_frame_id, "base_link");
tf2::Quaternion q;
tf2::fromMsg(t.transform.rotation, q);
EXPECT_NEAR(tf2::getYaw(q), M_PI/4.0, 1e-5)
<< "the mounting yaw must cancel out, leaving the imu's own yaw";
}
TEST_F(ImuToTFTest, DropsTheMessageWhenTheBaseTransformIsMissing)
{
// base_frame_id differs from the imu frame, so the node needs imu_link -> base_link
// from TF. Nothing publishes it, so nothing may be broadcast.
addNode(std::make_shared<rtabmap_util::ImuToTF>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("base_frame_id", "base_link"),
rclcpp::Parameter("wait_for_transform_duration", 0.0)})));
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
collect<tf2_msgs::msg::TFMessage>("/tf");
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
ASSERT_TRUE(waitForSubscriber(pub));
pub->publish(makeImu("imu_link", 1000.0, M_PI/2.0));
spinFor(std::chrono::milliseconds(500));
EXPECT_TRUE(tf->empty()) << "without the base transform the node must not broadcast";
}
rtabmap_util/test/test_lidar_deskewing.cpp
+204
View File
@@ -0,0 +1,204 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#include "node_test_utils.hpp"
#include "msg_builders.hpp"
#include <rtabmap_util/lidar_deskewing.hpp>
#include <cmath>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <tf2_msgs/msg/tf_message.hpp>
using namespace rtabmap_util_test;
namespace {
::testing::Environment * const kEnv = registerRclcppEnvironment();
}
class LidarDeskewingTest : public NodeTest
{
protected:
static constexpr double kSweep = 0.099; ///< first sample to last, seconds
static constexpr double kSpeed = 1.0; ///< m/s, straight at the wall
static constexpr float kWall = 5.0f; ///< distance to the wall, meters
/// Distance travelled since the first sample. Drives both the TF and the skew.
static double travelled(double elapsed) { return kSpeed * elapsed; }
/// Publishes odom -> lidar following exactly that trajectory.
void publishOdomMotion(double startStamp)
{
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub =
helper()->create_publisher<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
spinFor(std::chrono::milliseconds(100)); // let the node's listener subscribe
// Covers exactly the sweep, from the first sample to the last. Nothing beyond:
// asking for more than laser_geometry needs would be a regression.
for(int i=0; i<=2; ++i)
{
const double elapsed = kSweep * double(i) / 2.0;
geometry_msgs::msg::TransformStamped t;
t.header.stamp = stampOf(startStamp + elapsed);
t.header.frame_id = "odom";
t.child_frame_id = "lidar";
t.transform.translation.x = travelled(elapsed);
t.transform.rotation.w = 1.0;
tf2_msgs::msg::TFMessage msg;
msg.transforms.push_back(t);
tfPub->publish(msg);
}
spinFor(std::chrono::milliseconds(200)); // let the buffer fill
tfPub_ = tfPub; // keep the publisher alive
}
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub_;
};
TEST_F(LidarDeskewingTest, DeskewsACloudUsingTf)
{
// The wall is recorded bent because the sensor closes in during the sweep, and TF
// carries that same motion. A correct deskew must flatten it back to kWall.
addNode(std::make_shared<rtabmap_util::LidarDeskewing>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("fixed_frame_id", "odom"),
rclcpp::Parameter("wait_for_transform", 0.2)})));
publishOdomMotion(1000.0);
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
collect<sensor_msgs::msg::PointCloud2>("input_cloud/deskewed");
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("input_cloud", 10);
ASSERT_TRUE(waitForSubscriber(pub));
const size_t sampleCount = 20;
const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(
"lidar", 1000.0, sampleCount, kSweep, kWall, &travelled);
// The input really is bent: the last sample is a full sweep of travel closer.
ASSERT_NEAR(readXYZ(in, 0).x, kWall, 1e-4);
ASSERT_NEAR(readXYZ(in, sampleCount-1).x, kWall - float(kSpeed*kSweep), 1e-4);
pub->publish(in);
ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })) << "no deskewed cloud published";
const sensor_msgs::msg::PointCloud2 & cloud = out->back();
EXPECT_EQ(cloud.header.frame_id, "lidar") << "output stays in the sensor frame";
ASSERT_EQ(cloud.width, sampleCount);
// Every sample must land back on the wall.
for(size_t i=0; i<sampleCount; ++i)
{
EXPECT_NEAR(readXYZ(cloud, i).x, kWall, 5e-3) << "sample " << i;
}
}
TEST_F(LidarDeskewingTest, DeskewsAScanUsingTf)
{
// Same idea as the cloud case, for the 2D path. A ray at angle theta taken once the
// sensor has advanced d meters measures (wall - d)/cos(theta), so the raw scan bends.
// Deskewing must put every point back on the wall at x = kWall.
addNode(std::make_shared<rtabmap_util::LidarDeskewing>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("fixed_frame_id", "odom"),
rclcpp::Parameter("wait_for_transform", 0.2)})));
publishOdomMotion(1000.0);
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
collect<sensor_msgs::msg::PointCloud2>("input_scan/deskewed");
rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::LaserScan>("input_scan", 10);
ASSERT_TRUE(waitForSubscriber(pub));
sensor_msgs::msg::LaserScan scan;
scan.header.frame_id = "lidar";
scan.header.stamp = stampOf(1000.0);
scan.angle_min = -0.4f;
scan.angle_max = 0.4f;
scan.angle_increment = 0.05f;
scan.range_min = 0.1f;
scan.range_max = 30.0f;
const size_t rayCount = size_t((scan.angle_max - scan.angle_min) / scan.angle_increment) + 1;
scan.time_increment = float(kSweep / double(rayCount - 1));
scan.ranges.resize(rayCount);
for(size_t i=0; i<rayCount; ++i)
{
const double elapsed = double(i) * scan.time_increment;
const double angle = scan.angle_min + double(i) * scan.angle_increment;
// Distance to a wall at x=kWall, from a sensor that has already advanced.
scan.ranges[i] = float((kWall - travelled(elapsed)) / std::cos(angle));
}
pub->publish(scan);
ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })) << "no deskewed scan published";
const sensor_msgs::msg::PointCloud2 & cloud = out->back();
EXPECT_EQ(cloud.header.frame_id, "lidar") << "output stays in the sensor frame";
ASSERT_EQ(cloud.width, rayCount);
// Without deskewing the last ray would sit a full sweep of travel short of the wall.
for(size_t i=0; i<rayCount; ++i)
{
EXPECT_NEAR(readXYZ(cloud, i).x, kWall, 5e-3) << "ray " << i;
}
}
TEST_F(LidarDeskewingTest, RepublishesTheCloudUnchangedWhenDeskewingFails)
{
// With no TF the cloud cannot be deskewed, but the node deliberately republishes it
// as-is rather than dropping it, so downstream nodes keep receiving data.
addNode(std::make_shared<rtabmap_util::LidarDeskewing>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("fixed_frame_id", "odom"),
rclcpp::Parameter("wait_for_transform", 0.0)})));
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
collect<sensor_msgs::msg::PointCloud2>("input_cloud/deskewed");
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("input_cloud", 10);
ASSERT_TRUE(waitForSubscriber(pub));
const sensor_msgs::msg::PointCloud2 in =
makeXYZCloud("lidar", 1000.0, {{5.0f, 0.0f, 0.0f}, {5.0f, 1.0f, 0.0f}}, true);
pub->publish(in);
ASSERT_TRUE(spinUntil([&]() { return !out->empty(); }))
<< "the cloud must still be forwarded";
EXPECT_EQ(out->back().data, in.data) << "and forwarded byte for byte, still skewed";
}
TEST_F(LidarDeskewingTest, DropsAScanWhenTfIsMissing)
{
// The 2D scan path does the opposite of the cloud path: it returns early and
// publishes nothing when the transform is unavailable.
addNode(std::make_shared<rtabmap_util::LidarDeskewing>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("fixed_frame_id", "odom"),
rclcpp::Parameter("wait_for_transform", 0.0)})));
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
collect<sensor_msgs::msg::PointCloud2>("input_scan/deskewed");
rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::LaserScan>("input_scan", 10);
ASSERT_TRUE(waitForSubscriber(pub));
sensor_msgs::msg::LaserScan scan;
scan.header.frame_id = "lidar";
scan.header.stamp = stampOf(1000.0);
scan.angle_min = -1.0f;
scan.angle_max = 1.0f;
scan.angle_increment = 0.1f;
scan.time_increment = 0.001f;
scan.range_min = 0.1f;
scan.range_max = 30.0f;
scan.ranges.assign(21, 5.0f);
pub->publish(scan);
spinFor(std::chrono::milliseconds(400));
EXPECT_TRUE(out->empty()) << "the scan path drops the message instead of forwarding it";
}
rtabmap_util/test/test_map_assembler.cpp
+549
View File
@@ -0,0 +1,549 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#include "node_test_utils.hpp"
#include "msg_builders.hpp"
#include <rtabmap_util/map_assembler.hpp>
#include <rtabmap_conversions/MsgConversion.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap/core/Signature.h>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <std_srvs/srv/empty.hpp>
#include <thread>
#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
#include <octomap_msgs/srv/get_octomap.hpp>
#endif
using namespace rtabmap_util_test;
namespace {
::testing::Environment * const kEnv = registerRclcppEnvironment();
constexpr float kCellSize = 0.05f;
/// Anything above this is an obstacle once a grid is regenerated from a scan.
constexpr float kGroundHeight = 0.1f;
constexpr float kObstacleHeight = 0.5f;
cv::Mat toCellMat(const std::vector<cv::Point3f> & points)
{
if(points.empty()) { return cv::Mat(); }
cv::Mat mat(1, int(points.size()), CV_32FC3);
for(size_t i=0; i<points.size(); ++i)
{
mat.at<cv::Vec3f>(0, int(i)) = cv::Vec3f(points[i].x, points[i].y, points[i].z);
}
return mat;
}
/**
* @brief A graph node as it arrives on "mapData".
*
* map_assembler only caches a node that carries compressed images or a compressed scan,
* so the scan is what makes the node acceptable at all. The occupancy grid is what
* MapsManager normally uses; the two are deliberately given different geometry so a test
* can tell which one ended up in the map.
*
* @param scan points of the raw scan, in the node's frame
* @param ground ground cells of the ready-made grid
* @param obstacles obstacle cells of the ready-made grid
*/
rtabmap::Signature makeNode(
int id, const rtabmap::Transform & pose,
const std::vector<cv::Point3f> & scan,
const std::vector<cv::Point3f> & ground,
const std::vector<cv::Point3f> & obstacles)
{
rtabmap::SensorData data;
data.setId(id);
data.setStamp(1000.0 + id);
data.setLaserScan(rtabmap::LaserScan(rtabmap::compressData2(toCellMat(scan)),
/*maxPoints=*/0, /*maxRange=*/0.0f, rtabmap::LaserScan::kXYZ));
if(!ground.empty() || !obstacles.empty())
{
data.setOccupancyGrid(toCellMat(ground), toCellMat(obstacles), cv::Mat(), kCellSize,
cv::Point3f(0, 0, 0));
}
return rtabmap::Signature(id, /*mapId=*/0, /*weight=*/1, data.stamp(), /*label=*/"",
pose, rtabmap::Transform(), data);
}
cv::Point3f pointAt(const sensor_msgs::msg::PointCloud2 & cloud, size_t index)
{
uint32_t xo = 0, yo = 4, zo = 8;
for(size_t i=0; i<cloud.fields.size(); ++i)
{
if(cloud.fields[i].name == "x") { xo = cloud.fields[i].offset; }
else if(cloud.fields[i].name == "y") { yo = cloud.fields[i].offset; }
else if(cloud.fields[i].name == "z") { zo = cloud.fields[i].offset; }
}
const unsigned char * base = &cloud.data[index * cloud.point_step];
return cv::Point3f(
*reinterpret_cast<const float *>(base + xo),
*reinterpret_cast<const float *>(base + yo),
*reinterpret_cast<const float *>(base + zo));
}
bool containsPoint(const sensor_msgs::msg::PointCloud2 & cloud, const cv::Point3f & expected,
float tolerance = 1e-3f)
{
for(size_t i=0; i<size_t(cloud.width)*cloud.height; ++i)
{
if(cv::norm(pointAt(cloud, i) - expected) < tolerance) { return true; }
}
return false;
}
size_t pointCount(const sensor_msgs::msg::PointCloud2 & cloud)
{
return size_t(cloud.width) * cloud.height;
}
} // namespace
/**
* map_assembler subscribes to "mapData", caches the nodes it carries and hands them to a
* MapsManager, which is what actually publishes the maps.
*
* By default it first asks rtabmap for the map it missed: a one second timer whose
* callback calls get_map_data, blocks on the reply, and only then subscribes to
* "mapData". That needs two callbacks of the same node to run at once, hence the fake
* service and spinMultiThreadedUntil() in startInitializingFromRtabmap(). Most tests
* below have nothing to catch up on, so start() sets the timeout to 0 and the node
* subscribes immediately.
*/
class MapAssemblerTest : public NodeTest
{
protected:
/// The name the fake get_map_data service is advertised under.
static constexpr const char * kRtabmapName = "fake_rtabmap";
void SetUp() override
{
NodeTest::SetUp();
getMapCalls_ = 0;
}
/**
* @brief Advertises the get_map_data service map_assembler calls on start-up.
*
* @param initial the map handed back, i.e. what the node starts with in its cache.
*/
void advertiseGetMapData(const rtabmap_msgs::msg::MapData & initial =
rtabmap_msgs::msg::MapData())
{
initialMap_ = initial;
getMapService_ = helper()->create_service<rtabmap_msgs::srv::GetMap>(
std::string(kRtabmapName) + "/get_map_data",
[this](const std::shared_ptr<rtabmap_msgs::srv::GetMap::Request>,
std::shared_ptr<rtabmap_msgs::srv::GetMap::Response> response) {
++getMapCalls_;
response->data = initialMap_;
});
}
/// Creates the node with the start-up call skipped, so it subscribes right away.
void start(std::vector<rclcpp::Parameter> overrides = {})
{
overrides.push_back(rclcpp::Parameter("initialize_from_rtabmap_timeout", 0.0));
createNode(overrides);
ASSERT_TRUE(waitForSubscriber(mapDataPub_))
<< "map_assembler never subscribed to mapData";
}
/**
* @brief Creates the node with the start-up call enabled, as it is by default.
*
* It only subscribes to "mapData" once that call has returned, and the call blocks a
* callback on another callback of the same node, so it needs a multi-threaded
* executor to get through.
*/
void startInitializingFromRtabmap(double timeout = 5.0,
std::vector<rclcpp::Parameter> overrides = {})
{
overrides.push_back(
rclcpp::Parameter("initialize_from_rtabmap_timeout", timeout));
createNode(overrides);
ASSERT_TRUE(spinMultiThreadedUntil(
[&]() { return mapDataPub_->get_subscription_count() > 0; }))
<< "map_assembler never subscribed to mapData";
}
void createNode(std::vector<rclcpp::Parameter> overrides)
{
overrides.push_back(rclcpp::Parameter("rtabmap", std::string(kRtabmapName)));
assembler_ = addNode(std::make_shared<rtabmap_util::MapAssembler>(
rclcpp::NodeOptions().parameter_overrides(overrides)));
mapDataPub_ = helper()->create_publisher<rtabmap_msgs::msg::MapData>("mapData",
rclcpp::QoS(1));
}
/// A MapData carrying @p signatures and a graph over their poses.
static rtabmap_msgs::msg::MapData makeMapData(
const std::vector<rtabmap::Signature> & signatures,
const std::vector<int> & graphIds = {})
{
std::map<int, rtabmap::Transform> poses;
rtabmap_msgs::msg::MapData msg;
msg.header.frame_id = "map";
msg.header.stamp = stampOf(2000.0);
for(size_t i=0; i<signatures.size(); ++i)
{
rtabmap_msgs::msg::Node node;
rtabmap_conversions::nodeToROS(signatures[i], node);
msg.nodes.push_back(node);
poses.insert(std::make_pair(signatures[i].id(), signatures[i].getPose()));
}
// The graph may name nodes whose data is not resent, which is the normal case
// once map_assembler has them cached.
for(size_t i=0; i<graphIds.size(); ++i)
{
poses.insert(std::make_pair(graphIds[i], poseOf(graphIds[i])));
}
rtabmap_conversions::mapGraphToROS(poses, std::multimap<int, rtabmap::Link>(),
rtabmap::Transform::getIdentity(), msg.graph);
return msg;
}
static rtabmap::Transform poseOf(int id)
{
return rtabmap::Transform(2.0f * float(id - 1), 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
}
/// Two nodes whose ready-made grids hold one ground and one obstacle cell each.
static std::vector<rtabmap::Signature> twoNodes()
{
return {
makeNode(1, poseOf(1),
/*scan=*/{cv::Point3f(0.5f, -0.1f, 0.0f),
cv::Point3f(0.5f, 0.1f, kObstacleHeight)},
/*ground=*/{cv::Point3f(0.5f, -0.1f, 0.0f)},
/*obstacles=*/{cv::Point3f(1.0f, 0.1f, 0.0f)}),
makeNode(2, poseOf(2),
/*scan=*/{cv::Point3f(0.5f, -0.1f, 0.0f),
cv::Point3f(0.5f, 0.1f, kObstacleHeight)},
/*ground=*/{cv::Point3f(0.5f, -0.1f, 0.0f)},
/*obstacles=*/{cv::Point3f(1.0f, 0.1f, 0.0f)})};
}
void publishMapData(const rtabmap_msgs::msg::MapData & msg)
{
mapDataPub_->publish(msg);
}
std::shared_ptr<rtabmap_util::MapAssembler> assembler_;
rclcpp::Publisher<rtabmap_msgs::msg::MapData>::SharedPtr mapDataPub_;
rclcpp::Service<rtabmap_msgs::srv::GetMap>::SharedPtr getMapService_;
rtabmap_msgs::msg::MapData initialMap_;
std::atomic_int getMapCalls_{0};
};
constexpr const char * MapAssemblerTest::kRtabmapName;
//============================================================================
// Start-up
//============================================================================
TEST_F(MapAssemblerTest, AsksRtabmapForTheMapByDefault)
{
advertiseGetMapData(makeMapData(twoNodes()));
createNode({}); // no overrides at all, so the default timeout applies
ASSERT_TRUE(spinMultiThreadedUntil(
[&]() { return mapDataPub_->get_subscription_count() > 0; }));
EXPECT_EQ(getMapCalls_.load(), 1) << "the start-up service call is made exactly once";
}
TEST_F(MapAssemblerTest, SkipsTheStartUpCallWhenTheTimeoutIsZero)
{
// Nothing to catch up on, so the node should not spend its start-up waiting on a
// service: it subscribes immediately instead.
advertiseGetMapData(makeMapData(twoNodes()));
start();
EXPECT_EQ(getMapCalls_.load(), 0) << "rtabmap must not be called with a zero timeout";
}
TEST_F(MapAssemblerTest, SubscribesAnywayWhenRtabmapNeverAnswers)
{
// Nothing advertises get_map_data, so the start-up call times out. The node must
// still come up and subscribe, since rtabmap may be started afterwards.
// Short, because unlike every other test here this one waits the timeout out.
startInitializingFromRtabmap(/*timeout=*/0.5);
EXPECT_EQ(getMapCalls_.load(), 0);
}
TEST_F(MapAssemblerTest, WaitsForRtabmapToShowUpDuringTheTimeout)
{
// get_map_data is not advertised when the node starts asking for it: it appears part
// way through the wait. The call must still go through, which is what makes the
// timeout a real wait rather than a check of what happens to be up already.
//
// The node's timer fires one second after construction and then waits 750 ms, so
// advertising at 1250 ms lands inside that window with room on both sides.
std::thread rtabmapStartsLate([this]() {
std::this_thread::sleep_for(std::chrono::milliseconds(1250));
advertiseGetMapData(makeMapData(twoNodes()));
});
startInitializingFromRtabmap(/*timeout=*/0.75);
rtabmapStartsLate.join();
EXPECT_EQ(getMapCalls_.load(), 1) << "rtabmap showed up before the wait expired";
}
TEST_F(MapAssemblerTest, StartsFromTheMapRtabmapHandsBack)
{
// The nodes come from the start-up call, and the graph that arrives later names them
// without resending their data. The map must still be assembled from the cache.
advertiseGetMapData(makeMapData(twoNodes()));
startInitializingFromRtabmap();
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
collect<sensor_msgs::msg::PointCloud2>("cloud_map");
ASSERT_TRUE(waitForPublisher(cloud->subscription));
publishMapData(makeMapData({}, /*graphIds=*/{1, 2}));
ASSERT_TRUE(spinUntil([&]() { return !cloud->empty(); })) << "no cloud assembled";
EXPECT_EQ(pointCount(cloud->back()), 4u) << "one ground and one obstacle cell per node";
}
//============================================================================
// Assembling
//============================================================================
TEST_F(MapAssemblerTest, AssemblesTheCloudFromMapData)
{
start();
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
collect<sensor_msgs::msg::PointCloud2>("cloud_map");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
collect<sensor_msgs::msg::PointCloud2>("cloud_obstacles");
ASSERT_TRUE(waitForPublisher(cloud->subscription));
publishMapData(makeMapData(twoNodes()));
ASSERT_TRUE(spinUntil([&]() { return !cloud->empty() && !obstacles->empty(); }))
<< "no cloud assembled";
EXPECT_EQ(cloud->back().header.frame_id, "map") << "the frame comes from the message";
EXPECT_EQ(pointCount(cloud->back()), 4u);
EXPECT_EQ(pointCount(obstacles->back()), 2u);
// Node 2 sits 2 m along x, so its obstacle cell lands at 3 m.
EXPECT_TRUE(containsPoint(obstacles->back(), cv::Point3f(1.0f, 0.1f, 0.0f)));
EXPECT_TRUE(containsPoint(obstacles->back(), cv::Point3f(3.0f, 0.1f, 0.0f)));
}
TEST_F(MapAssemblerTest, PublishesTheOccupancyGrid)
{
start();
std::shared_ptr<Collector<nav_msgs::msg::OccupancyGrid>> grid =
collect<nav_msgs::msg::OccupancyGrid>("map");
ASSERT_TRUE(waitForPublisher(grid->subscription));
publishMapData(makeMapData(twoNodes()));
ASSERT_TRUE(spinUntil([&]() { return !grid->empty(); })) << "no grid assembled";
EXPECT_EQ(grid->back().header.frame_id, "map");
EXPECT_NEAR(grid->back().info.resolution, kCellSize, 1e-6);
EXPECT_GT(grid->back().info.width, 0u);
}
TEST_F(MapAssemblerTest, IgnoresAnEmptyMapData)
{
start();
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
collect<sensor_msgs::msg::PointCloud2>("cloud_map");
ASSERT_TRUE(waitForPublisher(cloud->subscription));
rtabmap_msgs::msg::MapData empty;
empty.header.frame_id = "map";
empty.header.stamp = stampOf(2000.0);
publishMapData(empty);
spinFor(std::chrono::milliseconds(400));
EXPECT_TRUE(cloud->empty()) << "a message with no graph and no nodes is nothing to do";
}
TEST_F(MapAssemblerTest, PublishesAnEmptyMapForAGraphWithNoCachedNodes)
{
// A graph can name nodes whose data map_assembler has never seen -- it has no cache
// at all here. It still publishes, using the poses as they are.
start();
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
collect<sensor_msgs::msg::PointCloud2>("cloud_map");
ASSERT_TRUE(waitForPublisher(cloud->subscription));
publishMapData(makeMapData({}, /*graphIds=*/{1, 2}));
ASSERT_TRUE(spinUntil([&]() { return !cloud->empty(); })) << "nothing published";
EXPECT_EQ(pointCount(cloud->back()), 0u) << "no data cached, so nothing to assemble";
EXPECT_EQ(cloud->back().header.frame_id, "map");
}
//============================================================================
// regenerate_local_grids
//============================================================================
TEST_F(MapAssemblerTest, UsesTheGridsThatCameWithTheNodes)
{
// By default the ready-made grid wins: its obstacle is at y=+0.1, the scan's is at
// y=-0.1 with the ground point, so the two are told apart by where the cells land.
start({rclcpp::Parameter(rtabmap::Parameters::kGridSensor(), std::string("0")),
rclcpp::Parameter(rtabmap::Parameters::kGridNormalsSegmentation(),
std::string("false")),
rclcpp::Parameter(rtabmap::Parameters::kGridMaxGroundHeight(),
std::string("0.1"))});
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
collect<sensor_msgs::msg::PointCloud2>("cloud_obstacles");
ASSERT_TRUE(waitForPublisher(obstacles->subscription));
publishMapData(makeMapData({twoNodes()[0]}));
ASSERT_TRUE(spinUntil([&]() { return !obstacles->empty(); }));
EXPECT_EQ(pointCount(obstacles->back()), 1u);
EXPECT_TRUE(containsPoint(obstacles->back(), cv::Point3f(1.0f, 0.1f, 0.0f)))
<< "the obstacle cell of the grid that came with the node";
}
TEST_F(MapAssemblerTest, RegenerateLocalGridsRebuildsThemFromTheScan)
{
// With regenerate_local_grids the grid that came with the node is thrown away, so
// MapsManager segments the scan instead: the raised scan point becomes the obstacle.
start({rclcpp::Parameter("regenerate_local_grids", true),
rclcpp::Parameter(rtabmap::Parameters::kGridSensor(), std::string("0")),
rclcpp::Parameter(rtabmap::Parameters::kGridNormalsSegmentation(),
std::string("false")),
rclcpp::Parameter(rtabmap::Parameters::kGridMaxGroundHeight(),
std::string("0.1"))});
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
collect<sensor_msgs::msg::PointCloud2>("cloud_obstacles");
ASSERT_TRUE(waitForPublisher(obstacles->subscription));
publishMapData(makeMapData({twoNodes()[0]}));
ASSERT_TRUE(spinUntil([&]() { return !obstacles->empty(); }));
EXPECT_EQ(pointCount(obstacles->back()), 1u);
EXPECT_TRUE(containsPoint(obstacles->back(),
cv::Point3f(0.5f, 0.1f, kObstacleHeight), kCellSize))
<< "the raised scan point, not the cell the node arrived with";
EXPECT_FALSE(containsPoint(obstacles->back(), cv::Point3f(1.0f, 0.1f, 0.0f)))
<< "the grid that came with the node must have been discarded";
}
//============================================================================
// Services
//============================================================================
TEST_F(MapAssemblerTest, ResetEmptiesTheMap)
{
start();
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
collect<sensor_msgs::msg::PointCloud2>("cloud_map");
ASSERT_TRUE(waitForPublisher(cloud->subscription));
publishMapData(makeMapData(twoNodes()));
ASSERT_TRUE(spinUntil([&]() { return !cloud->empty(); }));
ASSERT_EQ(pointCount(cloud->back()), 4u);
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr reset =
helper()->create_client<std_srvs::srv::Empty>("map_assembler/reset");
ASSERT_TRUE(spinUntil([&]() { return reset->service_is_ready(); }))
<< "the reset service was never advertised";
reset->async_send_request(std::make_shared<std_srvs::srv::Empty::Request>());
ASSERT_TRUE(spinUntil([&]() { return cloud->size() >= 1u; }));
spinFor(std::chrono::milliseconds(200));
// The cache is gone, so the same graph now assembles nothing.
const size_t before = cloud->size();
publishMapData(makeMapData({}, /*graphIds=*/{1, 2}));
ASSERT_TRUE(spinUntil([&]() { return cloud->size() > before; }));
EXPECT_EQ(pointCount(cloud->back()), 0u) << "reset must drop the cached nodes";
}
#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
TEST_F(MapAssemblerTest, ServesTheBinaryOctomap)
{
start();
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
collect<sensor_msgs::msg::PointCloud2>("cloud_map");
ASSERT_TRUE(waitForPublisher(cloud->subscription));
publishMapData(makeMapData(twoNodes()));
ASSERT_TRUE(spinUntil([&]() { return !cloud->empty(); }));
rclcpp::Client<octomap_msgs::srv::GetOctomap>::SharedPtr client =
helper()->create_client<octomap_msgs::srv::GetOctomap>(
"map_assembler/octomap_binary");
ASSERT_TRUE(spinUntil([&]() { return client->service_is_ready(); }));
auto future = client->async_send_request(
std::make_shared<octomap_msgs::srv::GetOctomap::Request>());
ASSERT_TRUE(spinUntil([&]() {
return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; }))
<< "octomap_binary never answered";
// Keep the response alive: future.get() hands back a temporary shared_ptr, so binding
// a reference into it would dangle.
const std::shared_ptr<octomap_msgs::srv::GetOctomap::Response> response = future.get();
EXPECT_EQ(response->map.header.frame_id, "map")
<< "the frame of the last map data received";
EXPECT_TRUE(response->map.binary);
EXPECT_FALSE(response->map.data.empty())
<< "the octomap is built on demand from the cache";
}
TEST_F(MapAssemblerTest, ServesTheFullOctomap)
{
start();
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
collect<sensor_msgs::msg::PointCloud2>("cloud_map");
ASSERT_TRUE(waitForPublisher(cloud->subscription));
publishMapData(makeMapData(twoNodes()));
ASSERT_TRUE(spinUntil([&]() { return !cloud->empty(); }));
rclcpp::Client<octomap_msgs::srv::GetOctomap>::SharedPtr client =
helper()->create_client<octomap_msgs::srv::GetOctomap>(
"map_assembler/octomap_full");
ASSERT_TRUE(spinUntil([&]() { return client->service_is_ready(); }));
auto future = client->async_send_request(
std::make_shared<octomap_msgs::srv::GetOctomap::Request>());
ASSERT_TRUE(spinUntil([&]() {
return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; }));
EXPECT_FALSE(future.get()->map.binary);
}
TEST_F(MapAssemblerTest, ServesAnEmptyOctomapWithoutData)
{
start();
rclcpp::Client<octomap_msgs::srv::GetOctomap>::SharedPtr client =
helper()->create_client<octomap_msgs::srv::GetOctomap>(
"map_assembler/octomap_binary");
ASSERT_TRUE(spinUntil([&]() { return client->service_is_ready(); }));
auto future = client->async_send_request(
std::make_shared<octomap_msgs::srv::GetOctomap::Request>());
ASSERT_TRUE(spinUntil([&]() {
return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; }));
EXPECT_TRUE(future.get()->map.data.empty()) << "nothing cached, nothing to serve";
}
#endif
rtabmap_util/test/test_maps_manager.cpp
+1060
View File
File diff suppressed because it is too large Load Diff
rtabmap_util/test/test_obstacles_detection.cpp
+423
View File
@@ -0,0 +1,423 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#include "node_test_utils.hpp"
#include "msg_builders.hpp"
#include <rtabmap_util/obstacles_detection.hpp>
#include <cmath>
#include <limits>
using namespace rtabmap_util_test;
namespace {
::testing::Environment * const kEnv = registerRclcppEnvironment();
/**
* @brief A dense ground plane at z=0 plus a vertical wall in front of it.
*
* The 0.05 m spacing matters: the segmentation clusters points with
* Grid/ClusterRadius (0.1 m by default) and drops clusters below
* Grid/MinClusterSize (10), so a sparser cloud is discarded entirely.
*/
std::vector<cv::Point3f> groundAndWall()
{
std::vector<cv::Point3f> points;
for(int i=0; i<=20; ++i) // ground: 1 m x 1 m at z=0
{
for(int j=0; j<=20; ++j)
{
points.push_back(cv::Point3f(0.3f + 0.05f*i, -0.5f + 0.05f*j, 0.0f));
}
}
for(int j=0; j<=20; ++j) // wall: vertical, 1 m wide, 0.75 m tall
{
for(int k=1; k<=15; ++k)
{
points.push_back(cv::Point3f(1.4f, -0.5f + 0.05f*j, 0.05f*k));
}
}
return points;
}
/**
* @brief A plane that is horizontal in the map frame, given a base frame pitched by
* @p pitch. In the base frame it therefore rises with x: z = x * tan(pitch).
*/
std::vector<cv::Point3f> planeLevelInMapFrame(double pitch)
{
std::vector<cv::Point3f> points;
for(int i=0; i<=20; ++i)
{
const float x = 0.8f + 0.05f*i;
for(int j=0; j<=20; ++j)
{
points.push_back(cv::Point3f(x, -0.5f + 0.05f*j, x * float(std::tan(pitch))));
}
}
return points;
}
/// Two flat 25-point patches: one about 0.5 m from the sensor, one about 3 m away.
std::vector<cv::Point3f> nearAndFarPatches()
{
std::vector<cv::Point3f> points;
for(int i=0; i<5; ++i)
{
for(int j=0; j<5; ++j)
{
points.push_back(cv::Point3f(0.4f + 0.05f*i, -0.1f + 0.05f*j, 0.0f));
points.push_back(cv::Point3f(2.9f + 0.05f*i, -0.1f + 0.05f*j, 0.0f));
}
}
return points;
}
/// Smallest and largest x in a cloud, to tell the near patch from the far one.
std::pair<float, float> xExtent(const sensor_msgs::msg::PointCloud2 & cloud)
{
float lo = std::numeric_limits<float>::max();
float hi = -std::numeric_limits<float>::max();
for(size_t i=0; i<cloud.width*cloud.height; ++i)
{
const float x = readXYZ(cloud, i).x;
lo = std::min(lo, x);
hi = std::max(hi, x);
}
return std::make_pair(lo, hi);
}
} // namespace
class ObstaclesDetectionTest : public NodeTest {};
TEST_F(ObstaclesDetectionTest, SeparatesGroundFromObstacles)
{
addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("frame_id", "base_link"),
rclcpp::Parameter("wait_for_transform", 0.1)})));
publishStaticTf("base_link", "lidar");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
collect<sensor_msgs::msg::PointCloud2>("ground");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
collect<sensor_msgs::msg::PointCloud2>("obstacles");
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(ground->subscription));
pub->publish(makeXYZCloud("lidar", 1000.0, groundAndWall()));
ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }))
<< "both ground and obstacles must be published";
EXPECT_GT(ground->back().width, 0u) << "the flat points must be classified as ground";
EXPECT_GT(obstacles->back().width, 0u) << "the wall must be classified as obstacles";
// The clouds are transformed back into the frame of the input topic, not frame_id.
EXPECT_EQ(ground->back().header.frame_id, "lidar");
EXPECT_EQ(obstacles->back().header.frame_id, "lidar");
}
TEST_F(ObstaclesDetectionTest, ProjectsObstaclesOntoTheGroundPlane)
{
// proj_obstacles is the obstacles cloud flattened to z=0, with flat surfaces removed.
// Note it is published in frame_id, unlike ground/obstacles which keep the input frame.
addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("frame_id", "base_link"),
rclcpp::Parameter("wait_for_transform", 0.1)})));
publishStaticTf("base_link", "lidar");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> proj =
collect<sensor_msgs::msg::PointCloud2>("proj_obstacles");
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(proj->subscription));
pub->publish(makeXYZCloud("lidar", 1000.0, groundAndWall()));
ASSERT_TRUE(spinUntil([&]() { return !proj->empty(); }));
const sensor_msgs::msg::PointCloud2 & cloud = proj->back();
ASSERT_GT(cloud.width, 0u) << "the wall must survive as a projected obstacle";
EXPECT_EQ(cloud.header.frame_id, "base_link")
<< "proj_obstacles uses frame_id, not the input frame";
for(size_t i=0; i<cloud.width; ++i)
{
EXPECT_NEAR(readXYZ(cloud, i).z, 0.0f, 1e-6)
<< "every projected point is flattened to z=0, point " << i;
}
}
TEST_F(ObstaclesDetectionTest, HeightSegmentationIsRelativeToTheBaseFrameByDefault)
{
// With normals segmentation off the split is a plain height threshold. Without a
// map frame the heights are those of the cloud in the base frame, so the flat points
// at z=0 fall below Grid/MaxGroundHeight and are ground.
addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("frame_id", "base_link"),
rclcpp::Parameter("wait_for_transform", 0.1),
rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")),
rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.2"))})));
publishStaticTf("base_link", "lidar");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
collect<sensor_msgs::msg::PointCloud2>("ground");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
collect<sensor_msgs::msg::PointCloud2>("obstacles");
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(ground->subscription));
pub->publish(makeXYZCloud("lidar", 1000.0, groundAndWall()));
ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }));
EXPECT_GT(ground->back().width, 0u) << "the z=0 plane is below the 0.2 m threshold";
EXPECT_GT(obstacles->back().width, 0u) << "the wall rises above it";
}
TEST_F(ObstaclesDetectionTest, MapFrameIdAloneDoesNotMoveTheHeightReference)
{
// The robot sits 1 m above the map origin, but Grid/MapFrameProjection is false by
// default, so pose.z() is ignored and the heights stay relative to the base frame.
// Setting map_frame_id on its own therefore changes nothing here.
addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("frame_id", "base_link"),
rclcpp::Parameter("map_frame_id", "map"),
rclcpp::Parameter("wait_for_transform", 0.1),
rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")),
rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.2"))})));
publishStaticTf("base_link", "lidar");
publishStaticTf("map", "base_link", 0.0, 0.0, 1.0);
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
collect<sensor_msgs::msg::PointCloud2>("ground");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
collect<sensor_msgs::msg::PointCloud2>("obstacles");
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(ground->subscription));
pub->publish(makeXYZCloud("lidar", 1000.0, groundAndWall()));
ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }));
EXPECT_GT(ground->back().width, 0u)
<< "without Grid/MapFrameProjection the map height is not applied";
}
TEST_F(ObstaclesDetectionTest, MapFrameIdLevelsTheGroundUsingRollAndPitch)
{
// Only pose.z() is gated by Grid/MapFrameProjection: roll and pitch are always
// applied. So map_frame_id on its own still levels the segmentation to the map's
// horizontal, which is what matters when the robot is on a slope.
const double pitch = 10.0 * M_PI / 180.0;
// A plane that is level in the map frame, seen from a base frame pitched by 10 deg:
// in the base frame it rises to well above the 0.1 m ground threshold.
const std::vector<cv::Point3f> plane = planeLevelInMapFrame(pitch);
ASSERT_GT(plane.back().z, 0.1f) << "precondition: tilted beyond the threshold";
// Without a map frame the tilt is taken at face value: not ground.
{
addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("frame_id", "base_link"),
rclcpp::Parameter("wait_for_transform", 0.1),
rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")),
rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.1"))})));
publishStaticTf("base_link", "lidar");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
collect<sensor_msgs::msg::PointCloud2>("ground");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
collect<sensor_msgs::msg::PointCloud2>("obstacles");
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(ground->subscription));
pub->publish(makeXYZCloud("lidar", 1000.0, plane));
ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }));
EXPECT_EQ(ground->back().width, 0u)
<< "a slope read in the base frame is not ground";
}
}
TEST_F(ObstaclesDetectionTest, MapFrameIdRecoversTheGroundOnASlope)
{
// Same tilted plane, but now the node knows the robot is pitched in the map frame,
// so it levels the cloud and the slope becomes ground again.
const double pitch = 10.0 * M_PI / 180.0;
addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("frame_id", "base_link"),
rclcpp::Parameter("map_frame_id", "map"),
rclcpp::Parameter("wait_for_transform", 0.1),
rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")),
rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.1"))})));
publishStaticTf("base_link", "lidar");
publishStaticTfRPY("map", "base_link", 0.0, pitch, 0.0);
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
collect<sensor_msgs::msg::PointCloud2>("ground");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
collect<sensor_msgs::msg::PointCloud2>("obstacles");
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(ground->subscription));
pub->publish(makeXYZCloud("lidar", 1000.0, planeLevelInMapFrame(pitch)));
ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }));
EXPECT_GT(ground->back().width, 0u)
<< "leveled by the map pitch, the slope is ground -- roll/pitch apply even "
"though Grid/MapFrameProjection is false";
}
TEST_F(ObstaclesDetectionTest, MapFrameProjectionSegmentsRelativeToTheMap)
{
// Same setup plus Grid/MapFrameProjection=true. Now pose.z() participates, the whole
// cloud sits 1 m up in the map frame, and nothing is below the 0.2 m ground
// threshold any more.
addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("frame_id", "base_link"),
rclcpp::Parameter("map_frame_id", "map"),
rclcpp::Parameter("wait_for_transform", 0.1),
rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")),
rclcpp::Parameter("Grid/MapFrameProjection", std::string("true")),
rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.2"))})));
publishStaticTf("base_link", "lidar");
publishStaticTf("map", "base_link", 0.0, 0.0, 1.0);
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
collect<sensor_msgs::msg::PointCloud2>("ground");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
collect<sensor_msgs::msg::PointCloud2>("obstacles");
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(ground->subscription));
pub->publish(makeXYZCloud("lidar", 1000.0, groundAndWall()));
ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }));
EXPECT_EQ(ground->back().width, 0u)
<< "lifted 1 m in the map frame, nothing is below the ground threshold";
EXPECT_GT(obstacles->back().width, 0u) << "everything becomes an obstacle instead";
}
/// Runs the node with the given Grid range settings and returns the ground cloud.
class ObstaclesDetectionRangeTest : public NodeTest
{
protected:
sensor_msgs::msg::PointCloud2 groundWithRange(
const std::string & rangeMin, const std::string & rangeMax,
const std::vector<cv::Point3f> & points)
{
addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("frame_id", "base_link"),
rclcpp::Parameter("wait_for_transform", 0.1),
rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")),
rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.2")),
rclcpp::Parameter("Grid/RangeMin", rangeMin),
rclcpp::Parameter("Grid/RangeMax", rangeMax)})));
publishStaticTf("base_link", "lidar");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
collect<sensor_msgs::msg::PointCloud2>("ground");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
collect<sensor_msgs::msg::PointCloud2>("obstacles");
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
EXPECT_TRUE(waitForSubscriber(pub));
EXPECT_TRUE(waitForPublisher(ground->subscription));
pub->publish(makeXYZCloud("lidar", 1000.0, points));
EXPECT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }));
return ground->empty() ? sensor_msgs::msg::PointCloud2() : ground->back();
}
};
TEST_F(ObstaclesDetectionRangeTest, RangeFilteringDisabledKeepsEverything)
{
// Grid/RangeMax=0 means no upper limit, so both patches survive.
const sensor_msgs::msg::PointCloud2 ground =
groundWithRange("0.0", "0.0", nearAndFarPatches());
EXPECT_EQ(ground.width, 50u);
}
TEST_F(ObstaclesDetectionRangeTest, GridRangeMaxDropsDistantPoints)
{
// Only the patch inside 1 m survives.
const sensor_msgs::msg::PointCloud2 ground =
groundWithRange("0.0", "1.0", nearAndFarPatches());
ASSERT_EQ(ground.width, 25u);
const std::pair<float, float> extent = xExtent(ground);
EXPECT_NEAR(extent.first, 0.4f, 1e-3);
EXPECT_LT(extent.second, 1.0f) << "nothing beyond the 1 m limit may remain";
}
TEST_F(ObstaclesDetectionRangeTest, GridRangeMinDropsNearbyPoints)
{
// The mirror image: everything closer than 1 m is discarded instead.
const sensor_msgs::msg::PointCloud2 ground =
groundWithRange("1.0", "0.0", nearAndFarPatches());
ASSERT_EQ(ground.width, 25u);
const std::pair<float, float> extent = xExtent(ground);
EXPECT_GT(extent.first, 1.0f) << "nothing closer than the 1 m limit may remain";
EXPECT_NEAR(extent.second, 3.1f, 1e-3);
}
TEST_F(ObstaclesDetectionRangeTest, DefaultRangeMaxIsFiveMeters)
{
// Grid/RangeMax defaults to 5.0, not infinity: a patch at 6 m is silently dropped
// even though no range parameter was set.
std::vector<cv::Point3f> points = nearAndFarPatches();
for(int i=0; i<5; ++i)
{
for(int j=0; j<5; ++j)
{
points.push_back(cv::Point3f(5.9f + 0.05f*i, -0.1f + 0.05f*j, 0.0f));
}
}
const sensor_msgs::msg::PointCloud2 ground =
groundWithRange("0.0", "5.0", points); // the defaults, stated explicitly
EXPECT_EQ(ground.width, 50u) << "the 6 m patch is beyond the default range";
EXPECT_LT(xExtent(ground).second, 5.0f);
}
TEST_F(ObstaclesDetectionTest, PublishesEmptyCloudsForAnEmptyInput)
{
addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
.parameter_overrides({rclcpp::Parameter("frame_id", "base_link")})));
publishStaticTf("base_link", "lidar");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
collect<sensor_msgs::msg::PointCloud2>("ground");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
collect<sensor_msgs::msg::PointCloud2>("obstacles");
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(ground->subscription));
pub->publish(makeXYZCloud("lidar", 1000.0, {}));
ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }))
<< "an empty input must still produce output, not a dropped message";
EXPECT_EQ(ground->back().width, 0u);
EXPECT_EQ(obstacles->back().width, 0u);
}
rtabmap_util/test/test_point_cloud_aggregator.cpp
+194
View File
@@ -0,0 +1,194 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#include "node_test_utils.hpp"
#include "msg_builders.hpp"
#include <rtabmap_util/point_cloud_aggregator.hpp>
#include <tf2_msgs/msg/tf_message.hpp>
using namespace rtabmap_util_test;
namespace {
::testing::Environment * const kEnv = registerRclcppEnvironment();
}
class PointCloudAggregatorTest : public NodeTest
{
protected:
/// odom -> base_link advancing along x at 1 m/s across the two cloud stamps.
void publishOdomMotion(double startStamp, double duration)
{
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub =
helper()->create_publisher<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
spinFor(std::chrono::milliseconds(100));
for(int i=0; i<=6; ++i)
{
const double elapsed = duration * double(i) / 6.0;
geometry_msgs::msg::TransformStamped t;
t.header.stamp = stampOf(startStamp + elapsed);
t.header.frame_id = "odom";
t.child_frame_id = "base_link";
t.transform.translation.x = elapsed; // 1 m/s
t.transform.rotation.w = 1.0;
tf2_msgs::msg::TFMessage msg;
msg.transforms.push_back(t);
tfPub->publish(msg);
}
spinFor(std::chrono::milliseconds(200));
tfPub_ = tfPub;
}
/**
* @brief Publishes three pairs of clouds observing one landmark 5 m ahead in odom.
*
* Pair k is stamped at 1000.0+0.2k and 0.1 s later. The robot drives at 1 m/s, so
* each sensor measures the landmark at 5 m minus the distance travelled by then.
*/
void publishPairs(
const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & pub1,
const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & pub2)
{
for(int k=0; k<3; ++k)
{
const double t1 = 1000.0 + 0.2*double(k);
const double t2 = t1 + 0.1;
pub1->publish(makeXYZCloud("lidar_a", t1, {{float(5.0-(t1-1000.0)), 0.0f, 0.0f}}));
pub2->publish(makeXYZCloud("lidar_b", t2, {{float(5.0-(t2-1000.0)), 0.0f, 0.0f}}));
spinFor(std::chrono::milliseconds(50));
}
}
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub_;
};
TEST_F(PointCloudAggregatorTest, CombinesTwoSynchronizedClouds)
{
addNode(std::make_shared<rtabmap_util::PointCloudAggregator>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("count", 2),
rclcpp::Parameter("frame_id", "base_link"),
rclcpp::Parameter("approx_sync", true),
rclcpp::Parameter("wait_for_transform", 0.2)})));
publishStaticTf("base_link", "lidar_a", 0.0, 0.2, 0.0);
publishStaticTf("base_link", "lidar_b", 0.0, -0.2, 0.0);
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
collect<sensor_msgs::msg::PointCloud2>("combined_cloud");
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub1 =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud1", 10);
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub2 =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud2", 10);
ASSERT_TRUE(waitForSubscriber(pub1));
ASSERT_TRUE(waitForSubscriber(pub2));
const std::vector<cv::Point3f> a = {{1.0f, 0.0f, 0.0f}, {2.0f, 0.0f, 0.0f}};
const std::vector<cv::Point3f> b = {{3.0f, 0.0f, 0.0f}};
pub1->publish(makeXYZCloud("lidar_a", 1000.0, a));
pub2->publish(makeXYZCloud("lidar_b", 1000.0, b));
ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })) << "no combined cloud published";
EXPECT_EQ(out->back().width, a.size() + b.size()) << "every input point must survive";
EXPECT_EQ(out->back().header.frame_id, "base_link")
<< "the combined cloud is expressed in frame_id";
}
TEST_F(PointCloudAggregatorTest, AlignsCloudsCapturedAtDifferentTimesWhileMoving)
{
// The two sensors fire 0.1 s apart while the robot drives forward at 1 m/s, so they
// see the same world point at different ranges. With fixed_frame_id set, the second
// cloud is motion-compensated back to the first one's stamp and the two coincide.
addNode(std::make_shared<rtabmap_util::PointCloudAggregator>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("count", 2),
rclcpp::Parameter("frame_id", "base_link"),
rclcpp::Parameter("fixed_frame_id", "odom"),
rclcpp::Parameter("approx_sync", true),
rclcpp::Parameter("wait_for_transform", 0.2)})));
publishStaticTf("base_link", "lidar_a");
publishStaticTf("base_link", "lidar_b");
publishOdomMotion(1000.0, 0.6);
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
collect<sensor_msgs::msg::PointCloud2>("combined_cloud");
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub1 =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud1", 10);
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub2 =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud2", 10);
ASSERT_TRUE(waitForSubscriber(pub1));
ASSERT_TRUE(waitForSubscriber(pub2));
// A landmark 5 m ahead in odom, the robot driving at 1 m/s. Several pairs are sent
// because the ApproximateTime policy needs a following message before it can commit
// to a match when the stamps differ; the first emitted pair is the one asserted on.
publishPairs(pub1, pub2);
ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })) << "no combined cloud";
const sensor_msgs::msg::PointCloud2 & cloud = out->front();
ASSERT_EQ(cloud.width, 2u);
// Both observations of the same landmark must land on the same point.
EXPECT_NEAR(readXYZ(cloud, 0).x, 5.0f, 5e-3);
EXPECT_NEAR(readXYZ(cloud, 1).x, 5.0f, 5e-3)
<< "the later cloud must be compensated for the 0.1 m of motion";
}
TEST_F(PointCloudAggregatorTest, WithoutAFixedFrameCloudsAreNotMotionCompensated)
{
// Same inputs, no fixed_frame_id: the second cloud is taken at face value and the
// two observations stay 0.1 m apart. This is what fixed_frame_id exists to fix.
addNode(std::make_shared<rtabmap_util::PointCloudAggregator>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("count", 2),
rclcpp::Parameter("frame_id", "base_link"),
rclcpp::Parameter("approx_sync", true),
rclcpp::Parameter("wait_for_transform", 0.2)})));
publishStaticTf("base_link", "lidar_a");
publishStaticTf("base_link", "lidar_b");
publishOdomMotion(1000.0, 0.6);
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
collect<sensor_msgs::msg::PointCloud2>("combined_cloud");
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub1 =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud1", 10);
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub2 =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud2", 10);
ASSERT_TRUE(waitForSubscriber(pub1));
ASSERT_TRUE(waitForSubscriber(pub2));
publishPairs(pub1, pub2);
ASSERT_TRUE(spinUntil([&]() { return !out->empty(); }));
const sensor_msgs::msg::PointCloud2 & cloud = out->front();
ASSERT_EQ(cloud.width, 2u);
EXPECT_NEAR(readXYZ(cloud, 0).x, 5.0f, 5e-3);
EXPECT_NEAR(readXYZ(cloud, 1).x, 4.9f, 5e-3)
<< "uncompensated, the second observation stays where it was measured";
}
TEST_F(PointCloudAggregatorTest, WaitsForEveryInput)
{
addNode(std::make_shared<rtabmap_util::PointCloudAggregator>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("count", 2),
rclcpp::Parameter("frame_id", "base_link"),
rclcpp::Parameter("approx_sync", true)})));
publishStaticTf("base_link", "lidar_a");
publishStaticTf("base_link", "lidar_b");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
collect<sensor_msgs::msg::PointCloud2>("combined_cloud");
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub1 =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud1", 10);
ASSERT_TRUE(waitForSubscriber(pub1));
// Only one of the two inputs arrives: the synchronizer must not fire.
pub1->publish(makeXYZCloud("lidar_a", 1000.0, {{1.0f, 0.0f, 0.0f}}));
spinFor(std::chrono::milliseconds(500));
EXPECT_TRUE(out->empty()) << "a single input must not produce a combined cloud";
}
rtabmap_util/test/test_point_cloud_assembler.cpp
+399
View File
@@ -0,0 +1,399 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#include "node_test_utils.hpp"
#include "msg_builders.hpp"
#include <rtabmap_util/point_cloud_assembler.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rtabmap_msgs/msg/odom_info.hpp>
using namespace rtabmap_util_test;
namespace {
::testing::Environment * const kEnv = registerRclcppEnvironment();
}
class PointCloudAssemblerTest : public NodeTest
{
protected:
/// Starts the assembler with @p overrides, plus a static odom -> lidar transform.
void start(const std::vector<rclcpp::Parameter> & overrides)
{
addNode(std::make_shared<rtabmap_util::PointCloudAssembler>(
rclcpp::NodeOptions().parameter_overrides(overrides)));
publishStaticTf("odom", "lidar");
publishStaticTf("lidar", "base_link");
out_ = collect<sensor_msgs::msg::PointCloud2>("assembled_cloud");
pub_ = helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
ASSERT_TRUE(waitForSubscriber(pub_));
}
static bool hasField(const sensor_msgs::msg::PointCloud2 & cloud, const std::string & name)
{
for(size_t i=0; i<cloud.fields.size(); ++i)
{
if(cloud.fields[i].name == name) { return true; }
}
return false;
}
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_;
};
TEST_F(PointCloudAssemblerTest, PublishesAfterMaxCloudsAreAccumulated)
{
addNode(std::make_shared<rtabmap_util::PointCloudAssembler>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("max_clouds", 3),
rclcpp::Parameter("fixed_frame_id", "odom"),
rclcpp::Parameter("wait_for_transform", 0.2)})));
publishStaticTf("odom", "lidar");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
collect<sensor_msgs::msg::PointCloud2>("assembled_cloud");
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
ASSERT_TRUE(waitForSubscriber(pub));
const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}, {2.0f, 0.0f, 0.0f}};
// The first two clouds are only accumulated.
pub->publish(makeXYZCloud("lidar", 1000.0, points));
pub->publish(makeXYZCloud("lidar", 1000.1, points));
spinFor(std::chrono::milliseconds(300));
EXPECT_TRUE(out->empty()) << "nothing is published before max_clouds is reached";
// The third completes the batch.
pub->publish(makeXYZCloud("lidar", 1000.2, points));
ASSERT_TRUE(spinUntil([&]() { return !out->empty(); }))
<< "the assembled cloud must be published on the third input";
EXPECT_EQ(out->back().width, 3 * points.size()) << "all three clouds must be included";
EXPECT_EQ(out->back().header.frame_id, "lidar")
<< "the assembled cloud comes back in the sensor frame";
}
TEST_F(PointCloudAssemblerTest, AssemblingTimePublishesAfterTheConfiguredSpan)
{
// An alternative trigger to max_clouds: publish once the newest cloud is at least
// assembling_time newer than the oldest one held.
start({rclcpp::Parameter("max_clouds", 0),
rclcpp::Parameter("assembling_time", 0.25),
rclcpp::Parameter("fixed_frame_id", "odom"),
rclcpp::Parameter("wait_for_transform", 0.2)});
const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
for(int i=0; i<3; ++i) // 1000.0, 1000.1, 1000.2 -- span 0.2 s, below 0.25
{
pub_->publish(makeXYZCloud("lidar", 1000.0 + 0.1*i, points));
}
spinFor(std::chrono::milliseconds(300));
EXPECT_TRUE(out_->empty()) << "0.2 s of clouds is short of assembling_time";
pub_->publish(makeXYZCloud("lidar", 1000.3, points)); // span now 0.3 s
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }))
<< "crossing assembling_time must publish";
EXPECT_EQ(out_->back().width, 4u) << "all four clouds are included";
}
TEST_F(PointCloudAssemblerTest, CircularBufferPublishesOnEveryCloud)
{
// With a circular buffer the node emits a sliding window instead of filling up,
// clearing and starting again: every input produces an output.
start({rclcpp::Parameter("max_clouds", 3),
rclcpp::Parameter("circular_buffer", true),
rclcpp::Parameter("fixed_frame_id", "odom"),
rclcpp::Parameter("wait_for_transform", 0.2)});
const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
for(int i=0; i<4; ++i)
{
pub_->publish(makeXYZCloud("lidar", 1000.0 + 0.1*i, points));
ASSERT_TRUE(spinUntil([&]() { return out_->size() >= size_t(i+1); }))
<< "cloud " << i << " did not produce an output";
}
EXPECT_EQ(out_->size(), 4u) << "one output per input, not one per full batch";
// The window is capped at max_clouds.
EXPECT_LE(out_->back().width, 3u);
}
TEST_F(PointCloudAssemblerTest, RangeMaxDropsDistantPoints)
{
start({rclcpp::Parameter("max_clouds", 1),
rclcpp::Parameter("range_max", 3.0),
rclcpp::Parameter("fixed_frame_id", "odom"),
rclcpp::Parameter("wait_for_transform", 0.2)});
// Two points inside 3 m, one well beyond it.
pub_->publish(makeXYZCloud("lidar", 1000.0,
{{1.0f, 0.0f, 0.0f}, {2.0f, 0.0f, 0.0f}, {9.0f, 0.0f, 0.0f}}));
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_EQ(out_->back().width, 2u) << "the 9 m point must be filtered out";
}
TEST_F(PointCloudAssemblerTest, RemoveZDropsTheZField)
{
start({rclcpp::Parameter("max_clouds", 1),
rclcpp::Parameter("remove_z", true),
rclcpp::Parameter("fixed_frame_id", "odom"),
rclcpp::Parameter("wait_for_transform", 0.2)});
pub_->publish(makeXYZCloud("lidar", 1000.0, {{1.0f, 0.0f, 0.5f}}));
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
// The field is removed entirely, not zeroed: the output is a 2D cloud.
EXPECT_TRUE(hasField(out_->back(), "x"));
EXPECT_TRUE(hasField(out_->back(), "y"));
EXPECT_FALSE(hasField(out_->back(), "z")) << "remove_z drops the field itself";
}
TEST_F(PointCloudAssemblerTest, FrameIdSetsTheOutputFrame)
{
start({rclcpp::Parameter("max_clouds", 1),
rclcpp::Parameter("frame_id", "base_link"),
rclcpp::Parameter("fixed_frame_id", "odom"),
rclcpp::Parameter("wait_for_transform", 0.2)});
pub_->publish(makeXYZCloud("lidar", 1000.0, {{1.0f, 0.0f, 0.0f}}));
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_EQ(out_->back().header.frame_id, "base_link")
<< "the assembled cloud is returned in frame_id when it is set";
}
TEST_F(PointCloudAssemblerTest, SkipCloudsIgnoresIntermediateClouds)
{
// skip_clouds=1 keeps every other cloud, so reaching max_clouds=2 takes four inputs.
start({rclcpp::Parameter("max_clouds", 2),
rclcpp::Parameter("skip_clouds", 1),
rclcpp::Parameter("fixed_frame_id", "odom"),
rclcpp::Parameter("wait_for_transform", 0.2)});
const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
pub_->publish(makeXYZCloud("lidar", 1000.0, points));
pub_->publish(makeXYZCloud("lidar", 1000.1, points));
spinFor(std::chrono::milliseconds(300));
EXPECT_TRUE(out_->empty()) << "one of those two was skipped, so the batch is short";
pub_->publish(makeXYZCloud("lidar", 1000.2, points));
pub_->publish(makeXYZCloud("lidar", 1000.3, points));
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_EQ(out_->back().width, 2u) << "two kept clouds, two skipped";
}
TEST_F(PointCloudAssemblerTest, LinearUpdateSkipsCloudsWhileStationary)
{
// With linear_update set, a cloud captured without the robot having moved far enough
// is discarded rather than accumulated, so a parked robot never fills a batch.
start({rclcpp::Parameter("max_clouds", 3),
rclcpp::Parameter("linear_update", 0.5),
rclcpp::Parameter("fixed_frame_id", "odom"),
rclcpp::Parameter("wait_for_transform", 0.2)});
const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
for(int i=0; i<5; ++i) // the TF is static, so the robot never moves
{
pub_->publish(makeXYZCloud("lidar", 1000.0 + 0.1*i, points));
}
spinFor(std::chrono::milliseconds(500));
EXPECT_TRUE(out_->empty())
<< "a stationary robot must not accumulate a batch when linear_update is set";
}
TEST_F(PointCloudAssemblerTest, WithoutLinearUpdateEveryCloudCounts)
{
// The same stationary robot, with the motion filter disabled: the batch fills.
start({rclcpp::Parameter("max_clouds", 3),
rclcpp::Parameter("linear_update", 0.0),
rclcpp::Parameter("fixed_frame_id", "odom"),
rclcpp::Parameter("wait_for_transform", 0.2)});
const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
for(int i=0; i<3; ++i)
{
pub_->publish(makeXYZCloud("lidar", 1000.0 + 0.1*i, points));
}
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_EQ(out_->back().width, 3u);
}
TEST_F(PointCloudAssemblerTest, VoxelSizeDownsamplesTheCloud)
{
start({rclcpp::Parameter("max_clouds", 1),
rclcpp::Parameter("voxel_size", 0.5),
rclcpp::Parameter("fixed_frame_id", "odom"),
rclcpp::Parameter("wait_for_transform", 0.2)});
// 100 points packed into a 0.2 m cube: a 0.5 m voxel grid collapses them.
std::vector<cv::Point3f> dense;
for(int i=0; i<10; ++i)
{
for(int j=0; j<10; ++j)
{
dense.push_back(cv::Point3f(1.0f + 0.02f*i, 0.02f*j, 0.0f));
}
}
pub_->publish(makeXYZCloud("lidar", 1000.0, dense));
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_LT(out_->back().width, dense.size())
<< "voxel_size must reduce the point count";
EXPECT_GT(out_->back().width, 0u);
}
/// Fixture for the odometry-synchronized modes, which need fixed_frame_id to be empty.
class PointCloudAssemblerOdomTest : public NodeTest
{
protected:
void start(std::vector<rclcpp::Parameter> overrides)
{
// fixed_frame_id defaults to "odom"; it has to be cleared for the node to
// subscribe to the odometry topic instead of reading TF directly.
overrides.push_back(rclcpp::Parameter("fixed_frame_id", ""));
addNode(std::make_shared<rtabmap_util::PointCloudAssembler>(
rclcpp::NodeOptions().parameter_overrides(overrides)));
publishStaticTf("odom", "lidar");
out_ = collect<sensor_msgs::msg::PointCloud2>("assembled_cloud");
cloudPub_ = helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
odomPub_ = helper()->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
odomInfoPub_ = helper()->create_publisher<rtabmap_msgs::msg::OdomInfo>("odom_info", 10);
ASSERT_TRUE(waitForSubscriber(cloudPub_));
ASSERT_TRUE(waitForSubscriber(odomPub_));
}
/// An odometry message at the origin; a null one has an all-zero orientation.
nav_msgs::msg::Odometry makeOdom(double stamp, bool null = false)
{
nav_msgs::msg::Odometry odom;
odom.header.stamp = stampOf(stamp);
odom.header.frame_id = "odom";
odom.child_frame_id = "lidar";
odom.pose.pose.orientation.w = null ? 0.0 : 1.0;
return odom;
}
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub_;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odomPub_;
rclcpp::Publisher<rtabmap_msgs::msg::OdomInfo>::SharedPtr odomInfoPub_;
};
TEST_F(PointCloudAssemblerOdomTest, TakesTheFixedFrameFromTheOdometryMessage)
{
// With fixed_frame_id empty the node syncs cloud with odom and uses the odometry
// header's frame as the fixed frame.
start({rclcpp::Parameter("max_clouds", 2),
rclcpp::Parameter("wait_for_transform", 0.2)});
const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
for(int i=0; i<2; ++i)
{
const double t = 1000.0 + 0.1*i;
cloudPub_->publish(makeXYZCloud("lidar", t, points));
odomPub_->publish(makeOdom(t)); // exact sync: identical stamps
spinFor(std::chrono::milliseconds(50));
}
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }))
<< "cloud+odom synchronization must drive the assembly";
EXPECT_EQ(out_->back().width, 2u);
}
TEST_F(PointCloudAssemblerOdomTest, NullOdometryResetsTheBuffer)
{
// A null odometry means tracking was lost, so the accumulated clouds are dropped
// rather than being stitched across the discontinuity.
start({rclcpp::Parameter("max_clouds", 3),
rclcpp::Parameter("wait_for_transform", 0.2)});
const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
// Two good clouds, then a lost-tracking frame, then two more.
for(int i=0; i<2; ++i)
{
const double t = 1000.0 + 0.1*i;
cloudPub_->publish(makeXYZCloud("lidar", t, points));
odomPub_->publish(makeOdom(t));
spinFor(std::chrono::milliseconds(50));
}
cloudPub_->publish(makeXYZCloud("lidar", 1000.2, points));
odomPub_->publish(makeOdom(1000.2, /*null=*/true));
spinFor(std::chrono::milliseconds(150));
EXPECT_TRUE(out_->empty()) << "the null odometry must not complete the batch";
// After the reset it takes three fresh clouds again, not one.
for(int i=0; i<2; ++i)
{
const double t = 1000.3 + 0.1*i;
cloudPub_->publish(makeXYZCloud("lidar", t, points));
odomPub_->publish(makeOdom(t));
spinFor(std::chrono::milliseconds(50));
}
EXPECT_TRUE(out_->empty()) << "the buffer restarted, so two clouds are not enough";
cloudPub_->publish(makeXYZCloud("lidar", 1000.5, points));
odomPub_->publish(makeOdom(1000.5));
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_EQ(out_->back().width, 3u) << "only the post-reset clouds are assembled";
}
TEST_F(PointCloudAssemblerOdomTest, SubscribeOdomInfoKeepsOnlyKeyFrames)
{
// With subscribe_odom_info the node also takes OdomInfo and accumulates a cloud only
// when that frame became a key frame.
start({rclcpp::Parameter("max_clouds", 2),
rclcpp::Parameter("subscribe_odom_info", true),
rclcpp::Parameter("wait_for_transform", 0.2)});
ASSERT_TRUE(waitForSubscriber(odomInfoPub_));
const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
auto publishFrame = [&](double t, bool keyFrame) {
rtabmap_msgs::msg::OdomInfo info;
info.header.stamp = stampOf(t);
info.header.frame_id = "odom";
info.key_frame_added = keyFrame;
cloudPub_->publish(makeXYZCloud("lidar", t, points));
odomPub_->publish(makeOdom(t));
odomInfoPub_->publish(info);
spinFor(std::chrono::milliseconds(60));
};
publishFrame(1000.0, false);
publishFrame(1000.1, false);
publishFrame(1000.2, false);
EXPECT_TRUE(out_->empty()) << "non key frames must be ignored";
publishFrame(1000.3, true);
publishFrame(1000.4, true);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_EQ(out_->back().width, 2u) << "only the two key frames are assembled";
}
TEST_F(PointCloudAssemblerTest, DropsCloudsWithoutTheFixedFrame)
{
// No TF at all, so the assembler cannot place the clouds relative to each other.
addNode(std::make_shared<rtabmap_util::PointCloudAssembler>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("max_clouds", 2),
rclcpp::Parameter("fixed_frame_id", "odom"),
rclcpp::Parameter("wait_for_transform", 0.0)})));
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
collect<sensor_msgs::msg::PointCloud2>("assembled_cloud");
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
ASSERT_TRUE(waitForSubscriber(pub));
pub->publish(makeXYZCloud("lidar", 1000.0, {{1.0f, 0.0f, 0.0f}}));
pub->publish(makeXYZCloud("lidar", 1000.1, {{1.0f, 0.0f, 0.0f}}));
spinFor(std::chrono::milliseconds(500));
EXPECT_TRUE(out->empty());
}
rtabmap_util/test/test_point_cloud_xyz.cpp
+344
View File
@@ -0,0 +1,344 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#include "node_test_utils.hpp"
#include "msg_builders.hpp"
#include <rtabmap_util/point_cloud_xyz.hpp>
#include <stereo_msgs/msg/disparity_image.hpp>
#include <cmath>
using namespace rtabmap_util_test;
namespace {
::testing::Environment * const kEnv = registerRclcppEnvironment();
constexpr int kWidth = 16;
constexpr int kHeight = 16;
constexpr double kFx = 100.0;
/// A depth image where every pixel is at @p meters.
sensor_msgs::msg::Image makeDepth(
double stamp, float meters,
const std::string & encoding = sensor_msgs::image_encodings::TYPE_32FC1)
{
cv::Mat image;
if(encoding == sensor_msgs::image_encodings::TYPE_32FC1)
{
image = cv::Mat(kHeight, kWidth, CV_32FC1, cv::Scalar(meters));
}
else
{
image = cv::Mat(kHeight, kWidth, CV_16UC1, cv::Scalar(uint16_t(meters*1000.0f)));
}
return makeImage("camera_link", stamp, image, encoding);
}
/// A disparity image where every pixel carries @p disparity, so depth = f*t/disparity.
stereo_msgs::msg::DisparityImage makeDisparity(
double stamp, float disparity, float focal = float(kFx), float baseline = 0.1f)
{
stereo_msgs::msg::DisparityImage msg;
msg.header.frame_id = "camera_link";
msg.header.stamp = stampOf(stamp);
msg.f = focal;
msg.t = baseline;
msg.min_disparity = 1.0f;
msg.max_disparity = 100.0f;
msg.image = makeImage("camera_link", stamp,
cv::Mat(kHeight, kWidth, CV_32FC1, cv::Scalar(disparity)),
sensor_msgs::image_encodings::TYPE_32FC1);
return msg;
}
/// The same, in the 16SC1 fixed-point form where the stored value is 16*disparity.
stereo_msgs::msg::DisparityImage makeDisparity16SC1(double stamp, float disparity)
{
stereo_msgs::msg::DisparityImage msg = makeDisparity(stamp, disparity);
msg.image = makeImage("camera_link", stamp,
cv::Mat(kHeight, kWidth, CV_16SC1, cv::Scalar(short(disparity*16.0f))),
sensor_msgs::image_encodings::TYPE_16SC1);
return msg;
}
bool hasField(const sensor_msgs::msg::PointCloud2 & cloud, const std::string & name)
{
for(size_t i=0; i<cloud.fields.size(); ++i)
{
if(cloud.fields[i].name == name) { return true; }
}
return false;
}
} // namespace
class PointCloudXYZTest : public NodeTest
{
protected:
void start(const std::vector<rclcpp::Parameter> & overrides = {})
{
addNode(std::make_shared<rtabmap_util::PointCloudXYZ>(
rclcpp::NodeOptions().parameter_overrides(overrides)));
out_ = collect<sensor_msgs::msg::PointCloud2>("cloud");
depthPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>("depth/camera_info", 10);
ASSERT_TRUE(waitForSubscriber(depthPub_));
ASSERT_TRUE(waitForSubscriber(infoPub_));
ASSERT_TRUE(waitForPublisher(out_->subscription));
}
/// Publishes a synchronized depth + camera_info pair.
void publishFrame(double stamp, float meters,
const std::string & encoding = sensor_msgs::image_encodings::TYPE_32FC1)
{
depthPub_->publish(makeDepth(stamp, meters, encoding));
infoPub_->publish(makeCameraInfo("camera_link", stamp, kWidth, kHeight, 0.0, kFx));
}
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depthPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub_;
};
TEST_F(PointCloudXYZTest, ProjectsDepthIntoACloud)
{
start();
publishFrame(1000.0, 2.0f);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight))
<< "one point per pixel at decimation 1";
EXPECT_EQ(cloud.header.frame_id, "camera_link")
<< "the cloud takes the depth image's frame";
// The principal-point pixel projects straight ahead at the measured depth.
const size_t center = size_t(kHeight/2) * kWidth + kWidth/2;
EXPECT_NEAR(readXYZ(cloud, center).z, 2.0f, 1e-3);
}
TEST_F(PointCloudXYZTest, Accepts16UC1Millimeters)
{
start();
publishFrame(1000.0, 2.0f, sensor_msgs::image_encodings::TYPE_16UC1);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
const size_t center = size_t(kHeight/2) * kWidth + kWidth/2;
EXPECT_NEAR(readXYZ(out_->back(), center).z, 2.0f, 1e-3)
<< "millimeter depth must be converted to meters";
}
TEST_F(PointCloudXYZTest, RejectsUnsupportedEncoding)
{
start();
depthPub_->publish(makeImage("camera_link", 1000.0,
cv::Mat(kHeight, kWidth, CV_8UC3, cv::Scalar(1,2,3)), "bgr8"));
infoPub_->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx));
spinFor(std::chrono::milliseconds(400));
EXPECT_TRUE(out_->empty()) << "only 32FC1, 16UC1 and mono16 depth are supported";
}
TEST_F(PointCloudXYZTest, DecimationReducesThePointCount)
{
start({rclcpp::Parameter("decimation", 2)});
publishFrame(1000.0, 2.0f);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_EQ(out_->back().width * out_->back().height, uint32_t(kWidth*kHeight)/4)
<< "decimation 2 keeps one pixel in four";
}
TEST_F(PointCloudXYZTest, MaxDepthMarksFarPointsInvalid)
{
// cloudFromDepth keeps the cloud organized: points outside the depth range become
// NaN rather than disappearing, so the point count is unchanged.
start({rclcpp::Parameter("max_depth", 1.0)});
publishFrame(1000.0, 5.0f); // beyond the limit
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight))
<< "the cloud stays organized";
EXPECT_TRUE(std::isnan(readXYZ(cloud, 0).z)) << "every point is past max_depth";
EXPECT_TRUE(std::isnan(readXYZ(cloud, kWidth*kHeight-1).z));
}
TEST_F(PointCloudXYZTest, WithinMaxDepthPointsStayValid)
{
start({rclcpp::Parameter("max_depth", 10.0)});
publishFrame(1000.0, 5.0f); // inside the limit
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
const size_t center = size_t(kHeight/2) * kWidth + kWidth/2;
EXPECT_FALSE(std::isnan(readXYZ(out_->back(), center).z));
EXPECT_NEAR(readXYZ(out_->back(), center).z, 5.0f, 1e-3);
}
TEST_F(PointCloudXYZTest, MinDepthMarksNearPointsInvalid)
{
start({rclcpp::Parameter("min_depth", 3.0)});
publishFrame(1000.0, 1.0f); // closer than the limit
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_TRUE(std::isnan(readXYZ(out_->back(), 0).z))
<< "every point is nearer than min_depth";
}
TEST_F(PointCloudXYZTest, FilterNaNsRemovesInvalidPoints)
{
// With filter_nans the invalid points are dropped instead, giving an unorganized
// cloud that is empty when nothing is in range.
start({rclcpp::Parameter("max_depth", 1.0),
rclcpp::Parameter("filter_nans", true)});
publishFrame(1000.0, 5.0f);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_EQ(out_->back().width * out_->back().height, 0u)
<< "filter_nans must remove the out-of-range points";
}
TEST_F(PointCloudXYZTest, NormalKAddsNormalFields)
{
start({rclcpp::Parameter("normal_k", 10)});
publishFrame(1000.0, 2.0f);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_TRUE(hasField(out_->back(), "normal_x"))
<< "asking for normals must change the point type";
EXPECT_TRUE(hasField(out_->back(), "normal_z"));
}
TEST_F(PointCloudXYZTest, NoNormalFieldsByDefault)
{
start();
publishFrame(1000.0, 2.0f);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_FALSE(hasField(out_->back(), "normal_x"));
}
TEST_F(PointCloudXYZTest, StaysSilentWithoutASubscriber)
{
// The projection is skipped entirely when nobody wants the cloud.
addNode(std::make_shared<rtabmap_util::PointCloudXYZ>(rclcpp::NodeOptions()));
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depthPub =
helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub =
helper()->create_publisher<sensor_msgs::msg::CameraInfo>("depth/camera_info", 10);
ASSERT_TRUE(waitForSubscriber(depthPub));
depthPub->publish(makeDepth(1000.0, 2.0f));
infoPub->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx));
spinFor(std::chrono::milliseconds(300));
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> late =
collect<sensor_msgs::msg::PointCloud2>("cloud");
spinFor(std::chrono::milliseconds(200));
EXPECT_TRUE(late->empty());
}
//============================================================================
// disparity/image + disparity/camera_info
//============================================================================
class PointCloudXYZDisparityTest : public NodeTest
{
protected:
void start(const std::vector<rclcpp::Parameter> & overrides = {})
{
addNode(std::make_shared<rtabmap_util::PointCloudXYZ>(
rclcpp::NodeOptions().parameter_overrides(overrides)));
out_ = collect<sensor_msgs::msg::PointCloud2>("cloud");
dispPub_ = helper()->create_publisher<stereo_msgs::msg::DisparityImage>(
"disparity/image", 10);
infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>(
"disparity/camera_info", 10);
ASSERT_TRUE(waitForSubscriber(dispPub_));
ASSERT_TRUE(waitForSubscriber(infoPub_));
ASSERT_TRUE(waitForPublisher(out_->subscription));
}
void publishFrame(double stamp, const stereo_msgs::msg::DisparityImage & disparity)
{
dispPub_->publish(disparity);
infoPub_->publish(makeCameraInfo("camera_link", stamp, kWidth, kHeight, 0.0, kFx));
}
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr dispPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub_;
};
TEST_F(PointCloudXYZDisparityTest, ProjectsDisparityIntoACloud)
{
start();
publishFrame(1000.0, makeDisparity(1000.0, 5.0f)); // depth = f*t/d = 100*0.1/5 = 2 m
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight));
EXPECT_EQ(cloud.header.frame_id, "camera_link")
<< "the cloud takes the disparity image's frame";
const size_t center = size_t(kHeight/2) * kWidth + kWidth/2;
EXPECT_NEAR(readXYZ(cloud, center).z, 2.0f, 1e-3);
}
TEST_F(PointCloudXYZDisparityTest, Accepts16SC1FixedPointDisparity)
{
// The 16-bit form stores 16*disparity, so the same 5 px must still give 2 m.
start();
publishFrame(1000.0, makeDisparity16SC1(1000.0, 5.0f));
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
const size_t center = size_t(kHeight/2) * kWidth + kWidth/2;
EXPECT_NEAR(readXYZ(out_->back(), center).z, 2.0f, 1e-3);
}
TEST_F(PointCloudXYZDisparityTest, RejectsUnsupportedDisparityEncoding)
{
start();
stereo_msgs::msg::DisparityImage msg = makeDisparity(1000.0, 5.0f);
msg.image = makeImage("camera_link", 1000.0,
cv::Mat(kHeight, kWidth, CV_8UC1, cv::Scalar(5)), "mono8");
publishFrame(1000.0, msg);
spinFor(std::chrono::milliseconds(400));
EXPECT_TRUE(out_->empty()) << "only 32FC1 and 16SC1 disparity are supported";
}
TEST_F(PointCloudXYZDisparityTest, MaxDepthMarksFarPointsInvalid)
{
// Like the depth path, cloudFromDisparity keeps the cloud organized and turns the
// out-of-range points into NaN instead of removing them.
start({rclcpp::Parameter("max_depth", 1.0)});
publishFrame(1000.0, makeDisparity(1000.0, 5.0f)); // 2 m, beyond the limit
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight));
EXPECT_TRUE(std::isnan(readXYZ(cloud, size_t(kHeight/2)*kWidth + kWidth/2).z));
}
TEST_F(PointCloudXYZDisparityTest, FilterNaNsRemovesInvalidPoints)
{
start({rclcpp::Parameter("max_depth", 1.0),
rclcpp::Parameter("filter_nans", true)});
publishFrame(1000.0, makeDisparity(1000.0, 5.0f));
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_EQ(out_->back().width * out_->back().height, 0u);
}
TEST_F(PointCloudXYZDisparityTest, DecimationReducesThePointCount)
{
start({rclcpp::Parameter("decimation", 2)});
publishFrame(1000.0, makeDisparity(1000.0, 5.0f));
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_EQ(out_->back().width * out_->back().height, uint32_t(kWidth*kHeight)/4);
}
rtabmap_util/test/test_point_cloud_xyzrgb.cpp
+531
View File
@@ -0,0 +1,531 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#include "node_test_utils.hpp"
#include "msg_builders.hpp"
#include <rtabmap_util/point_cloud_xyzrgb.hpp>
#include <stereo_msgs/msg/disparity_image.hpp>
#include <cmath>
using namespace rtabmap_util_test;
namespace {
::testing::Environment * const kEnv = registerRclcppEnvironment();
constexpr int kWidth = 16;
constexpr int kHeight = 16;
constexpr double kFx = 100.0;
constexpr int kCenter = (kHeight/2) * kWidth + kWidth/2;
/// The color every synthetic RGB image is painted with, in OpenCV's BGR order.
const cv::Scalar kColor(10, 20, 30);
sensor_msgs::msg::Image makeRgb(double stamp, const std::string & encoding = "bgr8")
{
if(encoding == "mono8")
{
return makeImage("camera_link", stamp,
cv::Mat(kHeight, kWidth, CV_8UC1, cv::Scalar(128)), encoding);
}
return makeImage("camera_link", stamp,
cv::Mat(kHeight, kWidth, CV_8UC3, kColor), encoding);
}
sensor_msgs::msg::Image makeDepth(double stamp, float meters,
const std::string & encoding = sensor_msgs::image_encodings::TYPE_32FC1)
{
cv::Mat image = encoding == sensor_msgs::image_encodings::TYPE_32FC1
? cv::Mat(kHeight, kWidth, CV_32FC1, cv::Scalar(meters))
: cv::Mat(kHeight, kWidth, CV_16UC1, cv::Scalar(uint16_t(meters*1000.0f)));
return makeImage("camera_link", stamp, image, encoding);
}
/// A disparity image where every pixel carries @p disparity, so depth = f*t/disparity.
stereo_msgs::msg::DisparityImage makeDisparity(double stamp, float disparity,
float focal = float(kFx), float baseline = 0.1f)
{
stereo_msgs::msg::DisparityImage msg;
msg.header.frame_id = "camera_link";
msg.header.stamp = stampOf(stamp);
msg.f = focal;
msg.t = baseline;
msg.min_disparity = 1.0f;
msg.max_disparity = 100.0f;
msg.image = makeImage("camera_link", stamp,
cv::Mat(kHeight, kWidth, CV_32FC1, cv::Scalar(disparity)),
sensor_msgs::image_encodings::TYPE_32FC1);
return msg;
}
bool hasField(const sensor_msgs::msg::PointCloud2 & cloud, const std::string & name)
{
for(size_t i=0; i<cloud.fields.size(); ++i)
{
if(cloud.fields[i].name == name) { return true; }
}
return false;
}
/// Reads the packed "rgb" float field of a point as (r,g,b).
cv::Vec3b readRGB(const sensor_msgs::msg::PointCloud2 & cloud, size_t index)
{
uint32_t offset = 16;
for(size_t i=0; i<cloud.fields.size(); ++i)
{
if(cloud.fields[i].name == "rgb") { offset = cloud.fields[i].offset; }
}
uint32_t packed = 0;
memcpy(&packed, &cloud.data[index * cloud.point_step + offset], 4);
return cv::Vec3b(
uint8_t((packed >> 16) & 0xFF),
uint8_t((packed >> 8) & 0xFF),
uint8_t(packed & 0xFF));
}
} // namespace
class PointCloudXYZRGBTest : public NodeTest
{
protected:
void start(const std::vector<rclcpp::Parameter> & overrides = {})
{
addNode(std::make_shared<rtabmap_util::PointCloudXYZRGB>(
rclcpp::NodeOptions().parameter_overrides(overrides)));
out_ = collect<sensor_msgs::msg::PointCloud2>("cloud");
rgbPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
depthPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>("rgb/camera_info", 10);
ASSERT_TRUE(waitForSubscriber(rgbPub_));
ASSERT_TRUE(waitForSubscriber(depthPub_));
ASSERT_TRUE(waitForSubscriber(infoPub_));
ASSERT_TRUE(waitForPublisher(out_->subscription));
}
/// Publishes a synchronized rgb + depth + camera_info triple.
void publishFrame(double stamp, float meters,
const std::string & depthEncoding = sensor_msgs::image_encodings::TYPE_32FC1,
const std::string & rgbEncoding = "bgr8")
{
rgbPub_->publish(makeRgb(stamp, rgbEncoding));
depthPub_->publish(makeDepth(stamp, meters, depthEncoding));
infoPub_->publish(makeCameraInfo("camera_link", stamp, kWidth, kHeight, 0.0, kFx));
}
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgbPub_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depthPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub_;
};
//============================================================================
// rgb + depth + camera_info
//============================================================================
TEST_F(PointCloudXYZRGBTest, ProjectsRgbAndDepthIntoAColoredCloud)
{
start();
publishFrame(1000.0, 2.0f);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight))
<< "one point per pixel at decimation 1";
EXPECT_EQ(cloud.header.frame_id, "camera_link")
<< "the cloud takes the RGB image's frame";
EXPECT_TRUE(hasField(cloud, "rgb")) << "the whole point of this node";
EXPECT_NEAR(readXYZ(cloud, kCenter).z, 2.0f, 1e-3);
// The RGB image is uniform, so every point carries the same color. cv_bridge hands
// the node a bgr8 image, which reaches the cloud as r=30, g=20, b=10.
const cv::Vec3b rgb = readRGB(cloud, kCenter);
EXPECT_EQ(int(rgb[0]), 30);
EXPECT_EQ(int(rgb[1]), 20);
EXPECT_EQ(int(rgb[2]), 10);
}
TEST_F(PointCloudXYZRGBTest, Accepts16UC1Millimeters)
{
start();
publishFrame(1000.0, 2.0f, sensor_msgs::image_encodings::TYPE_16UC1);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_NEAR(readXYZ(out_->back(), kCenter).z, 2.0f, 1e-3)
<< "millimeter depth must be converted to meters";
}
TEST_F(PointCloudXYZRGBTest, AcceptsMono8Color)
{
start();
publishFrame(1000.0, 2.0f, sensor_msgs::image_encodings::TYPE_32FC1, "mono8");
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
const cv::Vec3b rgb = readRGB(out_->back(), kCenter);
EXPECT_EQ(int(rgb[0]), 128) << "a grey image gives grey points";
EXPECT_EQ(int(rgb[1]), 128);
EXPECT_EQ(int(rgb[2]), 128);
}
TEST_F(PointCloudXYZRGBTest, RejectsUnsupportedDepthEncoding)
{
start();
rgbPub_->publish(makeRgb(1000.0));
depthPub_->publish(makeImage("camera_link", 1000.0,
cv::Mat(kHeight, kWidth, CV_8UC3, kColor), "bgr8"));
infoPub_->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx));
spinFor(std::chrono::milliseconds(400));
EXPECT_TRUE(out_->empty()) << "only 32FC1, 16UC1 and mono16 depth are supported";
}
TEST_F(PointCloudXYZRGBTest, DecimationReducesThePointCount)
{
start({rclcpp::Parameter("decimation", 2)});
publishFrame(1000.0, 2.0f);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_EQ(out_->back().width * out_->back().height, uint32_t(kWidth*kHeight)/4)
<< "decimation 2 keeps one pixel in four";
}
TEST_F(PointCloudXYZRGBTest, RoiRatiosCropTheCloud)
{
// A quarter off each side of a 16x16 image leaves an 8x8 window.
start({rclcpp::Parameter("roi_ratios", std::string("0.25 0.25 0.25 0.25"))});
publishFrame(1000.0, 2.0f);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_EQ(out_->back().width * out_->back().height, 64u);
}
TEST_F(PointCloudXYZRGBTest, MaxDepthMarksFarPointsInvalid)
{
// cloudFromDepthRGB keeps the cloud organized: out-of-range points become NaN
// rather than disappearing, so the point count is unchanged.
start({rclcpp::Parameter("max_depth", 1.0)});
publishFrame(1000.0, 5.0f);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_EQ(out_->back().width * out_->back().height, uint32_t(kWidth*kHeight));
EXPECT_TRUE(std::isnan(readXYZ(out_->back(), kCenter).z));
}
TEST_F(PointCloudXYZRGBTest, MinDepthMarksNearPointsInvalid)
{
start({rclcpp::Parameter("min_depth", 3.0)});
publishFrame(1000.0, 1.0f);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_TRUE(std::isnan(readXYZ(out_->back(), kCenter).z));
}
TEST_F(PointCloudXYZRGBTest, FilterNaNsRemovesInvalidPoints)
{
start({rclcpp::Parameter("max_depth", 1.0),
rclcpp::Parameter("filter_nans", true)});
publishFrame(1000.0, 5.0f);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_EQ(out_->back().width * out_->back().height, 0u)
<< "filter_nans must remove the out-of-range points";
}
TEST_F(PointCloudXYZRGBTest, NormalKAddsNormalFields)
{
start({rclcpp::Parameter("normal_k", 10)});
publishFrame(1000.0, 2.0f);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_TRUE(hasField(out_->back(), "normal_x"))
<< "asking for normals must change the point type";
EXPECT_TRUE(hasField(out_->back(), "rgb")) << "and must keep the color";
}
TEST_F(PointCloudXYZRGBTest, NoNormalFieldsByDefault)
{
start();
publishFrame(1000.0, 2.0f);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_FALSE(hasField(out_->back(), "normal_x"));
}
TEST_F(PointCloudXYZRGBTest, VoxelSizeThinsTheCloud)
{
// A frontal plane at 2 m spans about 0.32 m across a 16-pixel image at fx=100, so a
// 0.1 m voxel grid collapses the 256 points into far fewer.
start({rclcpp::Parameter("voxel_size", 0.1)});
publishFrame(1000.0, 2.0f);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
const uint32_t points = out_->back().width * out_->back().height;
EXPECT_GT(points, 0u);
EXPECT_LT(points, uint32_t(kWidth*kHeight));
}
TEST_F(PointCloudXYZRGBTest, StaysSilentWithoutASubscriber)
{
// The projection is skipped entirely when nobody wants the cloud.
addNode(std::make_shared<rtabmap_util::PointCloudXYZRGB>(rclcpp::NodeOptions()));
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgbPub =
helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depthPub =
helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub =
helper()->create_publisher<sensor_msgs::msg::CameraInfo>("rgb/camera_info", 10);
ASSERT_TRUE(waitForSubscriber(rgbPub));
ASSERT_TRUE(waitForSubscriber(depthPub));
rgbPub->publish(makeRgb(1000.0));
depthPub->publish(makeDepth(1000.0, 2.0f));
infoPub->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx));
spinFor(std::chrono::milliseconds(300));
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> late =
collect<sensor_msgs::msg::PointCloud2>("cloud");
spinFor(std::chrono::milliseconds(200));
EXPECT_TRUE(late->empty());
}
//============================================================================
// rgbd_image
//============================================================================
class PointCloudXYZRGBRgbdTest : public NodeTest
{
protected:
void start(const std::vector<rclcpp::Parameter> & overrides = {})
{
addNode(std::make_shared<rtabmap_util::PointCloudXYZRGB>(
rclcpp::NodeOptions().parameter_overrides(overrides)));
out_ = collect<sensor_msgs::msg::PointCloud2>("cloud");
rgbdPub_ = helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
ASSERT_TRUE(waitForSubscriber(rgbdPub_));
ASSERT_TRUE(waitForPublisher(out_->subscription));
}
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdPub_;
};
TEST_F(PointCloudXYZRGBRgbdTest, ProjectsAnRgbdImage)
{
start();
rgbdPub_->publish(makeRGBDImage("camera_link", 1000.0, kWidth, kHeight));
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight));
EXPECT_EQ(cloud.header.frame_id, "camera_link");
EXPECT_TRUE(hasField(cloud, "rgb"));
EXPECT_NEAR(readXYZ(cloud, kCenter).z, 1.5f, 1e-3)
<< "makeRGBDImage() fills the depth image with 1500 mm";
}
TEST_F(PointCloudXYZRGBRgbdTest, IgnoresAnInvalidRgbdImage)
{
// isValid() is false without any image data, and nothing must be published.
start();
rtabmap_msgs::msg::RGBDImage msg;
msg.header.frame_id = "camera_link";
msg.header.stamp = stampOf(1000.0);
rgbdPub_->publish(msg);
spinFor(std::chrono::milliseconds(400));
EXPECT_TRUE(out_->empty());
}
TEST_F(PointCloudXYZRGBRgbdTest, PublishesAnEmptyCloudForAColorOnlyRgbdImage)
{
// Depth is optional in an RGBDImage, so color alone must not be treated as a broken
// message: there is simply nothing to project.
start();
rtabmap_msgs::msg::RGBDImage msg = makeRGBDImage("camera_link", 1000.0, kWidth, kHeight);
msg.depth = sensor_msgs::msg::Image();
rgbdPub_->publish(msg);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
EXPECT_EQ(out_->back().width * out_->back().height, 0u);
}
TEST_F(PointCloudXYZRGBRgbdTest, ProjectsAStereoRgbdImage)
{
// A stereo pair in an RGBDImage is dense-matched instead of read as depth.
start();
rgbdPub_->publish(makeStereoRGBDImage("camera_link", 1000.0, 160, 120));
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
EXPECT_EQ(out_->back().width * out_->back().height, uint32_t(160*120));
EXPECT_TRUE(hasField(out_->back(), "rgb"));
}
//============================================================================
// left/image + disparity + left/camera_info
//============================================================================
class PointCloudXYZRGBDisparityTest : public NodeTest
{
protected:
void start(const std::vector<rclcpp::Parameter> & overrides = {})
{
addNode(std::make_shared<rtabmap_util::PointCloudXYZRGB>(
rclcpp::NodeOptions().parameter_overrides(overrides)));
out_ = collect<sensor_msgs::msg::PointCloud2>("cloud");
leftPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("left/image", 10);
dispPub_ = helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>("left/camera_info", 10);
ASSERT_TRUE(waitForSubscriber(leftPub_));
ASSERT_TRUE(waitForSubscriber(dispPub_));
ASSERT_TRUE(waitForSubscriber(infoPub_));
ASSERT_TRUE(waitForPublisher(out_->subscription));
}
void publishFrame(double stamp, float disparity)
{
leftPub_->publish(makeRgb(stamp));
dispPub_->publish(makeDisparity(stamp, disparity));
infoPub_->publish(makeCameraInfo("camera_link", stamp, kWidth, kHeight, 0.0, kFx));
}
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr leftPub_;
rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr dispPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub_;
};
TEST_F(PointCloudXYZRGBDisparityTest, ProjectsDisparityIntoAColoredCloud)
{
start();
publishFrame(1000.0, 5.0f); // depth = f*t/d = 100*0.1/5 = 2 m
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight));
EXPECT_EQ(cloud.header.frame_id, "camera_link")
<< "the cloud takes the disparity image's frame";
EXPECT_TRUE(hasField(cloud, "rgb"));
EXPECT_NEAR(readXYZ(cloud, kCenter).z, 2.0f, 1e-3);
const cv::Vec3b rgb = readRGB(cloud, kCenter);
EXPECT_EQ(int(rgb[0]), 30);
EXPECT_EQ(int(rgb[2]), 10);
}
TEST_F(PointCloudXYZRGBDisparityTest, RejectsUnsupportedDisparityEncoding)
{
start();
stereo_msgs::msg::DisparityImage msg = makeDisparity(1000.0, 5.0f);
msg.image = makeImage("camera_link", 1000.0,
cv::Mat(kHeight, kWidth, CV_8UC1, cv::Scalar(5)), "mono8");
leftPub_->publish(makeRgb(1000.0));
dispPub_->publish(msg);
infoPub_->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx));
spinFor(std::chrono::milliseconds(400));
EXPECT_TRUE(out_->empty()) << "only 32FC1 and 16SC1 disparity are supported";
}
TEST_F(PointCloudXYZRGBDisparityTest, MaxDepthMarksFarPointsInvalid)
{
start({rclcpp::Parameter("max_depth", 1.0)});
publishFrame(1000.0, 5.0f); // 2 m, beyond the limit
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
EXPECT_TRUE(std::isnan(readXYZ(out_->back(), kCenter).z));
}
//============================================================================
// left/image + right/image + both camera_infos
//============================================================================
class PointCloudXYZRGBStereoTest : public NodeTest
{
protected:
static constexpr int kStereoWidth = 160;
static constexpr int kStereoHeight = 120;
static constexpr float kBaseline = 0.12f;
static constexpr int kDisparity = 6; // depth = fx*baseline/d = 100*0.12/6 = 2 m
void start(const std::vector<rclcpp::Parameter> & overrides = {})
{
addNode(std::make_shared<rtabmap_util::PointCloudXYZRGB>(
rclcpp::NodeOptions().parameter_overrides(overrides)));
out_ = collect<sensor_msgs::msg::PointCloud2>("cloud");
leftPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("left/image", 10);
rightPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("right/image", 10);
leftInfoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>("left/camera_info", 10);
rightInfoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>("right/camera_info", 10);
ASSERT_TRUE(waitForSubscriber(leftPub_));
ASSERT_TRUE(waitForSubscriber(rightPub_));
ASSERT_TRUE(waitForSubscriber(leftInfoPub_));
ASSERT_TRUE(waitForSubscriber(rightInfoPub_));
ASSERT_TRUE(waitForPublisher(out_->subscription));
}
/**
* Publishes a textured pair whose true disparity is kDisparity everywhere: the right
* image is the left one shifted, which is what a plane at a constant depth looks like.
*/
void publishFrame(double stamp)
{
cv::Mat left(kStereoHeight, kStereoWidth + kDisparity, CV_8UC1);
cv::RNG rng(42);
rng.fill(left, cv::RNG::UNIFORM, 0, 256);
cv::Mat leftBgr;
cv::cvtColor(cv::Mat(left, cv::Rect(0, 0, kStereoWidth, kStereoHeight)),
leftBgr, cv::COLOR_GRAY2BGR);
cv::Mat right(left, cv::Rect(kDisparity, 0, kStereoWidth, kStereoHeight));
leftPub_->publish(makeImage("camera_link", stamp, leftBgr, "bgr8"));
rightPub_->publish(makeImage("camera_link", stamp, right.clone(), "mono8"));
leftInfoPub_->publish(makeCameraInfo(
"camera_link", stamp, kStereoWidth, kStereoHeight, 0.0, kFx));
rightInfoPub_->publish(makeCameraInfo(
"camera_link", stamp, kStereoWidth, kStereoHeight, -kFx*kBaseline, kFx));
}
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr leftPub_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rightPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr leftInfoPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rightInfoPub_;
};
TEST_F(PointCloudXYZRGBStereoTest, MatchesAStereoPairIntoAColoredCloud)
{
start({rclcpp::Parameter("StereoBM/NumDisparities", std::string("16")),
rclcpp::Parameter("StereoBM/BlockSize", std::string("9"))});
publishFrame(1000.0);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
EXPECT_EQ(cloud.width * cloud.height, uint32_t(kStereoWidth*kStereoHeight))
<< "the cloud stays organized, one point per pixel";
EXPECT_EQ(cloud.header.frame_id, "camera_link")
<< "the cloud takes the left image's frame";
EXPECT_TRUE(hasField(cloud, "rgb"));
// The pair is a shifted copy of itself, so the whole matched area sits at one depth.
const size_t center = size_t(kStereoHeight/2) * kStereoWidth + kStereoWidth/2;
EXPECT_NEAR(readXYZ(cloud, center).z, 2.0f, 0.2f);
}
TEST_F(PointCloudXYZRGBStereoTest, RejectsUnsupportedStereoEncoding)
{
start();
leftPub_->publish(makeImage("camera_link", 1000.0,
cv::Mat(kStereoHeight, kStereoWidth, CV_32FC1, cv::Scalar(1.0f)), "32FC1"));
rightPub_->publish(makeImage("camera_link", 1000.0,
cv::Mat(kStereoHeight, kStereoWidth, CV_8UC1, cv::Scalar(0)), "mono8"));
leftInfoPub_->publish(makeCameraInfo(
"camera_link", 1000.0, kStereoWidth, kStereoHeight, 0.0, kFx));
rightInfoPub_->publish(makeCameraInfo(
"camera_link", 1000.0, kStereoWidth, kStereoHeight, -kFx*kBaseline, kFx));
spinFor(std::chrono::milliseconds(400));
EXPECT_TRUE(out_->empty()) << "only 8-bit and mono16 stereo images are supported";
}
rtabmap_util/test/test_pointcloud_to_depthimage.cpp
+370
View File
@@ -0,0 +1,370 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#include "node_test_utils.hpp"
#include "msg_builders.hpp"
#include <rtabmap_util/pointcloud_to_depthimage.hpp>
#include <tf2_msgs/msg/tf_message.hpp>
using namespace rtabmap_util_test;
namespace {
::testing::Environment * const kEnv = registerRclcppEnvironment();
constexpr int kWidth = 16;
constexpr int kHeight = 16;
constexpr double kFx = 100.0;
float pixel32f(const sensor_msgs::msg::Image & img, int row, int col)
{
return *reinterpret_cast<const float *>(&img.data[row * img.step + col * sizeof(float)]);
}
uint16_t pixel16u(const sensor_msgs::msg::Image & img, int row, int col)
{
return *reinterpret_cast<const uint16_t *>(&img.data[row * img.step + col * sizeof(uint16_t)]);
}
} // namespace
class PointCloudToDepthImageTest : public NodeTest
{
protected:
void start(const std::vector<rclcpp::Parameter> & overrides = {}, bool withTf = true)
{
addNode(std::make_shared<rtabmap_util::PointCloudToDepthImage>(
rclcpp::NodeOptions().parameter_overrides(overrides)));
if(withTf)
{
publishStaticTf("camera_link", "lidar");
}
image32_ = collect<sensor_msgs::msg::Image>("image");
image16_ = collect<sensor_msgs::msg::Image>("image_raw");
cloudPub_ = helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>("camera_info", 10);
ASSERT_TRUE(waitForSubscriber(cloudPub_));
ASSERT_TRUE(waitForSubscriber(infoPub_));
ASSERT_TRUE(waitForPublisher(image32_->subscription));
}
/// Publishes a cloud and its camera info with identical stamps.
void publishFrame(double stamp, const std::vector<cv::Point3f> & points)
{
cloudPub_->publish(makeXYZCloud("lidar", stamp, points));
infoPub_->publish(makeCameraInfo("camera_link", stamp, kWidth, kHeight, 0.0, kFx));
}
/// A block of points straight ahead of the optical axis at @p depth meters.
static std::vector<cv::Point3f> blockAt(float depth)
{
std::vector<cv::Point3f> points;
for(int i=-2; i<=2; ++i)
{
for(int j=-2; j<=2; ++j)
{
points.push_back(cv::Point3f(0.01f*i, 0.01f*j, depth));
}
}
return points;
}
std::shared_ptr<Collector<sensor_msgs::msg::Image>> image32_;
std::shared_ptr<Collector<sensor_msgs::msg::Image>> image16_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub_;
};
TEST_F(PointCloudToDepthImageTest, ProjectsACloudIntoADepthImage)
{
start();
publishFrame(1000.0, blockAt(2.0f));
ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) << "no depth image";
const sensor_msgs::msg::Image & img = image32_->back();
EXPECT_EQ(img.encoding, sensor_msgs::image_encodings::TYPE_32FC1);
EXPECT_EQ(img.width, uint32_t(kWidth));
EXPECT_EQ(img.height, uint32_t(kHeight));
EXPECT_EQ(img.header.frame_id, "camera_link")
<< "the depth image belongs to the camera, not the cloud";
// The points sit on the optical axis, so they land on the principal point.
EXPECT_NEAR(pixel32f(img, kHeight/2, kWidth/2), 2.0f, 1e-3);
// A corner sees nothing.
EXPECT_FLOAT_EQ(pixel32f(img, 0, 0), 0.0f);
}
TEST_F(PointCloudToDepthImageTest, PublishesMillimetersOnImageRaw)
{
start();
publishFrame(1000.0, blockAt(2.0f));
ASSERT_TRUE(spinUntil([&]() { return !image16_->empty(); }));
const sensor_msgs::msg::Image & img = image16_->back();
EXPECT_EQ(img.encoding, sensor_msgs::image_encodings::TYPE_16UC1);
EXPECT_EQ(pixel16u(img, kHeight/2, kWidth/2), 2000) << "2 m expressed in millimeters";
}
TEST_F(PointCloudToDepthImageTest, EmptyCloudGivesAnAllZeroImage)
{
start();
publishFrame(1000.0, {});
ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); }))
<< "an empty cloud must still produce an image, not a dropped frame";
const sensor_msgs::msg::Image & img = image32_->back();
EXPECT_EQ(img.width, uint32_t(kWidth));
for(int row=0; row<kHeight; ++row)
{
for(int col=0; col<kWidth; ++col)
{
ASSERT_FLOAT_EQ(pixel32f(img, row, col), 0.0f) << "at " << row << "," << col;
}
}
}
TEST_F(PointCloudToDepthImageTest, DecimationShrinksTheImageAndScalesTheModel)
{
start({rclcpp::Parameter("decimation", 2)});
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> infoOut =
collect<sensor_msgs::msg::CameraInfo>("image/camera_info");
ASSERT_TRUE(waitForPublisher(infoOut->subscription));
publishFrame(1000.0, blockAt(2.0f));
ASSERT_TRUE(spinUntil([&]() { return !image32_->empty() && !infoOut->empty(); }));
EXPECT_EQ(image32_->back().width, uint32_t(kWidth)/2);
EXPECT_EQ(image32_->back().height, uint32_t(kHeight)/2);
EXPECT_NEAR(infoOut->back().p[0], kFx/2.0, 1e-6)
<< "the published camera info must match the decimated image";
EXPECT_EQ(infoOut->back().width, uint32_t(kWidth)/2);
}
TEST_F(PointCloudToDepthImageTest, FailsWithoutTheCloudToCameraTransform)
{
start({rclcpp::Parameter("wait_for_transform", 0.0)}, /*withTf=*/false);
publishFrame(1000.0, blockAt(2.0f));
spinFor(std::chrono::milliseconds(400));
EXPECT_TRUE(image32_->empty())
<< "without TF the cloud cannot be placed in the camera frame";
}
TEST_F(PointCloudToDepthImageTest, StaysSilentWithoutASubscriber)
{
// The projection is skipped entirely when neither image topic is subscribed.
addNode(std::make_shared<rtabmap_util::PointCloudToDepthImage>(rclcpp::NodeOptions()));
publishStaticTf("camera_link", "lidar");
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub =
helper()->create_publisher<sensor_msgs::msg::CameraInfo>("camera_info", 10);
ASSERT_TRUE(waitForSubscriber(cloudPub));
cloudPub->publish(makeXYZCloud("lidar", 1000.0, blockAt(2.0f)));
infoPub->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx));
spinFor(std::chrono::milliseconds(300));
std::shared_ptr<Collector<sensor_msgs::msg::Image>> late =
collect<sensor_msgs::msg::Image>("image");
spinFor(std::chrono::milliseconds(200));
EXPECT_TRUE(late->empty());
}
//============================================================================
// Motion compensation between the cloud stamp and the camera_info stamp
//============================================================================
/**
* With approximate synchronization the cloud and the camera info rarely share a stamp.
* When @c fixed_frame_id is set, the node asks TF how the lidar moved over that interval
* and folds the displacement into the camera's local transform, so the cloud is projected
* from where the camera was at its own stamp instead of where the lidar was.
*
* The frames follow the usual convention, as in rtabmap's own projectCloudToCamera tests:
* the cloud is expressed in a lidar frame with x forward, and the camera is attached to it
* through the optical rotation, so a point straight ahead lands on the principal point.
*/
class PointCloudToDepthImageMotionTest : public NodeTest
{
protected:
static constexpr double kSpeed = 1.0; ///< m/s
static constexpr double kCloudStamp = 1000.0;
static constexpr double kInfoDelay = 0.04; ///< the camera info lags the cloud by this
static constexpr float kRange = 2.0f; ///< distance to the point, meters
static constexpr int kFrames = 3; ///< see publishFrames()
static constexpr double kPeriod = 0.2; ///< seconds between frames
/// Distance travelled between the two stamps: what the node has to compensate for.
static double travelled() { return kSpeed * kInfoDelay; }
/// The same distance seen sideways by the camera, in pixels.
static int shiftInPixels() { return int(kFx * travelled() / double(kRange)); }
/**
* @param axis 'x' to drive straight at the point, 'y' to drive sideways past it,
* '0' to stand still
*/
void start(const std::vector<rclcpp::Parameter> & overrides, char axis = 'x')
{
addNode(std::make_shared<rtabmap_util::PointCloudToDepthImage>(
rclcpp::NodeOptions().parameter_overrides(overrides)));
// The camera is bolted to the lidar, looking the same way: x right, y down,
// z forward against the lidar's x forward, y left, z up.
publishStaticTfRPY("lidar", "camera_link", -M_PI/2.0, 0.0, -M_PI/2.0);
if(axis != '0')
{
publishOdomMotion(axis);
}
image32_ = collect<sensor_msgs::msg::Image>("image");
cloudPub_ = helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>("camera_info", 10);
ASSERT_TRUE(waitForSubscriber(cloudPub_));
ASSERT_TRUE(waitForSubscriber(infoPub_));
ASSERT_TRUE(waitForPublisher(image32_->subscription));
}
/// Publishes odom -> lidar moving at kSpeed, covering every stamp used below.
void publishOdomMotion(char axis)
{
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub =
helper()->create_publisher<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
spinFor(std::chrono::milliseconds(100)); // let the node's listener subscribe
// One sample per cloud stamp and per camera info stamp, plus a margin on each side
// so that nothing has to be extrapolated.
std::vector<double> elapsedSamples;
elapsedSamples.push_back(-kPeriod);
for(int k=0; k<kFrames; ++k)
{
elapsedSamples.push_back(kPeriod * double(k));
elapsedSamples.push_back(kPeriod * double(k) + kInfoDelay);
}
elapsedSamples.push_back(kPeriod * double(kFrames));
for(size_t i=0; i<elapsedSamples.size(); ++i)
{
const double elapsed = elapsedSamples[i];
geometry_msgs::msg::TransformStamped t;
t.header.stamp = stampOf(kCloudStamp + elapsed);
t.header.frame_id = "odom";
t.child_frame_id = "lidar";
(axis == 'x' ? t.transform.translation.x : t.transform.translation.y) =
kSpeed * elapsed;
t.transform.rotation.w = 1.0;
tf2_msgs::msg::TFMessage msg;
msg.transforms.push_back(t);
tfPub->publish(msg);
}
spinFor(std::chrono::milliseconds(200)); // let the buffer fill
tfPub_ = tfPub; // keep the publisher alive
}
/**
* @brief Publishes kFrames cloud/camera_info pairs, the info @p delay seconds late.
*
* Each cloud holds a single point straight ahead of the lidar. The speed is constant,
* so every pair needs the same correction and the first output is enough to assert on.
* A burst is needed because ApproximateTime emits nothing for a lone pair whose stamps
* differ: it cannot rule out a better match still to come.
*/
void publishFrames(double delay)
{
for(int k=0; k<kFrames; ++k)
{
const double elapsed = kPeriod * double(k);
cloudPub_->publish(makeXYZCloud("lidar", kCloudStamp + elapsed,
{cv::Point3f(kRange, 0.0f, 0.0f)}));
infoPub_->publish(makeCameraInfo(
"camera_link", kCloudStamp + elapsed + delay, kWidth, kHeight, 0.0, kFx));
}
}
/// The column the single point landed in on @p row, or -1 if that row is empty.
static int hitColumn(const sensor_msgs::msg::Image & img, int row)
{
for(int col=0; col<int(img.width); ++col)
{
if(pixel32f(img, row, col) != 0.0f) { return col; }
}
return -1;
}
std::shared_ptr<Collector<sensor_msgs::msg::Image>> image32_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub_;
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub_;
};
TEST_F(PointCloudToDepthImageMotionTest, NoShiftWhenTheStampsMatch)
{
// The control case: same frames, same scene, nothing to compensate. It also proves the
// optical rotation is right, since a wrongly oriented camera sees nothing at all.
start({rclcpp::Parameter("fixed_frame_id", std::string("odom"))});
publishFrames(0.0);
ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) << "no depth image";
const sensor_msgs::msg::Image & img = image32_->front();
EXPECT_EQ(hitColumn(img, kHeight/2), kWidth/2)
<< "a point straight ahead belongs at the principal point";
EXPECT_NEAR(pixel32f(img, kHeight/2, kWidth/2), kRange, 1e-3);
}
TEST_F(PointCloudToDepthImageMotionTest, ClosesTheGapWhenDrivingAtThePoint)
{
// The camera info is 40 ms younger than the cloud and the robot closes in at 1 m/s, so
// by the time of the exposure the point is 4 cm nearer than the lidar measured it.
start({rclcpp::Parameter("fixed_frame_id", std::string("odom"))}, 'x');
publishFrames(kInfoDelay);
ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) << "no depth image";
const sensor_msgs::msg::Image & img = image32_->front();
EXPECT_EQ(hitColumn(img, kHeight/2), kWidth/2)
<< "driving straight at the point does not move it across the image";
EXPECT_NEAR(pixel32f(img, kHeight/2, kWidth/2), kRange - float(travelled()), 1e-3)
<< "the depth must be corrected for the distance travelled";
}
TEST_F(PointCloudToDepthImageMotionTest, ShiftsThePointWhenDrivingPastIt)
{
// Moving sideways instead: the point slides across the image by fx*d/Z pixels, and
// stays at the same range.
start({rclcpp::Parameter("fixed_frame_id", std::string("odom"))}, 'y');
publishFrames(kInfoDelay);
ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) << "no depth image";
const sensor_msgs::msg::Image & img = image32_->front();
EXPECT_EQ(hitColumn(img, kHeight/2), kWidth/2 + shiftInPixels())
<< "the robot moved left, so the point must appear further right";
EXPECT_NEAR(pixel32f(img, kHeight/2, kWidth/2 + shiftInPixels()), kRange, 1e-3)
<< "only the bearing changed, not the range";
EXPECT_FLOAT_EQ(pixel32f(img, kHeight/2, kWidth/2), 0.0f)
<< "and it is no longer at the principal point";
}
TEST_F(PointCloudToDepthImageMotionTest, IgnoresTheStampDifferenceWithoutAFixedFrame)
{
// Without fixed_frame_id there is nothing to measure the motion against, so the cloud
// is projected as if both messages were captured at the same instant. That is why the
// node logs a fatal error when approximate sync is used without one.
start({rclcpp::Parameter("fixed_frame_id", std::string(""))}, 'x');
publishFrames(kInfoDelay);
ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) << "no depth image";
EXPECT_NEAR(pixel32f(image32_->front(), kHeight/2, kWidth/2), kRange, 1e-3)
<< "no fixed frame, no compensation";
}
TEST_F(PointCloudToDepthImageMotionTest, FailsWhenTheFixedFrameIsUnknown)
{
// fixed_frame_id is set but odom -> lidar was never published: the displacement cannot
// be measured, and projecting anyway would silently misplace the points.
start({rclcpp::Parameter("fixed_frame_id", std::string("odom")),
rclcpp::Parameter("wait_for_transform", 0.0)}, '0');
publishFrames(kInfoDelay);
spinFor(std::chrono::milliseconds(400));
EXPECT_TRUE(image32_->empty());
}
rtabmap_util/test/test_rgbd_relay.cpp
+350
View File
@@ -0,0 +1,350 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#include "node_test_utils.hpp"
#include "msg_builders.hpp"
#include <rtabmap_util/rgbd_relay.hpp>
#include <rtabmap/core/Compression.h>
#include <rtabmap/utilite/UException.h>
using namespace rtabmap_util_test;
namespace {
::testing::Environment * const kEnv = registerRclcppEnvironment();
}
class RGBDRelayTest : public NodeTest
{
protected:
/// Starts the node, wires up the input publisher and the output collector.
void start(bool compress, bool uncompress)
{
addNode(std::make_shared<rtabmap_util::RGBDRelay>(rclcpp::NodeOptions()
.parameter_overrides({
rclcpp::Parameter("compress", compress),
rclcpp::Parameter("uncompress", uncompress)})));
out_ = collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay");
pub_ = helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
ASSERT_TRUE(waitForSubscriber(pub_));
ASSERT_TRUE(waitForPublisher(out_->subscription));
}
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out_;
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub_;
};
TEST_F(RGBDRelayTest, RepublishesUnchangedByDefault)
{
start(/*compress=*/false, /*uncompress=*/false);
const rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
pub_->publish(in);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
const rtabmap_msgs::msg::RGBDImage & got = out_->back();
EXPECT_EQ(got.header.frame_id, in.header.frame_id);
EXPECT_EQ(got.rgb.data, in.rgb.data) << "the payload must be passed through untouched";
EXPECT_EQ(got.depth.data, in.depth.data);
EXPECT_TRUE(got.rgb_compressed.data.empty());
EXPECT_TRUE(got.depth_compressed.data.empty());
EXPECT_NEAR(got.rgb_camera_info.p[0], in.rgb_camera_info.p[0], 1e-9);
}
TEST_F(RGBDRelayTest, CompressesRawImagesWhenAsked)
{
start(/*compress=*/true, /*uncompress=*/false);
pub_->publish(makeRGBDImage("camera_link", 1000.0));
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
const rtabmap_msgs::msg::RGBDImage & got = out_->back();
EXPECT_FALSE(got.rgb_compressed.data.empty()) << "rgb must be compressed";
EXPECT_FALSE(got.depth_compressed.data.empty()) << "depth must be compressed";
// Depth is lossless png; color is jpg.
EXPECT_EQ(got.depth_compressed.format, "png");
EXPECT_TRUE(got.rgb.data.empty()) << "the raw image is not carried as well";
}
TEST_F(RGBDRelayTest, CompressesAStereoPairAsJpeg)
{
// When the camera infos describe a stereo pair, the "depth" slot holds the right
// image and is compressed as JPEG, not as a lossless depth PNG.
start(/*compress=*/true, /*uncompress=*/false);
pub_->publish(makeStereoRGBDImage("camera_link", 1000.0));
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
const rtabmap_msgs::msg::RGBDImage & got = out_->back();
ASSERT_FALSE(got.depth_compressed.data.empty());
EXPECT_NE(got.depth_compressed.format, "png")
<< "a stereo right image must not take the depth PNG path";
EXPECT_NE(got.depth_compressed.format.find("jp"), std::string::npos)
<< "expected a jpeg format, got \"" << got.depth_compressed.format << "\"";
EXPECT_LT(got.depth_camera_info.p[3], 0.0) << "the baseline must survive the relay";
}
TEST_F(RGBDRelayTest, CompressesDepthAsLosslessPng)
{
// The same call with no baseline is treated as color + depth instead.
start(/*compress=*/true, /*uncompress=*/false);
pub_->publish(makeRGBDImage("camera_link", 1000.0));
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
const rtabmap_msgs::msg::RGBDImage & got = out_->back();
ASSERT_FALSE(got.depth_compressed.data.empty());
EXPECT_EQ(got.depth_compressed.format, "png") << "depth must stay lossless";
EXPECT_DOUBLE_EQ(got.depth_camera_info.p[3], 0.0) << "no baseline: not stereo";
}
TEST_F(RGBDRelayTest, UncompressRestoresAStereoRightImage)
{
// The uncompress path branches on the format: "jpg" means a stereo right image and
// goes through cv_bridge, anything else is a depth image and goes through rtabmap.
start(/*compress=*/false, /*uncompress=*/true);
rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0);
const cv::Mat right(8, 8, CV_8UC1, cv::Scalar(60));
cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", right)
.toCompressedImageMsg(in.depth_compressed, cv_bridge::JPG);
in.depth = sensor_msgs::msg::Image();
pub_->publish(in);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
const rtabmap_msgs::msg::RGBDImage & got = out_->back();
ASSERT_FALSE(got.depth.data.empty()) << "the right image must be decompressed";
EXPECT_EQ(got.depth.encoding, "mono8") << "restored as an 8-bit image, not depth";
EXPECT_EQ(got.depth.width, 8u);
EXPECT_EQ(got.depth.height, 8u);
}
TEST_F(RGBDRelayTest, UncompressRestoresRawImages)
{
start(/*compress=*/false, /*uncompress=*/true);
// Feed it a message that carries only compressed depth.
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
const cv::Mat depth(8, 8, CV_16UC1, cv::Scalar(1500));
in.depth = sensor_msgs::msg::Image();
in.depth_compressed.header = in.header;
in.depth_compressed.format = "png";
in.depth_compressed.data = rtabmap::compressImage(depth, ".png");
pub_->publish(in);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
const rtabmap_msgs::msg::RGBDImage & got = out_->back();
ASSERT_FALSE(got.depth.data.empty()) << "depth must be decompressed";
EXPECT_EQ(got.depth.encoding, sensor_msgs::image_encodings::TYPE_16UC1);
EXPECT_EQ(got.depth.width, 8u);
EXPECT_EQ(got.depth.height, 8u);
}
TEST_F(RGBDRelayTest, UncompressPrefersTheRawImageOverTheCompressedOne)
{
// A message may carry both. The raw image is already usable, so decompressing the
// other copy would be wasted work -- and the two paths must agree, as the depth
// branch below does.
start(/*compress=*/false, /*uncompress=*/true);
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
// A compressed copy whose content differs, so it is obvious which one was used.
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8",
cv::Mat(8, 8, CV_8UC3, cv::Scalar(200, 200, 200)))
.toCompressedImageMsg(in.rgb_compressed, cv_bridge::PNG);
pub_->publish(in);
ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
ASSERT_FALSE(out_->back().rgb.data.empty());
EXPECT_EQ(out_->back().rgb.data, in.rgb.data)
<< "the raw image must be forwarded, not the decompressed copy";
}
TEST_F(RGBDRelayTest, StaysSilentWithoutASubscriber)
{
// No collector, so the relay's output has no subscriber and it must not do the work.
addNode(std::make_shared<rtabmap_util::RGBDRelay>(rclcpp::NodeOptions()));
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
ASSERT_TRUE(waitForSubscriber(pub));
pub->publish(makeRGBDImage("camera_link", 1000.0));
spinFor(std::chrono::milliseconds(300));
// Subscribing only now must not retroactively receive anything.
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> late =
collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay");
spinFor(std::chrono::milliseconds(200));
EXPECT_TRUE(late->empty());
}
/// Feeds a compressed right image in @p format through the uncompress path.
class RGBDRelayRightImageTest : public NodeTest
{
protected:
rtabmap_msgs::msg::RGBDImage relay(cv_bridge::Format format)
{
addNode(std::make_shared<rtabmap_util::RGBDRelay>(rclcpp::NodeOptions()
.parameter_overrides({rclcpp::Parameter("uncompress", true)})));
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out =
collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay");
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
EXPECT_TRUE(waitForSubscriber(pub));
EXPECT_TRUE(waitForPublisher(out->subscription));
rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0);
cv_bridge::CvImage(std_msgs::msg::Header(), "mono8",
cv::Mat(8, 8, CV_8UC1, cv::Scalar(60)))
.toCompressedImageMsg(in.depth_compressed, format);
in.depth = sensor_msgs::msg::Image();
pub->publish(in);
EXPECT_TRUE(spinUntil([&]() { return !out->empty(); }));
return out->empty() ? rtabmap_msgs::msg::RGBDImage() : out->back();
}
};
TEST_F(RGBDRelayRightImageTest, UncompressesAJpegRightImage)
{
const rtabmap_msgs::msg::RGBDImage got = relay(cv_bridge::JPG);
ASSERT_FALSE(got.depth.data.empty());
EXPECT_EQ(got.depth.encoding, sensor_msgs::image_encodings::MONO8);
EXPECT_EQ(got.depth.step, 8u);
}
TEST_F(RGBDRelayRightImageTest, UncompressesAPngRightImage)
{
// A losslessly compressed right image must not be mistaken for depth and abort.
const rtabmap_msgs::msg::RGBDImage got = relay(cv_bridge::PNG);
ASSERT_FALSE(got.depth.data.empty());
EXPECT_EQ(got.depth.encoding, sensor_msgs::image_encodings::MONO8);
EXPECT_EQ(got.depth.step, 8u);
}
/// QoS of the two sides, set independently through qos_sub and qos_pub.
///
/// A reliable subscription refuses to match a best-effort publisher, while a best-effort
/// subscription matches either. Every assertion below rests on that asymmetry: whether a
/// connection is established at all is what tells us which reliability the node picked.
class RGBDRelayQosTest : public NodeTest
{
protected:
enum Reliability { kSystemDefault = 0, kReliable = 1, kBestEffort = 2 };
void startRelay(const std::vector<rclcpp::Parameter> & params)
{
addNode(std::make_shared<rtabmap_util::RGBDRelay>(
rclcpp::NodeOptions().parameter_overrides(params)));
}
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr input(Reliability reliability)
{
rclcpp::QoS qos(10);
reliability == kBestEffort ? qos.best_effort() : qos.reliable();
return helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", qos);
}
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> output(Reliability reliability)
{
rclcpp::QoS qos(10);
reliability == kBestEffort ? qos.best_effort() : qos.reliable();
return collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay", qos);
}
};
TEST_F(RGBDRelayQosTest, BridgesABestEffortSourceToAReliableConsumer)
{
// The point of splitting the parameter: a sensor publishing best effort feeding a
// consumer that only accepts reliable. Neither could talk to the other directly.
startRelay({rclcpp::Parameter("qos_sub", int(kBestEffort)),
rclcpp::Parameter("qos_pub", int(kReliable))});
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out = output(kReliable);
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub = input(kBestEffort);
ASSERT_TRUE(waitForSubscriber(pub)) << "a best-effort source must reach the relay";
ASSERT_TRUE(waitForPublisher(out->subscription))
<< "a reliable consumer must be able to subscribe to the relayed topic";
pub->publish(makeRGBDImage("camera_link", 1000.0));
ASSERT_TRUE(spinUntil([&]() { return !out->empty(); }));
EXPECT_EQ(out->back().header.frame_id, "camera_link");
}
TEST_F(RGBDRelayQosTest, QosSubOverridesQosOnTheInputOnly)
{
// qos says reliable, which a best-effort source could not match; qos_sub overrides it.
startRelay({rclcpp::Parameter("qos", int(kReliable)),
rclcpp::Parameter("qos_sub", int(kBestEffort))});
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub = input(kBestEffort);
EXPECT_TRUE(waitForSubscriber(pub)) << "qos_sub must win over qos on the subscription";
// The output side kept qos, so a reliable consumer still matches it.
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out = output(kReliable);
EXPECT_TRUE(waitForPublisher(out->subscription))
<< "qos_sub must not affect the publisher";
}
TEST_F(RGBDRelayQosTest, QosPubOverridesQosOnTheOutputOnly)
{
// qos says best effort, which no reliable consumer could match; qos_pub overrides it.
startRelay({rclcpp::Parameter("qos", int(kBestEffort)),
rclcpp::Parameter("qos_pub", int(kReliable))});
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out = output(kReliable);
EXPECT_TRUE(waitForPublisher(out->subscription))
<< "qos_pub must win over qos on the publisher";
// The input side kept qos, so it is still best effort and accepts a best-effort source.
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub = input(kBestEffort);
EXPECT_TRUE(waitForSubscriber(pub)) << "qos_pub must not affect the subscription";
}
TEST_F(RGBDRelayQosTest, BothSidesFallBackToQos)
{
// Only qos is given, so both sides must be best effort -- as before the split.
startRelay({rclcpp::Parameter("qos", int(kBestEffort))});
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub = input(kBestEffort);
EXPECT_TRUE(waitForSubscriber(pub)) << "the subscription must have followed qos";
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out = output(kReliable);
spinFor(std::chrono::milliseconds(500));
EXPECT_EQ(out->subscription->get_publisher_count(), 0u)
<< "the publisher must have followed qos too: best effort, so a reliable "
"consumer cannot match it";
}
TEST_F(RGBDRelayQosTest, HonorsTheConfiguredQueueDepths)
{
// Queue depth is not directly observable from outside, so this only pins down that
// the parameters are accepted and the relay still works with them set.
startRelay({rclcpp::Parameter("queue_sub", 20), rclcpp::Parameter("queue_pub", 10)});
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out = output(kReliable);
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub = input(kReliable);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(out->subscription));
pub->publish(makeRGBDImage("camera_link", 1000.0));
ASSERT_TRUE(spinUntil([&]() { return !out->empty(); }));
EXPECT_EQ(out->back().header.frame_id, "camera_link");
}
TEST_F(RGBDRelayQosTest, RejectsAZeroQueueDepth)
{
// rclcpp::QoS(0) is not a meaningful depth, so say so at construction rather than
// leaving the relay silently misconfigured.
EXPECT_THROW(
startRelay({rclcpp::Parameter("queue_sub", 0)}),
UException);
}
rtabmap_util/test/test_rgbd_split.cpp
+397
View File
@@ -0,0 +1,397 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#include "node_test_utils.hpp"
#include "msg_builders.hpp"
#include <rtabmap_util/rgbd_split.hpp>
#include <rtabmap/core/Compression.h>
#include <rtabmap/utilite/UException.h>
using namespace rtabmap_util_test;
namespace {
::testing::Environment * const kEnv = registerRclcppEnvironment();
}
class RGBDSplitTest : public NodeTest {};
TEST_F(RGBDSplitTest, SplitsIntoImageAndCameraInfoTopics)
{
addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()));
// The node derives its output topics from the input topic name.
std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
collect<sensor_msgs::msg::Image>("rgbd_image/rgb/image");
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> rgbInfo =
collect<sensor_msgs::msg::CameraInfo>("rgbd_image/rgb/camera_info");
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> depthInfo =
collect<sensor_msgs::msg::CameraInfo>("rgbd_image/depth/camera_info");
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(rgb->subscription));
ASSERT_TRUE(waitForPublisher(depth->subscription));
const rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
pub->publish(in);
ASSERT_TRUE(spinUntil([&]() {
return !rgb->empty() && !depth->empty() && !rgbInfo->empty() && !depthInfo->empty();
})) << "not all four outputs were published";
EXPECT_EQ(rgb->back().encoding, "bgr8");
EXPECT_EQ(rgb->back().data, in.rgb.data);
EXPECT_EQ(depth->back().encoding, sensor_msgs::image_encodings::TYPE_16UC1);
EXPECT_EQ(depth->back().data, in.depth.data);
EXPECT_NEAR(rgbInfo->back().p[0], in.rgb_camera_info.p[0], 1e-9);
EXPECT_EQ(rgbInfo->back().width, in.rgb_camera_info.width);
EXPECT_NEAR(depthInfo->back().p[0], in.depth_camera_info.p[0], 1e-9);
}
TEST_F(RGBDSplitTest, FallsBackToTheInputHeaderForTheDepthCameraInfo)
{
addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()));
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> depthInfo =
collect<sensor_msgs::msg::CameraInfo>("rgbd_image/depth/camera_info");
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(depth->subscription));
// Depth camera info with no frame id: the node fills it from the message header.
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
in.depth_camera_info.header.frame_id = "";
pub->publish(in);
ASSERT_TRUE(spinUntil([&]() { return !depthInfo->empty(); }));
EXPECT_EQ(depthInfo->back().header.frame_id, "camera_link");
}
TEST_F(RGBDSplitTest, PassesAStereoPairThroughUnchanged)
{
// The node does not distinguish stereo from depth: it forwards whatever is in the
// "depth" slot, so a stereo right image is published on .../depth/image along with
// the right camera info carrying the baseline.
addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()));
std::shared_ptr<Collector<sensor_msgs::msg::Image>> right =
collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> rightInfo =
collect<sensor_msgs::msg::CameraInfo>("rgbd_image/depth/camera_info");
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(right->subscription));
const rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0);
pub->publish(in);
ASSERT_TRUE(spinUntil([&]() { return !right->empty() && !rightInfo->empty(); }));
EXPECT_EQ(right->back().encoding, "mono8") << "the right image is forwarded as-is";
EXPECT_EQ(right->back().data, in.depth.data);
EXPECT_LT(rightInfo->back().p[3], 0.0) << "the baseline must reach the consumer";
}
TEST_F(RGBDSplitTest, DecompressesDepthWithTheCorrectEncoding)
{
// rtabmap compresses depth as a PNG whose format string cv_bridge cannot interpret.
// The node must decode it itself and label it 16UC1, not mono8: the buffer is two
// bytes per pixel and a wrong encoding makes every consumer misread it.
addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()));
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(depth->subscription));
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
const cv::Mat original(8, 8, CV_16UC1, cv::Scalar(1500));
in.depth = sensor_msgs::msg::Image();
in.depth_compressed.header = in.header;
in.depth_compressed.format = "png";
in.depth_compressed.data = rtabmap::compressImage(original, ".png");
pub->publish(in);
ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); }));
const sensor_msgs::msg::Image & got = depth->back();
EXPECT_EQ(got.encoding, sensor_msgs::image_encodings::TYPE_16UC1)
<< "a 16-bit depth buffer must not be labeled mono8";
EXPECT_EQ(got.width, 8u);
EXPECT_EQ(got.height, 8u);
ASSERT_EQ(got.step, 16u) << "two bytes per pixel";
EXPECT_EQ(*reinterpret_cast<const uint16_t *>(&got.data[0]), 1500)
<< "and the values must survive the round trip";
}
/// Feeds a compressed right image in @p format and returns what lands on depth/image.
class RGBDSplitRightImageTest : public NodeTest
{
protected:
sensor_msgs::msg::Image split(cv_bridge::Format format)
{
addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()));
std::shared_ptr<Collector<sensor_msgs::msg::Image>> right =
collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
EXPECT_TRUE(waitForSubscriber(pub));
EXPECT_TRUE(waitForPublisher(right->subscription));
rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0);
cv_bridge::CvImage(std_msgs::msg::Header(), "mono8",
cv::Mat(8, 8, CV_8UC1, cv::Scalar(60)))
.toCompressedImageMsg(in.depth_compressed, format);
in.depth = sensor_msgs::msg::Image();
pub->publish(in);
EXPECT_TRUE(spinUntil([&]() { return !right->empty(); }))
<< "the right image must be decompressed, not rejected";
return right->empty() ? sensor_msgs::msg::Image() : right->back();
}
};
TEST_F(RGBDSplitRightImageTest, DecompressesAJpegRightImage)
{
// What stereo_sync emits.
const sensor_msgs::msg::Image got = split(cv_bridge::JPG);
EXPECT_EQ(got.encoding, sensor_msgs::image_encodings::MONO8);
EXPECT_EQ(got.step, 8u) << "one byte per pixel, not mistaken for 16-bit depth";
}
TEST_F(RGBDSplitRightImageTest, DecompressesAPngRightImage)
{
// Nothing forbids a producer from compressing the right image losslessly, and a
// stereo pipeline may prefer it since JPEG artifacts hurt matching. Going by the
// format string alone would send this down the depth path and abort on the assert.
const sensor_msgs::msg::Image got = split(cv_bridge::PNG);
EXPECT_EQ(got.encoding, sensor_msgs::image_encodings::MONO8);
EXPECT_EQ(got.step, 8u);
}
/// Queue depths and the reach of the qos parameter.
class RGBDSplitQosTest : public NodeTest
{
protected:
void startSplit(const std::vector<rclcpp::Parameter> & params)
{
addNode(std::make_shared<rtabmap_util::RGBDSplit>(
rclcpp::NodeOptions().parameter_overrides(params)));
}
};
TEST_F(RGBDSplitQosTest, HonorsTheConfiguredQueueDepths)
{
// Queue depth is not observable from outside, so this pins down that the parameters
// are accepted and the node still splits with them set.
startSplit({rclcpp::Parameter("queue_sub", 20), rclcpp::Parameter("queue_pub", 10)});
std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
collect<sensor_msgs::msg::Image>("rgbd_image/rgb/image");
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(rgb->subscription));
pub->publish(makeRGBDImage("camera_link", 1000.0));
ASSERT_TRUE(spinUntil([&]() { return !rgb->empty(); }));
EXPECT_EQ(rgb->back().encoding, "bgr8");
}
TEST_F(RGBDSplitQosTest, RejectsAZeroQueueDepth)
{
EXPECT_THROW(startSplit({rclcpp::Parameter("queue_pub", 0)}), UException);
}
TEST_F(RGBDSplitQosTest, AppliesQosToTheCameraInfoPublishersToo)
{
// A best-effort node must be best effort on every output, camera infos included:
// a reliable consumer must not match any of them.
startSplit({rclcpp::Parameter("qos", 2)});
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> rgbInfo =
collect<sensor_msgs::msg::CameraInfo>(
"rgbd_image/rgb/camera_info", rclcpp::QoS(10).reliable());
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> depthInfo =
collect<sensor_msgs::msg::CameraInfo>(
"rgbd_image/depth/camera_info", rclcpp::QoS(10).reliable());
spinFor(std::chrono::milliseconds(500));
EXPECT_EQ(rgbInfo->subscription->get_publisher_count(), 0u)
<< "the rgb camera info publisher ignored qos";
EXPECT_EQ(depthInfo->subscription->get_publisher_count(), 0u)
<< "the depth camera info publisher ignored qos";
}
/// Output topic naming, controlled by the stereo parameter.
class RGBDSplitStereoNamingTest : public NodeTest
{
protected:
void startSplit(bool stereo)
{
addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()
.parameter_overrides({rclcpp::Parameter("stereo", stereo)})));
}
};
TEST_F(RGBDSplitStereoNamingTest, PublishesOnLeftAndRightWhenStereoIsSet)
{
startSplit(/*stereo=*/true);
std::shared_ptr<Collector<sensor_msgs::msg::Image>> left =
collect<sensor_msgs::msg::Image>("rgbd_image/left/image");
std::shared_ptr<Collector<sensor_msgs::msg::Image>> right =
collect<sensor_msgs::msg::Image>("rgbd_image/right/image");
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> leftInfo =
collect<sensor_msgs::msg::CameraInfo>("rgbd_image/left/camera_info");
std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> rightInfo =
collect<sensor_msgs::msg::CameraInfo>("rgbd_image/right/camera_info");
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(left->subscription));
ASSERT_TRUE(waitForPublisher(right->subscription));
const rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0);
pub->publish(in);
ASSERT_TRUE(spinUntil([&]() {
return !left->empty() && !right->empty() && !leftInfo->empty() && !rightInfo->empty();
})) << "not all four outputs were published";
EXPECT_EQ(left->back().data, in.rgb.data) << "the rgb slot feeds the left topic";
EXPECT_EQ(right->back().data, in.depth.data) << "the depth slot feeds the right topic";
EXPECT_LT(rightInfo->back().p[3], 0.0) << "the baseline must reach the right camera info";
}
TEST_F(RGBDSplitStereoNamingTest, DoesNotPublishOnRgbAndDepthWhenStereoIsSet)
{
// The two namings are exclusive: nothing must be left publishing the old names.
startSplit(/*stereo=*/true);
std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
collect<sensor_msgs::msg::Image>("rgbd_image/rgb/image");
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
spinFor(std::chrono::milliseconds(500));
EXPECT_EQ(rgb->subscription->get_publisher_count(), 0u);
EXPECT_EQ(depth->subscription->get_publisher_count(), 0u);
}
TEST_F(RGBDSplitStereoNamingTest, KeepsRgbAndDepthByDefault)
{
startSplit(/*stereo=*/false);
std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
collect<sensor_msgs::msg::Image>("rgbd_image/rgb/image");
std::shared_ptr<Collector<sensor_msgs::msg::Image>> left =
collect<sensor_msgs::msg::Image>("rgbd_image/left/image");
ASSERT_TRUE(waitForPublisher(rgb->subscription));
EXPECT_EQ(left->subscription->get_publisher_count(), 0u)
<< "left/right naming must be opt-in";
}
TEST_F(RGBDSplitStereoNamingTest, StillPublishesADepthImageOnRightWithStereoSet)
{
// A depth image with stereo set is a misconfiguration: the node warns (once) but
// keeps forwarding, so an existing pipeline is never silently broken.
startSplit(/*stereo=*/true);
std::shared_ptr<Collector<sensor_msgs::msg::Image>> right =
collect<sensor_msgs::msg::Image>("rgbd_image/right/image");
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(right->subscription));
// makeRGBDImage carries 16UC1 depth, not a right image.
pub->publish(makeRGBDImage("camera_link", 1000.0));
ASSERT_TRUE(spinUntil([&]() { return !right->empty(); }))
<< "the image must still be forwarded, warning or not";
EXPECT_EQ(right->back().encoding, sensor_msgs::image_encodings::TYPE_16UC1);
}
TEST_F(RGBDSplitStereoNamingTest, StillPublishesARightImageOnDepthWithStereoUnset)
{
// The inverse misconfiguration, and the one this node has always allowed: a stereo
// pair with stereo left false. It warns, but the right image must still come out.
startSplit(/*stereo=*/false);
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(depth->subscription));
const rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0);
pub->publish(in);
ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); }))
<< "the right image must still be forwarded, warning or not";
EXPECT_EQ(depth->back().encoding, "mono8");
EXPECT_EQ(depth->back().data, in.depth.data);
}
TEST_F(RGBDSplitStereoNamingTest, DoesNotWarnOnAnEmptySecondHalf)
{
// A color-only RGBDImage leaves the depth slot empty, whose encoding is "". That
// must not be mistaken for a right image: nothing is published, nothing to warn about.
startSplit(/*stereo=*/false);
std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
collect<sensor_msgs::msg::Image>("rgbd_image/rgb/image");
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(rgb->subscription));
rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
in.depth = sensor_msgs::msg::Image();
pub->publish(in);
ASSERT_TRUE(spinUntil([&]() { return !rgb->empty(); }));
spinFor(std::chrono::milliseconds(300));
EXPECT_EQ(rgb->back().encoding, "bgr8") << "the color half is unaffected";
if(!depth->empty())
{
EXPECT_TRUE(depth->back().data.empty())
<< "an absent depth image must not turn into a non-empty one";
}
}
TEST_F(RGBDSplitQosTest, QosSubAndQosPubOverrideQosPerSide)
{
// qos says reliable, which a best-effort source could not match; qos_sub overrides
// it, while qos_pub keeps the outputs reliable for a strict consumer.
startSplit({rclcpp::Parameter("qos", 1),
rclcpp::Parameter("qos_sub", 2),
rclcpp::Parameter("qos_pub", 1)});
std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
collect<sensor_msgs::msg::Image>("rgbd_image/rgb/image", rclcpp::QoS(10).reliable());
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>(
"rgbd_image", rclcpp::QoS(10).best_effort());
ASSERT_TRUE(waitForSubscriber(pub)) << "qos_sub must win over qos on the subscription";
ASSERT_TRUE(waitForPublisher(rgb->subscription)) << "qos_pub must keep the output reliable";
pub->publish(makeRGBDImage("camera_link", 1000.0));
ASSERT_TRUE(spinUntil([&]() { return !rgb->empty(); }));
EXPECT_EQ(rgb->back().encoding, "bgr8");
}