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)

CMakeLists.txt

@@ -47,6 +47,7 @@ add_message_files(
    Info.msg
    KeyPoint.msg
    MapData.msg
+   MapGraph.msg
    NodeData.msg
    Link.msg
    OdomInfo.msg

include/rtabmap_ros/MsgConversion.h

@@ -45,6 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_ros/KeyPoint.h>
 #include <rtabmap_ros/Point2f.h>
 #include <rtabmap_ros/MapData.h>
+#include <rtabmap_ros/MapGraph.h>
 #include <rtabmap_ros/NodeData.h>
 #include <rtabmap_ros/OdomInfo.h>
 #include <rtabmap_ros/Info.h>
@@ -88,22 +89,23 @@ void mapDataFromROS(
 		std::multimap<int, rtabmap::Link> & links,
 		std::map<int, rtabmap::Signature> & signatures,
 		rtabmap::Transform & mapToOdom);
-void mapDataFromROS(
-		const rtabmap_ros::MapData & msg,
-		std::map<int, rtabmap::Transform> & poses,
-		std::multimap<int, rtabmap::Link> & links,
-		rtabmap::Transform & 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);
-void mapDataToROS(
+
+void mapGraphFromROS(
+		const rtabmap_ros::MapGraph & msg,
+		std::map<int, rtabmap::Transform> & poses,
+		std::multimap<int, rtabmap::Link> & links,
+		rtabmap::Transform & mapToOdom);
+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);
 
 rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg);
 void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg);

launch/config/demo_robot_mapping.rviz

@@ -169,7 +169,7 @@ Visualization Manager:
       Color: 25; 255; 0
       Enabled: true
       Name: MapGraph
-      Topic: /rtabmap/mapData
+      Topic: /rtabmap/mapGraph
       Value: true
     - Alpha: 0.7
       Class: rviz/Map

launch/config/demo_stereo_outdoor.rviz

@@ -156,7 +156,7 @@ Visualization Manager:
       Color: 25; 255; 0
       Enabled: true
       Name: MapGraph
-      Topic: /rtabmap/mapData_optimized
+      Topic: /rtabmap/mapDataGraph_optimized
       Value: true
     - Class: rviz/Image
       Enabled: true

msg/MapData.msg

@@ -4,18 +4,7 @@ Header header
 ##################
 # Optimized graph
 ##################
-##
+MapGraph graph
-# /map to /odom transform
-# Always identity when the graph is optimized from the latest pose.
-##
-geometry_msgs/Transform mapToOdom
-
-# The poses
-int32[] posesId
-geometry_msgs/Pose[] poses
-
-# The links
-Link[] links
 
 ##################
 # Graph data

msg/MapGraph.msg

@@ -0,0 +1,18 @@
+
+Header header
+
+##
+# /map to /odom transform
+# Always identity when the graph is optimized from the latest pose.
+##
+geometry_msgs/Transform mapToOdom
+
+# The poses
+int32[] posesId
+geometry_msgs/Pose[] poses
+
+# The links
+Link[] links
+
+
+

plugin_description.xml

@@ -11,9 +11,9 @@
          type="rtabmap_ros::MapGraphDisplay"
          base_class_type="rviz::Display">
     <description>
-      Displays graphs from rtabmap_ros/MapData messages.
+      Displays graphs from rtabmap_ros/MapGraph messages.
     </description>
-    <message_type>rtabmap_ros/MapData</message_type>
+    <message_type>rtabmap_ros/MapGraph</message_type>
   </class>
   <class name="rtabmap_ros/Info"
          type="rtabmap_ros::InfoDisplay"

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());
+		UASSERT(msg->graph.posesId.size() == msg->graph.poses.size());
-		for(unsigned int i=0; i<msg->posesId.size(); ++i)
+		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());
+		UASSERT(msg->graph.posesId.size() == msg->graph.poses.size());
-		for(unsigned int i=0; i<msg->posesId.size(); ++i)
+		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::MapData outputDataMsg;
-			rtabmap_ros::mapDataToROS(optimizedPoses,
+			rtabmap_ros::MapGraph outputGraphMsg;
+			rtabmap_ros::mapGraphToROS(optimizedPoses,
 					linksOut,
 					mapCorrection,
-					outputMsg);
+					outputGraphMsg);
-			outputMsg.header = msg->header;
+
-			outputMsg.nodes = msg->nodes;
+			if(mapGraphPub_.getNumSubscribers())
-			mapDataPub_.publish(outputMsg);
+			{
+				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(
+void mapDataToROS(
-		const rtabmap_ros::MapData & msg,
+		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(
+void mapGraphToROS(
-		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(
 		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();