added rtabmap/list_labels service, added labels to MapGraph msg, rtabmap node publishes labels as visualization_msgs/MarkerArray

CMakeLists.txt

@@ -5,7 +5,7 @@ project(rtabmap_ros)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS 
-             cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs
+             cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs visualization_msgs
              image_transport tf tf_conversions laser_geometry pcl_conversions 
              pcl_ros nodelet dynamic_reconfigure rviz message_filters class_loader
              genmsg stereo_msgs octomap_ros
@@ -51,6 +51,7 @@ add_message_files(
  add_service_files(
    FILES
    GetMap.srv
+   ListLabels.srv
    PublishMap.srv
    ResetPose.srv
    SetGoal.srv
@@ -80,7 +81,7 @@ generate_dynamic_reconfigure_options(cfg/Camera.cfg)
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES rtabmap_ros
-  CATKIN_DEPENDS cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs
+  CATKIN_DEPENDS cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs visualization_msgs
                  image_transport tf tf_conversions laser_geometry pcl_conversions 
                  pcl_ros nodelet dynamic_reconfigure rviz message_filters class_loader
                  stereo_msgs octomap_ros

include/rtabmap_ros/MsgConversion.h

@@ -81,11 +81,13 @@ void mapGraphFromROS(
 		const rtabmap_ros::Graph & msg,
 		std::map<int, rtabmap::Transform> & poses,
 		std::map<int, int> & mapIds,
+		std::map<int, std::string> & labels,
 		std::multimap<int, rtabmap::Link> & links,
 		rtabmap::Transform & mapToOdom);
 void mapGraphToROS(
 		const std::map<int, rtabmap::Transform> & poses,
 		const std::map<int, int> & mapIds,
+		const std::map<int, std::string> & labels,
 		const std::multimap<int, rtabmap::Link> & links,
 		const rtabmap::Transform & mapToOdom,
 		rtabmap_ros::Graph & msg);

msg/Graph.msg

@@ -12,6 +12,7 @@ geometry_msgs/Transform mapToOdom
 # std::map<nodeId, mapId>
 int32[] nodeIds
 int32[] mapIds
+string[] labels
 
 # std::map<nodeId, Pose>
 geometry_msgs/Pose[] poses

package.xml

@@ -21,6 +21,7 @@
   <build_depend>nav_msgs</build_depend>
   <build_depend>stereo_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
+  <build_depend>visualization_msgs</build_depend>
   <build_depend>image_transport</build_depend>
   <build_depend>tf</build_depend>
   <build_depend>tf_conversions</build_depend>
@@ -46,6 +47,7 @@
   <run_depend>nav_msgs</run_depend>
   <run_depend>stereo_msgs</run_depend>
   <run_depend>geometry_msgs</run_depend>
+  <run_depend>visualization_msgs</run_depend>
   <run_depend>image_transport</run_depend>
   <run_depend>image_transport_plugins</run_depend>
   <run_depend>tf</run_depend>

src/CoreWrapper.cpp

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <nav_msgs/OccupancyGrid.h>
 #include <std_msgs/Int32MultiArray.h>
 #include <std_msgs/Bool.h>
+#include <visualization_msgs/MarkerArray.h>
 #include <rtabmap/core/RtabmapEvent.h>
 #include <rtabmap/core/Camera.h>
 #include <rtabmap/core/Parameters.h>
@@ -188,6 +189,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::Graph>("graph", 1);
+	labelsPub_ = nh.advertise<visualization_msgs::MarkerArray>("labels", 1);
 
 	// mapping topics
 	cloudMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud_map", 1);
@@ -355,6 +357,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this);
 	setGoalSrv_ = nh.advertiseService("set_goal", &CoreWrapper::setGoalCallback, this);
 	setLabelSrv_ = nh.advertiseService("set_label", &CoreWrapper::setLabelCallback, this);
+	listLabelsSrv_ = nh.advertiseService("list_labels", &CoreWrapper::listLabelsCallback, this);
 	octomapBinarySrv_ = nh.advertiseService("octomap_binary", &CoreWrapper::octomapBinaryCallback, this);
 	octomapFullSrv_ = nh.advertiseService("octomap_full", &CoreWrapper::octomapFullCallback, this);
 
@@ -1349,6 +1352,7 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros:
 	std::map<int, Transform> poses;
 	std::multimap<int, Link> constraints;
 	std::map<int, int> mapIds;
+	std::map<int, std::string> labels;
 
 	if(req.graphOnly)
 	{
@@ -1356,6 +1360,7 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros:
 				poses,
 				constraints,
 				mapIds,
+				labels,
 				req.optimized,
 				req.global);
 	}
@@ -1366,6 +1371,7 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros:
 				poses,
 				constraints,
 				mapIds,
+				labels,
 				req.optimized,
 				req.global);
 	}
@@ -1379,6 +1385,7 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros:
 	//RGB-D SLAM data
 	rtabmap_ros::mapGraphToROS(poses,
 		mapIds,
+		labels,
 		constraints,
 		Transform::getIdentity(),
 		res.data.graph);
@@ -1507,6 +1514,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 		std::map<int, Transform> poses;
 		std::multimap<int, Link> constraints;
 		std::map<int, int> mapIds;
+		std::map<int, std::string> labels;
 
 		if(req.graphOnly)
 		{
@@ -1514,6 +1522,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 					poses,
 					constraints,
 					mapIds,
+					labels,
 					req.optimized,
 					req.global);
 		}
@@ -1524,6 +1533,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 					poses,
 					constraints,
 					mapIds,
+					labels,
 					req.optimized,
 					req.global);
 		}
@@ -1542,6 +1552,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 
 			rtabmap_ros::mapGraphToROS(poses,
 				mapIds,
+				labels,
 				constraints,
 				Transform::getIdentity(),
 				*graphMsg);
@@ -1612,6 +1623,84 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 				}
 			}
 		}
+
+		if(labelsPub_.getNumSubscribers())
+		{
+			if(poses.size() && labels.size())
+			{
+				visualization_msgs::MarkerArray markers;
+				for(std::map<int, std::string>::const_iterator iter=labels.begin();
+					iter!=labels.end();
+					++iter)
+				{
+					std::map<int, Transform>::const_iterator poseIter= poses.find(iter->first);
+					if(poseIter!=poses.end())
+					{
+						// Add labels
+						if(!iter->second.empty())
+						{
+							visualization_msgs::Marker marker;
+							marker.header.frame_id = mapFrameId_;
+							marker.header.stamp = now;
+							marker.ns = "labels";
+							marker.id = -iter->first;
+							marker.action = visualization_msgs::Marker::ADD;
+							marker.pose.position.x = poseIter->second.x();
+							marker.pose.position.y = poseIter->second.y();
+							marker.pose.position.z = poseIter->second.z();
+							marker.pose.orientation.x = 0.0;
+							marker.pose.orientation.y = 0.0;
+							marker.pose.orientation.z = 0.0;
+							marker.pose.orientation.w = 1.0;
+							marker.scale.x = 1;
+							marker.scale.y = 1;
+							marker.scale.z = 0.5;
+							marker.color.a = 0.7;
+							marker.color.r = 1.0;
+							marker.color.g = 0.0;
+							marker.color.b = 0.0;
+
+							marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+							marker.text = iter->second;
+
+							markers.markers.push_back(marker);
+						}
+						// Add node ids
+						visualization_msgs::Marker marker;
+						marker.header.frame_id = mapFrameId_;
+						marker.header.stamp = now;
+						marker.ns = "ids";
+						marker.id = iter->first;
+						marker.action = visualization_msgs::Marker::ADD;
+						marker.pose.position.x = poseIter->second.x();
+						marker.pose.position.y = poseIter->second.y();
+						marker.pose.position.z = poseIter->second.z();
+						marker.pose.orientation.x = 0.0;
+						marker.pose.orientation.y = 0.0;
+						marker.pose.orientation.z = 0.0;
+						marker.pose.orientation.w = 1.0;
+						marker.scale.x = 1;
+						marker.scale.y = 1;
+						marker.scale.z = 0.2;
+						marker.color.a = 0.5;
+						marker.color.r = 1.0;
+						marker.color.g = 1.0;
+						marker.color.b = 1.0;
+						marker.lifetime = ros::Duration(2.0f/rate_);
+
+						marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+						marker.text = uNumber2Str(iter->first);
+
+						markers.markers.push_back(marker);
+					}
+				}
+
+				if(markers.markers.size())
+				{
+					labelsPub_.publish(markers);
+				}
+			}
+		}
 	}
 	else
 	{
@@ -1679,6 +1768,17 @@ bool CoreWrapper::setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_
 	return true;
 }
 
+bool CoreWrapper::listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtabmap_ros::ListLabels::Response& res)
+{
+	if(rtabmap_.getMemory())
+	{
+		std::map<int, std::string> labels = rtabmap_.getMemory()->getAllLabels();
+		res.labels = uValues(labels);
+		ROS_INFO("List labels service: %d labels found.", (int)res.labels.size());
+	}
+	return true;
+}
+
 void CoreWrapper::publishStats(const ros::Time & stamp)
 {
 	const rtabmap::Statistics & stats = rtabmap_.getStatistics();
@@ -1696,7 +1796,8 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
 
 	if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers())
 	{
-		if(stats.poses().size() == 0 || stats.poses().size() == stats.getMapIds().size())
+		if(stats.poses().size() == stats.getMapIds().size() &&
+		   stats.poses().size() == stats.getLabels().size())
 		{
 			rtabmap_ros::GraphPtr graphMsg(new rtabmap_ros::Graph);
 			graphMsg->header.stamp = stamp;
@@ -1705,6 +1806,7 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
 			rtabmap_ros::mapGraphToROS(
 				stats.poses(),
 				stats.getMapIds(),
+				stats.getLabels(),
 				stats.constraints(),
 				stats.mapCorrection(),
 				*graphMsg);
@@ -1729,7 +1831,86 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
 		}
 		else
 		{
-			ROS_ERROR("Poses and map ids are not the same size!? %d vs %d", (int)stats.poses().size(), (int)stats.getMapIds().size());
+			ROS_ERROR("Poses, map ids and labels are not the same size!? %d vs %d vs %d",
+					(int)stats.poses().size(), (int)stats.getMapIds().size(), (int)stats.getLabels().size());
+		}
+	}
+
+	if(labelsPub_.getNumSubscribers())
+	{
+		if(stats.poses().size() && stats.getLabels().size())
+		{
+			visualization_msgs::MarkerArray markers;
+			for(std::map<int, std::string>::const_iterator iter=stats.getLabels().begin();
+				iter!=stats.getLabels().end();
+				++iter)
+			{
+				std::map<int, Transform>::const_iterator poseIter= stats.poses().find(iter->first);
+				if(poseIter!=stats.poses().end())
+				{
+					// Add labels
+					if(!iter->second.empty())
+					{
+						visualization_msgs::Marker marker;
+						marker.header.frame_id = mapFrameId_;
+						marker.header.stamp = stamp;
+						marker.ns = "labels";
+						marker.id = -iter->first;
+						marker.action = visualization_msgs::Marker::ADD;
+						marker.pose.position.x = poseIter->second.x();
+						marker.pose.position.y = poseIter->second.y();
+						marker.pose.position.z = poseIter->second.z();
+						marker.pose.orientation.x = 0.0;
+						marker.pose.orientation.y = 0.0;
+						marker.pose.orientation.z = 0.0;
+						marker.pose.orientation.w = 1.0;
+						marker.scale.x = 1;
+						marker.scale.y = 1;
+						marker.scale.z = 0.5;
+						marker.color.a = 0.7;
+						marker.color.r = 1.0;
+						marker.color.g = 0.0;
+						marker.color.b = 0.0;
+
+						marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+						marker.text = iter->second;
+
+						markers.markers.push_back(marker);
+					}
+					// Add node ids
+					visualization_msgs::Marker marker;
+					marker.header.frame_id = mapFrameId_;
+					marker.header.stamp = stamp;
+					marker.ns = "ids";
+					marker.id = iter->first;
+					marker.action = visualization_msgs::Marker::ADD;
+					marker.pose.position.x = poseIter->second.x();
+					marker.pose.position.y = poseIter->second.y();
+					marker.pose.position.z = poseIter->second.z();
+					marker.pose.orientation.x = 0.0;
+					marker.pose.orientation.y = 0.0;
+					marker.pose.orientation.z = 0.0;
+					marker.pose.orientation.w = 1.0;
+					marker.scale.x = 1;
+					marker.scale.y = 1;
+					marker.scale.z = 0.2;
+					marker.color.a = 0.5;
+					marker.color.r = 1.0;
+					marker.color.g = 1.0;
+					marker.color.b = 1.0;
+					marker.lifetime = ros::Duration(2.0f/rate_);
+
+					marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+					marker.text = uNumber2Str(iter->first);
+
+					markers.markers.push_back(marker);
+				}
+			}
+
+			if(markers.markers.size())
+			{
+				labelsPub_.publish(markers);
+			}
 		}
 	}
 }

src/CoreWrapper.h

@@ -54,6 +54,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Rtabmap.h>
 
 #include "rtabmap_ros/GetMap.h"
+#include "rtabmap_ros/ListLabels.h"
 #include "rtabmap_ros/PublishMap.h"
 #include "rtabmap_ros/SetGoal.h"
 #include "rtabmap_ros/SetLabel.h"
@@ -191,6 +192,7 @@ private:
 	bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&);
 	bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res);
 	bool setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res);
+	bool listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtabmap_ros::ListLabels::Response& res);
 	bool octomapBinaryCallback(octomap_msgs::GetOctomap::Request  &req, octomap_msgs::GetOctomap::Response &res);
 	bool octomapFullCallback(octomap_msgs::GetOctomap::Request  &req, octomap_msgs::GetOctomap::Response &res);
 
@@ -256,6 +258,7 @@ private:
 	ros::Publisher infoPub_;
 	ros::Publisher mapDataPub_;
 	ros::Publisher mapGraphPub_;
+	ros::Publisher labelsPub_;
 	ros::Publisher cloudMapPub_;
 	ros::Publisher projMapPub_;
 	ros::Publisher gridMapPub_;
@@ -377,6 +380,7 @@ private:
 	ros::ServiceServer publishMapDataSrv_;
 	ros::ServiceServer setGoalSrv_;
 	ros::ServiceServer setLabelSrv_;
+	ros::ServiceServer listLabelsSrv_;
 	ros::ServiceServer octomapBinarySrv_;
 	ros::ServiceServer octomapFullSrv_;
 

src/GuiWrapper.cpp

@@ -185,9 +185,10 @@ void GuiWrapper::infoMapCallback(
 	rtabmap::Transform mapToOdom;
 	std::map<int, rtabmap::Transform> poses;
 	std::map<int, int> mapIds;
+	std::map<int, std::string> labels;
 	std::multimap<int, Link> links;
 
-	rtabmap_ros::mapGraphFromROS(mapMsg->graph, poses, mapIds, links, mapToOdom);
+	rtabmap_ros::mapGraphFromROS(mapMsg->graph, poses, mapIds, labels, links, mapToOdom);
 
 	stat.setMapCorrection(mapToOdom);
 	stat.setPoses(poses);
@@ -213,6 +214,7 @@ void GuiWrapper::processRequestedMap(const rtabmap_ros::MapData & map)
 	std::map<int, Transform> poses;
 	std::multimap<int, rtabmap::Link> constraints;
 	std::map<int, int> mapIds;
+	std::map<int, std::string> labels;
 	Transform mapToOdom;
 
 	if(map.graph.nodeIds.size() != map.graph.mapIds.size())
@@ -229,7 +231,7 @@ void GuiWrapper::processRequestedMap(const rtabmap_ros::MapData & map)
 		return;
 	}
 
-	rtabmap_ros::mapGraphFromROS(map.graph, poses, mapIds, constraints, mapToOdom);
+	rtabmap_ros::mapGraphFromROS(map.graph, poses, mapIds, labels, constraints, mapToOdom);
 
 	//data
 	for(unsigned int i=0; i<map.nodes.size(); ++i)
@@ -240,7 +242,8 @@ void GuiWrapper::processRequestedMap(const rtabmap_ros::MapData & map)
 	this->post(new RtabmapEvent3DMap(signatures,
 			poses,
 			constraints,
-			mapIds));
+			mapIds,
+			labels));
 }
 
 void GuiWrapper::handleEvent(UEvent * anEvent)

src/MapOptimizerNode.cpp

@@ -115,6 +115,7 @@ public:
 		// Assuming that nodes/constraints are all linked together
 		UASSERT(msg->graph.nodeIds.size() == msg->graph.poses.size());
 		UASSERT(msg->graph.nodeIds.size() == msg->graph.mapIds.size());
+		UASSERT(msg->graph.nodeIds.size() == msg->graph.labels.size());
 
 		bool dataChanged = false;
 
@@ -146,6 +147,7 @@ public:
 
 		std::map<int, Transform> newPoses;
 		std::map<int, int> newMapIds;
+		std::map<int, std::string> newLabels;
 		// add new odometry poses
 		for(unsigned int i=0; i<msg->nodes.size(); ++i)
 		{
@@ -153,6 +155,7 @@ public:
 			Transform pose = rtabmap_ros::transformFromPoseMsg(msg->nodes[i].pose);
 			newPoses.insert(std::make_pair(id, pose));
 			newMapIds.insert(std::make_pair(id, msg->nodes[i].mapId));
+			newLabels.insert(std::make_pair(id, msg->nodes[i].label));
 
 			std::pair<std::map<int, Transform>::iterator, bool> p = cachedPoses_.insert(std::make_pair(id, pose));
 			if(!p.second && pose != cachedPoses_.at(id))
@@ -162,6 +165,7 @@ public:
 			else if(p.second)
 			{
 				cachedMapIds_.insert(std::make_pair(id, msg->nodes[i].mapId));
+				cachedLabels_.insert(std::make_pair(id, msg->nodes[i].label));
 			}
 		}
 
@@ -170,17 +174,20 @@ public:
 			ROS_WARN("Graph data has changed! Reset cache...");
 			cachedPoses_ = newPoses;
 			cachedMapIds_ = newMapIds;
+			cachedLabels_ = newLabels;
 			cachedConstraints_ = newConstraints;
 		}
 
 		//match poses in the graph
 		std::map<int, Transform> poses;
 		std::map<int, int> mapIds;
+		std::map<int, std::string> labels;
 		std::multimap<int, Link> constraints;
 		if(globalOptimization_)
 		{
 			poses = cachedPoses_;
 			mapIds = cachedMapIds_;
+			labels = cachedLabels_;
 			constraints = cachedConstraints_;
 		}
 		else
@@ -193,6 +200,7 @@ public:
 				{
 					poses.insert(*iter);
 					mapIds.insert(*cachedMapIds_.find(iter->first));
+					labels.insert(*cachedLabels_.find(iter->first));
 				}
 				else
 				{
@@ -235,11 +243,13 @@ public:
 					   "not be null if there are edges, and edges must be null if poses <= 1)",
 					  (int)poses.size(), (int)constraints.size());
 				mapIds.clear();
+				labels.clear();
 			}
 
 			UASSERT(optimizedPoses.size() == mapIds.size());
+			UASSERT(optimizedPoses.size() == labels.size());
 			rtabmap_ros::MapData outputMsg;
-			rtabmap_ros::mapGraphToROS(optimizedPoses, mapIds, std::multimap<int, rtabmap::Link>(), mapCorrection, outputMsg.graph);
+			rtabmap_ros::mapGraphToROS(optimizedPoses, mapIds, labels, std::multimap<int, rtabmap::Link>(), mapCorrection, outputMsg.graph);
 			outputMsg.graph.links = msg->graph.links;
 			outputMsg.header = msg->header;
 			outputMsg.nodes = msg->nodes;
@@ -266,6 +276,7 @@ private:
 
 	std::map<int, Transform> cachedPoses_;
 	std::map<int, int> cachedMapIds_;
+	std::map<int, std::string> cachedLabels_;
 	std::multimap<int, Link> cachedConstraints_;
 
 	tf::TransformBroadcaster tfBroadcaster_;

src/MsgConversion.cpp

@@ -252,18 +252,21 @@ void mapGraphFromROS(
 		const rtabmap_ros::Graph & msg,
 		std::map<int, rtabmap::Transform> & poses,
 		std::map<int, int> & mapIds,
+		std::map<int, std::string> & labels,
 		std::multimap<int, rtabmap::Link> & links,
 		rtabmap::Transform & mapToOdom)
 {
 	mapToOdom = transformFromGeometryMsg(msg.mapToOdom);
 
-	for(unsigned int i=0; i<msg.nodeIds.size() && i<msg.mapIds.size(); ++i)
+	UASSERT(msg.nodeIds.size() == msg.mapIds.size());
+	UASSERT(msg.nodeIds.size() == msg.poses.size());
+	UASSERT(msg.nodeIds.size() == msg.labels.size());
+
+	for(unsigned int i=0; i<msg.nodeIds.size(); ++i)
 	{
-		if(msg.poses.size())
-		{
-			poses.insert(std::make_pair(msg.nodeIds[i], rtabmap_ros::transformFromPoseMsg(msg.poses[i])));
-		}
+		poses.insert(std::make_pair(msg.nodeIds[i], rtabmap_ros::transformFromPoseMsg(msg.poses[i])));
 		mapIds.insert(std::make_pair(msg.nodeIds[i], msg.mapIds[i]));
+		labels.insert(std::make_pair(msg.nodeIds[i], msg.labels[i]));
 	}
 
 	for(unsigned int i=0; i<msg.links.size(); ++i)
@@ -275,30 +278,32 @@ void mapGraphFromROS(
 void mapGraphToROS(
 		const std::map<int, rtabmap::Transform> & poses,
 		const std::map<int, int> & mapIds,
+		const std::map<int, std::string> & labels,
 		const std::multimap<int, rtabmap::Link> & links,
 		const rtabmap::Transform & mapToOdom,
 		rtabmap_ros::Graph & msg)
 {
-	UASSERT(poses.size() == 0 || poses.size() == mapIds.size());
+	UASSERT(poses.size() == 0 || (poses.size() == mapIds.size() && poses.size() == labels.size()));
 
 	transformToGeometryMsg(mapToOdom, msg.mapToOdom);
 
-	msg.nodeIds.resize(mapIds.size());
+	msg.nodeIds.resize(poses.size());
 	msg.poses.resize(poses.size());
-	msg.mapIds.resize(mapIds.size());
+	msg.mapIds.resize(poses.size());
+	msg.labels.resize(poses.size());
 	int index = 0;
 	std::map<int, rtabmap::Transform>::const_iterator iterPoses = poses.begin();
-	for(std::map<int, int>::const_iterator iter = mapIds.begin();
-		iter!=mapIds.end();
-		++iter)
+	std::map<int, int>::const_iterator iterMapIds = mapIds.begin();
+	std::map<int, std::string>::const_iterator iterLabels = labels.begin();
+	while(iterPoses != poses.end())
 	{
-		msg.nodeIds[index] = iter->first;
-		msg.mapIds[index] = iter->second;
-		if(iterPoses != poses.end())
-		{
-			transformToPoseMsg(iterPoses->second, msg.poses[index]);
-			++iterPoses;
-		}
+		msg.nodeIds[index] = iterPoses->first;
+		msg.mapIds[index] = iterMapIds->second;
+		msg.labels[index] = iterLabels->second;
+		transformToPoseMsg(iterPoses->second, msg.poses[index]);
+		++iterPoses;
+		++iterMapIds;
+		++iterLabels;
 		++index;
 	}
 

src/rviz/MapGraphDisplay.cpp

@@ -104,9 +104,10 @@ void MapGraphDisplay::processMessage( const rtabmap_ros::MapData::ConstPtr& msg
 	// Get links
 	std::map<int, rtabmap::Transform> poses;
 	std::map<int, int> mapIds;
+	std::map<int, std::string> labels;
 	std::multimap<int, rtabmap::Link> links;
 	rtabmap::Transform mapToOdom;
-	rtabmap_ros::mapGraphFromROS(msg->graph, poses, mapIds, links, mapToOdom);
+	rtabmap_ros::mapGraphFromROS(msg->graph, poses, mapIds, labels, links, mapToOdom);
 
 	destroyObjects();
 

srv/ListLabels.srv

@@ -0,0 +1,4 @@
+#request
+---
+#response 
+string[] labels