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

rtabmap node: Added ExactTime policy for RGBD+laser callbacks

Browse Source
This commit is contained in:
matlabbe
2016-07-22 15:11:08 -04:00
parent e14c1d9e68
commit b572bc1191
2 changed files with 118 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+92 -38
View File
@@ -2708,17 +2708,31 @@ void CoreWrapper::setupCallbacks(
if(subscribeScan2d)
{
scanSub_.subscribe(nh, "scan", 1);
depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(
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>(
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>(
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>(
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>(
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>(
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>(
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>(
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>(
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>(
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>(
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>(
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(),
src/CoreWrapper.h
+26
View File
@@ -323,6 +323,13 @@ private:
sensor_msgs::CameraInfo,
sensor_msgs::LaserScan> MyDepthScanSyncPolicy;
message_filters::Synchronizer<MyDepthScanSyncPolicy> * 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<MyDepthScanExactSyncPolicy> * 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<MyDepthScan3dSyncPolicy> * 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<MyDepthScan3dExactSyncPolicy> * 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<MyDepthScanTFSyncPolicy> * depthScanTFSync_;
typedef message_filters::sync_policies::ExactTime<
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::LaserScan> MyDepthScanTFExactSyncPolicy;
message_filters::Synchronizer<MyDepthScanTFExactSyncPolicy> * 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<MyDepthScan3dTFSyncPolicy> * depthScan3dTFSync_;
typedef message_filters::sync_policies::ExactTime<
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::PointCloud2> MyDepthScan3dTFExactSyncPolicy;
message_filters::Synchronizer<MyDepthScan3dTFExactSyncPolicy> * depthScan3dTFExactSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,