Added pause/resume services to visual_odometry node (also linked to pause action in rtabmapviz)

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

launch/rgbd_localization.launch

@@ -55,6 +55,8 @@
       <remap from="odom" to="/odom"/>
       
       <remap from="reset_odom" to="/reset_odom"/> <!-- to call rtabmap/visual_odometry "reset_odom" service -->
+      <remap from="pause_odom" to="/pause_odom"/> <!-- to call rtabmap/visual_odometry "pause_odom" service -->
+      <remap from="resume_odom" to="/resume_odom"/> <!-- to call rtabmap/visual_odometry "resume_odom" service -->
     </node>
   
   </group>

launch/rgbd_mapping.launch

@@ -49,6 +49,8 @@
       <remap from="odom" to="/odom"/>
       
       <remap from="reset_odom" to="/reset_odom"/> <!-- to call rtabmap/visual_odometry "reset_odom" service -->
+      <remap from="pause_odom" to="/pause_odom"/> <!-- to call rtabmap/visual_odometry "pause_odom" service -->
+      <remap from="resume_odom" to="/resume_odom"/> <!-- to call rtabmap/visual_odometry "resume_odom" service -->
     </node>
   
   </group>

src/GuiWrapper.cpp

@@ -386,6 +386,9 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
 					system(str.c_str());
 				}
 
+				// Pause visual_odometry
+				ros::service::call("pause_odom", srv);
+
 				// Pause rtabmap
 				if(!ros::service::call("pause", srv))
 				{
@@ -400,6 +403,9 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
 					ROS_ERROR("Can't call \"resume\" service");
 				}
 
+				// Pause visual_odometry
+				ros::service::call("resume_odom", srv);
+
 				// Resume the camera if the rtabmap/camera node is used
 				if(!cameraNodeName_.empty())
 				{

src/VisualOdometryNode.cpp

@@ -42,7 +42,8 @@ public:
 		odometry_(0),
 		frameId_("base_link"),
 		odomFrameId_("odom"),
-		sync_(0)
+		sync_(0),
+		paused_(false)
 	{
 		ros::NodeHandle nh;
 
@@ -132,6 +133,8 @@ public:
 		sync_->registerCallback(boost::bind(&VisualOdometry::callback, this, _1, _2, _3));
 
 		resetSrv_ = nh.advertiseService("reset_odom", &VisualOdometry::reset, this);
+		pauseSrv_ = nh.advertiseService("pause_odom", &VisualOdometry::pause, this);
+		resumeSrv_ = nh.advertiseService("resume_odom", &VisualOdometry::resume, this);
 	}
 
 	~VisualOdometry()
@@ -143,6 +146,8 @@ public:
 	void callback(const sensor_msgs::ImageConstPtr& image,
 			const sensor_msgs::ImageConstPtr& depth,
 			const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		if(!paused_)
 		{
 			if(!(image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 				 image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
@@ -234,6 +239,7 @@ public:
 
 			ROS_INFO("Odom update time(%f s)", (ros::WallTime::now()-time).toSec());
 		}
+	}
 
 	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 	{
@@ -242,6 +248,34 @@ public:
 		return true;
 	}
 
+	bool pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+	{
+		if(paused_)
+		{
+			ROS_WARN("visual_odometry: Already paused!");
+		}
+		else
+		{
+			paused_ = true;
+			ROS_INFO("visual_odometry: paused!");
+		}
+		return true;
+	}
+
+	bool resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+	{
+		if(!paused_)
+		{
+			ROS_WARN("visual_odometry: Already running!");
+		}
+		else
+		{
+			paused_ = false;
+			ROS_INFO("visual_odometry: resumed!");
+		}
+		return true;
+	}
+
 private:
 	rtabmap::Odometry * odometry_;
 
@@ -251,6 +285,8 @@ private:
 
 	ros::Publisher odomPub_;
 	ros::ServiceServer resetSrv_;
+	ros::ServiceServer pauseSrv_;
+	ros::ServiceServer resumeSrv_;
 	tf::TransformBroadcaster tfBroadcaster_;
 	tf::TransformListener tfListener_;
 
@@ -259,6 +295,8 @@ private:
 	message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
 	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
 	message_filters::Synchronizer<MySyncPolicy> * sync_;
+
+	bool paused_;
 };
 
 int main(int argc, char *argv[])