rtabmap_sync tests and doc (#1454)

* rtabmap_sync tests and doc

* Added some diagrams

* cleanup some diagrams

* fixing running tests in parallels
This commit is contained in:
matlabbe
2026-09-13 11:25:32 -07:00
committed by GitHub
parent 61edb4ee85
commit 5062bf0614
45 changed files with 4481 additions and 76 deletions
@@ -59,34 +59,177 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_sync/CommonDataSubscriberDefines.h>
#include <rtabmap_sync/SyncDiagnostic.h>
/**
* @namespace rtabmap_sync
* @brief Synchronization of the sensor topics RTAB-Map consumes.
*
* Two things live here: the standalone nodes that group a camera's topics into a single
* [RGBDImage](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html)
* (`rgbd_sync`, `stereo_sync`, `rgb_sync`, `rgbdx_sync`), and CommonDataSubscriber, the
* base class through which the consuming nodes subscribe.
*/
namespace rtabmap_sync {
/**
* @brief Subscribes to whichever set of sensor topics a node was configured for, and
* hands them over synchronized.
*
* RTAB-Map can be fed in a dozen shapes -- RGB-D, stereo, RGB-only, a pre-packed
* `RGBDImage` or several of them, a 2D or 3D scan, a whole `SensorData` -- each
* optionally alongside odometry, an `OdomInfo` and user data. That is far too many
* combinations for a node to wire by hand, so this class owns all of them: it reads the
* `subscribe_*` parameters, builds the one `message_filters` synchronizer that matches,
* and calls back with a uniform set of arguments no matter which inputs were used.
*
* `rtabmap_slam`'s `rtabmap` node and `rtabmap_viz` both derive from it, which is why
* they take identical topics and parameters.
*
* @par Using it
* Derive from both rclcpp::Node and this class, and call setupCallbacks() once the
* subclass is ready to receive data:
* @code
* class MyNode : public rclcpp::Node, public rtabmap_sync::CommonDataSubscriber
* {
* public:
* explicit MyNode(const rclcpp::NodeOptions & options) :
* Node("my_node", options),
* CommonDataSubscriber(*this, false)
* {
* setupCallbacks(*this);
* }
* protected:
* void commonMultiCameraCallback(...) override { ... }
* // ... and the three other callbacks
* };
* @endcode
* The constructor declares the parameters, so they are readable from the subclass
* constructor before setupCallbacks() is called.
*
* @par Which callback fires
* Exactly one of the four, decided once at setup:
* - commonMultiCameraCallback() for anything with a camera in it,
* - commonLaserScanCallback() for a scan with no camera,
* - commonSensorDataCallback() for `subscribe_sensor_data`,
* - commonOdomCallback() when odometry is the only input.
*
* @par Conflicting parameters
* Several `subscribe_*` flags describe the same slot. Rather than refusing to start,
* setupCallbacks() drops one of the two and logs which: stereo beats depth and RGB,
* `subscribe_rgbd` beats all three, `subscribe_sensor_data` beats everything including
* `subscribe_rgbd`, `subscribe_scan` beats `subscribe_scan_cloud`, and
* `subscribe_scan_descriptor` beats both. Setting `odom_frame_id` turns off
* `subscribe_odom`, since the pose is then read from TF instead.
*
* @par Build options
* Synchronizing several `RGBDImage` topics (`rgbd_cameras` > 1) needs
* `RTABMAP_SYNC_MULTI_RGBD`, and `subscribe_user_data` needs `RTABMAP_SYNC_USER_DATA`.
* Both are off by default because each multiplies the number of synchronizer templates
* the package instantiates. Turning the first on is the better of the two ways to take
* several cameras: the node subscribes to them directly, with nothing in between.
* Without it, `rgbd_cameras=0` selects the `RGBDImages` interface -- what `rgbdx_sync`
* publishes -- which needs no rebuild and has no camera-count limit, at the cost of one
* extra node and one full-frame copy per camera.
*/
class CommonDataSubscriber {
public:
/**
* @brief Declares the `subscribe_*`, queue and QoS parameters on @p node.
*
* Subscribing itself happens in setupCallbacks(), so that a subclass can read the
* parameters and finish constructing before any message can arrive.
*
* @param node the node the parameters are declared on and the topics subscribed to
* @param gui true for a visualization node: `subscribe_depth` and `subscribe_rgb`
* then default to false, leaving odometry as the only default input
*/
RTABMAP_SYNC_PUBLIC
CommonDataSubscriber(rclcpp::Node & node, bool gui);
virtual ~CommonDataSubscriber();
/// True if subscribed to separate color, depth and camera_info topics.
bool isSubscribedToDepth() const {return subscribedToDepth_;}
/// True if subscribed to a left/right image pair with their two camera_info topics.
bool isSubscribedToStereo() const {return subscribedToStereo_;}
/// True if subscribed to color and camera_info with no depth.
bool isSubscribedToRGB() const {return subscribedToRGB_;}
/// True if odometry comes from the `odom` topic; false when `odom_frame_id` is set.
bool isSubscribedToOdom() const {return subscribedToOdom_;}
/// True if subscribed to `RGBDImage` topics, or to the `RGBDImages` container.
bool isSubscribedToRGBD() const {return subscribedToRGBD_;}
/// True if subscribed to a `LaserScan`.
bool isSubscribedToScan2d() const {return subscribedToScan2d_;}
/// True if subscribed to a `PointCloud2` scan.
bool isSubscribedToScan3d() const {return subscribedToScan3d_;}
/// True if subscribed to a whole `SensorData`.
bool isSubscribedToSensorData() const {return subscribedToSensorData_;}
/// True if an `OdomInfo` is synchronized with the data.
bool isSubscribedToOdomInfo() const {return subscribedToOdomInfo_;}
/// True if any input at all is subscribed. False means no callback can ever fire.
bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD() || isSubscribedToScan2d() || isSubscribedToScan3d() || isSubscribedToRGB() || isSubscribedToOdom() || isSubscribedToSensorData();}
/**
* @brief Number of `RGBDImage` topics subscribed.
* @return 0 when not subscribed to RGBD at all, and also on the `RGBDImages`
* interface (`rgbd_cameras=0`), where the count varies per message.
*/
int rgbdCameras() const {return isSubscribedToRGBD()?(int)rgbdSubs_.size():0;}
/// Queue depth of each individual subscription (`topic_queue_size`).
int getTopicQueueSize() const {return topicQueueSize_;}
/// Queue depth of the synchronizer (`sync_queue_size`).
int getSyncQueueSize() const {return syncQueueSize_;}
/**
* @brief True if inputs are matched by nearest stamp rather than exact equality.
*
* The default depends on the inputs: false for stereo and for a scan with no camera,
* true otherwise. The `approx_sync` parameter overrides it either way.
*/
bool isApproxSync() const {return approxSync_;}
/// The node name, as captured at construction.
const std::string & name() const {return name_;}
protected:
/**
* @brief Resolves the parameters into one synchronizer and subscribes.
*
* Call once from the subclass constructor, after the subclass is able to handle a
* callback. This is also where the conflicting-parameter rules are applied and where
* the /diagnostics reporting is set up.
*
* @param node the node to subscribe on; pass the same one given to the constructor
* @param otherTasks extra diagnostic tasks to publish alongside the input and output
* rate, so the node reports its own state in the same message
*/
void setupCallbacks(
rclcpp::Node & node,
std::vector<diagnostic_updater::DiagnosticTask*> otherTasks = std::vector<diagnostic_updater::DiagnosticTask*>());
/**
* @brief Called with one synchronized frame from one or more cameras.
*
* Fires for every configuration that has a camera in it, whichever way the camera was
* subscribed. The vectors hold one entry per camera and are parallel; unused inputs
* arrive empty or null rather than being signalled separately.
*
* @param odomMsg the pose, or null when odometry is not subscribed
* @param userDataMsg user data, or null
* @param imageMsgs one color image per camera
* @param depthMsgs one depth image per camera, or the right image in
* stereo; empty when there is no depth (RGB-only)
* @param cameraInfoMsgs calibration of each color camera
* @param depthCameraInfoMsgs calibration of each depth camera, or of the right
* camera in stereo, whose P(0,3) carries the baseline
* @param scanMsg a 2D scan, or a default-constructed one if none
* @param scan3dMsg a 3D scan, or a default-constructed one if none
* @param odomInfoMsg odometry details, or null
* @param globalDescriptorMsgs global descriptors, empty when none were computed
* @param localKeyPoints per-camera keypoints, in image coordinates; only ever
* set by the RGBD inputs, which can carry the features
* the odometry already extracted
* @param localPoints3d per-camera 3D points matching @p localKeyPoints, each
* expressed in **its own camera's optical frame** -- not
* in the robot's base frame. rtabmap_conversions'
* `convertRGBDMsgs()` is what moves them to the base
* frame, applying each camera's local transform.
* @param localDescriptors per-camera feature descriptors, already uncompressed
*/
virtual void commonMultiCameraCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
@@ -101,6 +244,17 @@ protected:
const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> >(),
const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_msgs::msg::Point3f> >(),
const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>()) = 0;
/**
* @brief Called with one synchronized scan, when no camera is subscribed.
*
* @param odomMsg the pose, or null when odometry is not subscribed
* @param userDataMsg user data, or null
* @param scanMsg the 2D scan, default-constructed if the scan is 3D
* @param scan3dMsg the 3D scan, default-constructed if the scan is 2D
* @param odomInfoMsg odometry details, or null
* @param globalDescriptor the descriptor from a `ScanDescriptor` input; its `data`
* is empty when none was computed
*/
virtual void commonLaserScanCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
@@ -108,15 +262,42 @@ protected:
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
const rtabmap_msgs::msg::GlobalDescriptor & globalDescriptor = rtabmap_msgs::msg::GlobalDescriptor()) = 0;
/**
* @brief Called with odometry alone, when it is the only subscribed input.
* @param odomMsg the pose
* @param userDataMsg user data, or null
* @param odomInfoMsg odometry details, or null
*/
virtual void commonOdomCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg) = 0;
/**
* @brief Called with a whole `SensorData`, for `subscribe_sensor_data`.
*
* A `SensorData` already carries the images, the scan and the calibration of one
* frame, so nothing is unpacked here: it is passed on as it arrived.
*
* @param sensorDataMsg the frame
* @param odomMsg the pose, or null when odometry is not subscribed
* @param odomInfoMsg odometry details, or null
*/
virtual void commonSensorDataCallback(
const rtabmap_msgs::msg::SensorData::ConstSharedPtr & sensorDataMsg,
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg) = 0;
/**
* @brief Reports that the subclass produced an output, for /diagnostics.
*
* The input side is ticked automatically as messages arrive; this is the other half,
* and it is what lets "one camera went quiet" be told apart from "the node is
* receiving everything and falling behind". Call it once per published result.
*
* @param stamp stamp of what was produced
* @param targetFrequency the rate to be judged against, or 0 to inherit the rate
* measured on the input side
*/
void tick(const rclcpp::Time & stamp, double targetFrequency = 0);
private:
@@ -29,7 +29,40 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap_sync/GetTopicName.h>
#include <string>
/**
* @brief One name for the topic of a subscription, whichever kind it is.
*
* A `message_filters::Subscriber` exposes `get_topic_name()` through the subscription it
* holds, while an `image_transport::SubscriberFilter` exposes `getTopic()`. The SYNC_DECL
* macros below log what a node subscribed to and have to handle both, so this picks
* whichever the object actually has, resolved at compile time.
*
* @{
*/
template<class T>
auto getTopicNameImpl(T const& obj, int)
-> decltype(obj->get_topic_name(), std::string())
{
return obj->get_topic_name();
}
template<class T>
auto getTopicNameImpl(T const& obj, long)
-> decltype(obj.getTopic(), std::string())
{
return obj.getTopic();
}
template<class T>
auto getTopicName(T const& obj)
-> decltype(getTopicNameImpl(obj, 0), std::string())
{
return getTopicNameImpl(obj, 0);
}
/** @} */
#define DATA_SYNC2(PREFIX, SYNC_NAME, MSG0, MSG1) \
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1> PREFIX##SYNC_NAME##SyncPolicy; \
@@ -1,35 +0,0 @@
/*
* GetTopicName.h
*
* Created on: Oct 1, 2021
* Author: mathieu
*/
#ifndef INCLUDE_RTABMAP_ROS_GETTOPICNAME_H_
#define INCLUDE_RTABMAP_ROS_GETTOPICNAME_H_
#include <string>
template<class T>
auto getTopicNameImpl(T const& obj, int)
-> decltype(obj->get_topic_name(), std::string())
{
return obj->get_topic_name();
}
template<class T>
auto getTopicNameImpl(T const& obj, long)
-> decltype(obj.getTopic(), std::string())
{
return obj.getTopic();
}
template<class T>
auto getTopicName(T const& obj)
-> decltype(getTopicNameImpl(obj, 0), std::string())
{
return getTopicNameImpl(obj, 0);
}
#endif /* INCLUDE_RTABMAP_ROS_GETTOPICNAME_H_ */
@@ -14,8 +14,43 @@ using namespace std::chrono_literals;
namespace rtabmap_sync {
/**
* @brief Reports the rate going into a synchronizer and the rate coming out of it, on
* /diagnostics.
*
* Every node in this package, and every node built on CommonDataSubscriber, publishes
* through one of these. Two statuses rather than one is the whole point: a node can be
* receiving all of its inputs and still publish nothing -- one camera lagging is enough
* to stop a synchronizer emitting -- and only the pair tells those cases apart.
*
* @par Expected rate
* With no rate given, the target is learned from the gaps between the message stamps,
* averaged over a sliding window, and only ever revised upwards to the fastest rate seen.
* A node that deliberately publishes slower than it receives -- a throttled or decimated
* output -- passes its own rate to tickOutput() instead, so it is judged against what it
* meant to do.
*
* @par Usage
* @code
* syncDiagnostic_.reset(new SyncDiagnostic(this));
* syncDiagnostic_->init(imageSub_.getTopic(), "Did not receive data since 5 seconds!...");
* // then, in the callback:
* syncDiagnostic_->tickInput(image->header.stamp);
* ...
* syncDiagnostic_->tickOutput(image->header.stamp);
* @endcode
*
* @note The node passed in is held as a raw pointer and must outlive this object.
*/
class SyncDiagnostic {
public:
/**
* @param node the node to publish /diagnostics from; must outlive this object
* @param tolerance fraction by which the measured rate may differ from the
* expected one before the status stops being OK
* @param windowSize number of stamp intervals averaged when learning the expected
* rate; must be at least 1
*/
SyncDiagnostic(rclcpp::Node * node, double tolerance = 0.2, int windowSize = 5) :
node_(node),
diagnosticUpdater_(node, 2.0),
@@ -34,6 +69,20 @@ class SyncDiagnostic {
UASSERT(windowSize_ >= 1);
}
/**
* @brief Registers the tasks and starts publishing.
*
* @param topic one of the subscribed topics, used only to name the hardware the
* status belongs to: the last two segments are dropped, so
* `/back_camera/left/image` reports as `back_camera`. Pass an empty
* string when no single topic identifies the device; the hardware id is
* then `none`.
* @param topicsNotReceivedWarningMsg logged every 5 seconds while nothing is coming
* in. Worth making specific: it is what a user sees when a pipeline is
* silent, so it should name the topics and the likely causes.
* @param otherTasks extra tasks to publish in the same message, so a node's own state
* arrives alongside its rates rather than in a separate update.
*/
void init(
const std::string & topic,
const std::string & topicsNotReceivedWarningMsg,
@@ -62,6 +111,12 @@ class SyncDiagnostic {
diagnosticTimer_ = node_->create_wall_timer(5s, std::bind(&SyncDiagnostic::diagnosticTimerCallback, this), nullptr);
}
/**
* @brief Records that one input message arrived.
* @param stamp the message stamp; it is also checked against the clock,
* which is how an unsynchronized sender is caught
* @param expectedFrequency the rate to judge against, or 0 to learn it from the stamps
*/
void tickInput(const rclcpp::Time & stamp, double expectedFrequency = 0.0)
{
updateFrequency(
@@ -74,6 +129,13 @@ class SyncDiagnostic {
lastTickInputStamp_);
}
/**
* @brief Records that one output message was published.
* @param stamp the stamp of what was published
* @param expectedFrequency the rate to judge against, or 0 to inherit the rate
* measured on the input side -- the right default for a
* node that publishes one output per input
*/
void tickOutput(const rclcpp::Time & stamp, double expectedFrequency = 0.0)
{
if(expectedFrequency == 0.0) {
@@ -44,6 +44,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync
{
/**
* @brief Groups a camera's color and calibration topics into one `RGBDImage`, with no
* depth.
*
* The RGB-only counterpart of RGBDSync, for a monocular camera feeding an appearance-only
* pipeline -- loop closure detection and relocalization without 3D reconstruction. With
* `fill_empty_depth` it adds an all-zero depth image for consumers that insist on one.
*
* See the [node documentation](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_sync/doc/rgb_sync.md)
* for topics and parameters.
*/
class RGBSync : public rclcpp::Node
{
public:
@@ -60,6 +71,9 @@ private:
double compressedRate_;
bool fillEmptyDepth_;
/// Stamp of the last compressed message published, for compressed_rate throttling.
/// Explicitly on the ROS clock: the default is the system clock, and rclcpp refuses
/// to compare two times that do not come from the same source.
rclcpp::Time lastCompressedPublished_;
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;
@@ -44,6 +44,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync
{
/**
* @brief Groups an RGB-D camera's color, depth and calibration topics into one
* `RGBDImage`.
*
* Three topics that have to stay together are easier to keep together as one message:
* remapping is a single line, nothing downstream re-synchronizes them, and a recording
* cannot end up with a depth frame and no color. It can also decimate, rescale depth and
* publish a compressed copy for a slow link.
*
* See the [node documentation](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_sync/doc/rgbd_sync.md)
* for topics and parameters.
*/
class RGBDSync : public rclcpp::Node
{
public:
@@ -63,6 +75,9 @@ private:
double compressedRate_;
double approxSyncMaxInterval_;
/// Stamp of the last compressed message published, for compressed_rate throttling.
/// Explicitly on the ROS clock: the default is the system clock, and rclcpp refuses
/// to compare two times that do not come from the same source.
rclcpp::Time lastCompressedPublished_;
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;
@@ -46,6 +46,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync
{
/**
* @brief Groups the `RGBDImage` topics of 2 to 8 cameras into one `RGBDImages`.
*
* For a robot carrying several RGB-D cameras. Synchronizing them here, once, means the
* consuming node subscribes to a single topic and needs no multi-camera build option --
* `rgbd_cameras=0` on CommonDataSubscriber takes the container this publishes.
*
* See the [node documentation](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_sync/doc/rgbdx_sync.md)
* for topics and parameters.
*/
class RGBDXSync : public rclcpp::Node
{
public:
@@ -44,6 +44,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync
{
/**
* @brief Groups a stereo pair's four topics into one `RGBDImage`.
*
* The left image goes in the color slot and the right image in the depth slot; what tells
* a consumer to read it as a stereo pair rather than as color plus depth is the baseline
* in the second calibration's P(0,3). Defaults to exact synchronization, since a stereo
* pair is normally hardware-triggered.
*
* See the [node documentation](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_sync/doc/stereo_sync.md)
* for topics and parameters.
*/
class StereoSync : public rclcpp::Node
{
public:
@@ -60,6 +71,9 @@ public:
private:
double compressedRate_;
double approxSyncMaxInterval_;
/// Stamp of the last compressed message published, for compressed_rate throttling.
/// Explicitly on the ROS clock: the default is the system clock, and rclcpp refuses
/// to compare two times that do not come from the same source.
rclcpp::Time lastCompressedPublished_;
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;