diff --git a/CMakeLists.txt b/CMakeLists.txt index 2a4afcf7..5a35b57e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,7 +32,7 @@ find_package(fiducial_msgs) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) -find_package(RTABMap 0.20.20 REQUIRED) +find_package(RTABMap 0.20.22 REQUIRED) find_package(OpenCV REQUIRED QUIET COMPONENTS core calib3d imgproc highgui stitching photo video OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d) diff --git a/include/rtabmap_ros/CoreWrapper.h b/include/rtabmap_ros/CoreWrapper.h index 9e6c13cc..5fbe3b71 100644 --- a/include/rtabmap_ros/CoreWrapper.h +++ b/include/rtabmap_ros/CoreWrapper.h @@ -393,8 +393,6 @@ private: bool alreadyRectifiedImages_; bool twoDMapping_; ros::Time previousStamp_; - std::set nodesToRepublish_; - int maxNodesRepublished_; }; } diff --git a/include/rtabmap_ros/GuiWrapper.h b/include/rtabmap_ros/GuiWrapper.h index 59c50ad1..d458cfb3 100644 --- a/include/rtabmap_ros/GuiWrapper.h +++ b/include/rtabmap_ros/GuiWrapper.h @@ -129,6 +129,8 @@ private: double maxOdomUpdateRate_; tf::TransformListener tfListener_; + ros::Publisher republishNodeDataPub_; + message_filters::Subscriber infoTopic_; message_filters::Subscriber mapDataTopic_; diff --git a/package.xml b/package.xml index 3c1af103..acbfa9e7 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap_ros - 0.20.20 + 0.20.22 RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 2595a2ef..e02e4a0c 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -125,8 +125,7 @@ CoreWrapper::CoreWrapper() : alreadyRectifiedImages_(Parameters::defaultRtabmapImagesAlreadyRectified()), twoDMapping_(Parameters::defaultRegForce3DoF()), previousStamp_(0), - mbClient_(0), - maxNodesRepublished_(2) + mbClient_(0) { char * rosHomePath = getenv("ROS_HOME"); std::string workingDir = rosHomePath?rosHomePath:UDirectory::homeDir()+"/.ros"; @@ -189,7 +188,6 @@ 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_); @@ -2391,22 +2389,7 @@ 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; - } - } + rtabmap_.addNodesToRepublish(msg->data); } void CoreWrapper::interOdomCallback(const nav_msgs::OdometryConstPtr & msg) @@ -2720,7 +2703,6 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt mapToOdomMutex_.lock(); mapToOdom_.setIdentity(); mapToOdomMutex_.unlock(); - nodesToRepublish_.clear(); return true; } @@ -2811,7 +2793,6 @@ bool CoreWrapper::loadDatabaseCallback(rtabmap_ros::LoadDatabase::Request& req, mapToOdomMutex_.lock(); mapToOdom_.setIdentity(); mapToOdomMutex_.unlock(); - nodesToRepublish_.clear(); // Open new database databasePath_ = newDatabasePath; @@ -2928,7 +2909,6 @@ 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"); @@ -3982,57 +3962,6 @@ 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, true, false, true); - std::multimap 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::multimap::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/GuiWrapper.cpp b/src/GuiWrapper.cpp index 632e6c15..48ed744d 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include @@ -154,6 +155,8 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) : UEventsManager::addHandler(this); UEventsManager::addHandler(mainWindow_); + republishNodeDataPub_ = nh.advertise("republish_node_data", 1); + infoTopic_.subscribe(nh, "info", 1); mapDataTopic_.subscribe(nh, "mapData", 1); infoMapSync_ = new message_filters::Synchronizer( @@ -204,10 +207,7 @@ void GuiWrapper::infoMapCallback( stat.setMapCorrection(mapToOdom); stat.setPoses(poses); - if(signatures.size()) - { - stat.setLastSignatureData(signatures.rbegin()->second); - } + stat.setSignaturesData(signatures); stat.setConstraints(links); this->post(new RtabmapEvent(stat)); @@ -417,6 +417,13 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) ROS_ERROR("Can't call \"remove_label\" service"); } } + else if(cmd == rtabmap::RtabmapEventCmd::kCmdRepublishData) + { + UASSERT(cmdEvent->value1().isIntArray()); + std_msgs::Int32MultiArray msg; + msg.data = cmdEvent->value1().toIntArray(); + republishNodeDataPub_.publish(msg); + } else { ROS_WARN("Not handled command (%d)...", cmd);