Flush odometry's synchronizer queues on reset

src/OdometryROS.cpp

@@ -543,6 +543,7 @@ bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 {
 	ROS_INFO("visual_odometry: reset odom!");
 	odometry_->reset();
+	this->flushCallbacks();
 	return true;
 }
 
@@ -551,6 +552,7 @@ bool OdometryROS::resetToPose(rtabmap_ros::ResetPose::Request& req, rtabmap_ros:
 	Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw);
 	ROS_INFO("visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str());
 	odometry_->reset(pose);
+	this->flushCallbacks();
 	return true;
 }
 

src/OdometryROS.h

@@ -53,7 +53,7 @@ public:
 
 public:
 	OdometryROS(int argc, char * argv[], bool stereo = false);
-	~OdometryROS();
+	virtual ~OdometryROS();
 	void processData(const rtabmap::SensorData & data, const ros::Time & stamp);
 
 	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -73,6 +73,9 @@ public:
 	bool isOdometryF2M() const;
 	rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
 
+protected:
+	virtual void flushCallbacks() = 0;
+
 private:
 	rtabmap::Odometry * odometry_;
 

src/RGBDOdometryNode.cpp

@@ -54,14 +54,14 @@ public:
 	RGBDOdometry(int argc, char * argv[]) :
 		rtabmap_ros::OdometryROS(argc, argv),
 		sync_(0),
-		sync2_(0)
+		sync2_(0),
+		queueSize_(5)
 	{
 		ros::NodeHandle nh;
 		ros::NodeHandle pnh("~");
 
-		int queueSize = 5;
 		int depthCameras = 1;
-		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("queue_size", queueSize_, queueSize_);
 		pnh.param("depth_cameras", depthCameras, depthCameras);
 		if(depthCameras <= 0)
 		{
@@ -110,7 +110,7 @@ public:
 					info2_sub_.getTopic().c_str());
 
 			sync2_ = new message_filters::Synchronizer<MySync2Policy>(
-					MySync2Policy(queueSize),
+					MySync2Policy(queueSize_),
 					image_mono_sub_,
 					image_depth_sub_,
 					info_sub_,
@@ -140,12 +140,12 @@ public:
 					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_ = 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));
 		}
 	}
 
-	~RGBDOdometry()
+	virtual ~RGBDOdometry()
 	{
 		if(sync_)
 		{
@@ -328,6 +328,31 @@ public:
 		}
 	}
 
+protected:
+	virtual void flushCallbacks()
+	{
+		// flush callbacks
+		if(sync_)
+		{
+			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));
+		}
+		if(sync2_)
+		{
+			delete sync2_;
+			sync2_ = new message_filters::Synchronizer<MySync2Policy>(
+					MySync2Policy(queueSize_),
+					image_mono_sub_,
+					image_depth_sub_,
+					info_sub_,
+					image_mono2_sub_,
+					image_depth2_sub_,
+					info2_sub_);
+			sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6));
+		}
+	}
+
 private:
 	image_transport::SubscriberFilter image_mono_sub_;
 	image_transport::SubscriberFilter image_depth_sub_;
@@ -339,6 +364,7 @@ private:
 	message_filters::Synchronizer<MySyncPolicy> * sync_;
 	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_;
 };
 
 int main(int argc, char *argv[])

src/StereoOdometryNode.cpp

@@ -54,16 +54,16 @@ public:
 	StereoOdometry(int argc, char * argv[]) :
 		rtabmap_ros::OdometryROS(argc, argv, true),
 		approxSync_(0),
-		exactSync_(0)
+		exactSync_(0),
+		queueSize_(5)
 	{
 		ros::NodeHandle nh;
 
 		ros::NodeHandle pnh("~");
 
 		bool approxSync = false;
-		int queueSize = 5;
 		pnh.param("approx_sync", approxSync, approxSync);
-		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("queue_size", queueSize_, queueSize_);
 		ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
 
 		ros::NodeHandle left_nh(nh, "left");
@@ -89,17 +89,17 @@ public:
 
 		if(approxSync)
 		{
-			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
 			approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
 		}
 		else
 		{
-			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
 			exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
 		}
 	}
 
-	~StereoOdometry()
+	virtual ~StereoOdometry()
 	{
 		if(approxSync_)
 		{
@@ -188,6 +188,24 @@ public:
 		}
 	}
 
+protected:
+	virtual void flushCallbacks()
+	{
+		//flush callbacks
+		if(approxSync_)
+		{
+			delete approxSync_;
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+			approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
+		}
+		if(exactSync_)
+		{
+			delete exactSync_;
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+			exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
+		}
+	}
+
 private:
 	image_transport::SubscriberFilter imageRectLeft_;
 	image_transport::SubscriberFilter imageRectRight_;
@@ -197,6 +215,7 @@ private:
 	message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
 	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
 	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
+	int queueSize_;
 };
 
 int main(int argc, char *argv[])