mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 16:57:46 +08:00
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:
@@ -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_;
|
||||
|
||||
Reference in New Issue
Block a user