ros-pkg: rtabmap node: added "stereo_approx_sync" parameter (default false)

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@2003 f169173b-cf89-36c8-b27e-44dbe73f0c83

src/CoreWrapper.cpp

@@ -68,6 +68,11 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 		configPath_(""),
 		databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()),
 		mapToOdom_(tf::Transform::getIdentity()),
+		depthSync_(0),
+		depthScanSync_(0),
+		stereoScanSync_(0),
+		stereoApproxSync_(0),
+		stereoExactSync_(0),
 		transformThread_(0),
 		rate_(Parameters::defaultRtabmapDetectionRate()),
 		time_(ros::Time::now())
@@ -81,6 +86,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	int queueSize = 10;
 	bool publishTf = true;
 	double tfDelay = 0.05; // 20 Hz
+	bool stereoApproxSync = false;
 
 	// ROS related parameters (private)
 	pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
@@ -106,6 +112,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	pnh.param("frame_id", frameId_, frameId_);
 	pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
 	pnh.param("queue_size", queueSize, queueSize);
+	pnh.param("stereo_approx_sync", stereoApproxSync, stereoApproxSync);
 
 	pnh.param("publish_tf", publishTf, publishTf);
 	pnh.param("tf_delay", tfDelay, tfDelay);
@@ -250,7 +257,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	getMapDataSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this);
 	publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this);
 
-	setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize);
+	setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize, stereoApproxSync);
 
 	int toroIterations = 0;
 	Parameters::parse(parameters, Parameters::kRGBDToroIterations(), toroIterations);
@@ -277,6 +284,12 @@ CoreWrapper::~CoreWrapper()
 		delete depthSync_;
 	if(depthScanSync_)
 		delete depthScanSync_;
+	if(stereoScanSync_)
+		delete stereoScanSync_;
+	if(stereoApproxSync_)
+		delete stereoApproxSync_;
+	if(stereoExactSync_)
+		delete stereoExactSync_;
 
 	this->saveParameters(configPath_);
 
@@ -1349,7 +1362,8 @@ void CoreWrapper::setupCallbacks(
 		bool subscribeDepth,
 		bool subscribeLaserScan,
 		bool subscribeStereo,
-		int queueSize)
+		int queueSize,
+		bool stereoApproxSync)
 {
 	ros::NodeHandle nh; // public
 	ros::NodeHandle pnh("~"); // private
@@ -1410,9 +1424,19 @@ void CoreWrapper::setupCallbacks(
 		}
 		else //!subscribeLaserScan
 		{
-			ROS_INFO("Registering Stereo callback...");
+			if(stereoApproxSync)
-			stereoSync_ = new message_filters::Synchronizer<MyStereoSyncPolicy>(MyStereoSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomSub_);
+			{
-			stereoSync_->registerCallback(boost::bind(&CoreWrapper::stereoCallback, this, _1, _2, _3, _4, _5));
+				ROS_INFO("Registering Stereo Approx callback...");
+				stereoApproxSync_ = new message_filters::Synchronizer<MyStereoApproxSyncPolicy>(MyStereoApproxSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomSub_);
+				stereoApproxSync_->registerCallback(boost::bind(&CoreWrapper::stereoCallback, this, _1, _2, _3, _4, _5));
+			}
+			else
+			{
+				ROS_INFO("Registering Stereo Exact callback...");
+				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));
+			}
+
 		}
 	}
 	else

src/CoreWrapper.h

@@ -56,6 +56,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <message_filters/subscriber.h>
 #include <message_filters/synchronizer.h>
 #include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
 
 #include <image_transport/image_transport.h>
 #include <image_transport/subscriber_filter.h>
@@ -67,7 +68,7 @@ public:
 	virtual ~CoreWrapper();
 
 private:
-	void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, bool subscribeStereo, int queueSize);
+	void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, bool subscribeStereo, int queueSize, bool stereoApproxSync);
 	void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
 	void depthCallback(const sensor_msgs::ImageConstPtr& imageMsg,
 					   const nav_msgs::OdometryConstPtr & odomMsg,
@@ -183,8 +184,16 @@ private:
 			sensor_msgs::Image,
 			sensor_msgs::CameraInfo,
 			sensor_msgs::CameraInfo,
-			nav_msgs::Odometry> MyStereoSyncPolicy;
+			nav_msgs::Odometry> MyStereoApproxSyncPolicy;
-	message_filters::Synchronizer<MyStereoSyncPolicy> * stereoSync_;
+	message_filters::Synchronizer<MyStereoApproxSyncPolicy> * stereoApproxSync_;
+
+	typedef message_filters::sync_policies::ExactTime<
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::CameraInfo,
+			nav_msgs::Odometry> MyStereoExactSyncPolicy;
+	message_filters::Synchronizer<MyStereoExactSyncPolicy> * stereoExactSync_;
 
 	tf::TransformBroadcaster tfBroadcaster_;
 	tf::TransformListener tfListener_;