updated find-object with markers demo and wifi demo to correctly handle the case where only last signature data is published

src/CoreWrapper.cpp

@@ -2068,12 +2068,15 @@ void CoreWrapper::userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr & da
 	if(!paused_)
 	{
 		UScopeMutex lock(userDataMutex_);
-		if(!userData_.empty())
+		static bool warningShow = false;
+		if(!userData_.empty() && !warningShow)
 		{
-			ROS_WARN("Overwriting previous user data set. Asynchronous user "
+			ROS_WARN("Overwriting previous user data set. When asynchronous user "
-					"data input topic should be used with user data published at "
+					"data input topic rate is higher than "
-					"lower rate than map update rate (current %s=%f).",
+					"map update rate (current %s=%f), only latest data is saved "
+					"in the next node created. This message will is shown only once.",
 					Parameters::kRtabmapDetectionRate().c_str(), rate_);
+			warningShow = true;
 		}
 		userData_ = rtabmap_ros::userDataFromROS(*dataMsg);
 	}

src/SaveObjectsExample.cpp

@@ -99,16 +99,20 @@ public:
     // from rtabmap to rviz visualization
     void mapDataCallback(const rtabmap_ros::MapDataConstPtr & msg)
     {
-        std::map<double, int> nodeStamps; // <stamp, id>
         std::map<int, rtabmap::Signature> signatures;
         std::map<int, rtabmap::Transform> poses;
         std::multimap<int, rtabmap::Link> links;
         rtabmap::Transform mapToOdom;
         rtabmap_ros::mapDataFromROS(*msg, poses, links, signatures, mapToOdom);
+
+        // handle the case where we can receive only latest data, or if all data are published
         for(std::map<int, rtabmap::Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
         {
         	int id = iter->first;
         	rtabmap::Signature & node = iter->second;
+
+			nodeStamps_.insert(std::make_pair(node.getStamp(), node.id()));
+
 			if(!node.sensorData().userDataCompressed().empty() && nodeToObjects_.find(id)==nodeToObjects_.end())
 			{
 				cv::Mat data = rtabmap::uncompressData(node.sensorData().userDataCompressed());
@@ -116,8 +120,18 @@ public:
 				ROS_INFO("Node %d has %d object(s)", id, data.rows);
 				nodeToObjects_.insert(std::make_pair(id, data));
 			}
-            // Sort stamps by stamps->id
+        }
-            nodeStamps.insert(std::make_pair(iter->second.getStamp(), iter->first));
+
+		// for the logic below, we should keep only stamps for
+		// nodes still in the graph (in case nodes are ignored when not moving)
+		std::map<double, int> nodeStamps;
+		for(std::map<double, int>::iterator iter=nodeStamps_.begin(); iter!=nodeStamps_.end(); ++iter)
+		{
+			std::map<int, rtabmap::Transform>::const_iterator jter = poses.find(iter->second);
+			if(jter != poses.end())
+			{
+				nodeStamps.insert(*iter);
+			}
 		}
 
         // Publish markers accordingly to current optimized graph
@@ -244,6 +258,7 @@ private:
     ros::Publisher pub_;
     ros::Publisher pubMarkers_;
     std::map<int, cv::Mat> nodeToObjects_;
+    std::map<double, int> nodeStamps_; // <stamp, id>
     std::string frameId_;
 };
 

src/WifiSignalSubNode.cpp

@@ -62,6 +62,7 @@ inline int dBm2Quality(int dBm)
 
 ros::Publisher wifiSignalCloudPub;
 std::map<double, int> wifiLevels;
+std::map<double, int> nodeStamps_;
 
 void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg)
 {
@@ -73,11 +74,18 @@ void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg)
 	std::map<int, rtabmap::Signature> signatures;
 	rtabmap_ros::mapDataFromROS(*mapDataMsg, poses, links, signatures, mapToOdom);
 
-	std::map<double, int> nodeStamps; // <stamp, id>
+	// handle the case where we can receive only latest data, or if all data are published
 	for(std::map<int, rtabmap::Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
+	{
+		int id = iter->first;
+		rtabmap::Signature & node = iter->second;
+
+		nodeStamps_.insert(std::make_pair(node.getStamp(), node.id()));
+
+		if(!node.sensorData().userDataCompressed().empty())
 		{
 			cv::Mat data;
-		iter->second.sensorData().uncompressDataConst(0 ,0, 0, &data);
+			node.sensorData().uncompressDataConst(0 ,0, 0, &data);
 
 			if(data.type() == CV_64FC1 && data.rows == 1 && data.cols == 2)
 			{
@@ -90,9 +98,19 @@ void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg)
 			{
 				ROS_ERROR("Wrong user data format for wifi signal.");
 			}
+		}
+	}
 
-		// Sort stamps by stamps->id
+	// for the logic below, we should keep only stamps for
-		nodeStamps.insert(std::make_pair(iter->second.getStamp(), iter->first));
+	// nodes still in the graph (in case nodes are ignored when not moving)
+	std::map<double, int> nodeStamps;
+	for(std::map<double, int>::iterator iter=nodeStamps_.begin(); iter!=nodeStamps_.end(); ++iter)
+	{
+		std::map<int, rtabmap::Transform>::const_iterator jter = poses.find(iter->second);
+		if(jter != poses.end())
+		{
+			nodeStamps.insert(*iter);
+		}
 	}
 
 	if(wifiLevels.size() == 0)