From b572bc11911b443941db286623edd9c9d41cc4bc Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 22 Jul 2016 15:11:08 -0400 Subject: [PATCH] rtabmap node: Added ExactTime policy for RGBD+laser callbacks --- src/CoreWrapper.cpp | 130 +++++++++++++++++++++++++++++++------------- src/CoreWrapper.h | 26 +++++++++ 2 files changed, 118 insertions(+), 38 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 9c82fb72..45793c79 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -2708,17 +2708,31 @@ void CoreWrapper::setupCallbacks( if(subscribeScan2d) { scanSub_.subscribe(nh, "scan", 1); - depthScanSync_ = new message_filters::Synchronizer( - MyDepthScanSyncPolicy(queueSize), - *imageSubs_[0], - odomSub_, - *imageDepthSubs_[0], - *cameraInfoSubs_[0], - scanSub_); - depthScanSync_->registerCallback(boost::bind(&CoreWrapper::depthScanCallback, this, _1, _2, _3, _4, _5)); - - ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s", + if(approxSync) + { + depthScanSync_ = new message_filters::Synchronizer( + MyDepthScanSyncPolicy(queueSize), + *imageSubs_[0], + odomSub_, + *imageDepthSubs_[0], + *cameraInfoSubs_[0], + scanSub_); + depthScanSync_->registerCallback(boost::bind(&CoreWrapper::depthScanCallback, this, _1, _2, _3, _4, _5)); + } + else + { + depthScanExactSync_ = new message_filters::Synchronizer( + MyDepthScanExactSyncPolicy(queueSize), + *imageSubs_[0], + odomSub_, + *imageDepthSubs_[0], + *cameraInfoSubs_[0], + scanSub_); + depthScanExactSync_->registerCallback(boost::bind(&CoreWrapper::depthScanCallback, this, _1, _2, _3, _4, _5)); + } + ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s,\n %s", ros::this_node::getName().c_str(), + approxSync?"approx":"exact", imageSubs_[0]->getTopic().c_str(), imageDepthSubs_[0]->getTopic().c_str(), cameraInfoSubs_[0]->getTopic().c_str(), @@ -2728,17 +2742,31 @@ void CoreWrapper::setupCallbacks( else if(subscribeScan3d) { scan3dSub_.subscribe(nh, "scan_cloud", 1); - depthScan3dSync_ = new message_filters::Synchronizer( - MyDepthScan3dSyncPolicy(queueSize), - *imageSubs_[0], - odomSub_, - *imageDepthSubs_[0], - *cameraInfoSubs_[0], - scan3dSub_); - depthScan3dSync_->registerCallback(boost::bind(&CoreWrapper::depthScan3dCallback, this, _1, _2, _3, _4, _5)); - - ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s", + if(approxSync) + { + depthScan3dSync_ = new message_filters::Synchronizer( + MyDepthScan3dSyncPolicy(queueSize), + *imageSubs_[0], + odomSub_, + *imageDepthSubs_[0], + *cameraInfoSubs_[0], + scan3dSub_); + depthScan3dSync_->registerCallback(boost::bind(&CoreWrapper::depthScan3dCallback, this, _1, _2, _3, _4, _5)); + } + else + { + depthScan3dExactSync_ = new message_filters::Synchronizer( + MyDepthScan3dExactSyncPolicy(queueSize), + *imageSubs_[0], + odomSub_, + *imageDepthSubs_[0], + *cameraInfoSubs_[0], + scan3dSub_); + depthScan3dExactSync_->registerCallback(boost::bind(&CoreWrapper::depthScan3dCallback, this, _1, _2, _3, _4, _5)); + } + ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s,\n %s", ros::this_node::getName().c_str(), + approxSync?"approx":"exact", imageSubs_[0]->getTopic().c_str(), imageDepthSubs_[0]->getTopic().c_str(), cameraInfoSubs_[0]->getTopic().c_str(), @@ -2808,16 +2836,29 @@ void CoreWrapper::setupCallbacks( if(subscribeScan2d) { scanSub_.subscribe(nh, "scan", 1); - depthScanTFSync_ = new message_filters::Synchronizer( - MyDepthScanTFSyncPolicy(queueSize), - *imageSubs_[0], - *imageDepthSubs_[0], - *cameraInfoSubs_[0], - scanSub_); - depthScanTFSync_->registerCallback(boost::bind(&CoreWrapper::depthScanTFCallback, this, _1, _2, _3, _4)); - - ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s", + if(approxSync) + { + depthScanTFSync_ = new message_filters::Synchronizer( + MyDepthScanTFSyncPolicy(queueSize), + *imageSubs_[0], + *imageDepthSubs_[0], + *cameraInfoSubs_[0], + scanSub_); + depthScanTFSync_->registerCallback(boost::bind(&CoreWrapper::depthScanTFCallback, this, _1, _2, _3, _4)); + } + else + { + depthScanTFExactSync_ = new message_filters::Synchronizer( + MyDepthScanTFExactSyncPolicy(queueSize), + *imageSubs_[0], + *imageDepthSubs_[0], + *cameraInfoSubs_[0], + scanSub_); + depthScanTFExactSync_->registerCallback(boost::bind(&CoreWrapper::depthScanTFCallback, this, _1, _2, _3, _4)); + } + ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s", ros::this_node::getName().c_str(), + approxSync?"approx":"exact", imageSubs_[0]->getTopic().c_str(), imageDepthSubs_[0]->getTopic().c_str(), cameraInfoSubs_[0]->getTopic().c_str(), @@ -2826,16 +2867,29 @@ void CoreWrapper::setupCallbacks( else if(subscribeScan3d) { scan3dSub_.subscribe(nh, "scan_cloud", 1); - depthScan3dTFSync_ = new message_filters::Synchronizer( - MyDepthScan3dTFSyncPolicy(queueSize), - *imageSubs_[0], - *imageDepthSubs_[0], - *cameraInfoSubs_[0], - scan3dSub_); - depthScan3dTFSync_->registerCallback(boost::bind(&CoreWrapper::depthScan3dTFCallback, this, _1, _2, _3, _4)); - - ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s", + if(approxSync) + { + depthScan3dTFSync_ = new message_filters::Synchronizer( + MyDepthScan3dTFSyncPolicy(queueSize), + *imageSubs_[0], + *imageDepthSubs_[0], + *cameraInfoSubs_[0], + scan3dSub_); + depthScan3dTFSync_->registerCallback(boost::bind(&CoreWrapper::depthScan3dTFCallback, this, _1, _2, _3, _4)); + } + else + { + depthScan3dTFExactSync_ = new message_filters::Synchronizer( + MyDepthScan3dTFExactSyncPolicy(queueSize), + *imageSubs_[0], + *imageDepthSubs_[0], + *cameraInfoSubs_[0], + scan3dSub_); + depthScan3dTFExactSync_->registerCallback(boost::bind(&CoreWrapper::depthScan3dTFCallback, this, _1, _2, _3, _4)); + } + ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s", ros::this_node::getName().c_str(), + approxSync?"approx":"exact", imageSubs_[0]->getTopic().c_str(), imageDepthSubs_[0]->getTopic().c_str(), cameraInfoSubs_[0]->getTopic().c_str(), diff --git a/src/CoreWrapper.h b/src/CoreWrapper.h index e9815ab5..ddac1c92 100644 --- a/src/CoreWrapper.h +++ b/src/CoreWrapper.h @@ -323,6 +323,13 @@ private: sensor_msgs::CameraInfo, sensor_msgs::LaserScan> MyDepthScanSyncPolicy; message_filters::Synchronizer * depthScanSync_; + typedef message_filters::sync_policies::ExactTime< + sensor_msgs::Image, + nav_msgs::Odometry, + sensor_msgs::Image, + sensor_msgs::CameraInfo, + sensor_msgs::LaserScan> MyDepthScanExactSyncPolicy; + message_filters::Synchronizer * depthScanExactSync_; typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, @@ -331,6 +338,13 @@ private: sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> MyDepthScan3dSyncPolicy; message_filters::Synchronizer * depthScan3dSync_; + typedef message_filters::sync_policies::ExactTime< + sensor_msgs::Image, + nav_msgs::Odometry, + sensor_msgs::Image, + sensor_msgs::CameraInfo, + sensor_msgs::PointCloud2> MyDepthScan3dExactSyncPolicy; + message_filters::Synchronizer * depthScan3dExactSync_; typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, @@ -396,6 +410,12 @@ private: sensor_msgs::CameraInfo, sensor_msgs::LaserScan> MyDepthScanTFSyncPolicy; message_filters::Synchronizer * depthScanTFSync_; + typedef message_filters::sync_policies::ExactTime< + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo, + sensor_msgs::LaserScan> MyDepthScanTFExactSyncPolicy; + message_filters::Synchronizer * depthScanTFExactSync_; typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, @@ -403,6 +423,12 @@ private: sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> MyDepthScan3dTFSyncPolicy; message_filters::Synchronizer * depthScan3dTFSync_; + typedef message_filters::sync_policies::ExactTime< + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo, + sensor_msgs::PointCloud2> MyDepthScan3dTFExactSyncPolicy; + message_filters::Synchronizer * depthScan3dTFExactSync_; typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image,