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.

include/rtabmap_ros/CoreWrapper.h

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <std_msgs/Empty.h>
 #include <std_msgs/Int32.h>
+#include "std_msgs/Int32MultiArray.h"
 #include <sensor_msgs/NavSatFix.h>
 #include <nav_msgs/GetMap.h>
 #include <nav_msgs/GetPlan.h>
@@ -165,6 +166,7 @@ private:
 	void tagDetectionsAsyncCallback(const apriltag_ros::AprilTagDetectionArray & tagDetections);
 #endif
 	void imuAsyncCallback(const sensor_msgs::ImuConstPtr & tagDetections);
+	void republishNodeDataCallback(const std_msgs::Int32MultiArray::ConstPtr& msg);
 	void interOdomCallback(const nav_msgs::OdometryConstPtr & msg);
 	void interOdomInfoCallback(const nav_msgs::OdometryConstPtr & msg1, const rtabmap_ros::OdomInfoConstPtr & msg2);
 
@@ -192,8 +194,6 @@ private:
 			const std::map<int, rtabmap::Transform> & nodes,
 			const rtabmap::Transform & currentPose);
 
-	void republishMaps();
-
 	bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -243,6 +243,7 @@ private:
 	void goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback);
 	void publishLocalPath(const ros::Time & stamp);
 	void publishGlobalPath(const ros::Time & stamp);
+	void republishMaps();
 
 private:
 	rtabmap::Rtabmap rtabmap_;
@@ -371,6 +372,7 @@ private:
 	ros::Subscriber imuSub_;
 	std::map<double, rtabmap::Transform> imus_;
 	std::string imuFrameId_;
+	ros::Subscriber republishNodeDataSub_;
 
 	ros::Subscriber interOdomSub_;
 	std::list<std::pair<nav_msgs::Odometry, rtabmap_ros::OdomInfo> > interOdoms_;
@@ -388,6 +390,8 @@ private:
 	bool alreadyRectifiedImages_;
 	bool twoDMapping_;
 	ros::Time previousStamp_;
+	std::set<int> nodesToRepublish_;
+	int maxNodesRepublished_;
 };
 
 }

src/CoreWrapper.cpp

@@ -125,7 +125,8 @@ CoreWrapper::CoreWrapper() :
 		alreadyRectifiedImages_(Parameters::defaultRtabmapImagesAlreadyRectified()),
 		twoDMapping_(Parameters::defaultRegForce3DoF()),
 		previousStamp_(0),
-		mbClient_(0)
+		mbClient_(0),
+		maxNodesRepublished_(2)
 {
 	char * rosHomePath = getenv("ROS_HOME");
 	std::string workingDir = rosHomePath?rosHomePath:UDirectory::homeDir()+"/.ros";
@@ -188,6 +189,7 @@ void CoreWrapper::onInit()
 	pnh.param("wait_for_transform_duration",  waitForTransformDuration_, waitForTransformDuration_);
 	pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
 	pnh.param("use_saved_map", useSavedMap_, useSavedMap_);
+	pnh.param("max_nodes_republished", maxNodesRepublished_, maxNodesRepublished_);
 	pnh.param("gen_scan",            genScan_, genScan_);
 	pnh.param("gen_scan_max_depth",  genScanMaxDepth_, genScanMaxDepth_);
 	pnh.param("gen_scan_min_depth",  genScanMinDepth_, genScanMinDepth_);
@@ -819,6 +821,7 @@ void CoreWrapper::onInit()
 	tagDetectionsSub_ = nh.subscribe("tag_detections", 1, &CoreWrapper::tagDetectionsAsyncCallback, this);
 #endif
 	imuSub_ = nh.subscribe("imu", 100, &CoreWrapper::imuAsyncCallback, this);
+	republishNodeDataSub_ = nh.subscribe("republish_node_data", 100, &CoreWrapper::republishNodeDataCallback, this);
 }
 
 CoreWrapper::~CoreWrapper()
@@ -2495,6 +2498,26 @@ void CoreWrapper::imuAsyncCallback(const sensor_msgs::ImuConstPtr & msg)
 	}
 }
 
+void CoreWrapper::republishNodeDataCallback(const std_msgs::Int32MultiArray::ConstPtr& msg)
+{
+	if(maxNodesRepublished_>0)
+	{
+		nodesToRepublish_.insert(msg->data.begin(), msg->data.end());
+	}
+	else
+	{
+		static bool warned = false;
+		if(!warned)
+		{
+			NODELET_WARN("A node is requesting some node data "
+					"to be republished after the next update, "
+					"but parameter \"max_nodes_republished\" is not over 0, "
+					"ignoring the call. This warning is only printed once.");
+			warned = true;
+		}
+	}
+}
+
 void CoreWrapper::interOdomCallback(const nav_msgs::OdometryConstPtr & msg)
 {
 	if(!paused_)
@@ -2805,6 +2828,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
 	mapToOdomMutex_.lock();
 	mapToOdom_.setIdentity();
 	mapToOdomMutex_.unlock();
+	nodesToRepublish_.clear();
 
 	return true;
 }
@@ -2894,6 +2918,7 @@ bool CoreWrapper::loadDatabaseCallback(rtabmap_ros::LoadDatabase::Request& req,
 	mapToOdomMutex_.lock();
 	mapToOdom_.setIdentity();
 	mapToOdomMutex_.unlock();
+	nodesToRepublish_.clear();
 
 	// Open new database
 	databasePath_ = newDatabasePath;
@@ -3009,6 +3034,7 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em
 	globalPose_.header.stamp = ros::Time(0);
 	gps_ = rtabmap::GPS();
 	tags_.clear();
+	nodesToRepublish_.clear();
 
 	NODELET_INFO("Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str());
 	UFile::copy(databasePath_, databasePath_+".back");
@@ -4036,6 +4062,58 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
 		{
 			signatures.insert(std::make_pair(stats.getLastSignatureData().id(), stats.getLastSignatureData()));
 		}
+
+		if(nodesToRepublish_.size() && !rtabmap_.getLastLocalizationPose().isNull())
+		{
+			// Republish data from closest nodes of the current localization
+			std::map<int, Transform> nodesOnly(rtabmap_.getLocalOptimizedPoses().lower_bound(1), rtabmap_.getLocalOptimizedPoses().end());
+			int id = rtabmap::graph::findNearestNode(nodesOnly, rtabmap_.getLastLocalizationPose());
+			if(id>0)
+			{
+				std::map<int, int> ids = rtabmap_.getMemory()->getNeighborsId(id, 0, 0, false, false, true);
+				std::map<int, int> missingIds;
+				for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
+				{
+					if(nodesToRepublish_.find(iter->first) != nodesToRepublish_.end())
+					{
+						missingIds.insert(std::make_pair(iter->second, iter->first));
+					}
+				}
+
+				if(nodesToRepublish_.size() != missingIds.size())
+				{
+					// remove requested nodes not anymore in the graph
+					for(std::set<int>::iterator iter=nodesToRepublish_.begin(); iter!=nodesToRepublish_.end();)
+					{
+						if(ids.find(*iter) == ids.end())
+						{
+							iter = nodesToRepublish_.erase(iter);
+						}
+						else
+						{
+							++iter;
+						}
+					}
+				}
+
+				int loaded = 0;
+				std::stringstream stream;
+				for(std::map<int, int>::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<maxNodesRepublished_; ++iter)
+				{
+					signatures.insert(std::make_pair(iter->second, rtabmap_.getMemory()->getNodeData(iter->second, true, true, true, true)));
+					nodesToRepublish_.erase(iter->second);
+					++loaded;
+					stream << iter->second << " ";
+				}
+				if(loaded)
+				{
+					NODELET_WARN("Republishing data of requested node(s) %sfrom \"%s\" input topic (max_nodes_republished=%d)",
+							stream.str().c_str(),
+							republishNodeDataSub_.getTopic().c_str(),
+							maxNodesRepublished_);
+				}
+			}
+		}
 		rtabmap_ros::mapDataToROS(
 			stats.poses(),
 			stats.constraints(),

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_;