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,27 +99,41 @@ 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;
+        	int id = iter->first;
-            rtabmap::Signature & node = iter->second;
+        	rtabmap::Signature & node = iter->second;
-            if(!node.sensorData().userDataCompressed().empty() && nodeToObjects_.find(id)==nodeToObjects_.end())
+
-            {
+			nodeStamps_.insert(std::make_pair(node.getStamp(), node.id()));
-                cv::Mat data = rtabmap::uncompressData(node.sensorData().userDataCompressed());
+
-                ROS_ASSERT(data.cols == 9 && data.type() == CV_64FC1);
+			if(!node.sensorData().userDataCompressed().empty() && nodeToObjects_.find(id)==nodeToObjects_.end())
-                ROS_INFO("Node %d has %d object(s)", id, data.rows);
+			{
-                nodeToObjects_.insert(std::make_pair(id, data));
+				cv::Mat data = rtabmap::uncompressData(node.sensorData().userDataCompressed());
-            }
+				ROS_ASSERT(data.cols == 9 && data.type() == CV_64FC1);
-            // Sort stamps by stamps->id
+				ROS_INFO("Node %d has %d object(s)", id, data.rows);
-            nodeStamps.insert(std::make_pair(iter->second.getStamp(), iter->first));
+				nodeToObjects_.insert(std::make_pair(id, data));
+			}
         }
 
+		// 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
         std::map<int, float> objectsAdded;
         visualization_msgs::MarkerArray markers;
@@ -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,26 +74,43 @@ 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)
 	{
-		cv::Mat data;
+		int id = iter->first;
-		iter->second.sensorData().uncompressDataConst(0 ,0, 0, &data);
+		rtabmap::Signature & node = iter->second;
 
-		if(data.type() == CV_64FC1 && data.rows == 1 && data.cols == 2)
+		nodeStamps_.insert(std::make_pair(node.getStamp(), node.id()));
-		{
-			// format [int level, double stamp], see wifi_signal_pub_node.cpp
-			int level = data.at<double>(0);
-			double stamp = data.at<double>(1);
-			wifiLevels.insert(std::make_pair(stamp, level));
-		}
-		else if(!data.empty())
-		{
-			ROS_ERROR("Wrong user data format for wifi signal.");
-		}
 
-		// Sort stamps by stamps->id
+		if(!node.sensorData().userDataCompressed().empty())
-		nodeStamps.insert(std::make_pair(iter->second.getStamp(), iter->first));
+		{
+			cv::Mat data;
+			node.sensorData().uncompressDataConst(0 ,0, 0, &data);
+
+			if(data.type() == CV_64FC1 && data.rows == 1 && data.cols == 2)
+			{
+				// format [int level, double stamp], see wifi_signal_pub_node.cpp
+				int level = data.at<double>(0);
+				double stamp = data.at<double>(1);
+				wifiLevels.insert(std::make_pair(stamp, level));
+			}
+			else if(!data.empty())
+			{
+				ROS_ERROR("Wrong user data format for wifi signal.");
+			}
+		}
+	}
+
+	// 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);
+		}
 	}
 
 	if(wifiLevels.size() == 0)