Fixed labels not correctly published

include/rtabmap_ros/CoreWrapper.h

@@ -198,7 +198,6 @@ private:
 	bool latestNodeWasReached_;
 	rtabmap::ParametersMap parameters_;
 	std::map<std::string, float> rtabmapROSStats_;
-	std::map<int, std::string> labels_;
 
 	std::string frameId_;
 	std::string odomFrameId_;

msg/Info.msg

@@ -30,6 +30,10 @@ float32[] rawLikelihoodValues
 int32[] weightsKeys
 int32[] weightsValues
 
+# std::map<int, std::string> labels;
+int32[] labelsKeys
+string[] labelsValues
+
 # std::map<std::string, float> stats
 string[] statsKeys
 float32[] statsValues

src/CoreWrapper.cpp

@@ -2337,7 +2337,6 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
 	lastPublishedMetricGoal_.setNull();
 	latestNodeWasReached_ = false;
 	mapsManager_.clear();
-	labels_.clear();
 	previousStamp_ = ros::Time(0);
 	globalPose_.header.stamp = ros::Time(0);
 	gps_ = rtabmap::GPS();
@@ -3001,12 +3000,6 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
 	{
 		if(stats.poses().size())
 		{
-			// update labels buffer
-			if(stats.getLastSignatureData().id() > 0 && !stats.getLastSignatureData().getLabel().empty())
-			{
-				labels_.insert(std::make_pair(stats.getLastSignatureData().id(), stats.getLastSignatureData().getLabel()));
-			}
-
 			visualization_msgs::MarkerArray markers;
 			nav_msgs::Path path;
 			if(pubPath)
@@ -3018,11 +3011,11 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
 				poseIter!=stats.poses().end();
 				++poseIter)
 			{
-				if(pubLabels)
+				if(pubLabels && rtabmap_.getMemory())
 				{
 					// Add labels
-					std::map<int, std::string>::iterator lter = labels_.find(poseIter->first);
-					if(lter != labels_.end() && !lter->second.empty())
+					std::map<int, std::string>::const_iterator lter = rtabmap_.getMemory()->getAllLabels().find(poseIter->first);
+					if(lter != rtabmap_.getMemory()->getAllLabels().end() && !lter->second.empty())
 					{
 						visualization_msgs::Marker marker;
 						marker.header.frame_id = mapFrameId_;

src/MsgConversion.cpp

@@ -394,18 +394,21 @@ void infoFromROS(const rtabmap_ros::Info & info, rtabmap::Statistics & stat)
 		mapIntFloat.insert(std::pair<int, float>(info.posteriorKeys.at(i), info.posteriorValues.at(i)));
 	}
 	stat.setPosterior(mapIntFloat);
+
 	mapIntFloat.clear();
 	for(unsigned int i=0; i<info.likelihoodKeys.size() && i<info.likelihoodValues.size(); ++i)
 	{
 		mapIntFloat.insert(std::pair<int, float>(info.likelihoodKeys.at(i), info.likelihoodValues.at(i)));
 	}
 	stat.setLikelihood(mapIntFloat);
+
 	mapIntFloat.clear();
 	for(unsigned int i=0; i<info.rawLikelihoodKeys.size() && i<info.rawLikelihoodValues.size(); ++i)
 	{
 		mapIntFloat.insert(std::pair<int, float>(info.rawLikelihoodKeys.at(i), info.rawLikelihoodValues.at(i)));
 	}
 	stat.setRawLikelihood(mapIntFloat);
+
 	std::map<int, int> mapIntInt;
 	for(unsigned int i=0; i<info.weightsKeys.size() && i<info.weightsValues.size(); ++i)
 	{
@@ -413,6 +416,13 @@ void infoFromROS(const rtabmap_ros::Info & info, rtabmap::Statistics & stat)
 	}
 	stat.setWeights(mapIntInt);
 
+	std::map<int, std::string> mapIntStr;
+	for(unsigned int i=0; i<info.labelsKeys.size() && i<info.labelsValues.size(); ++i)
+	{
+		mapIntStr.insert(std::pair<int, std::string>(info.labelsKeys.at(i), info.labelsValues.at(i)));
+	}
+	stat.setLabels(mapIntStr);
+
 	stat.setLocalPath(info.localPath);
 	stat.setCurrentGoalId(info.currentGoalId);
 
@@ -443,6 +453,8 @@ void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::Info & info)
 		info.rawLikelihoodValues = uValues(stats.rawLikelihood());
 		info.weightsKeys = uKeys(stats.weights());
 		info.weightsValues = uValues(stats.weights());
+		info.labelsKeys = uKeys(stats.labels());
+		info.labelsValues = uValues(stats.labels());
 		info.localPath = stats.localPath();
 		info.currentGoalId = stats.currentGoalId();