Uniformized "approx_sync" parameter name across all nodes. Updated rgbd_mapping.launch and stereo_mapping.launch to include rtabmap.launch instead of redefining all nodes.

src/CoreWrapper.cpp

@@ -132,7 +132,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 	double tfDelay = 0.05; // 20 Hz
 	double tfTolerance = 0.1; // 100 ms
 	std::string tfPrefix = "";
-	bool stereoApproxSync = false;
+	bool approxSync = true;
 
 	// ROS related parameters (private)
 	pnh.param("subscribe_depth",     subscribeDepth, subscribeDepth);
@@ -171,7 +171,17 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 	pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
 	pnh.param("depth_cameras",       depthCameras, depthCameras);
 	pnh.param("queue_size",          queueSize, queueSize);
-	pnh.param("stereo_approx_sync",  stereoApproxSync, stereoApproxSync);
+	if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync"))
+	{
+		ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed "
+				 "to \"approx_sync\"! Your value is still copied to "
+				 "corresponding parameter.");
+		pnh.param("stereo_approx_sync", approxSync, approxSync);
+	}
+	else
+	{
+		pnh.param("approx_sync", approxSync, approxSync);
+	}
 
 	pnh.param("publish_tf",          publishTf, publishTf);
 	pnh.param("tf_delay",            tfDelay, tfDelay);
@@ -227,6 +237,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 	ROS_INFO("rtabmap: tf_delay = %f", tfDelay);
 	ROS_INFO("rtabmap: tf_tolerance = %f", tfTolerance);
 	ROS_INFO("rtabmap: depth_cameras = %d", depthCameras);
+	ROS_INFO("rtabmap: approx_sync = %s", approxSync?"true":"false");
 
 	infoPub_ = nh.advertise<rtabmap_ros::Info>("info", 1);
 	mapDataPub_ = nh.advertise<rtabmap_ros::MapData>("mapData", 1);
@@ -427,7 +438,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 	setLogWarnSrv_ = pnh.advertiseService("log_warning", &CoreWrapper::setLogWarn, this);
 	setLogErrorSrv_ = pnh.advertiseService("log_error", &CoreWrapper::setLogError, this);
 
-	setupCallbacks(subscribeDepth, subscribeScan2d, subscribeScan3d, subscribeStereo, queueSize, stereoApproxSync, depthCameras);
+	setupCallbacks(subscribeDepth, subscribeScan2d, subscribeScan3d, subscribeStereo, queueSize, approxSync, depthCameras);
 
 	int optimizeIterations = 0;
 	Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations);
@@ -2634,7 +2645,7 @@ void CoreWrapper::setupCallbacks(
 		bool subscribeScan3d,
 		bool subscribeStereo,
 		int queueSize,
-		bool stereoApproxSync,
+		bool approxSync,
 		int depthCameras)
 {
 	ros::NodeHandle nh; // public
@@ -2679,7 +2690,6 @@ void CoreWrapper::setupCallbacks(
 			odomSub_.subscribe(nh, "odom", 1);
 			if(subscribeScan2d)
 			{
-				ROS_INFO("Registering Depth+LaserScan callback...");
 				scanSub_.subscribe(nh, "scan", 1);
 				depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(
 						MyDepthScanSyncPolicy(queueSize),
@@ -2700,7 +2710,6 @@ void CoreWrapper::setupCallbacks(
 			}
 			else if(subscribeScan3d)
 			{
-				ROS_INFO("Registering Depth+LaserScan3d callback...");
 				scan3dSub_.subscribe(nh, "scan_cloud", 1);
 				depthScan3dSync_ = new message_filters::Synchronizer<MyDepthScan3dSyncPolicy>(
 						MyDepthScan3dSyncPolicy(queueSize),
@@ -2723,7 +2732,6 @@ void CoreWrapper::setupCallbacks(
 			{
 				if(depthCameras > 1)
 				{
-					ROS_INFO("Registering Depth2 callback...");
 					depth2Sync_ = new message_filters::Synchronizer<MyDepth2SyncPolicy>(
 							MyDepth2SyncPolicy(queueSize),
 							odomSub_,
@@ -2747,17 +2755,29 @@ void CoreWrapper::setupCallbacks(
 				}
 				else
 				{
-					ROS_INFO("Registering Depth callback...");
-					depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(
-							MyDepthSyncPolicy(queueSize),
-							*imageSubs_[0],
-							odomSub_,
-							*imageDepthSubs_[0],
-							*cameraInfoSubs_[0]);
-					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",
+					if(approxSync)
+					{
+						depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(
+								MyDepthSyncPolicy(queueSize),
+								*imageSubs_[0],
+								odomSub_,
+								*imageDepthSubs_[0],
+								*cameraInfoSubs_[0]);
+						depthSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4));
+					}
+					else
+					{
+						depthExactSync_ = new message_filters::Synchronizer<MyDepthExactSyncPolicy>(
+								MyDepthExactSyncPolicy(queueSize),
+								*imageSubs_[0],
+								odomSub_,
+								*imageDepthSubs_[0],
+								*cameraInfoSubs_[0]);
+						depthExactSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, 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(),
@@ -2806,15 +2826,27 @@ void CoreWrapper::setupCallbacks(
 			}
 			else //!subscribeLaserScan
 			{
-				depthTFSync_ = new message_filters::Synchronizer<MyDepthTFSyncPolicy>(
-						MyDepthTFSyncPolicy(queueSize),
-						*imageSubs_[0],
-						*imageDepthSubs_[0],
-						*cameraInfoSubs_[0]);
-				depthTFSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3));
-
-				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s",
+				if(approxSync)
+				{
+					depthTFSync_ = new message_filters::Synchronizer<MyDepthTFSyncPolicy>(
+							MyDepthTFSyncPolicy(queueSize),
+							*imageSubs_[0],
+							*imageDepthSubs_[0],
+							*cameraInfoSubs_[0]);
+					depthTFSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3));
+				}
+				else
+				{
+					depthTFExactSync_ = new message_filters::Synchronizer<MyDepthTFExactSyncPolicy>(
+							MyDepthTFExactSyncPolicy(queueSize),
+							*imageSubs_[0],
+							*imageDepthSubs_[0],
+							*cameraInfoSubs_[0]);
+					depthTFExactSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3));
+				}
+				ROS_INFO("\n%s subscribed to (%s sync):\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());
@@ -2886,9 +2918,8 @@ void CoreWrapper::setupCallbacks(
 			}
 			else //!subscribeLaserScan
 			{
-				if(stereoApproxSync)
+				if(approxSync)
 				{
-					ROS_INFO("Registering Stereo Approx callback...");
 					stereoApproxSync_ = new message_filters::Synchronizer<MyStereoApproxSyncPolicy>(
 							MyStereoApproxSyncPolicy(queueSize),
 							imageRectLeft_,
@@ -2900,7 +2931,6 @@ void CoreWrapper::setupCallbacks(
 				}
 				else
 				{
-					ROS_INFO("Registering Stereo Exact callback...");
 					stereoExactSync_ = new message_filters::Synchronizer<MyStereoExactSyncPolicy>(
 							MyStereoExactSyncPolicy(queueSize),
 							imageRectLeft_,
@@ -2911,8 +2941,9 @@ void CoreWrapper::setupCallbacks(
 					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_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",
 						imageRectLeft_.getTopic().c_str(),
 						imageRectRight_.getTopic().c_str(),
 						cameraInfoLeft_.getTopic().c_str(),
@@ -2925,7 +2956,6 @@ void CoreWrapper::setupCallbacks(
 			// use odom from TF, so subscribe to sensors only
 			if(subscribeScan2d)
 			{
-				ROS_INFO("Registering Stereo+LaserScan2d+OdomTF callback...");
 				scanSub_.subscribe(nh, "scan", 1);
 				stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>(
 						MyStereoScanTFSyncPolicy(queueSize),
@@ -2946,7 +2976,6 @@ void CoreWrapper::setupCallbacks(
 			}
 			else if(subscribeScan3d)
 			{
-				ROS_INFO("Registering Stereo+LaserScan3d+OdomTF callback...");
 				scan3dSub_.subscribe(nh, "scan_cloud", 1);
 				stereoScan3dTFSync_ = new message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy>(
 						MyStereoScan3dTFSyncPolicy(queueSize),
@@ -2967,9 +2996,8 @@ void CoreWrapper::setupCallbacks(
 			}
 			else //!subscribeLaserScan
 			{
-				if(stereoApproxSync)
+				if(approxSync)
 				{
-					ROS_INFO("Registering Stereo+OdomTF Approx callback...");
 					stereoApproxTFSync_ = new message_filters::Synchronizer<MyStereoApproxTFSyncPolicy>(
 							MyStereoApproxTFSyncPolicy(queueSize),
 							imageRectLeft_,
@@ -2980,7 +3008,6 @@ void CoreWrapper::setupCallbacks(
 				}
 				else
 				{
-					ROS_INFO("Registering Stereo+OdomTF Exact callback...");
 					stereoExactTFSync_ = new message_filters::Synchronizer<MyStereoExactTFSyncPolicy>(
 							MyStereoExactTFSyncPolicy(queueSize),
 							imageRectLeft_,
@@ -2990,8 +3017,9 @@ void CoreWrapper::setupCallbacks(
 					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_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s",
 						ros::this_node::getName().c_str(),
+						approxSync?"approx":"exact",
 						imageRectLeft_.getTopic().c_str(),
 						imageRectRight_.getTopic().c_str(),
 						cameraInfoLeft_.getTopic().c_str(),

src/CoreWrapper.h

@@ -90,7 +90,7 @@ private:
 			bool subscribeScan3d,
 			bool subscribeStereo,
 			int queueSize,
-			bool stereoApproxSync,
+			bool approxSync,
 			int depthCameras);
 	void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
 
@@ -338,6 +338,12 @@ private:
 			sensor_msgs::Image,
 			sensor_msgs::CameraInfo> MyDepthSyncPolicy;
 	message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
+	typedef message_filters::sync_policies::ExactTime<
+			sensor_msgs::Image,
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthExactSyncPolicy;
+	message_filters::Synchronizer<MyDepthExactSyncPolicy> * depthExactSync_;
 
 	typedef message_filters::sync_policies::ApproximateTime<
 			sensor_msgs::Image,
@@ -403,6 +409,11 @@ private:
 			sensor_msgs::Image,
 			sensor_msgs::CameraInfo> MyDepthTFSyncPolicy;
 	message_filters::Synchronizer<MyDepthTFSyncPolicy> * depthTFSync_;
+	typedef message_filters::sync_policies::ExactTime<
+				sensor_msgs::Image,
+				sensor_msgs::Image,
+				sensor_msgs::CameraInfo> MyDepthTFExactSyncPolicy;
+	message_filters::Synchronizer<MyDepthTFExactSyncPolicy> * depthTFExactSync_;
 
 	typedef message_filters::sync_policies::ApproximateTime<
 			sensor_msgs::Image,

src/RGBDOdometryNode.cpp

@@ -54,7 +54,8 @@ class RGBDOdometry : public rtabmap_ros::OdometryROS
 public:
 	RGBDOdometry(int argc, char * argv[]) :
 		rtabmap_ros::OdometryROS(argc, argv),
-		sync_(0),
+		approxSync_(0),
+		exactSync_(0),
 		sync2_(0),
 		queueSize_(5)
 	{
@@ -62,6 +63,8 @@ public:
 		ros::NodeHandle pnh("~");
 
 		int depthCameras = 1;
+		bool approxSync = true;
+		pnh.param("approx_sync", approxSync, approxSync);
 		pnh.param("queue_size", queueSize_, queueSize_);
 		pnh.param("depth_cameras", depthCameras, depthCameras);
 		if(depthCameras <= 0)
@@ -135,22 +138,35 @@ 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",
+			if(approxSync)
+			{
+				approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+				approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
+			}
+			else
+			{
+				exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+				exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
+			}
+
+			ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s",
 					ros::this_node::getName().c_str(),
+					approxSync?"approx":"exact",
 					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));
 		}
 	}
 
 	virtual ~RGBDOdometry()
 	{
-		if(sync_)
+		if(approxSync_)
 		{
-			delete sync_;
+			delete approxSync_;
+		}
+		if(exactSync_)
+		{
+			delete exactSync_;
 		}
 		if(sync2_)
 		{
@@ -343,11 +359,17 @@ protected:
 	virtual void flushCallbacks()
 	{
 		// flush callbacks
-		if(sync_)
+		if(approxSync_)
 		{
-			delete sync_;
-			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));
+			delete approxSync_;
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+			approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
+		}
+		if(exactSync_)
+		{
+			delete exactSync_;
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+			exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
 		}
 		if(sync2_)
 		{
@@ -371,8 +393,10 @@ private:
 	image_transport::SubscriberFilter image_mono2_sub_;
 	image_transport::SubscriberFilter image_depth2_sub_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> info2_sub_;
-	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
-	message_filters::Synchronizer<MySyncPolicy> * sync_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
+	message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
+	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
 	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySync2Policy;
 	message_filters::Synchronizer<MySync2Policy> * sync2_;
 	int queueSize_;

src/StereoOdometryNode.cpp

@@ -80,13 +80,6 @@ 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_);
@@ -97,6 +90,15 @@ public:
 			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
 			exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, 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",
+				imageRectLeft_.getTopic().c_str(),
+				imageRectRight_.getTopic().c_str(),
+				cameraInfoLeft_.getTopic().c_str(),
+				cameraInfoRight_.getTopic().c_str());
 	}
 
 	virtual ~StereoOdometry()