feat(slam): add rtabmap_ros
This commit is contained in:
X-lanni
@@ -0,0 +1,636 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, 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 COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_
|
||||
#define INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_
|
||||
|
||||
#include <rtabmap_sync/visibility.h>
|
||||
#include <message_filters/subscriber.hpp>
|
||||
#include <message_filters/synchronizer.hpp>
|
||||
#include <message_filters/sync_policies/approximate_time.hpp>
|
||||
#include <message_filters/sync_policies/exact_time.hpp>
|
||||
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <image_transport/subscriber_filter.hpp>
|
||||
|
||||
#ifdef PRE_ROS_IRON
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#else
|
||||
#include <cv_bridge/cv_bridge.hpp>
|
||||
#endif
|
||||
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <sensor_msgs/msg/camera_info.hpp>
|
||||
#include <sensor_msgs/msg/laser_scan.hpp>
|
||||
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
|
||||
#include <rtabmap_msgs/msg/rgbd_image.hpp>
|
||||
#include <rtabmap_msgs/msg/rgbd_images.hpp>
|
||||
#include <rtabmap_msgs/msg/user_data.hpp>
|
||||
#include <rtabmap_msgs/msg/odom_info.hpp>
|
||||
#include <rtabmap_msgs/msg/scan_descriptor.hpp>
|
||||
#include <rtabmap_msgs/msg/sensor_data.hpp>
|
||||
#include <rtabmap_sync/CommonDataSubscriberDefines.h>
|
||||
#include <rtabmap_sync/SyncDiagnostic.h>
|
||||
|
||||
namespace rtabmap_sync {
|
||||
|
||||
class CommonDataSubscriber {
|
||||
public:
|
||||
RTABMAP_SYNC_PUBLIC
|
||||
CommonDataSubscriber(rclcpp::Node & node, bool gui);
|
||||
virtual ~CommonDataSubscriber();
|
||||
|
||||
bool isSubscribedToDepth() const {return subscribedToDepth_;}
|
||||
bool isSubscribedToStereo() const {return subscribedToStereo_;}
|
||||
bool isSubscribedToRGB() const {return subscribedToRGB_;}
|
||||
bool isSubscribedToOdom() const {return subscribedToOdom_;}
|
||||
bool isSubscribedToRGBD() const {return subscribedToRGBD_;}
|
||||
bool isSubscribedToScan2d() const {return subscribedToScan2d_;}
|
||||
bool isSubscribedToScan3d() const {return subscribedToScan3d_;}
|
||||
bool isSubscribedToSensorData() const {return subscribedToSensorData_;}
|
||||
bool isSubscribedToOdomInfo() const {return subscribedToOdomInfo_;}
|
||||
bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD() || isSubscribedToScan2d() || isSubscribedToScan3d() || isSubscribedToRGB() || isSubscribedToOdom() || isSubscribedToSensorData();}
|
||||
int rgbdCameras() const {return isSubscribedToRGBD()?(int)rgbdSubs_.size():0;}
|
||||
int getTopicQueueSize() const {return topicQueueSize_;}
|
||||
int getSyncQueueSize() const {return syncQueueSize_;}
|
||||
bool isApproxSync() const {return approxSync_;}
|
||||
const std::string & name() const {return name_;}
|
||||
|
||||
protected:
|
||||
void setupCallbacks(
|
||||
rclcpp::Node & node,
|
||||
std::vector<diagnostic_updater::DiagnosticTask*> otherTasks = std::vector<diagnostic_updater::DiagnosticTask*>());
|
||||
virtual void commonMultiCameraCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
|
||||
const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
|
||||
const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
|
||||
const sensor_msgs::msg::LaserScan& scanMsg,
|
||||
const sensor_msgs::msg::PointCloud2& scan3dMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
|
||||
const std::vector<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_msgs::msg::GlobalDescriptor>(),
|
||||
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;
|
||||
virtual void commonLaserScanCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
|
||||
const sensor_msgs::msg::LaserScan & scanMsg,
|
||||
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
|
||||
const rtabmap_msgs::msg::GlobalDescriptor & globalDescriptor = rtabmap_msgs::msg::GlobalDescriptor()) = 0;
|
||||
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;
|
||||
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;
|
||||
|
||||
void tick(const rclcpp::Time & stamp, double targetFrequency = 0);
|
||||
|
||||
private:
|
||||
void commonSingleCameraCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
|
||||
const cv_bridge::CvImageConstPtr & imageMsg,
|
||||
const cv_bridge::CvImageConstPtr & depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo & rgbCameraInfoMsg,
|
||||
const sensor_msgs::msg::CameraInfo & depthCameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan & scanMsg,
|
||||
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
|
||||
const std::vector<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_msgs::msg::GlobalDescriptor>(),
|
||||
const std::vector<rtabmap_msgs::msg::KeyPoint> & localKeyPoints = std::vector<rtabmap_msgs::msg::KeyPoint>(),
|
||||
const std::vector<rtabmap_msgs::msg::Point3f> & localPoints3d = std::vector<rtabmap_msgs::msg::Point3f>(),
|
||||
const cv::Mat & localDescriptors = cv::Mat());
|
||||
void processSyncData();
|
||||
void setupDepthCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo);
|
||||
void setupStereoCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBDCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBDXCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo);
|
||||
#ifdef RTABMAP_SYNC_MULTI_RGBD
|
||||
void setupRGBD2Callbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBD3Callbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBD4Callbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBD5Callbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBD6Callbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo);
|
||||
#endif
|
||||
void setupSensorDataCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeOdomInfo);
|
||||
void setupScanCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeOdomInfo);
|
||||
void setupOdomCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeUserData,
|
||||
bool subscribeOdomInfo);
|
||||
|
||||
protected:
|
||||
std::string subscribedTopicsMsg_;
|
||||
int topicQueueSize_;
|
||||
int syncQueueSize_;
|
||||
rmw_qos_reliability_policy_t qosOdom_;
|
||||
rmw_qos_reliability_policy_t qosImage_;
|
||||
rmw_qos_reliability_policy_t qosCameraInfo_;
|
||||
rmw_qos_reliability_policy_t qosScan_;
|
||||
rmw_qos_reliability_policy_t qosUserData_;
|
||||
rmw_qos_reliability_policy_t qosSensorData_;
|
||||
|
||||
private:
|
||||
bool approxSync_;
|
||||
bool subscribedToDepth_;
|
||||
bool subscribedToStereo_;
|
||||
bool subscribedToRGB_;
|
||||
bool subscribedToOdom_;
|
||||
bool subscribedToRGBD_;
|
||||
bool subscribedToSensorData_;
|
||||
bool subscribedToScan2d_;
|
||||
bool subscribedToScan3d_;
|
||||
bool subscribedToScanDescriptor_;
|
||||
bool subscribedToOdomInfo_;
|
||||
bool subscribedToUserData_;
|
||||
std::string odomFrameId_;
|
||||
int rgbdCameras_;
|
||||
std::string name_;
|
||||
|
||||
rclcpp::CallbackGroup::SharedPtr syncCallbackGroup_;
|
||||
|
||||
//for depth and rgb-only callbacks
|
||||
image_transport::SubscriberFilter imageSub_;
|
||||
image_transport::SubscriberFilter imageDepthSub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoSub_;
|
||||
|
||||
//for rgbd callback
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::RGBDImage>::ConstSharedPtr rgbdSub_;
|
||||
std::vector<message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>*> rgbdSubs_;
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::RGBDImages>::ConstSharedPtr rgbdXSubOnly_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImages> rgbdXSub_;
|
||||
|
||||
//for sensor data callback
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::SensorData>::ConstSharedPtr sensorDataSubOnly_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::SensorData> sensorDataSub_;
|
||||
|
||||
//stereo callback
|
||||
image_transport::SubscriberFilter imageRectLeft_;
|
||||
image_transport::SubscriberFilter imageRectRight_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoLeft_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoRight_;
|
||||
|
||||
message_filters::Subscriber<nav_msgs::msg::Odometry> odomSub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::UserData> userDataSub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::LaserScan> scanSub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::PointCloud2> scan3dSub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::ScanDescriptor> scanDescSub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::OdomInfo> odomInfoSub_;
|
||||
|
||||
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::ConstSharedPtr scan2dSubOnly_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::ConstSharedPtr scan3dSubOnly_;
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::ScanDescriptor>::ConstSharedPtr scanDescSubOnly_;
|
||||
rclcpp::Subscription<nav_msgs::msg::Odometry>::ConstSharedPtr odomSubOnly_;
|
||||
|
||||
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
|
||||
|
||||
// RGB + Depth
|
||||
DATA_SYNCS3(depth, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo)
|
||||
DATA_SYNCS4(depthScan2d, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS4(depthScan3d, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS4(depthScanDesc, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS4(depthInfo, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS5(depthScan2dInfo, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS5(depthScan3dInfo, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS5(depthScanDescInfo, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// RGB + Depth + Odom
|
||||
DATA_SYNCS4(depthOdom, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo)
|
||||
DATA_SYNCS5(depthOdomScan2d, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS5(depthOdomScan3d, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS5(depthOdomScanDesc, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS5(depthOdomInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS6(depthOdomScan2dInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS6(depthOdomScan3dInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS6(depthOdomScanDescInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// RGB + Depth + User Data
|
||||
DATA_SYNCS4(depthData, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo)
|
||||
DATA_SYNCS5(depthDataScan2d, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS5(depthDataScan3d, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS5(depthDataScanDesc, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS5(depthDataInfo, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS6(depthDataScan2dInfo, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS6(depthDataScan3dInfo, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS6(depthDataScanDescInfo, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// RGB + Depth + Odom + User Data
|
||||
DATA_SYNCS5(depthOdomData, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo)
|
||||
DATA_SYNCS6(depthOdomDataScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS6(depthOdomDataScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS6(depthOdomDataScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS6(depthOdomDataInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS7(depthOdomDataScan2dInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS7(depthOdomDataScan3dInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS7(depthOdomDataScanDescInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
#endif
|
||||
|
||||
// Stereo
|
||||
DATA_SYNCS4(stereo, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo)
|
||||
DATA_SYNCS5(stereoInfo, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// Stereo + Odom
|
||||
DATA_SYNCS5(stereoOdom, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo)
|
||||
DATA_SYNCS6(stereoOdomInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// RGB-only
|
||||
DATA_SYNCS2(rgb, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo)
|
||||
DATA_SYNCS3(rgbScan2d, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS3(rgbScan3d, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS3(rgbScanDesc, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS3(rgbInfo, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS4(rgbScan2dInfo, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS4(rgbScan3dInfo, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS4(rgbScanDescInfo, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// RGB-only + Odom
|
||||
DATA_SYNCS3(rgbOdom, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo)
|
||||
DATA_SYNCS4(rgbOdomScan2d, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS4(rgbOdomScan3d, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS4(rgbOdomScanDesc, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS4(rgbOdomInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS5(rgbOdomScan2dInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS5(rgbOdomScan3dInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS5(rgbOdomScanDescInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// RGB-only + User Data
|
||||
DATA_SYNCS3(rgbData, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo)
|
||||
DATA_SYNCS4(rgbDataScan2d, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS4(rgbDataScan3d, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS4(rgbDataScanDesc, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS4(rgbDataInfo, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS5(rgbDataScan2dInfo, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS5(rgbDataScan3dInfo, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS5(rgbDataScanDescInfo, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// RGB-only + Odom + User Data
|
||||
DATA_SYNCS4(rgbOdomData, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo)
|
||||
DATA_SYNCS5(rgbOdomDataScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS5(rgbOdomDataScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS5(rgbOdomDataScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS5(rgbOdomDataInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS6(rgbOdomDataScan2dInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS6(rgbOdomDataScan3dInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS6(rgbOdomDataScanDescInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
#endif
|
||||
|
||||
// 1 RGBD
|
||||
void rgbdCallback(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr);
|
||||
DATA_SYNCS2(rgbdScan2d, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS2(rgbdScan3d, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS2(rgbdScanDesc, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS2(rgbdInfo, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// 1 RGBD + Odom
|
||||
DATA_SYNCS2(rgbdOdom, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS3(rgbdOdomScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS3(rgbdOdomScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS3(rgbdOdomScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS3(rgbdOdomInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// 1 RGBD + User Data
|
||||
DATA_SYNCS2(rgbdData, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS3(rgbdDataScan2d, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS3(rgbdDataScan3d, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS3(rgbdDataScanDesc, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS3(rgbdDataInfo, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// 1 RGBD + Odom + User Data
|
||||
DATA_SYNCS3(rgbdOdomData, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS4(rgbdOdomDataScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS4(rgbdOdomDataScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS4(rgbdOdomDataScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS4(rgbdOdomDataInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
#endif
|
||||
|
||||
// X RGBD
|
||||
void rgbdXCallback(const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr);
|
||||
DATA_SYNCS2(rgbdXScan2d, rtabmap_msgs::msg::RGBDImages, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS2(rgbdXScan3d, rtabmap_msgs::msg::RGBDImages, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS2(rgbdXScanDesc, rtabmap_msgs::msg::RGBDImages, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS2(rgbdXInfo, rtabmap_msgs::msg::RGBDImages, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// X RGBD + Odom
|
||||
DATA_SYNCS2(rgbdXOdom, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImages)
|
||||
DATA_SYNCS3(rgbdXOdomScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImages, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS3(rgbdXOdomScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImages, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS3(rgbdXOdomScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImages, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS3(rgbdXOdomInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImages, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// X RGBD + User Data
|
||||
DATA_SYNCS2(rgbdXData, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages)
|
||||
DATA_SYNCS3(rgbdXDataScan2d, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS3(rgbdXDataScan3d, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS3(rgbdXDataScanDesc, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS3(rgbdXDataInfo, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// X RGBD + Odom + User Data
|
||||
DATA_SYNCS3(rgbdXOdomData, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages)
|
||||
DATA_SYNCS4(rgbdXOdomDataScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS4(rgbdXOdomDataScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS4(rgbdXOdomDataScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS4(rgbdXOdomDataInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages, rtabmap_msgs::msg::OdomInfo)
|
||||
#endif
|
||||
|
||||
// SensorData
|
||||
void sensorDataCallback(const rtabmap_msgs::msg::SensorData::ConstSharedPtr);
|
||||
DATA_SYNCS2(sensorDataInfo, rtabmap_msgs::msg::SensorData, rtabmap_msgs::msg::OdomInfo);
|
||||
|
||||
// SensorData + Odom
|
||||
DATA_SYNCS2(sensorDataOdom, nav_msgs::msg::Odometry, rtabmap_msgs::msg::SensorData);
|
||||
DATA_SYNCS3(sensorDataOdomInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::SensorData, rtabmap_msgs::msg::OdomInfo);
|
||||
|
||||
#ifdef RTABMAP_SYNC_MULTI_RGBD
|
||||
// 2 RGBD
|
||||
DATA_SYNCS2(rgbd2, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS3(rgbd2Scan2d, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS3(rgbd2Scan3d, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS3(rgbd2ScanDesc, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS3(rgbd2Info, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
// 2 RGBD + Odom
|
||||
DATA_SYNCS3(rgbd2Odom, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS4(rgbd2OdomScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS4(rgbd2OdomScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS4(rgbd2OdomScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS4(rgbd2OdomInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// 2 RGBD + User Data
|
||||
DATA_SYNCS3(rgbd2Data, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS4(rgbd2DataScan2d, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS4(rgbd2DataScan3d, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS4(rgbd2DataScanDesc, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS4(rgbd2DataInfo, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// 2 RGBD + Odom + User Data
|
||||
DATA_SYNCS4(rgbd2OdomData, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS5(rgbd2OdomDataScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS5(rgbd2OdomDataScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS5(rgbd2OdomDataScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS5(rgbd2OdomDataInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
#endif
|
||||
|
||||
// 3 RGBD
|
||||
DATA_SYNCS3(rgbd3, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS4(rgbd3Scan2d, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS4(rgbd3Scan3d, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS4(rgbd3ScanDesc, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS4(rgbd3Info, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// 3 RGBD + Odom
|
||||
DATA_SYNCS4(rgbd3Odom, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS5(rgbd3OdomScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS5(rgbd3OdomScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS5(rgbd3OdomScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS5(rgbd3OdomInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// 3 RGBD + User Data
|
||||
DATA_SYNCS4(rgbd3Data, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS5(rgbd3DataScan2d, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS5(rgbd3DataScan3d, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS5(rgbd3DataScanDesc, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS5(rgbd3DataInfo, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// 3 RGBD + Odom + User Data
|
||||
DATA_SYNCS5(rgbd3OdomData, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS6(rgbd3OdomDataScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS6(rgbd3OdomDataScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS6(rgbd3OdomDataScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS6(rgbd3OdomDataInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
#endif
|
||||
|
||||
// 4 RGBD
|
||||
DATA_SYNCS4(rgbd4, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS5(rgbd4Scan2d, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS5(rgbd4Scan3d, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS5(rgbd4ScanDesc, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS5(rgbd4Info, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// 4 RGBD + Odom
|
||||
DATA_SYNCS5(rgbd4Odom, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS6(rgbd4OdomScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS6(rgbd4OdomScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS6(rgbd4OdomScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS6(rgbd4OdomInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// 4 RGBD + User Data
|
||||
DATA_SYNCS5(rgbd4Data, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS6(rgbd4DataScan2d, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS6(rgbd4DataScan3d, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS6(rgbd4DataScanDesc, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS6(rgbd4DataInfo, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
// 4 RGBD + Odom + User Data
|
||||
DATA_SYNCS6(rgbd4OdomData, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS7(rgbd4OdomDataScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS7(rgbd4OdomDataScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS7(rgbd4OdomDataScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS7(rgbd4OdomDataInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
#endif
|
||||
|
||||
// 5 RGBD
|
||||
DATA_SYNCS5(rgbd5, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS6(rgbd5Scan2d, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS6(rgbd5Scan3d, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS6(rgbd5ScanDesc, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS6(rgbd5Info, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// 5 RGBD + Odom
|
||||
DATA_SYNCS6(rgbd5Odom, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS7(rgbd5OdomScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS7(rgbd5OdomScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS7(rgbd5OdomScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS7(rgbd5OdomInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// 6 RGBD
|
||||
DATA_SYNCS6(rgbd6, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS7(rgbd6Scan2d, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS7(rgbd6Scan3d, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS7(rgbd6ScanDesc, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS7(rgbd6Info, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// 6 RGBD + Odom
|
||||
DATA_SYNCS7(rgbd6Odom, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS8(rgbd6OdomScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS8(rgbd6OdomScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS8(rgbd6OdomScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS8(rgbd6OdomInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
#endif //RTABMAP_SYNC_MULTI_RGBD
|
||||
|
||||
// Scan
|
||||
void scan2dCallback(const sensor_msgs::msg::LaserScan::ConstSharedPtr);
|
||||
void scan3dCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr);
|
||||
void scanDescCallback(const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr);
|
||||
DATA_SYNCS2(scan2dInfo, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS2(scan3dInfo, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS2(scanDescInfo, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// Scan + Odom
|
||||
DATA_SYNCS2(odomScan2d, nav_msgs::msg::Odometry, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS2(odomScan3d, nav_msgs::msg::Odometry, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS2(odomScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS3(odomScan2dInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS3(odomScan3dInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS3(odomScanDescInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// Scan + User Data
|
||||
DATA_SYNCS2(dataScan2d, rtabmap_msgs::msg::UserData, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS2(dataScan3d, rtabmap_msgs::msg::UserData, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS2(dataScanDesc, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS3(dataScan2dInfo, rtabmap_msgs::msg::UserData, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS3(dataScan3dInfo, rtabmap_msgs::msg::UserData, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS3(dataScanDescInfo, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// Scan + Odom + User Data
|
||||
DATA_SYNCS3(odomDataScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS3(odomDataScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS3(odomDataScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS4(odomDataScan2dInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS4(odomDataScan3dInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS4(odomDataScanDescInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
#endif
|
||||
|
||||
// Odom
|
||||
void odomCallback(const nav_msgs::msg::Odometry::ConstSharedPtr);
|
||||
DATA_SYNCS2(odomInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// Odom + User Data
|
||||
DATA_SYNCS2(odomData, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData)
|
||||
DATA_SYNCS3(odomDataInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::OdomInfo)
|
||||
#endif
|
||||
};
|
||||
|
||||
} /* namespace rtabmap_ros */
|
||||
|
||||
#endif /* INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_ */
|
||||
@@ -0,0 +1,264 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, 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 COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_
|
||||
#define INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_
|
||||
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap_sync/GetTopicName.h>
|
||||
|
||||
#define DATA_SYNC2(PREFIX, SYNC_NAME, MSG0, MSG1) \
|
||||
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1> PREFIX##SYNC_NAME##SyncPolicy; \
|
||||
message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
|
||||
|
||||
#define DATA_SYNCS2(PREFIX, MSG0, MSG1) \
|
||||
DATA_SYNC2(PREFIX, Approximate, MSG0, MSG1) \
|
||||
DATA_SYNC2(PREFIX, Exact, MSG0, MSG1) \
|
||||
void PREFIX##Callback(const MSG0 ::ConstSharedPtr, const MSG1 ::ConstSharedPtr);
|
||||
|
||||
#define DATA_SYNC3(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2) \
|
||||
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2> PREFIX##SYNC_NAME##SyncPolicy; \
|
||||
message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
|
||||
|
||||
#define DATA_SYNCS3(PREFIX, MSG0, MSG1, MSG2) \
|
||||
DATA_SYNC3(PREFIX, Approximate, MSG0, MSG1, MSG2) \
|
||||
DATA_SYNC3(PREFIX, Exact, MSG0, MSG1, MSG2) \
|
||||
void PREFIX##Callback(const MSG0 ::ConstSharedPtr, const MSG1 ::ConstSharedPtr, const MSG2 ::ConstSharedPtr);
|
||||
|
||||
#define DATA_SYNC4(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3) \
|
||||
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2, MSG3> PREFIX##SYNC_NAME##SyncPolicy; \
|
||||
message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
|
||||
|
||||
#define DATA_SYNCS4(PREFIX, MSG0, MSG1, MSG2, MSG3) \
|
||||
DATA_SYNC4(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3) \
|
||||
DATA_SYNC4(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3) \
|
||||
void PREFIX##Callback(const MSG0 ::ConstSharedPtr, const MSG1 ::ConstSharedPtr, const MSG2 ::ConstSharedPtr, const MSG3 ::ConstSharedPtr);
|
||||
|
||||
#define DATA_SYNC5(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3, MSG4) \
|
||||
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2, MSG3, MSG4> PREFIX##SYNC_NAME##SyncPolicy; \
|
||||
message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
|
||||
|
||||
#define DATA_SYNCS5(PREFIX, MSG0, MSG1, MSG2, MSG3, MSG4) \
|
||||
DATA_SYNC5(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3, MSG4) \
|
||||
DATA_SYNC5(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3, MSG4) \
|
||||
void PREFIX##Callback(const MSG0 ::ConstSharedPtr, const MSG1 ::ConstSharedPtr, const MSG2 ::ConstSharedPtr, const MSG3 ::ConstSharedPtr, const MSG4 ::ConstSharedPtr);
|
||||
|
||||
#define DATA_SYNC6(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \
|
||||
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2, MSG3, MSG4, MSG5> PREFIX##SYNC_NAME##SyncPolicy; \
|
||||
message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
|
||||
|
||||
#define DATA_SYNCS6(PREFIX, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \
|
||||
DATA_SYNC6(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \
|
||||
DATA_SYNC6(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \
|
||||
void PREFIX##Callback(const MSG0 ::ConstSharedPtr, const MSG1 ::ConstSharedPtr, const MSG2 ::ConstSharedPtr, const MSG3 ::ConstSharedPtr, const MSG4 ::ConstSharedPtr, const MSG5 ::ConstSharedPtr);
|
||||
|
||||
#define DATA_SYNC7(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6) \
|
||||
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6> PREFIX##SYNC_NAME##SyncPolicy; \
|
||||
message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
|
||||
|
||||
#define DATA_SYNCS7(PREFIX, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6) \
|
||||
DATA_SYNC7(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6) \
|
||||
DATA_SYNC7(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6) \
|
||||
void PREFIX##Callback(const MSG0 ::ConstSharedPtr, const MSG1 ::ConstSharedPtr, const MSG2 ::ConstSharedPtr, const MSG3 ::ConstSharedPtr, const MSG4 ::ConstSharedPtr, const MSG5 ::ConstSharedPtr, const MSG6 ::ConstSharedPtr);
|
||||
|
||||
#define DATA_SYNC8(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6, MSG7) \
|
||||
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6, MSG7> PREFIX##SYNC_NAME##SyncPolicy; \
|
||||
message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
|
||||
|
||||
#define DATA_SYNCS8(PREFIX, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6, MSG7) \
|
||||
DATA_SYNC8(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6, MSG7) \
|
||||
DATA_SYNC8(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6, MSG7) \
|
||||
void PREFIX##Callback(const MSG0 ::ConstSharedPtr, const MSG1 ::ConstSharedPtr, const MSG2 ::ConstSharedPtr, const MSG3 ::ConstSharedPtr, const MSG4 ::ConstSharedPtr, const MSG5 ::ConstSharedPtr, const MSG6 ::ConstSharedPtr, const MSG7 ::ConstSharedPtr);
|
||||
|
||||
|
||||
// Constructor
|
||||
#define SYNC_INIT(PREFIX) \
|
||||
PREFIX##ApproximateSync_(0), \
|
||||
PREFIX##ExactSync_(0)
|
||||
|
||||
// Destructor
|
||||
#define SYNC_DEL(PREFIX) \
|
||||
if(PREFIX##ApproximateSync_) delete PREFIX##ApproximateSync_; \
|
||||
if(PREFIX##ExactSync_) delete PREFIX##ExactSync_;
|
||||
|
||||
// Sync declarations
|
||||
#define SYNC_DECL2(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1) \
|
||||
if(APPROX) \
|
||||
{ \
|
||||
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
|
||||
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1); \
|
||||
PREFIX##ApproximateSync_->registerCallback(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2)); \
|
||||
} \
|
||||
else \
|
||||
{ \
|
||||
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
|
||||
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1); \
|
||||
PREFIX##ExactSync_->registerCallback(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2)); \
|
||||
} \
|
||||
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s", \
|
||||
name_.c_str(), \
|
||||
APPROX?"approx":"exact", \
|
||||
getTopicName(SUB0.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB1.getSubscriber()).c_str());
|
||||
|
||||
#define SYNC_DECL3(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2) \
|
||||
if(APPROX) \
|
||||
{ \
|
||||
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
|
||||
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2); \
|
||||
PREFIX##ApproximateSync_->registerCallback(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); \
|
||||
} \
|
||||
else \
|
||||
{ \
|
||||
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
|
||||
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2); \
|
||||
PREFIX##ExactSync_->registerCallback(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); \
|
||||
} \
|
||||
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s", \
|
||||
name_.c_str(), \
|
||||
APPROX?"approx":"exact", \
|
||||
getTopicName(SUB0.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB1.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB2.getSubscriber()).c_str());
|
||||
|
||||
#define SYNC_DECL4(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3) \
|
||||
if(APPROX) \
|
||||
{ \
|
||||
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
|
||||
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3); \
|
||||
PREFIX##ApproximateSync_->registerCallback(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); \
|
||||
} \
|
||||
else \
|
||||
{ \
|
||||
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
|
||||
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3); \
|
||||
PREFIX##ExactSync_->registerCallback(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); \
|
||||
} \
|
||||
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s", \
|
||||
name_.c_str(), \
|
||||
APPROX?"approx":"exact", \
|
||||
getTopicName(SUB0.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB1.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB2.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB3.getSubscriber()).c_str());
|
||||
|
||||
#define SYNC_DECL5(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4) \
|
||||
if(APPROX) \
|
||||
{ \
|
||||
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
|
||||
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4); \
|
||||
PREFIX##ApproximateSync_->registerCallback(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5)); \
|
||||
} \
|
||||
else \
|
||||
{ \
|
||||
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
|
||||
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4); \
|
||||
PREFIX##ExactSync_->registerCallback(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5)); \
|
||||
} \
|
||||
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s", \
|
||||
name_.c_str(), \
|
||||
APPROX?"approx":"exact", \
|
||||
getTopicName(SUB0.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB1.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB2.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB3.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB4.getSubscriber()).c_str());
|
||||
|
||||
#define SYNC_DECL6(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5) \
|
||||
if(APPROX) \
|
||||
{ \
|
||||
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
|
||||
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5); \
|
||||
PREFIX##ApproximateSync_->registerCallback(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6)); \
|
||||
} \
|
||||
else \
|
||||
{ \
|
||||
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
|
||||
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5); \
|
||||
PREFIX##ExactSync_->registerCallback(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6)); \
|
||||
} \
|
||||
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s", \
|
||||
name_.c_str(), \
|
||||
APPROX?"approx":"exact", \
|
||||
getTopicName(SUB0.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB1.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB2.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB3.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB4.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB5.getSubscriber()).c_str());
|
||||
|
||||
#define SYNC_DECL7(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6) \
|
||||
if(APPROX) \
|
||||
{ \
|
||||
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
|
||||
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6); \
|
||||
PREFIX##ApproximateSync_->registerCallback(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7)); \
|
||||
} \
|
||||
else \
|
||||
{ \
|
||||
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
|
||||
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6); \
|
||||
PREFIX##ExactSync_->registerCallback(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7)); \
|
||||
} \
|
||||
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s", \
|
||||
name_.c_str(), \
|
||||
APPROX?"approx":"exact", \
|
||||
getTopicName(SUB0.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB1.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB2.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB3.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB4.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB5.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB6.getSubscriber()).c_str());
|
||||
|
||||
#define SYNC_DECL8(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6, SUB7) \
|
||||
if(APPROX) \
|
||||
{ \
|
||||
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
|
||||
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6, SUB7); \
|
||||
PREFIX##ApproximateSync_->registerCallback(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, std::placeholders::_8)); \
|
||||
} \
|
||||
else \
|
||||
{ \
|
||||
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
|
||||
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6, SUB7); \
|
||||
PREFIX##ExactSync_->registerCallback(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, std::placeholders::_8)); \
|
||||
} \
|
||||
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s", \
|
||||
name_.c_str(), \
|
||||
APPROX?"approx":"exact", \
|
||||
getTopicName(SUB0.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB1.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB2.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB3.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB4.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB5.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB6.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB7.getSubscriber()).c_str());
|
||||
|
||||
|
||||
#endif /* INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_ */
|
||||
@@ -0,0 +1,35 @@
|
||||
/*
|
||||
* 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_ */
|
||||
@@ -0,0 +1,182 @@
|
||||
#ifndef INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_
|
||||
#define INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_
|
||||
|
||||
#include "rtabmap/utilite/UStl.h"
|
||||
|
||||
#include <diagnostic_updater/diagnostic_updater.hpp>
|
||||
#include <diagnostic_updater/publisher.hpp>
|
||||
|
||||
#include "rtabmap_conversions/MsgConversion.h"
|
||||
#include "rtabmap/utilite/ULogger.h"
|
||||
#include "rtabmap/utilite/UMutex.h"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
namespace rtabmap_sync {
|
||||
|
||||
class SyncDiagnostic {
|
||||
public:
|
||||
SyncDiagnostic(rclcpp::Node * node, double tolerance = 0.2, int windowSize = 5) :
|
||||
node_(node),
|
||||
diagnosticUpdater_(node, 2.0),
|
||||
inFrequencyStatus_(diagnostic_updater::FrequencyStatusParam(&inTargetFrequency_, &inTargetFrequency_, tolerance), node->get_clock()),
|
||||
inTimeStampStatus_(diagnostic_updater::TimeStampStatusParam(), node->get_clock()),
|
||||
outFrequencyStatus_(diagnostic_updater::FrequencyStatusParam(&outTargetFrequency_, &outTargetFrequency_, tolerance), node->get_clock()),
|
||||
outTimeStampStatus_(diagnostic_updater::TimeStampStatusParam(), node->get_clock()),
|
||||
inCompositeTask_("Input Status"),
|
||||
outCompositeTask_("Output Status"),
|
||||
lastTickInputStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1),
|
||||
inTargetFrequency_(0.0),
|
||||
outTargetFrequency_(0.0),
|
||||
windowSize_(windowSize),
|
||||
lastTickTime_(0.0)
|
||||
{
|
||||
UASSERT(windowSize_ >= 1);
|
||||
}
|
||||
|
||||
void init(
|
||||
const std::string & topic,
|
||||
const std::string & topicsNotReceivedWarningMsg,
|
||||
std::vector<diagnostic_updater::DiagnosticTask*> otherTasks = std::vector<diagnostic_updater::DiagnosticTask*>())
|
||||
{
|
||||
topicsNotReceivedWarningMsg_ = topicsNotReceivedWarningMsg;
|
||||
|
||||
std::list<std::string> strList = uSplit(topic, '/');
|
||||
for(int i=0; i<2 && strList.size()>1; ++i)
|
||||
{
|
||||
// Assuming format is /back_camera/left/image, we want "back_camera"
|
||||
strList.pop_back();
|
||||
}
|
||||
inCompositeTask_.addTask(&inFrequencyStatus_);
|
||||
inCompositeTask_.addTask(&inTimeStampStatus_);
|
||||
diagnosticUpdater_.add(inCompositeTask_);
|
||||
outCompositeTask_.addTask(&outFrequencyStatus_);
|
||||
outCompositeTask_.addTask(&outTimeStampStatus_);
|
||||
diagnosticUpdater_.add(outCompositeTask_);
|
||||
for(size_t i=0; i<otherTasks.size(); ++i)
|
||||
{
|
||||
diagnosticUpdater_.add(*otherTasks[i]);
|
||||
}
|
||||
diagnosticUpdater_.setHardwareID(strList.empty()?"none":uJoin(strList, "/"));
|
||||
diagnosticUpdater_.force_update();
|
||||
diagnosticTimer_ = node_->create_wall_timer(5s, std::bind(&SyncDiagnostic::diagnosticTimerCallback, this), nullptr);
|
||||
}
|
||||
|
||||
void tickInput(const rclcpp::Time & stamp, double expectedFrequency = 0)
|
||||
{
|
||||
updateFrequency(
|
||||
stamp,
|
||||
expectedFrequency,
|
||||
inFrequencyStatus_,
|
||||
inTimeStampStatus_,
|
||||
inWindow_,
|
||||
inTargetFrequency_,
|
||||
lastTickInputStamp_);
|
||||
}
|
||||
|
||||
void tickOutput(const rclcpp::Time & stamp, double expectedFrequency = 0)
|
||||
{
|
||||
double lastTickOutputStamp;
|
||||
updateFrequency(
|
||||
stamp,
|
||||
expectedFrequency,
|
||||
outFrequencyStatus_,
|
||||
outTimeStampStatus_,
|
||||
outWindow_,
|
||||
outTargetFrequency_,
|
||||
lastTickOutputStamp);
|
||||
}
|
||||
|
||||
private:
|
||||
void diagnosticTimerCallback()
|
||||
{
|
||||
UScopeMutex lock(tickMutex_);
|
||||
if(rtabmap_conversions::timestampFromROS(node_->now())-lastTickInputStamp_ >= 5 && !topicsNotReceivedWarningMsg_.empty())
|
||||
{
|
||||
RCLCPP_WARN(node_->get_logger(), "%s", topicsNotReceivedWarningMsg_.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
void updateFrequency(
|
||||
const rclcpp::Time & stamp,
|
||||
const double & expectedFrequency,
|
||||
diagnostic_updater::FrequencyStatus & freqStatus,
|
||||
diagnostic_updater::TimeStampStatus & timeStatus,
|
||||
std::deque<double> & window,
|
||||
double & targetFrequency,
|
||||
double & lastTickStamp)
|
||||
{
|
||||
UScopeMutex lock(tickMutex_);
|
||||
|
||||
freqStatus.tick();
|
||||
timeStatus.tick(stamp);
|
||||
|
||||
double stampSec = rtabmap_conversions::timestampFromROS(stamp);
|
||||
double singlePeriod = stampSec - lastTickStamp;
|
||||
|
||||
window.push_back(singlePeriod);
|
||||
if(window.size() > windowSize_)
|
||||
{
|
||||
window.pop_front();
|
||||
|
||||
double period = 0.0;
|
||||
if(window.size() == windowSize_)
|
||||
{
|
||||
for(size_t i=0; i<window.size(); ++i)
|
||||
{
|
||||
period += window[i];
|
||||
}
|
||||
period /= windowSize_;
|
||||
}
|
||||
|
||||
if(period>0.0 && expectedFrequency == 0 && (targetFrequency == 0.0 || period < 1.0/targetFrequency))
|
||||
{
|
||||
targetFrequency = 1.0/period;
|
||||
}
|
||||
else if(expectedFrequency>0)
|
||||
{
|
||||
targetFrequency = expectedFrequency;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
lastTickStamp = stampSec;
|
||||
|
||||
double clockNow = rtabmap_conversions::timestampFromROS(node_->now());
|
||||
if(lastTickTime_ > clockNow)
|
||||
{
|
||||
RCLCPP_WARN(node_->get_logger(), "%s: Detected time jump in the past of %f sec, forcing diagnostic update.",
|
||||
node_->get_name(), lastTickTime_ - clockNow);
|
||||
inFrequencyStatus_.clear();
|
||||
outFrequencyStatus_.clear();
|
||||
diagnosticUpdater_.force_update();
|
||||
lastTickInputStamp_ = clockNow;
|
||||
}
|
||||
lastTickTime_ = clockNow;
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Node * node_;
|
||||
std::string topicsNotReceivedWarningMsg_;
|
||||
diagnostic_updater::Updater diagnosticUpdater_;
|
||||
diagnostic_updater::FrequencyStatus inFrequencyStatus_;
|
||||
diagnostic_updater::TimeStampStatus inTimeStampStatus_;
|
||||
diagnostic_updater::FrequencyStatus outFrequencyStatus_;
|
||||
diagnostic_updater::TimeStampStatus outTimeStampStatus_;
|
||||
diagnostic_updater::CompositeDiagnosticTask inCompositeTask_;
|
||||
diagnostic_updater::CompositeDiagnosticTask outCompositeTask_;
|
||||
rclcpp::TimerBase::SharedPtr diagnosticTimer_;
|
||||
double lastTickInputStamp_;
|
||||
double inTargetFrequency_;
|
||||
double outTargetFrequency_;
|
||||
int windowSize_;
|
||||
std::deque<double> inWindow_;
|
||||
std::deque<double> outWindow_;
|
||||
UMutex tickMutex_;
|
||||
double lastTickTime_;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_ */
|
||||
@@ -0,0 +1,80 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, 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 COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <rtabmap_sync/visibility.h>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <sensor_msgs/msg/camera_info.hpp>
|
||||
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <image_transport/subscriber_filter.hpp>
|
||||
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
|
||||
#include "rtabmap_msgs/msg/rgbd_image.hpp"
|
||||
#include "rtabmap_sync/SyncDiagnostic.h"
|
||||
|
||||
namespace rtabmap_sync
|
||||
{
|
||||
|
||||
class RGBSync : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
RTABMAP_SYNC_PUBLIC
|
||||
explicit RGBSync(const rclcpp::NodeOptions & options);
|
||||
|
||||
virtual ~RGBSync();
|
||||
|
||||
void callback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr image,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo);
|
||||
|
||||
private:
|
||||
double compressedRate_;
|
||||
|
||||
rclcpp::Time lastCompressedPublished_;
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageCompressedPub_;
|
||||
|
||||
image_transport::SubscriberFilter imageSub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoSub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyApproxSyncPolicy;
|
||||
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyExactSyncPolicy;
|
||||
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
|
||||
|
||||
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,85 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, 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 COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <rtabmap_sync/visibility.h>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <sensor_msgs/msg/camera_info.hpp>
|
||||
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <image_transport/subscriber_filter.hpp>
|
||||
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
|
||||
#include "rtabmap_msgs/msg/rgbd_image.hpp"
|
||||
#include "rtabmap_sync/SyncDiagnostic.h"
|
||||
|
||||
namespace rtabmap_sync
|
||||
{
|
||||
|
||||
class RGBDSync : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
RTABMAP_SYNC_PUBLIC
|
||||
explicit RGBDSync(const rclcpp::NodeOptions & options);
|
||||
|
||||
virtual ~RGBDSync();
|
||||
|
||||
void callback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr image,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depth,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo);
|
||||
|
||||
private:
|
||||
double depthScale_;
|
||||
int decimation_;
|
||||
double compressedRate_;
|
||||
double approxSyncMaxInterval_;
|
||||
|
||||
rclcpp::Time lastCompressedPublished_;
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageCompressedPub_;
|
||||
|
||||
image_transport::SubscriberFilter imageSub_;
|
||||
image_transport::SubscriberFilter imageDepthSub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoSub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyApproxSyncDepthPolicy;
|
||||
message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
|
||||
|
||||
typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyExactSyncDepthPolicy;
|
||||
message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
|
||||
|
||||
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,80 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, 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 COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <rtabmap_sync/visibility.h>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <sensor_msgs/msg/camera_info.hpp>
|
||||
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <image_transport/subscriber_filter.hpp>
|
||||
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
|
||||
#include "rtabmap_msgs/msg/rgbd_image.hpp"
|
||||
#include "rtabmap_msgs/msg/rgbd_images.hpp"
|
||||
#include "rtabmap_sync/CommonDataSubscriber.h"
|
||||
#include "rtabmap_sync/SyncDiagnostic.h"
|
||||
|
||||
namespace rtabmap_sync
|
||||
{
|
||||
|
||||
class RGBDXSync : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
RTABMAP_SYNC_PUBLIC
|
||||
explicit RGBDXSync(const rclcpp::NodeOptions & options);
|
||||
|
||||
virtual ~RGBDXSync();
|
||||
|
||||
void callback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr image,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depth,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo);
|
||||
|
||||
private:
|
||||
DATA_SYNCS2(rgbd2, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS3(rgbd3, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS4(rgbd4, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS5(rgbd5, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS6(rgbd6, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS7(rgbd7, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS8(rgbd8, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
|
||||
private:
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr rgbdImagesPub_;
|
||||
|
||||
std::vector<message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>*> rgbdSubs_;
|
||||
|
||||
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,82 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, 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 COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <rtabmap_sync/visibility.h>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <sensor_msgs/msg/camera_info.hpp>
|
||||
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <image_transport/subscriber_filter.hpp>
|
||||
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
|
||||
#include "rtabmap_msgs/msg/rgbd_image.hpp"
|
||||
#include "rtabmap_sync/SyncDiagnostic.h"
|
||||
|
||||
namespace rtabmap_sync
|
||||
{
|
||||
|
||||
class StereoSync : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
RTABMAP_SYNC_PUBLIC
|
||||
explicit StereoSync(const rclcpp::NodeOptions & options);
|
||||
|
||||
virtual ~StereoSync();
|
||||
|
||||
void callback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageLeft,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageRight,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight);
|
||||
private:
|
||||
double compressedRate_;
|
||||
rclcpp::Time lastCompressedPublished_;
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageCompressedPub_;
|
||||
|
||||
image_transport::SubscriberFilter imageLeftSub_;
|
||||
image_transport::SubscriberFilter imageRightSub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoLeftSub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoRightSub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo> MyApproxSyncPolicy;
|
||||
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo> MyExactSyncPolicy;
|
||||
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
|
||||
|
||||
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,59 @@
|
||||
// Copyright 2016 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef RTABMAP_SYNC__VISIBILITY_CONTROL_H_
|
||||
#define RTABMAP_SYNC__VISIBILITY_CONTROL_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
|
||||
// https://gcc.gnu.org/wiki/Visibility
|
||||
|
||||
#if defined _WIN32 || defined __CYGWIN__
|
||||
#ifdef __GNUC__
|
||||
#define RTABMAP_SYNC_EXPORT __attribute__ ((dllexport))
|
||||
#define RTABMAP_SYNC_IMPORT __attribute__ ((dllimport))
|
||||
#else
|
||||
#define RTABMAP_SYNC_EXPORT __declspec(dllexport)
|
||||
#define RTABMAP_SYNC_IMPORT __declspec(dllimport)
|
||||
#endif
|
||||
#ifdef RTABMAP_SYNC_BUILDING_DLL
|
||||
#define RTABMAP_SYNC_PUBLIC RTABMAP_SYNC_EXPORT
|
||||
#else
|
||||
#define RTABMAP_SYNC_PUBLIC RTABMAP_SYNC_IMPORT
|
||||
#endif
|
||||
#define RTABMAP_SYNC_PUBLIC_TYPE RTABMAP_SYNC_PUBLIC
|
||||
#define RTABMAP_SYNC_LOCAL
|
||||
#else
|
||||
#define RTABMAP_SYNC_EXPORT __attribute__ ((visibility("default")))
|
||||
#define RTABMAP_SYNC_IMPORT
|
||||
#if __GNUC__ >= 4
|
||||
#define RTABMAP_SYNC_PUBLIC __attribute__ ((visibility("default")))
|
||||
#define RTABMAP_SYNC_LOCAL __attribute__ ((visibility("hidden")))
|
||||
#else
|
||||
#define RTABMAP_SYNC_PUBLIC
|
||||
#define RTABMAP_SYNC_LOCAL
|
||||
#endif
|
||||
#define RTABMAP_SYNC_PUBLIC_TYPE
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // RTABMAP_SYNC__VISIBILITY_CONTROL_H_
|
||||
|
||||
Reference in New Issue
Block a user