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

Added some ROS_INFO about to which topics the nodes are subscribed on initialization

Browse Source
This commit is contained in:
Mathieu Labbe
2015-04-24 11:24:18 -04:00
parent 9c723006e7
commit c253a75016
6 changed files with 138 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+62 -4
View File
@@ -2636,12 +2636,27 @@ void CoreWrapper::setupCallbacks(
scanSub_.subscribe(nh, "scan", 1);
depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_, 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",
ros::this_node::getName().c_str(),
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str(),
odomSub_.getTopic().c_str(),
scanSub_.getTopic().c_str());
}
else //!subscribeLaserScan
{
ROS_INFO("Registering Depth callback...");
depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_);
depthSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str(),
odomSub_.getTopic().c_str());
}
}
else
@@ -2649,16 +2664,27 @@ void CoreWrapper::setupCallbacks(
// use odom from TF, so subscribe to sensors only
if(subscribeLaserScan)
{
ROS_INFO("Registering Depth+LaserScan+OdomTF callback...");
scanSub_.subscribe(nh, "scan", 1);
depthScanTFSync_ = new message_filters::Synchronizer<MyDepthScanTFSyncPolicy>(MyDepthScanTFSyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_, 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",
ros::this_node::getName().c_str(),
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str(),
scanSub_.getTopic().c_str());
}
else //!subscribeLaserScan
{
ROS_INFO("Registering Depth+OdomTF callback...");
depthTFSync_ = new message_filters::Synchronizer<MyDepthTFSyncPolicy>(MyDepthTFSyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
depthTFSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str());
}
}
}
@@ -2683,10 +2709,18 @@ void CoreWrapper::setupCallbacks(
odomSub_.subscribe(nh, "odom", 1);
if(subscribeLaserScan)
{
ROS_INFO("Registering Stereo+LaserScan callback...");
scanSub_.subscribe(nh, "scan", 1);
stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(MyStereoScanSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, scanSub_, odomSub_);
stereoScanSync_->registerCallback(boost::bind(&CoreWrapper::stereoScanCallback, this, _1, _2, _3, _4, _5, _6));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str(),
odomSub_.getTopic().c_str(),
scanSub_.getTopic().c_str());
}
else //!subscribeLaserScan
{
@@ -2702,6 +2736,14 @@ void CoreWrapper::setupCallbacks(
stereoExactSync_ = new message_filters::Synchronizer<MyStereoExactSyncPolicy>(MyStereoExactSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomSub_);
stereoExactSync_->registerCallback(boost::bind(&CoreWrapper::stereoCallback, this, _1, _2, _3, _4, _5));
}
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str(),
odomSub_.getTopic().c_str());
}
}
else
@@ -2713,6 +2755,14 @@ void CoreWrapper::setupCallbacks(
scanSub_.subscribe(nh, "scan", 1);
stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>(MyStereoScanTFSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, scanSub_);
stereoScanTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoScanTFCallback, this, _1, _2, _3, _4, _5));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str(),
scanSub_.getTopic().c_str());
}
else //!subscribeLaserScan
{
@@ -2728,17 +2778,25 @@ void CoreWrapper::setupCallbacks(
stereoExactTFSync_ = new message_filters::Synchronizer<MyStereoExactTFSyncPolicy>(MyStereoExactTFSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
stereoExactTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoTFCallback, this, _1, _2, _3, _4));
}
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str());
}
}
}
else
{
ROS_INFO("Registering image-only callback...");
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle rgb_pnh(pnh, "rgb");
image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
defaultSub_ = rgb_it.subscribe("image", 1, &CoreWrapper::defaultCallback, this);
ROS_INFO("\n%s subscribed to:\n %s", ros::this_node::getName().c_str(), defaultSub_.getTopic().c_str());
}
}
src/GuiWrapper.cpp
+53 -7
View File
@@ -934,23 +934,43 @@ void GuiWrapper::setupCallbacks(
if(subscribeLaserScan)
{
ROS_INFO("Registering Depth+LaserScan callback...");
scanSub_.subscribe(nh, "scan", 1);
depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
depthScanSync_->registerCallback(boost::bind(&GuiWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str(),
odomSub_.getTopic().c_str(),
scanSub_.getTopic().c_str());
}
else if(subscribeOdomInfo)
{
ROS_INFO("Registering Depth callback + OdomInfo...");
odomInfoSub_.subscribe(nh, "odom_info", 1);
depthOdomInfoSync_ = new message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy>(MyDepthOdomInfoSyncPolicy(queueSize), imageSub_, odomSub_, odomInfoSub_, imageDepthSub_, cameraInfoSub_);
depthOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthOdomInfoCallback, this, _1, _2, _3, _4, _5));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str(),
odomSub_.getTopic().c_str(),
odomInfoSub_.getTopic().c_str());
}
else
{
ROS_INFO("Registering Depth callback...");
depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_);
depthSync_->registerCallback(boost::bind(&GuiWrapper::depthCallback, this, _1, _2, _3, _4));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str(),
odomSub_.getTopic().c_str());
}
}
else if(subscribeStereo)
@@ -972,29 +992,55 @@ void GuiWrapper::setupCallbacks(
if(subscribeLaserScan)
{
ROS_INFO("Registering Stereo callback + LaserScan...");
scanSub_.subscribe(nh, "scan", 1);
stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(MyStereoScanSyncPolicy(queueSize), odomSub_, scanSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
stereoScanSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanCallback, this, _1, _2, _3, _4, _5, _6));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str(),
odomSub_.getTopic().c_str(),
scanSub_.getTopic().c_str());
}
else if(subscribeOdomInfo)
{
ROS_INFO("Registering Stereo callback + OdomInfo...");
odomInfoSub_.subscribe(nh, "odom_info", 1);
stereoOdomInfoSync_ = new message_filters::Synchronizer<MyStereoOdomInfoSyncPolicy>(MyStereoOdomInfoSyncPolicy(queueSize), odomSub_, odomInfoSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
stereoOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::stereoOdomInfoCallback, this, _1, _2, _3, _4, _5, _6));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str(),
odomSub_.getTopic().c_str(),
odomInfoSub_.getTopic().c_str());
}
else
{
ROS_INFO("Registering Stereo callback...");
stereoSync_ = new message_filters::Synchronizer<MyStereoSyncPolicy>(MyStereoSyncPolicy(queueSize), odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
stereoSync_->registerCallback(boost::bind(&GuiWrapper::stereoCallback, this, _1, _2, _3, _4, _5));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str(),
odomSub_.getTopic().c_str());
}
}
else // default odom only
{
ROS_INFO("Registering default callback (\"odom\" only)...");
defaultSub_ = nh.subscribe("odom", 1, &GuiWrapper::defaultCallback, this);
ROS_INFO("\n%s subscribed to:\n %s",
ros::this_node::getName().c_str(),
odomSub_.getTopic().c_str());
}
}
src/RGBDOdometryNode.cpp
+6
View File
@@ -72,6 +72,12 @@ public:
image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
info_sub_.subscribe(rgb_nh, "camera_info", 1);
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
image_mono_sub_.getTopic().c_str(),
image_depth_sub_.getTopic().c_str(),
info_sub_.getTopic().c_str());
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), image_mono_sub_, image_depth_sub_, info_sub_);
sync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
}
src/StereoOdometryNode.cpp
+7
View File
@@ -80,6 +80,13 @@ public:
cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str());
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);