ros-pkg fixed action "Publish graph" from GUI not linked to rtabmap related services

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

rtabmap/src/CoreWrapper.cpp

@@ -174,6 +174,8 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	triggerNewMapSrv_ = nh.advertiseService("trigger_new_map", &CoreWrapper::triggerNewMapCallback, this);
 	publishGlobalMapDataSrv_ = nh.advertiseService("publish_global_map_data", &CoreWrapper::publishGlobalMapDataCallback, this);
 	publishLocalMapDataSrv_ = nh.advertiseService("publish_local_map_data", &CoreWrapper::publishLocalMapDataCallback, this);
+	publishGlobalGraphSrv_ = nh.advertiseService("publish_global_graph", &CoreWrapper::publishGlobalGraphCallback, this);
+	publishLocalGraphSrv_ = nh.advertiseService("publish_local_graph", &CoreWrapper::publishLocalGraphCallback, this);
 
 
 	setupCallbacks(subscribeDepth, subscribeLaserScan, queueSize);
@@ -616,6 +618,18 @@ bool CoreWrapper::publishLocalMapDataCallback(std_srvs::Empty::Request&, std_srv
 	return true;
 }
 
+bool CoreWrapper::publishGlobalGraphCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	publishGraph(true);
+	return true;
+}
+
+bool CoreWrapper::publishLocalGraphCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	publishGraph(false);
+	return true;
+}
+
 void CoreWrapper::publishMapData(bool global)
 {
 	ROS_INFO("rtabmap: Publishing map data (global=%s)...", global?"true":"false");
@@ -630,7 +644,6 @@ void CoreWrapper::publishMapData(bool global)
 	if(mapData_.getNumSubscribers())
 	{
 		rtabmap::MapDataPtr msg(new rtabmap::MapData);
-		msg->header.stamp = ros::Time::now();
 
 		rtabmap_.get3DMap(images,
 				depths,
@@ -718,6 +731,55 @@ void CoreWrapper::publishMapData(bool global)
 			++i;
 		}
 
+		msg->header.stamp = ros::Time::now();
+		mapData_.publish(msg);
+	}
+}
+
+void CoreWrapper::publishGraph(bool global)
+{
+	ROS_INFO("rtabmap: Publishing graph (global=%s)...", global?"true":"false");
+	std::map<int, Transform> poses;
+	std::multimap<int, Link> constraints;
+
+	if(mapData_.getNumSubscribers())
+	{
+		rtabmap::MapDataPtr msg(new rtabmap::MapData);
+
+		std::map<int, Transform> poses;
+		std::multimap<int, Link> constraints;
+
+		rtabmap_.getGraph(poses,
+				constraints,
+				true,
+				global);
+
+		int i=0;
+		msg->poseIDs.resize(poses.size());
+		msg->poses.resize(poses.size());
+		i=0;
+		for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			msg->poseIDs[i] = iter->first;
+			transformToPoseMsg(iter->second, msg->poses[i]);
+			++i;
+		}
+
+		msg->constraintFromIDs.resize(constraints.size());
+		msg->constraintToIDs.resize(constraints.size());
+		msg->constraintTypes.resize(constraints.size());
+		msg->constraints.resize(constraints.size());
+		i=0;
+		for(std::multimap<int, Link>::iterator iter = constraints.begin(); iter!=constraints.end(); ++iter)
+		{
+			msg->constraintFromIDs[i] = iter->first;
+			msg->constraintToIDs[i] = iter->second.to();
+			msg->constraintTypes[i] = iter->second.type();
+			transformToGeometryMsg(iter->second.transform(), msg->constraints[i]);
+			++i;
+		}
+
+		msg->header.stamp = ros::Time::now();
 		mapData_.publish(msg);
 	}
 }

rtabmap/src/CoreWrapper.h

@@ -76,8 +76,11 @@ private:
 	bool triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool publishGlobalMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool publishLocalMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool publishGlobalGraphCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool publishLocalGraphCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 
 	void publishMapData(bool global);
+	void publishGraph(bool global);
 
 	rtabmap::ParametersMap loadParameters(const std::string & configFile);
 	void saveParameters(const std::string & configFile);
@@ -140,6 +143,8 @@ private:
 	ros::ServiceServer triggerNewMapSrv_;
 	ros::ServiceServer publishGlobalMapDataSrv_;
 	ros::ServiceServer publishLocalMapDataSrv_;
+	ros::ServiceServer publishGlobalGraphSrv_;
+	ros::ServiceServer publishLocalGraphSrv_;
 
 	boost::thread* transformThread_;
 

rtabmap/src/GuiWrapper.cpp

@@ -415,7 +415,7 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
 				ROS_ERROR("Can't call \"trigger_new_map\" service");
 			}
 		}
-		else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMap)
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMapLocal)
 		{
 			if(mapDataTopic_.getNumPublishers())
 			{
@@ -431,7 +431,7 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
 				this->post(new RtabmapEvent3DMap(2)); // topic error
 			}
 		}
-		else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMapFull)
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMapGlobal)
 		{
 			if(mapDataTopic_.getNumPublishers())
 			{
@@ -447,6 +447,38 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
 				this->post(new RtabmapEvent3DMap(2)); // topic error
 			}
 		}
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublishTOROGraphLocal)
+		{
+			if(mapDataTopic_.getNumPublishers())
+			{
+				if(!ros::service::call("publish_local_graph", srv))
+				{
+					ROS_WARN("Can't call \"publish_local_graph\" service");
+					this->post(new RtabmapEvent3DMap(1)); // service error
+				}
+			}
+			else
+			{
+				ROS_WARN("No publisher subscribed for topic \"%s\", map cannot be downloaded!", mapDataTopic_.getTopic().c_str());
+				this->post(new RtabmapEvent3DMap(2)); // topic error
+			}
+		}
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublishTOROGraphGlobal)
+		{
+			if(mapDataTopic_.getNumPublishers())
+			{
+				if(!ros::service::call("publish_global_graph", srv))
+				{
+					ROS_WARN("Can't call \"publish_global_graph\" service");
+					this->post(new RtabmapEvent3DMap(1)); // service error
+				}
+			}
+			else
+			{
+				ROS_WARN("No publisher subscribed for topic \"%s\", map cannot be downloaded!", mapDataTopic_.getTopic().c_str());
+				this->post(new RtabmapEvent3DMap(2)); // topic error
+			}
+		}
 		else
 		{
 			ROS_WARN("Not handled command (%d)...", cmd);