updated ROS messages with stamps/user_data

CMakeLists.txt

@@ -45,6 +45,7 @@ add_message_files(
    NodeData.msg
    Link.msg
    OdomInfo.msg
+   UserData.msg
 )
 
 ## Generate services in the 'srv' folder

include/rtabmap_ros/MsgConversion.h

@@ -81,13 +81,17 @@ void mapGraphFromROS(
 		const rtabmap_ros::Graph & msg,
 		std::map<int, rtabmap::Transform> & poses,
 		std::map<int, int> & mapIds,
+		std::map<int, double> & stamps,
 		std::map<int, std::string> & labels,
+		std::map<int, std::vector<unsigned char> > & userDatas,
 		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, double> & stamps,
 		const std::map<int, std::string> & labels,
+		const std::map<int, std::vector<unsigned char> > & userDatas,
 		const std::multimap<int, rtabmap::Link> & links,
 		const rtabmap::Transform & mapToOdom,
 		rtabmap_ros::Graph & msg);

msg/Graph.msg

@@ -13,6 +13,8 @@ geometry_msgs/Transform mapToOdom
 int32[] nodeIds
 int32[] mapIds
 string[] labels
+float64[] stamps
+UserData[] userDatas
 
 # std::map<nodeId, Pose>
 geometry_msgs/Pose[] poses

msg/NodeData.msg

@@ -4,6 +4,7 @@ int32 mapId
 int32 weight
 float64 stamp
 string label
+UserData userData
 
 # Pose from odometry not corrected
 geometry_msgs/Pose pose

msg/UserData.msg

@@ -0,0 +1,2 @@
+
+uint8[] data

src/CoreWrapper.cpp

@@ -1379,7 +1379,9 @@ 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, double> stamps;
 	std::map<int, std::string> labels;
+	std::map<int, std::vector<unsigned char> > userDatas;
 
 	if(req.graphOnly)
 	{
@@ -1387,7 +1389,9 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros:
 				poses,
 				constraints,
 				mapIds,
+				stamps,
 				labels,
+				userDatas,
 				req.optimized,
 				req.global);
 	}
@@ -1398,7 +1402,9 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros:
 				poses,
 				constraints,
 				mapIds,
+				stamps,
 				labels,
+				userDatas,
 				req.optimized,
 				req.global);
 	}
@@ -1412,7 +1418,9 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros:
 	//RGB-D SLAM data
 	rtabmap_ros::mapGraphToROS(poses,
 		mapIds,
+		stamps,
 		labels,
+		userDatas,
 		constraints,
 		Transform::getIdentity(),
 		res.data.graph);
@@ -1543,7 +1551,9 @@ 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, double> stamps;
 		std::map<int, std::string> labels;
+		std::map<int, std::vector<unsigned char> > userDatas;
 
 		if(req.graphOnly)
 		{
@@ -1551,7 +1561,9 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 					poses,
 					constraints,
 					mapIds,
+					stamps,
 					labels,
+					userDatas,
 					req.optimized,
 					req.global);
 		}
@@ -1562,7 +1574,9 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 					poses,
 					constraints,
 					mapIds,
+					stamps,
 					labels,
+					userDatas,
 					req.optimized,
 					req.global);
 		}
@@ -1581,7 +1595,9 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 
 			rtabmap_ros::mapGraphToROS(poses,
 				mapIds,
+				stamps,
 				labels,
+				userDatas,
 				constraints,
 				Transform::getIdentity(),
 				*graphMsg);
@@ -1810,6 +1826,7 @@ bool CoreWrapper::listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtab
 
 void CoreWrapper::publishStats(const ros::Time & stamp)
 {
+	UDEBUG("Publishing stats...");
 	const rtabmap::Statistics & stats = rtabmap_.getStatistics();
 
 	if(infoPub_.getNumSubscribers())
@@ -1826,7 +1843,9 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
 	if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers())
 	{
 		if(stats.poses().size() == stats.getMapIds().size() &&
-		   stats.poses().size() == stats.getLabels().size())
+		   stats.poses().size() == stats.getStamps().size() &&
+		   stats.poses().size() == stats.getLabels().size() &&
+		   stats.poses().size() == stats.getUserDatas().size())
 		{
 			rtabmap_ros::GraphPtr graphMsg(new rtabmap_ros::Graph);
 			graphMsg->header.stamp = stamp;
@@ -1835,7 +1854,9 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
 			rtabmap_ros::mapGraphToROS(
 				stats.poses(),
 				stats.getMapIds(),
+				stats.getStamps(),
 				stats.getLabels(),
+				stats.getUserDatas(),
 				stats.constraints(),
 				stats.mapCorrection(),
 				*graphMsg);
@@ -1951,6 +1972,8 @@ std::map<int, rtabmap::Transform> CoreWrapper::updateMapCaches(
 		bool updateGrid,
 		const std::map<int, Signature> & signatures)
 {
+	UDEBUG("Updating map caches...");
+
 	if(!rtabmap_.getMemory() && signatures.size() == 0)
 	{
 		ROS_FATAL("Memory not initialized!?");
@@ -2216,6 +2239,8 @@ void CoreWrapper::publishMaps(
 		const std::map<int, rtabmap::Transform> & poses,
 		const ros::Time & stamp)
 {
+	UDEBUG("Publishing maps...");
+
 	// publish maps
 	if(cloudMapPub_.getNumSubscribers())
 	{

src/GuiWrapper.cpp

@@ -185,14 +185,19 @@ void GuiWrapper::infoMapCallback(
 	rtabmap::Transform mapToOdom;
 	std::map<int, rtabmap::Transform> poses;
 	std::map<int, int> mapIds;
+	std::map<int, double> stamps;
 	std::map<int, std::string> labels;
+	std::map<int, std::vector<unsigned char> > userDatas;
 	std::multimap<int, Link> links;
 
-	rtabmap_ros::mapGraphFromROS(mapMsg->graph, poses, mapIds, labels, links, mapToOdom);
+	rtabmap_ros::mapGraphFromROS(mapMsg->graph, poses, mapIds, stamps, labels, userDatas, links, mapToOdom);
 
 	stat.setMapCorrection(mapToOdom);
 	stat.setPoses(poses);
 	stat.setMapIds(mapIds);
+	stat.setStamps(stamps);
+	stat.setLabels(labels);
+	stat.setUserDatas(userDatas);
 	stat.setConstraints(links);
 
 	//data
@@ -214,7 +219,9 @@ 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, double> stamps;
 	std::map<int, std::string> labels;
+	std::map<int, std::vector<unsigned char> > userDatas;
 	Transform mapToOdom;
 
 	if(map.graph.nodeIds.size() != map.graph.mapIds.size())
@@ -231,7 +238,7 @@ void GuiWrapper::processRequestedMap(const rtabmap_ros::MapData & map)
 		return;
 	}
 
-	rtabmap_ros::mapGraphFromROS(map.graph, poses, mapIds, labels, constraints, mapToOdom);
+	rtabmap_ros::mapGraphFromROS(map.graph, poses, mapIds, stamps, labels, userDatas, constraints, mapToOdom);
 
 	//data
 	for(unsigned int i=0; i<map.nodes.size(); ++i)
@@ -243,7 +250,9 @@ void GuiWrapper::processRequestedMap(const rtabmap_ros::MapData & map)
 				poses,
 				constraints,
 				mapIds,
-				labels);
+				stamps,
+				labels,
+				userDatas);
 	QMetaObject::invokeMethod(mainWindow_, "processRtabmapEvent3DMap", Q_ARG(rtabmap::RtabmapEvent3DMap, e));
 }
 

src/MapOptimizerNode.cpp

@@ -115,7 +115,9 @@ 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.stamps.size());
 		UASSERT(msg->graph.nodeIds.size() == msg->graph.labels.size());
+		UASSERT(msg->graph.nodeIds.size() == msg->graph.userDatas.size());
 
 		bool dataChanged = false;
 
@@ -147,7 +149,9 @@ public:
 
 		std::map<int, Transform> newPoses;
 		std::map<int, int> newMapIds;
+		std::map<int, double> newStamps;
 		std::map<int, std::string> newLabels;
+		std::map<int, std::vector<unsigned char> > newUserDatas;
 		// add new odometry poses
 		for(unsigned int i=0; i<msg->nodes.size(); ++i)
 		{
@@ -155,7 +159,9 @@ 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));
+			newStamps.insert(std::make_pair(id, msg->nodes[i].stamp));
 			newLabels.insert(std::make_pair(id, msg->nodes[i].label));
+			newUserDatas.insert(std::make_pair(id, msg->nodes[i].userData.data));
 
 			std::pair<std::map<int, Transform>::iterator, bool> p = cachedPoses_.insert(std::make_pair(id, pose));
 			if(!p.second && pose != cachedPoses_.at(id))
@@ -165,7 +171,9 @@ public:
 			else if(p.second)
 			{
 				cachedMapIds_.insert(std::make_pair(id, msg->nodes[i].mapId));
+				cachedStamps_.insert(std::make_pair(id, msg->nodes[i].stamp));
 				cachedLabels_.insert(std::make_pair(id, msg->nodes[i].label));
+				cachedUserDatas_.insert(std::make_pair(id, msg->nodes[i].userData.data));
 			}
 		}
 
@@ -174,20 +182,26 @@ public:
 			ROS_WARN("Graph data has changed! Reset cache...");
 			cachedPoses_ = newPoses;
 			cachedMapIds_ = newMapIds;
+			cachedStamps_ = newStamps;
 			cachedLabels_ = newLabels;
+			cachedUserDatas_ = newUserDatas;
 			cachedConstraints_ = newConstraints;
 		}
 
 		//match poses in the graph
 		std::map<int, Transform> poses;
 		std::map<int, int> mapIds;
+		std::map<int, double> stamps;
 		std::map<int, std::string> labels;
+		std::map<int, std::vector<unsigned char> > userDatas;
 		std::multimap<int, Link> constraints;
 		if(globalOptimization_)
 		{
 			poses = cachedPoses_;
 			mapIds = cachedMapIds_;
+			stamps = cachedStamps_;
 			labels = cachedLabels_;
+			userDatas = cachedUserDatas_;
 			constraints = cachedConstraints_;
 		}
 		else
@@ -200,7 +214,9 @@ public:
 				{
 					poses.insert(*iter);
 					mapIds.insert(*cachedMapIds_.find(iter->first));
+					stamps.insert(*cachedStamps_.find(iter->first));
 					labels.insert(*cachedLabels_.find(iter->first));
+					userDatas.insert(*cachedUserDatas_.find(iter->first));
 				}
 				else
 				{
@@ -246,12 +262,16 @@ public:
 					  (int)poses.size(), (int)constraints.size());
 				mapIds.clear();
 				labels.clear();
+				stamps.clear();
+				userDatas.clear();
 			}
 
 			UASSERT(optimizedPoses.size() == mapIds.size());
 			UASSERT(optimizedPoses.size() == labels.size());
+			UASSERT(optimizedPoses.size() == stamps.size());
+			UASSERT(optimizedPoses.size() == userDatas.size());
 			rtabmap_ros::MapData outputMsg;
-			rtabmap_ros::mapGraphToROS(optimizedPoses, mapIds, labels, std::multimap<int, rtabmap::Link>(), mapCorrection, outputMsg.graph);
+			rtabmap_ros::mapGraphToROS(optimizedPoses, mapIds, stamps, labels, userDatas, std::multimap<int, rtabmap::Link>(), mapCorrection, outputMsg.graph);
 			outputMsg.graph.links = msg->graph.links;
 			outputMsg.header = msg->header;
 			outputMsg.nodes = msg->nodes;
@@ -278,7 +298,9 @@ private:
 
 	std::map<int, Transform> cachedPoses_;
 	std::map<int, int> cachedMapIds_;
+	std::map<int, double> cachedStamps_;
 	std::map<int, std::string> cachedLabels_;
+	std::map<int, std::vector<unsigned char> > cachedUserDatas_;
 	std::multimap<int, Link> cachedConstraints_;
 
 	tf::TransformBroadcaster tfBroadcaster_;

src/MsgConversion.cpp

@@ -252,7 +252,9 @@ void mapGraphFromROS(
 		const rtabmap_ros::Graph & msg,
 		std::map<int, rtabmap::Transform> & poses,
 		std::map<int, int> & mapIds,
+		std::map<int, double> & stamps,
 		std::map<int, std::string> & labels,
+		std::map<int, std::vector<unsigned char> > & userDatas,
 		std::multimap<int, rtabmap::Link> & links,
 		rtabmap::Transform & mapToOdom)
 {
@@ -260,13 +262,17 @@ void mapGraphFromROS(
 
 	UASSERT(msg.nodeIds.size() == msg.mapIds.size());
 	UASSERT(msg.nodeIds.size() == msg.poses.size());
+	UASSERT(msg.nodeIds.size() == msg.stamps.size());
 	UASSERT(msg.nodeIds.size() == msg.labels.size());
+	UASSERT(msg.nodeIds.size() == msg.userDatas.size());
 
 	for(unsigned int i=0; i<msg.nodeIds.size(); ++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]));
+		stamps.insert(std::make_pair(msg.nodeIds[i], msg.stamps[i]));
 		labels.insert(std::make_pair(msg.nodeIds[i], msg.labels[i]));
+		userDatas.insert(std::make_pair(msg.nodeIds[i], msg.userDatas[i].data));
 	}
 
 	for(unsigned int i=0; i<msg.links.size(); ++i)
@@ -278,32 +284,46 @@ void mapGraphFromROS(
 void mapGraphToROS(
 		const std::map<int, rtabmap::Transform> & poses,
 		const std::map<int, int> & mapIds,
+		const std::map<int, double> & stamps,
 		const std::map<int, std::string> & labels,
+		const std::map<int, std::vector<unsigned char> > & userDatas,
 		const std::multimap<int, rtabmap::Link> & links,
 		const rtabmap::Transform & mapToOdom,
 		rtabmap_ros::Graph & msg)
 {
-	UASSERT(poses.size() == 0 || (poses.size() == mapIds.size() && poses.size() == labels.size()));
+	UASSERT(poses.size() == 0 ||
+			(poses.size() == mapIds.size() &&
+			 poses.size() == labels.size() &&
+			 poses.size() == stamps.size() &&
+			 poses.size() == userDatas.size()));
 
 	transformToGeometryMsg(mapToOdom, msg.mapToOdom);
 
 	msg.nodeIds.resize(poses.size());
 	msg.poses.resize(poses.size());
 	msg.mapIds.resize(poses.size());
+	msg.stamps.resize(poses.size());
 	msg.labels.resize(poses.size());
+	msg.userDatas.resize(poses.size());
 	int index = 0;
 	std::map<int, rtabmap::Transform>::const_iterator iterPoses = poses.begin();
 	std::map<int, int>::const_iterator iterMapIds = mapIds.begin();
+	std::map<int, double>::const_iterator iterStamps = stamps.begin();
 	std::map<int, std::string>::const_iterator iterLabels = labels.begin();
+	std::map<int, std::vector<unsigned char> >::const_iterator iterUserDatas = userDatas.begin();
 	while(iterPoses != poses.end())
 	{
 		msg.nodeIds[index] = iterPoses->first;
 		msg.mapIds[index] = iterMapIds->second;
+		msg.stamps[index] = iterStamps->second;
 		msg.labels[index] = iterLabels->second;
+		msg.userDatas[index].data = iterUserDatas->second;
 		transformToPoseMsg(iterPoses->second, msg.poses[index]);
 		++iterPoses;
 		++iterMapIds;
+		++iterStamps;
 		++iterLabels;
+		++iterUserDatas;
 		++index;
 	}
 
@@ -351,7 +371,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 			words,
 			words3D,
 			transformFromPoseMsg(msg.pose),
-			std::vector<unsigned char>(),
+			msg.userData.data,
 			compressedMatFromBytes(msg.laserScan),
 			compressedMatFromBytes(msg.image),
 			compressedMatFromBytes(msg.depth),
@@ -369,6 +389,7 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 	msg.weight = signature.getWeight();
 	msg.stamp = signature.getStamp();
 	msg.label = signature.getLabel();
+	msg.userData.data = signature.getUserData();
 	transformToPoseMsg(signature.getPose(), msg.pose);
 	compressedMatToBytes(signature.getImageCompressed(), msg.image);
 	compressedMatToBytes(signature.getDepthCompressed(), msg.depth);

src/rviz/MapGraphDisplay.cpp

@@ -104,10 +104,12 @@ 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,  double> stamps;
 	std::map<int, std::string> labels;
+	std::map<int, std::vector<unsigned char> > userDatas;
 	std::multimap<int, rtabmap::Link> links;
 	rtabmap::Transform mapToOdom;
-	rtabmap_ros::mapGraphFromROS(msg->graph, poses, mapIds, labels, links, mapToOdom);
+	rtabmap_ros::mapGraphFromROS(msg->graph, poses, mapIds, stamps, labels, userDatas, links, mapToOdom);
 
 	destroyObjects();