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

Added OdomInfo + scan2d/scan3d synchronization interface

Browse Source
This commit is contained in:
matlabbe
2017-04-02 16:55:38 -04:00
parent 89bc0c29ca
commit ed3508fe4b
6 changed files with 652 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/CommonDataSubscriber.h
+24
View File
@@ -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);
};
include/rtabmap_ros/CommonDataSubscriberDefines.h
+34
View File
@@ -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<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##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>( \
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>( \
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_ */
src/CommonDataSubscriber.cpp
+49 -1
View File
@@ -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; i<rgbdSubs_.size(); ++i)
{
src/impl/CommonDataSubscriberDepth.cpp
+177 -8
View File
@@ -78,6 +78,30 @@ void CommonDataSubscriber::depthInfoCallback(
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthScan2dInfoCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthScan3dInfoCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
}
// RGB + Depth + Odom
void CommonDataSubscriber::depthOdomCallback(
@@ -128,6 +152,30 @@ void CommonDataSubscriber::depthOdomInfoCallback(
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomScan2dInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomScan3dInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
}
// RGB + Depth + User Data
void CommonDataSubscriber::depthDataCallback(
@@ -178,6 +226,30 @@ void CommonDataSubscriber::depthDataInfoCallback(
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthDataScan2dInfoCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
nav_msgs::OdometryConstPtr odomMsg; // null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthDataScan3dInfoCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
nav_msgs::OdometryConstPtr odomMsg; // null
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
}
// RGB + Depth + Odom + User Data
void CommonDataSubscriber::depthOdomDataCallback(
@@ -228,6 +300,30 @@ void CommonDataSubscriber::depthOdomDataInfoCallback(
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomDataScan2dInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomDataScan3dInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::setupDepthCallbacks(
ros::NodeHandle & nh,
@@ -266,13 +362,31 @@ void CommonDataSubscriber::setupDepthCallbacks(
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", 1);
SYNC_DECL6(depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL7(depthOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL6(depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", 1);
SYNC_DECL6(depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL7(depthOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL6(depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
@@ -293,13 +407,31 @@ void CommonDataSubscriber::setupDepthCallbacks(
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", 1);
SYNC_DECL5(depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL6(depthOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", 1);
SYNC_DECL5(depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL6(depthOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
@@ -320,13 +452,32 @@ void CommonDataSubscriber::setupDepthCallbacks(
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", 1);
SYNC_DECL5(depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL6(depthDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", 1);
SYNC_DECL5(depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL6(depthDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
@@ -345,13 +496,31 @@ void CommonDataSubscriber::setupDepthCallbacks(
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", 1);
SYNC_DECL4(depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL5(depthScan2dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", 1);
SYNC_DECL4(depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL5(depthScan3dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
src/impl/CommonDataSubscriberRGBD.cpp
+184 -8
View File
@@ -85,6 +85,32 @@ void CommonDataSubscriber::rgbdInfoCallback(
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, 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)
{
src/impl/CommonDataSubscriberRGBD2.cpp
+184 -8
View File
@@ -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)
{