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

launch/rgbd_localization.launch

@@ -35,10 +35,10 @@
   <group ns="rtabmap">
   
     <!-- Odometry -->
-    <node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen">
+    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
       <remap from="rgb/image"       to="/camera/rgb/image_rect_color"/>
       <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
-      <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
+      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
       
       <param name="Odom/Strategy" type="string" value="$(arg strategy)"/> 
 	  <param name="Odom/FeatureType" type="string" value="$(arg feature)"/>  
@@ -58,7 +58,7 @@
 	
       <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
       <remap from="depth/image" to="/camera/depth_registered/image_raw"/>
-      <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
+      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
       
       <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
       <param name="Rtabmap/DatabasePath" type="string" value="~/.ros/rtabmap.db"/>  <!-- Database used for localization -->
@@ -74,7 +74,7 @@
     
       <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
       <remap from="depth/image" to="/camera/depth_registered/image_raw"/>
-      <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
+      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
     </node>
   
   </group>
@@ -86,7 +86,7 @@
   <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
     <remap from="rgb/image_in"       to="camera/rgb/image_rect_color"/>
     <remap from="depth/image_in"     to="camera/depth_registered/image_raw"/>
-    <remap from="rgb/camera_info_in" to="camera/depth_registered/camera_info"/>
+    <remap from="rgb/camera_info_in" to="camera/rgb/camera_info"/>
     <remap from="odom_in"             to="rtabmap/odom"/>
     
     <remap from="rgb/image_out"       to="data_odom_sync/image"/>

launch/rgbd_mapping.launch

@@ -37,10 +37,10 @@
   <group ns="rtabmap">
   
     <!-- Odometry -->
-    <node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen">
+    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
       <remap from="rgb/image"       to="/camera/rgb/image_rect_color"/>
       <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
-      <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
+      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
 	  
 	  <param name="Odom/Strategy" type="string" value="$(arg strategy)"/> 
 	  <param name="Odom/FeatureType" type="string" value="$(arg feature)"/>  
@@ -59,7 +59,7 @@
 	
       <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
       <remap from="depth/image" to="/camera/depth_registered/image_raw"/>
-      <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
+      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
 
       <param name="LccBow/MinInliers" type="string" value="10"/>
       <param name="LccBow/InlierDistance" type="string" value="0.02"/>
@@ -72,7 +72,7 @@
     
       <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
       <remap from="depth/image" to="/camera/depth_registered/image_raw"/>
-      <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
+      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
     </node>
   
   </group>
@@ -84,7 +84,7 @@
   <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
     <remap from="rgb/image_in"       to="camera/rgb/image_rect_color"/>
     <remap from="depth/image_in"     to="camera/depth_registered/image_raw"/>
-    <remap from="rgb/camera_info_in" to="camera/depth_registered/camera_info"/>
+    <remap from="rgb/camera_info_in" to="camera/rgb/camera_info"/>
     <remap from="odom_in"             to="rtabmap/odom"/>
     
     <remap from="rgb/image_out"       to="data_odom_sync/image"/>

src/CoreWrapper.cpp

@@ -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

@@ -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

@@ -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

@@ -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_);