Added MapGraph message to avoid MapGraphDisplay to subscribe to MapData (which can contain node data not used by MapGraphDisplay, that also limit the bandwidth used when MapCloudDisplay and MapGraphDisplay are added to RVIZ)

src/CoreNode.cpp

@@ -37,7 +37,7 @@ int main(int argc, char** argv)
 	ROS_INFO("Starting node...");
 
 	ULogger::setType(ULogger::kTypeConsole);
-	ULogger::setLevel(ULogger::kWarning);
+	ULogger::setLevel(ULogger::kInfo);
 
 	ros::init(argc, argv, "rtabmap");
 

src/CoreWrapper.cpp

@@ -64,6 +64,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 //msgs
 #include "rtabmap_ros/Info.h"
 #include "rtabmap_ros/MapData.h"
+#include "rtabmap_ros/MapGraph.h"
 #include "rtabmap_ros/GetMap.h"
 #include "rtabmap_ros/PublishMap.h"
 
@@ -187,6 +188,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 
 	infoPub_ = nh.advertise<rtabmap_ros::Info>("info", 1);
 	mapDataPub_ = nh.advertise<rtabmap_ros::MapData>("mapData", 1);
+	mapGraphPub_ = nh.advertise<rtabmap_ros::MapGraph>("mapGraph", 1);
 	labelsPub_ = nh.advertise<visualization_msgs::MarkerArray>("labels", 1);
 
 	// planning topics
@@ -1726,6 +1728,20 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 			mapDataPub_.publish(msg);
 		}
 
+		if(mapGraphPub_.getNumSubscribers())
+		{
+			rtabmap_ros::MapGraphPtr msg(new rtabmap_ros::MapGraph);
+			msg->header.stamp = now;
+			msg->header.frame_id = mapFrameId_;
+
+			rtabmap_ros::mapGraphToROS(poses,
+				constraints,
+				Transform::getIdentity(),
+				*msg);
+
+			mapGraphPub_.publish(msg);
+		}
+
 		if(!req.graphOnly && mapsManager_.hasSubscribers())
 		{
 			std::map<int, Transform> filteredPoses;
@@ -1950,6 +1966,21 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
 		mapDataPub_.publish(msg);
 	}
 
+	if(mapGraphPub_.getNumSubscribers())
+	{
+		rtabmap_ros::MapGraphPtr msg(new rtabmap_ros::MapGraph);
+		msg->header.stamp = stamp;
+		msg->header.frame_id = mapFrameId_;
+
+		rtabmap_ros::mapGraphToROS(
+			stats.poses(),
+			stats.constraints(),
+			stats.mapCorrection(),
+			*msg);
+
+		mapGraphPub_.publish(msg);
+	}
+
 	if(labelsPub_.getNumSubscribers())
 	{
 		if(stats.poses().size() && stats.getSignatures().size())

src/CoreWrapper.h

@@ -246,6 +246,7 @@ private:
 
 	ros::Publisher infoPub_;
 	ros::Publisher mapDataPub_;
+	ros::Publisher mapGraphPub_;
 	ros::Publisher labelsPub_;
 
 	//Planning stuff

src/GridMapAssemblerNode.cpp

@@ -99,10 +99,10 @@ public:
 		}
 
 		std::map<int, Transform> poses;
-		UASSERT(msg->posesId.size() == msg->poses.size());
-		for(unsigned int i=0; i<msg->posesId.size(); ++i)
+		UASSERT(msg->graph.posesId.size() == msg->graph.poses.size());
+		for(unsigned int i=0; i<msg->graph.posesId.size(); ++i)
 		{
-			poses.insert(std::make_pair(msg->posesId[i], rtabmap_ros::transformFromPoseMsg(msg->poses[i])));
+			poses.insert(std::make_pair(msg->graph.posesId[i], rtabmap_ros::transformFromPoseMsg(msg->graph.poses[i])));
 		}
 
 		if(filterRadius_ > 0.0 && filterAngle_ > 0.0)

src/MapAssemblerNode.cpp

@@ -191,10 +191,10 @@ public:
 
 		// filter poses
 		std::map<int, Transform> poses;
-		UASSERT(msg->posesId.size() == msg->poses.size());
-		for(unsigned int i=0; i<msg->posesId.size(); ++i)
+		UASSERT(msg->graph.posesId.size() == msg->graph.poses.size());
+		for(unsigned int i=0; i<msg->graph.posesId.size(); ++i)
 		{
-			poses.insert(std::make_pair(msg->posesId[i], rtabmap_ros::transformFromPoseMsg(msg->poses[i])));
+			poses.insert(std::make_pair(msg->graph.posesId[i], rtabmap_ros::transformFromPoseMsg(msg->graph.poses[i])));
 		}
 		if(nodeFilteringAngle_ > 0.0 && nodeFilteringRadius_ > 0.0)
 		{

src/MapOptimizerNode.cpp

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <ros/ros.h>
 #include "rtabmap_ros/MapData.h"
+#include "rtabmap_ros/MapGraph.h"
 #include "rtabmap_ros/MsgConversion.h"
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/Graph.h>
@@ -73,6 +74,7 @@ public:
 
 		mapDataTopic_ = nh.subscribe("mapData", 1, &MapOptimizer::mapDataReceivedCallback, this);
 		mapDataPub_ = nh.advertise<rtabmap_ros::MapData>(nh.resolveName("mapData")+"_optimized", 1);
+		mapGraphPub_ = nh.advertise<rtabmap_ros::MapGraph>(nh.resolveName("mapData")+"Graph_optimized", 1);
 
 		if(publishTf)
 		{
@@ -117,14 +119,14 @@ public:
 	{
 		// save new poses and constraints
 		// Assuming that nodes/constraints are all linked together
-		UASSERT(msg->posesId.size() == msg->poses.size());
+		UASSERT(msg->graph.posesId.size() == msg->graph.poses.size());
 
 		bool dataChanged = false;
 
 		std::multimap<int, Link> newConstraints;
-		for(unsigned int i=0; i<msg->links.size(); ++i)
+		for(unsigned int i=0; i<msg->graph.links.size(); ++i)
 		{
-			Link link = rtabmap_ros::linkFromROS(msg->links[i]);
+			Link link = rtabmap_ros::linkFromROS(msg->graph.links[i]);
 			newConstraints.insert(std::make_pair(link.from(), link));
 
 			bool edgeAlreadyAdded = false;
@@ -180,22 +182,22 @@ public:
 		else
 		{
 			constraints = newConstraints;
-			for(unsigned int i=0; i<msg->posesId.size(); ++i)
+			for(unsigned int i=0; i<msg->graph.posesId.size(); ++i)
 			{
-				std::map<int, Transform>::iterator iter = cachedPoses_.find(msg->posesId[i]);
+				std::map<int, Transform>::iterator iter = cachedPoses_.find(msg->graph.posesId[i]);
 				if(iter != cachedPoses_.end())
 				{
 					poses.insert(*iter);
 				}
 				else
 				{
-					ROS_ERROR("Odometry pose of node %d not found in cache!", msg->posesId[i]);
+					ROS_ERROR("Odometry pose of node %d not found in cache!", msg->graph.posesId[i]);
 					return;
 				}
 			}
 		}
 		// Optimize only if there is a subscriber
-		if(mapDataPub_.getNumSubscribers())
+		if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers())
 		{
 			UTimer timer;
 			std::map<int, Transform> optimizedPoses;
@@ -229,14 +231,26 @@ public:
 					  (int)poses.size(), (int)constraints.size());
 			}
 
-			rtabmap_ros::MapData outputMsg;
-			rtabmap_ros::mapDataToROS(optimizedPoses,
+			rtabmap_ros::MapData outputDataMsg;
+			rtabmap_ros::MapGraph outputGraphMsg;
+			rtabmap_ros::mapGraphToROS(optimizedPoses,
 					linksOut,
 					mapCorrection,
-					outputMsg);
-			outputMsg.header = msg->header;
-			outputMsg.nodes = msg->nodes;
-			mapDataPub_.publish(outputMsg);
+					outputGraphMsg);
+
+			if(mapGraphPub_.getNumSubscribers())
+			{
+				outputGraphMsg.header = msg->header;
+				mapGraphPub_.publish(outputGraphMsg);
+			}
+
+			if(mapDataPub_.getNumSubscribers())
+			{
+				outputDataMsg.header = msg->header;
+				outputDataMsg.graph = outputGraphMsg;
+				outputDataMsg.nodes = msg->nodes;
+				mapDataPub_.publish(outputDataMsg);
+			}
 
 			ROS_INFO("Time graph optimization = %f s", timer.ticks());
 		}
@@ -256,6 +270,7 @@ private:
 	ros::Subscriber mapDataTopic_;
 
 	ros::Publisher mapDataPub_;
+	ros::Publisher mapGraphPub_;
 
 	std::map<int, Transform> cachedPoses_;
 	std::multimap<int, Link> cachedConstraints_;

src/MsgConversion.cpp

@@ -298,7 +298,7 @@ void mapDataFromROS(
 		rtabmap::Transform & mapToOdom)
 {
 	//optimized graph
-	mapDataFromROS(msg, poses, links, mapToOdom);
+	mapGraphFromROS(msg.graph, poses, links, mapToOdom);
 
 	//Data
 	for(unsigned int i=0; i<msg.nodes.size(); ++i)
@@ -306,8 +306,29 @@ void mapDataFromROS(
 		signatures.insert(std::make_pair(msg.nodes[i].id, nodeDataFromROS(msg.nodes[i])));
 	}
 }
-void mapDataFromROS(
-		const rtabmap_ros::MapData & msg,
+void mapDataToROS(
+		const std::map<int, rtabmap::Transform> & poses,
+		const std::multimap<int, rtabmap::Link> & links,
+		const std::map<int, rtabmap::Signature> & signatures,
+		const rtabmap::Transform & mapToOdom,
+		rtabmap_ros::MapData & msg)
+{
+	//Optimized graph
+	mapGraphToROS(poses, links, mapToOdom, msg.graph);
+
+	//Data
+	msg.nodes.resize(signatures.size());
+	int index=0;
+	for(std::multimap<int, rtabmap::Signature>::const_iterator iter = signatures.begin();
+		iter!=signatures.end();
+		++iter)
+	{
+		nodeDataToROS(iter->second, msg.nodes[index++]);
+	}
+}
+
+void mapGraphFromROS(
+		const rtabmap_ros::MapGraph & msg,
 		std::map<int, rtabmap::Transform> & poses,
 		std::multimap<int, rtabmap::Link> & links,
 		rtabmap::Transform & mapToOdom)
@@ -325,32 +346,11 @@ void mapDataFromROS(
 	}
 	mapToOdom = transformFromGeometryMsg(msg.mapToOdom);
 }
-void mapDataToROS(
-		const std::map<int, rtabmap::Transform> & poses,
-		const std::multimap<int, rtabmap::Link> & links,
-		const std::map<int, rtabmap::Signature> & signatures,
-		const rtabmap::Transform & mapToOdom,
-		rtabmap_ros::MapData & msg)
-{
-	//Optimized graph
-	mapDataToROS(poses, links, mapToOdom, msg);
-
-	//Data
-	msg.nodes.resize(signatures.size());
-	int index=0;
-	for(std::multimap<int, rtabmap::Signature>::const_iterator iter = signatures.begin();
-		iter!=signatures.end();
-		++iter)
-	{
-		nodeDataToROS(iter->second, msg.nodes[index++]);
-	}
-}
-
-void mapDataToROS(
+void mapGraphToROS(
 		const std::map<int, rtabmap::Transform> & poses,
 		const std::multimap<int, rtabmap::Link> & links,
 		const rtabmap::Transform & mapToOdom,
-		rtabmap_ros::MapData & msg)
+		rtabmap_ros::MapGraph & msg)
 {
 	//Optimized graph
 	msg.posesId.resize(poses.size());

src/rviz/MapCloudDisplay.cpp

@@ -299,9 +299,9 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 
 	// Update graph
 	std::map<int, rtabmap::Transform> poses;
-	for(unsigned int i=0; i<map.posesId.size() && i<map.poses.size(); ++i)
+	for(unsigned int i=0; i<map.graph.posesId.size() && i<map.graph.poses.size(); ++i)
 	{
-		poses.insert(std::make_pair(map.posesId[i], rtabmap_ros::transformFromPoseMsg(map.poses[i])));
+		poses.insert(std::make_pair(map.graph.posesId[i], rtabmap_ros::transformFromPoseMsg(map.graph.poses[i])));
 	}
 
 	if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
@@ -482,12 +482,12 @@ void MapCloudDisplay::downloadMap()
 		else
 		{
 			messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
-					.arg(getMapSrv.response.data.poses.size()).arg(getMapSrv.response.data.nodes.size()));
+					.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
 			QApplication::processEvents();
 			this->reset();
 			processMapData(getMapSrv.response.data);
 			messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
-					.arg(getMapSrv.response.data.poses.size()).arg(getMapSrv.response.data.nodes.size()));
+					.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
 
 			QTimer::singleShot(1000, messageBox, SLOT(close()));
 		}
@@ -539,10 +539,10 @@ void MapCloudDisplay::downloadGraph()
 		}
 		else
 		{
-			messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.poses.size()));
+			messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.graph.poses.size()));
 			QApplication::processEvents();
 			processMapData(getMapSrv.response.data);
-			messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.poses.size()));
+			messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.graph.poses.size()));
 
 			QTimer::singleShot(1000, messageBox, SLOT(close()));
 		}

src/rviz/MapGraphDisplay.cpp

@@ -93,7 +93,7 @@ void MapGraphDisplay::destroyObjects()
 	manual_objects_.clear();
 }
 
-void MapGraphDisplay::processMessage( const rtabmap_ros::MapData::ConstPtr& msg )
+void MapGraphDisplay::processMessage( const rtabmap_ros::MapGraph::ConstPtr& msg )
 {
 	if(!(msg->poses.size() == msg->posesId.size()))
 	{
@@ -105,7 +105,7 @@ void MapGraphDisplay::processMessage( const rtabmap_ros::MapData::ConstPtr& msg
 	std::map<int, rtabmap::Transform> poses;
 	std::multimap<int, rtabmap::Link> links;
 	rtabmap::Transform mapToOdom;
-	rtabmap_ros::mapDataFromROS(*msg, poses, links, mapToOdom);
+	rtabmap_ros::mapGraphFromROS(*msg, poses, links, mapToOdom);
 
 	destroyObjects();
 

src/rviz/MapGraphDisplay.h

@@ -29,7 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef MAP_GRAPH_DISPLAY_H
 #define MAP_GRAPH_DISPLAY_H
 
-#include <rtabmap_ros/MapData.h>
+#include <rtabmap_ros/MapGraph.h>
 
 #include <rviz/message_filter_display.h>
 
@@ -51,9 +51,9 @@ namespace rtabmap_ros
 
 /**
  * \class MapGraphDisplay
- * \brief Displays the graph in rtabmap::MapData message
+ * \brief Displays the graph of rtabmap::MapGraph message
  */
-class MapGraphDisplay: public MessageFilterDisplay<rtabmap_ros::MapData>
+class MapGraphDisplay: public MessageFilterDisplay<rtabmap_ros::MapGraph>
 {
 Q_OBJECT
 public:
@@ -68,7 +68,7 @@ protected:
   virtual void onInitialize();
 
   /** @brief Overridden from MessageFilterDisplay. */
-  void processMessage( const rtabmap_ros::MapData::ConstPtr& msg );
+  void processMessage( const rtabmap_ros::MapGraph::ConstPtr& msg );
 
 private:
   void destroyObjects();