From d21842408d09955ab7953d820c7650818ce9e650 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 13 Sep 2021 18:22:56 -0400 Subject: [PATCH] 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 | 8 +++- src/CoreWrapper.cpp | 80 ++++++++++++++++++++++++++++++- src/rviz/MapCloudDisplay.cpp | 38 ++++++++++++--- src/rviz/MapCloudDisplay.h | 3 ++ 4 files changed, 119 insertions(+), 10 deletions(-) diff --git a/include/rtabmap_ros/CoreWrapper.h b/include/rtabmap_ros/CoreWrapper.h index 4140e8a3..13bd0e75 100644 --- a/include/rtabmap_ros/CoreWrapper.h +++ b/include/rtabmap_ros/CoreWrapper.h @@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include "std_msgs/Int32MultiArray.h" #include #include #include @@ -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 & 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 imus_; std::string imuFrameId_; + ros::Subscriber republishNodeDataSub_; ros::Subscriber interOdomSub_; std::list > interOdoms_; @@ -388,6 +390,8 @@ private: bool alreadyRectifiedImages_; bool twoDMapping_; ros::Time previousStamp_; + std::set nodesToRepublish_; + int maxNodesRepublished_; }; } diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index fc01be8d..f7de221e 100644 --- a/src/CoreWrapper.cpp +++ b/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 nodesOnly(rtabmap_.getLocalOptimizedPoses().lower_bound(1), rtabmap_.getLocalOptimizedPoses().end()); + int id = rtabmap::graph::findNearestNode(nodesOnly, rtabmap_.getLastLocalizationPose()); + if(id>0) + { + std::map ids = rtabmap_.getMemory()->getNeighborsId(id, 0, 0, false, false, true); + std::map missingIds; + for(std::map::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::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::iterator iter=missingIds.begin(); iter!=missingIds.end() && loadedsecond, 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(), diff --git a/src/rviz/MapCloudDisplay.cpp b/src/rviz/MapCloudDisplay.cpp index 05bb4323..22e299a4 100644 --- a/src/rviz/MapCloudDisplay.cpp +++ b/src/rviz/MapCloudDisplay.cpp @@ -57,6 +57,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include 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(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 missingNodes; for (std::map::iterator it=current_map_.begin(); it != current_map_.end(); ++it) { std::map::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(); } diff --git a/src/rviz/MapCloudDisplay.h b/src/rviz/MapCloudDisplay.h index 8c44533c..2ecc0e07 100644 --- a/src/rviz/MapCloudDisplay.h +++ b/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 cloud_infos_; @@ -175,6 +177,7 @@ private: std::map current_map_; boost::mutex current_map_mutex_; + bool current_map_updated_; int lastCloudAdded_;