From ed3508fe4b91f9a8647afe15b94d8f38516532d4 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 2 Apr 2017 16:55:38 -0400 Subject: [PATCH] Added OdomInfo + scan2d/scan3d synchronization interface --- include/rtabmap_ros/CommonDataSubscriber.h | 24 +++ .../rtabmap_ros/CommonDataSubscriberDefines.h | 34 ++++ src/CommonDataSubscriber.cpp | 50 ++++- src/impl/CommonDataSubscriberDepth.cpp | 185 ++++++++++++++++- src/impl/CommonDataSubscriberRGBD.cpp | 192 +++++++++++++++++- src/impl/CommonDataSubscriberRGBD2.cpp | 192 +++++++++++++++++- 6 files changed, 652 insertions(+), 25 deletions(-) diff --git a/include/rtabmap_ros/CommonDataSubscriber.h b/include/rtabmap_ros/CommonDataSubscriber.h index 9b50bc0d..50b406dc 100644 --- a/include/rtabmap_ros/CommonDataSubscriber.h +++ b/include/rtabmap_ros/CommonDataSubscriber.h @@ -182,24 +182,32 @@ private: DATA_SYNCS4(depthScan2d, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan); DATA_SYNCS4(depthScan3d, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2); DATA_SYNCS4(depthInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo); + DATA_SYNCS5(depthScan2dInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); + DATA_SYNCS5(depthScan3dInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); // RGB + Depth + Odom DATA_SYNCS4(depthOdom, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo); DATA_SYNCS5(depthOdomScan2d, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan); DATA_SYNCS5(depthOdomScan3d, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2); DATA_SYNCS5(depthOdomInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo); + DATA_SYNCS6(depthOdomScan2dInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); + DATA_SYNCS6(depthOdomScan3dInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); // RGB + Depth + User Data DATA_SYNCS4(depthData, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo); DATA_SYNCS5(depthDataScan2d, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan); DATA_SYNCS5(depthDataScan3d, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2); DATA_SYNCS5(depthDataInfo, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo); + DATA_SYNCS6(depthDataScan2dInfo, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); + DATA_SYNCS6(depthDataScan3dInfo, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); // RGB + Depth + Odom + User Data DATA_SYNCS5(depthOdomData, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo); DATA_SYNCS6(depthOdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan); DATA_SYNCS6(depthOdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2); DATA_SYNCS6(depthOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo); + DATA_SYNCS7(depthOdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); + DATA_SYNCS7(depthOdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); // Stereo DATA_SYNCS4(stereo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo); @@ -214,48 +222,64 @@ private: DATA_SYNCS2(rgbdScan2d, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); DATA_SYNCS2(rgbdScan3d, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2); DATA_SYNCS2(rgbdInfo, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo); + DATA_SYNCS3(rgbdScan2dInfo, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); + DATA_SYNCS3(rgbdScan3dInfo, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); // 1 RGBD + Odom DATA_SYNCS2(rgbdOdom, nav_msgs::Odometry, rtabmap_ros::RGBDImage); DATA_SYNCS3(rgbdOdomScan2d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); DATA_SYNCS3(rgbdOdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2); DATA_SYNCS3(rgbdOdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo); + DATA_SYNCS4(rgbdOdomScan2dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); + DATA_SYNCS4(rgbdOdomScan3dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); // 1 RGBD + User Data DATA_SYNCS2(rgbdData, rtabmap_ros::UserData, rtabmap_ros::RGBDImage); DATA_SYNCS3(rgbdDataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); DATA_SYNCS3(rgbdDataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2); DATA_SYNCS3(rgbdDataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo); + DATA_SYNCS4(rgbdDataScan2dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); + DATA_SYNCS4(rgbdDataScan3dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); // 1 RGBD + Odom + User Data DATA_SYNCS3(rgbdOdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage); DATA_SYNCS4(rgbdOdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); DATA_SYNCS4(rgbdOdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2); DATA_SYNCS4(rgbdOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo); + DATA_SYNCS5(rgbdOdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); + DATA_SYNCS5(rgbdOdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); // 2 RGBD DATA_SYNCS2(rgbd2, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage); DATA_SYNCS3(rgbd2Scan2d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); DATA_SYNCS3(rgbd2Scan3d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2); DATA_SYNCS3(rgbd2Info, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo); + DATA_SYNCS4(rgbd2Scan2dInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); + DATA_SYNCS4(rgbd2Scan3dInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); // 2 RGBD + Odom DATA_SYNCS3(rgbd2Odom, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage); DATA_SYNCS4(rgbd2OdomScan2d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); DATA_SYNCS4(rgbd2OdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2); DATA_SYNCS4(rgbd2OdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo); + DATA_SYNCS5(rgbd2OdomScan2dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); + DATA_SYNCS5(rgbd2OdomScan3dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); // 2 RGBD + User Data DATA_SYNCS3(rgbd2Data, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage); DATA_SYNCS4(rgbd2DataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); DATA_SYNCS4(rgbd2DataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2); DATA_SYNCS4(rgbd2DataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo); + DATA_SYNCS5(rgbd2DataScan2dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); + DATA_SYNCS5(rgbd2DataScan3dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); // 2 RGBD + Odom + User Data DATA_SYNCS4(rgbd2OdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage); DATA_SYNCS5(rgbd2OdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); DATA_SYNCS5(rgbd2OdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2); DATA_SYNCS5(rgbd2OdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo); + DATA_SYNCS6(rgbd2OdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); + DATA_SYNCS6(rgbd2OdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); }; diff --git a/include/rtabmap_ros/CommonDataSubscriberDefines.h b/include/rtabmap_ros/CommonDataSubscriberDefines.h index a88aa5ae..ef840463 100644 --- a/include/rtabmap_ros/CommonDataSubscriberDefines.h +++ b/include/rtabmap_ros/CommonDataSubscriberDefines.h @@ -75,6 +75,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. DATA_SYNC6(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \ void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&, const MSG2##ConstPtr&, const MSG3##ConstPtr&, const MSG4##ConstPtr&, const MSG5##ConstPtr&); \ +#define DATA_SYNC7(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6) \ + typedef message_filters::sync_policies::SYNC_NAME##Time PREFIX##SYNC_NAME##SyncPolicy; \ + message_filters::Synchronizer * 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##ConstPtr&, const MSG1##ConstPtr&, const MSG2##ConstPtr&, const MSG3##ConstPtr&, const MSG4##ConstPtr&, const MSG5##ConstPtr&, const MSG6##ConstPtr&); \ + + // Constructor #define SYNC_INIT(PREFIX) \ PREFIX##ApproximateSync_(0), \ @@ -191,5 +201,29 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. SUB4.getTopic().c_str(), \ SUB5.getTopic().c_str()); +#define SYNC_DECL7(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6) \ + if(APPROX) \ + { \ + PREFIX##ApproximateSync_ = new message_filters::Synchronizer( \ + PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6); \ + PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7)); \ + } \ + else \ + { \ + PREFIX##ExactSync_ = new message_filters::Synchronizer( \ + PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6); \ + PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _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", \ + SUB0.getTopic().c_str(), \ + SUB1.getTopic().c_str(), \ + SUB2.getTopic().c_str(), \ + SUB3.getTopic().c_str(), \ + SUB4.getTopic().c_str(), \ + SUB5.getTopic().c_str(), \ + SUB6.getTopic().c_str()); + #endif /* INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_ */ diff --git a/src/CommonDataSubscriber.cpp b/src/CommonDataSubscriber.cpp index fc01376d..f6c8521b 100644 --- a/src/CommonDataSubscriber.cpp +++ b/src/CommonDataSubscriber.cpp @@ -46,24 +46,32 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) : SYNC_INIT(depthScan2d), SYNC_INIT(depthScan3d), SYNC_INIT(depthInfo), + SYNC_INIT(depthScan2dInfo), + SYNC_INIT(depthScan3dInfo), // RGB + Depth + Odom SYNC_INIT(depthOdom), SYNC_INIT(depthOdomScan2d), SYNC_INIT(depthOdomScan3d), SYNC_INIT(depthOdomInfo), + SYNC_INIT(depthOdomScan2dInfo), + SYNC_INIT(depthOdomScan3dInfo), // RGB + Depth + User Data SYNC_INIT(depthData), SYNC_INIT(depthDataScan2d), SYNC_INIT(depthDataScan3d), SYNC_INIT(depthDataInfo), + SYNC_INIT(depthDataScan2dInfo), + SYNC_INIT(depthDataScan3dInfo), // RGB + Depth + Odom + User Data SYNC_INIT(depthOdomData), SYNC_INIT(depthOdomDataScan2d), SYNC_INIT(depthOdomDataScan3d), SYNC_INIT(depthOdomDataInfo), + SYNC_INIT(depthOdomDataScan2dInfo), + SYNC_INIT(depthOdomDataScan3dInfo), // Stereo SYNC_INIT(stereo), @@ -77,48 +85,64 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) : SYNC_INIT(rgbdScan2d), SYNC_INIT(rgbdScan3d), SYNC_INIT(rgbdInfo), + SYNC_INIT(rgbdScan2dInfo), + SYNC_INIT(rgbdScan3dInfo), // 1 RGBD + Odom SYNC_INIT(rgbdOdom), SYNC_INIT(rgbdOdomScan2d), SYNC_INIT(rgbdOdomScan3d), SYNC_INIT(rgbdOdomInfo), + SYNC_INIT(rgbdOdomScan2dInfo), + SYNC_INIT(rgbdOdomScan3dInfo), // 1 RGBD + User Data SYNC_INIT(rgbdData), SYNC_INIT(rgbdDataScan2d), SYNC_INIT(rgbdDataScan3d), SYNC_INIT(rgbdDataInfo), + SYNC_INIT(rgbdDataScan2dInfo), + SYNC_INIT(rgbdDataScan3dInfo), // 1 RGBD + Odom + User Data SYNC_INIT(rgbdOdomData), SYNC_INIT(rgbdOdomDataScan2d), SYNC_INIT(rgbdOdomDataScan3d), SYNC_INIT(rgbdOdomDataInfo), + SYNC_INIT(rgbdOdomDataScan2dInfo), + SYNC_INIT(rgbdOdomDataScan3dInfo), // 2 RGBD SYNC_INIT(rgbd2), SYNC_INIT(rgbd2Scan2d), SYNC_INIT(rgbd2Scan3d), SYNC_INIT(rgbd2Info), + SYNC_INIT(rgbd2Scan2dInfo), + SYNC_INIT(rgbd2Scan3dInfo), // 2 RGBD + Odom SYNC_INIT(rgbd2Odom), SYNC_INIT(rgbd2OdomScan2d), SYNC_INIT(rgbd2OdomScan3d), SYNC_INIT(rgbd2OdomInfo), + SYNC_INIT(rgbd2OdomScan2dInfo), + SYNC_INIT(rgbd2OdomScan3dInfo), // 2 RGBD + User Data SYNC_INIT(rgbd2Data), SYNC_INIT(rgbd2DataScan2d), SYNC_INIT(rgbd2DataScan3d), SYNC_INIT(rgbd2DataInfo), + SYNC_INIT(rgbd2DataScan2dInfo), + SYNC_INIT(rgbd2DataScan3dInfo), // 2 RGBD + Odom + User Data SYNC_INIT(rgbd2OdomData), SYNC_INIT(rgbd2OdomDataScan2d), SYNC_INIT(rgbd2OdomDataScan3d), - SYNC_INIT(rgbd2OdomDataInfo) + SYNC_INIT(rgbd2OdomDataInfo), + SYNC_INIT(rgbd2OdomDataScan2dInfo), + SYNC_INIT(rgbd2OdomDataScan3dInfo) { } @@ -280,24 +304,32 @@ CommonDataSubscriber::~CommonDataSubscriber() SYNC_DEL(depthScan2d); SYNC_DEL(depthScan3d); SYNC_DEL(depthInfo); + SYNC_DEL(depthScan2dInfo); + SYNC_DEL(depthScan3dInfo); // RGB + Depth + Odom SYNC_DEL(depthOdom); SYNC_DEL(depthOdomScan2d); SYNC_DEL(depthOdomScan3d); SYNC_DEL(depthOdomInfo); + SYNC_DEL(depthOdomScan2dInfo); + SYNC_DEL(depthOdomScan3dInfo); // RGB + Depth + User Data SYNC_DEL(depthData); SYNC_DEL(depthDataScan2d); SYNC_DEL(depthDataScan3d); SYNC_DEL(depthDataInfo); + SYNC_DEL(depthDataScan2dInfo); + SYNC_DEL(depthDataScan3dInfo); // RGB + Depth + Odom + User Data SYNC_DEL(depthOdomData); SYNC_DEL(depthOdomDataScan2d); SYNC_DEL(depthOdomDataScan3d); SYNC_DEL(depthOdomDataInfo); + SYNC_DEL(depthOdomDataScan2dInfo); + SYNC_DEL(depthOdomDataScan3dInfo); // Stereo SYNC_DEL(stereo); @@ -311,48 +343,64 @@ CommonDataSubscriber::~CommonDataSubscriber() SYNC_DEL(rgbdScan2d); SYNC_DEL(rgbdScan3d); SYNC_DEL(rgbdInfo); + SYNC_DEL(rgbdScan2dInfo); + SYNC_DEL(rgbdScan3dInfo); // 1 RGBD + Odom SYNC_DEL(rgbdOdom); SYNC_DEL(rgbdOdomScan2d); SYNC_DEL(rgbdOdomScan3d); SYNC_DEL(rgbdOdomInfo); + SYNC_DEL(rgbdOdomScan2dInfo); + SYNC_DEL(rgbdOdomScan3dInfo); // 1 RGBD + User Data SYNC_DEL(rgbdData); SYNC_DEL(rgbdDataScan2d); SYNC_DEL(rgbdDataScan3d); SYNC_DEL(rgbdDataInfo); + SYNC_DEL(rgbdDataScan2dInfo); + SYNC_DEL(rgbdDataScan3dInfo); // 1 RGBD + Odom + User Data SYNC_DEL(rgbdOdomData); SYNC_DEL(rgbdOdomDataScan2d); SYNC_DEL(rgbdOdomDataScan3d); SYNC_DEL(rgbdOdomDataInfo); + SYNC_DEL(rgbdOdomDataScan2dInfo); + SYNC_DEL(rgbdOdomDataScan3dInfo); // 2 RGBD SYNC_DEL(rgbd2); SYNC_DEL(rgbd2Scan2d); SYNC_DEL(rgbd2Scan3d); SYNC_DEL(rgbd2Info); + SYNC_DEL(rgbd2Scan2dInfo); + SYNC_DEL(rgbd2Scan3dInfo); // 2 RGBD + Odom SYNC_DEL(rgbd2Odom); SYNC_DEL(rgbd2OdomScan2d); SYNC_DEL(rgbd2OdomScan3d); SYNC_DEL(rgbd2OdomInfo); + SYNC_DEL(rgbd2OdomScan2dInfo); + SYNC_DEL(rgbd2OdomScan3dInfo); // 2 RGBD + User Data SYNC_DEL(rgbd2Data); SYNC_DEL(rgbd2DataScan2d); SYNC_DEL(rgbd2DataScan3d); SYNC_DEL(rgbd2DataInfo); + SYNC_DEL(rgbd2DataScan2dInfo); + SYNC_DEL(rgbd2DataScan3dInfo); // 2 RGBD + Odom + User Data SYNC_DEL(rgbd2OdomData); SYNC_DEL(rgbd2OdomDataScan2d); SYNC_DEL(rgbd2OdomDataScan3d); SYNC_DEL(rgbd2OdomDataInfo); + SYNC_DEL(rgbd2OdomDataScan2dInfo); + SYNC_DEL(rgbd2OdomDataScan3dInfo); for(unsigned int i=0; icameraInfo, scanMsg, scan3dMsg, odomInfoMsg); } +void CommonDataSubscriber::rgbdScan2dInfoCallback( + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbdScan3dInfoCallback( + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} // 1 RGBD camera + Odom void CommonDataSubscriber::rgbdOdomCallback( @@ -139,6 +165,32 @@ void CommonDataSubscriber::rgbdOdomInfoCallback( sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); } +void CommonDataSubscriber::rgbdOdomScan2dInfoCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbdOdomScan3dInfoCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} // 1 RGBD camera + User Data void CommonDataSubscriber::rgbdDataCallback( @@ -193,6 +245,32 @@ void CommonDataSubscriber::rgbdDataInfoCallback( sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); } +void CommonDataSubscriber::rgbdDataScan2dInfoCallback( + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbdDataScan3dInfoCallback( + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} // 1 RGBD camera + Odom + User Data void CommonDataSubscriber::rgbdOdomDataCallback( @@ -247,6 +325,32 @@ void CommonDataSubscriber::rgbdOdomDataInfoCallback( sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); } +void CommonDataSubscriber::rgbdOdomDataScan2dInfoCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbdOdomDataScan3dInfoCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + sensor_msgs::LaserScanConstPtr scanMsg; // Null + commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} void CommonDataSubscriber::setupRGBDCallbacks( ros::NodeHandle & nh, @@ -276,13 +380,31 @@ void CommonDataSubscriber::setupRGBDCallbacks( { subscribedToScan2d_ = true; scanSub_.subscribe(nh, "scan", 1); - SYNC_DECL4(rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_); + if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL5(rgbdOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_); + } + else + { + SYNC_DECL4(rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_); + } } else if(subscribeScan3d) { subscribedToScan3d_ = true; scan3dSub_.subscribe(nh, "scan_cloud", 1); - SYNC_DECL4(rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_); + if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL5(rgbdOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_); + } + else + { + SYNC_DECL4(rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_); + } } else if(subscribeOdomInfo) { @@ -302,13 +424,31 @@ void CommonDataSubscriber::setupRGBDCallbacks( { subscribedToScan2d_ = true; scanSub_.subscribe(nh, "scan", 1); - SYNC_DECL3(rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_); + if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL4(rgbdOdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_); + } + else + { + SYNC_DECL3(rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_); + } } else if(subscribeScan3d) { subscribedToScan3d_ = true; scan3dSub_.subscribe(nh, "scan_cloud", 1); - SYNC_DECL3(rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_); + if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL4(rgbdOdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_); + } + else + { + SYNC_DECL3(rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_); + } } else if(subscribeOdomInfo) { @@ -328,13 +468,31 @@ void CommonDataSubscriber::setupRGBDCallbacks( { subscribedToScan2d_ = true; scanSub_.subscribe(nh, "scan", 1); - SYNC_DECL3(rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_); + if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL4(rgbdDataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_); + } + else + { + SYNC_DECL3(rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_); + } } else if(subscribeScan3d) { subscribedToScan3d_ = true; scan3dSub_.subscribe(nh, "scan_cloud", 1); - SYNC_DECL3(rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_); + if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL4(rgbdDataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_); + } + else + { + SYNC_DECL3(rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_); + } } else if(subscribeOdomInfo) { @@ -353,13 +511,31 @@ void CommonDataSubscriber::setupRGBDCallbacks( { subscribedToScan2d_ = true; scanSub_.subscribe(nh, "scan", 1); - SYNC_DECL2(rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_); + if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL3(rgbdScan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_, odomInfoSub_); + } + else + { + SYNC_DECL2(rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_); + } } else if(subscribeScan3d) { subscribedToScan3d_ = true; scan3dSub_.subscribe(nh, "scan_cloud", 1); - SYNC_DECL2(rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_); + if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL3(rgbdScan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_); + } + else + { + SYNC_DECL2(rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_); + } } else if(subscribeOdomInfo) { diff --git a/src/impl/CommonDataSubscriberRGBD2.cpp b/src/impl/CommonDataSubscriberRGBD2.cpp index ac9dde64..f74e187e 100644 --- a/src/impl/CommonDataSubscriberRGBD2.cpp +++ b/src/impl/CommonDataSubscriberRGBD2.cpp @@ -95,6 +95,32 @@ void CommonDataSubscriber::rgbd2InfoCallback( sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); } +void CommonDataSubscriber::rgbd2Scan2dInfoCallback( + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + IMAGE_CONVERSION(); + + nav_msgs::OdometryConstPtr odomMsg; // Null + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbd2Scan3dInfoCallback( + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + IMAGE_CONVERSION(); + + nav_msgs::OdometryConstPtr odomMsg; // Null + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} // 2 RGBD + Odom void CommonDataSubscriber::rgbd2OdomCallback( @@ -149,6 +175,32 @@ void CommonDataSubscriber::rgbd2OdomInfoCallback( sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); } +void CommonDataSubscriber::rgbd2OdomScan2dInfoCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + IMAGE_CONVERSION(); + + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbd2OdomScan3dInfoCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + IMAGE_CONVERSION(); + + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} // 2 RGBD + User Data void CommonDataSubscriber::rgbd2DataCallback( @@ -203,6 +255,32 @@ void CommonDataSubscriber::rgbd2DataInfoCallback( sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); } +void CommonDataSubscriber::rgbd2DataScan2dInfoCallback( + const rtabmap_ros::UserDataConstPtr& userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + IMAGE_CONVERSION(); + + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbd2DataScan3dInfoCallback( + const rtabmap_ros::UserDataConstPtr& userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + IMAGE_CONVERSION(); + + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} // 2 RGBD + Odom + User Data void CommonDataSubscriber::rgbd2OdomDataCallback( @@ -257,6 +335,32 @@ void CommonDataSubscriber::rgbd2OdomDataInfoCallback( sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); } +void CommonDataSubscriber::rgbd2OdomDataScan2dInfoCallback( + const nav_msgs::OdometryConstPtr& odomMsg, + const rtabmap_ros::UserDataConstPtr& userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + IMAGE_CONVERSION(); + + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbd2OdomDataScan3dInfoCallback( + const nav_msgs::OdometryConstPtr& odomMsg, + const rtabmap_ros::UserDataConstPtr& userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + IMAGE_CONVERSION(); + + sensor_msgs::LaserScanConstPtr scanMsg; // Null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} void CommonDataSubscriber::setupRGBD2Callbacks( ros::NodeHandle & nh, @@ -285,13 +389,31 @@ void CommonDataSubscriber::setupRGBD2Callbacks( { subscribedToScan2d_ = true; scanSub_.subscribe(nh, "scan", 1); - SYNC_DECL5(rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); + if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL6(rgbd2OdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_); + } + else + { + SYNC_DECL5(rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); + } } else if(subscribeScan3d) { subscribedToScan3d_ = true; scan3dSub_.subscribe(nh, "scan_cloud", 1); - SYNC_DECL5(rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); + if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL6(rgbd2OdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_); + } + else + { + SYNC_DECL5(rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); + } } else if(subscribeOdomInfo) { @@ -311,13 +433,31 @@ void CommonDataSubscriber::setupRGBD2Callbacks( { subscribedToScan2d_ = true; scanSub_.subscribe(nh, "scan", 1); - SYNC_DECL4(rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); + if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL5(rgbd2OdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_); + } + else + { + SYNC_DECL4(rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); + } } else if(subscribeScan3d) { subscribedToScan3d_ = true; scan3dSub_.subscribe(nh, "scan_cloud", 1); - SYNC_DECL4(rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); + if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL5(rgbd2OdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_); + } + else + { + SYNC_DECL4(rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); + } } else if(subscribeOdomInfo) { @@ -337,13 +477,31 @@ void CommonDataSubscriber::setupRGBD2Callbacks( { subscribedToScan2d_ = true; scanSub_.subscribe(nh, "scan", 1); - SYNC_DECL4(rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); + if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL5(rgbd2DataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_); + } + else + { + SYNC_DECL4(rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); + } } else if(subscribeScan3d) { subscribedToScan3d_ = true; scan3dSub_.subscribe(nh, "scan_cloud", 1); - SYNC_DECL4(rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); + if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL5(rgbd2DataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_); + } + else + { + SYNC_DECL4(rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); + } } else if(subscribeOdomInfo) { @@ -362,13 +520,31 @@ void CommonDataSubscriber::setupRGBD2Callbacks( { subscribedToScan2d_ = true; scanSub_.subscribe(nh, "scan", 1); - SYNC_DECL3(rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); + if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL4(rgbd2Scan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_); + } + else + { + SYNC_DECL3(rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); + } } else if(subscribeScan3d) { subscribedToScan3d_ = true; scan3dSub_.subscribe(nh, "scan_cloud", 1); - SYNC_DECL3(rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); + if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL4(rgbd2Scan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_); + } + else + { + SYNC_DECL3(rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); + } } else if(subscribeOdomInfo) {