rtabmap: added republish_node_data input topic to request rtabmap to republish data of some nodes. max_nodes_republished parameter (default 2) limits the number of nodes republished at each update.

src/rviz/MapCloudDisplay.cpp

@@ -57,6 +57,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Graph.h>
 #include <rtabmap_ros/MsgConversion.h>
 #include <rtabmap_ros/GetMap.h>
+#include <std_msgs/Int32MultiArray.h>
 
 
 namespace rtabmap_ros
@@ -90,7 +91,8 @@ MapCloudDisplay::MapCloudDisplay()
     new_xyz_transformer_(false),
     new_color_transformer_(false),
     needs_retransform_(false),
-    transformer_class_loader_(NULL)
+    transformer_class_loader_(NULL),
+	current_map_updated_(false)
 {
 	//QIcon icon;
 	//this->setIcon(icon);
@@ -186,7 +188,7 @@ MapCloudDisplay::MapCloudDisplay()
 	node_filtering_angle_->setMin( 0.0f );
 	node_filtering_angle_->setMax( 359.0f );
 
-	download_namespace = new rviz::StringProperty("Download namespace", "rtabmap", "Namespace used to call Download services below", this);
+	download_namespace = new rviz::StringProperty("Download namespace", "rtabmap", "Namespace used to call Download services below", this, SLOT( downloadNamespaceChanged() ), this);
 
 	download_map_ = new rviz::BoolProperty( "Download map", false,
 										 "Download the optimized global map using rtabmap/GetMap service. This will force to re-create all clouds.",
@@ -196,6 +198,8 @@ MapCloudDisplay::MapCloudDisplay()
 											 "Download the optimized global graph (without cloud data) using rtabmap/GetMap service.",
 											 this, SLOT( downloadGraph() ), this );
 
+	downloadNamespaceChanged();
+
 	// PointCloudCommon sets up a callback queue with a thread for each
 	// instance.  Use that for processing incoming messages.
 	update_nh_.setCallbackQueue( &cbqueue_ );
@@ -387,6 +391,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 	{
 		boost::mutex::scoped_lock lock(current_map_mutex_);
 		current_map_ = poses;
+		current_map_updated_ = true;
 	}
 }
 
@@ -536,8 +541,7 @@ void MapCloudDisplay::downloadMap(bool graphOnly)
 	getMapSrv.request.optimized = true;
 	getMapSrv.request.graphOnly = graphOnly;
 	std::string rtabmapNs = download_namespace->getStdString();
-	ros::NodeHandle nh;
-	std::string srvName = nh.resolveName(uFormat("%s/get_map_data", rtabmapNs.c_str()));
+	std::string srvName = update_nh_.resolveName(uFormat("%s/get_map_data", rtabmapNs.c_str()));
 	QMessageBox * messageBox = new QMessageBox(
 			QMessageBox::NoIcon,
 			tr("Calling \"%1\" service...").arg(srvName.c_str()),
@@ -550,12 +554,12 @@ void MapCloudDisplay::downloadMap(bool graphOnly)
 	QApplication::processEvents();
 	if(!ros::service::call(srvName, getMapSrv))
 	{
-		ROS_ERROR("MapCloudDisplay: Can't call \"%s\" service. "
+		ROS_ERROR("MapCloudDisplay: Cannot call \"%s\" service. "
 				  "Tip: if rtabmap node is not in \"%s\" namespace, you can "
 				  "change the \"Download namespace\" option.",
 				  srvName.c_str(),
 				  rtabmapNs.c_str());
-		messageBox->setText(tr("MapCloudDisplay: Can't call \"%1\" service. "
+		messageBox->setText(tr("MapCloudDisplay: Cannot call \"%1\" service. "
 				  "Tip: if rtabmap node is not in \"%2\" namespace, you can "
 				  "change the \"Download namespace\" option.").
 				  arg(srvName.c_str()).arg(rtabmapNs.c_str()));
@@ -583,6 +587,13 @@ void MapCloudDisplay::downloadMap(bool graphOnly)
 	}
 }
 
+void MapCloudDisplay::downloadNamespaceChanged()
+{
+	std::string rtabmapNs = download_namespace->getStdString();
+	std::string topicName = update_nh_.resolveName(uFormat("%s/republish_node_data", rtabmapNs.c_str()));
+	republishNodeDataPub_ = update_nh_.advertise<std_msgs::Int32MultiArray>(topicName, 1);
+}
+
 void MapCloudDisplay::downloadMap()
 {
 	if(download_map_->getBool())
@@ -709,6 +720,7 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
 		boost::mutex::scoped_lock lock(current_map_mutex_);
 		if(!current_map_.empty())
 		{
+			std::vector<int> missingNodes;
 			for (std::map<int, rtabmap::Transform>::iterator it=current_map_.begin(); it != current_map_.end(); ++it)
 			{
 				std::map<int, CloudInfoPtr>::iterator cloudInfoIt = cloud_infos_.find(it->first);
@@ -745,7 +757,10 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
 								cloudInfoIt->second->message_->header.frame_id.c_str(),
 								cloudInfoIt->second->message_->header.frame_id.c_str());
 					}
-
+				}
+				else if(it->first>0 && current_map_updated_)
+				{
+					missingNodes.push_back(it->first);
 				}
 			}
 			//hide not used clouds
@@ -770,7 +785,15 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
 					++iter;
 				}
 			}
+
+			if(!missingNodes.empty())
+			{
+				std_msgs::Int32MultiArray msg;
+				msg.data = missingNodes;
+				republishNodeDataPub_.publish(msg);
+			}
 		}
+		current_map_updated_ = false;
 	}
 	if(lastCloudAdded>0)
 	{
@@ -792,6 +815,7 @@ void MapCloudDisplay::reset()
 	{
 		boost::mutex::scoped_lock lock(current_map_mutex_);
 		current_map_.clear();
+		current_map_updated_ = false;
 	}
 	MFDClass::reset();
 }

src/rviz/MapCloudDisplay.h

@@ -135,6 +135,7 @@ private Q_SLOTS:
 	void setXyzTransformerOptions( EnumProperty* prop );
 	void setColorTransformerOptions( EnumProperty* prop );
 	void updateCloudParameters();
+	void downloadNamespaceChanged();
 	void downloadMap();
 	void downloadGraph();
 
@@ -167,6 +168,7 @@ private:
 private:
 	ros::AsyncSpinner spinner_;
 	ros::CallbackQueue cbqueue_;
+	ros::Publisher republishNodeDataPub_;
 
 	std::map<int, CloudInfoPtr> cloud_infos_;
 
@@ -175,6 +177,7 @@ private:
 
 	std::map<int, rtabmap::Transform> current_map_;
 	boost::mutex current_map_mutex_;
+	bool current_map_updated_;
 
 	int lastCloudAdded_;