diff --git a/CMakeLists.txt b/CMakeLists.txt index 2add7fbd..e01d52c7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,7 +31,7 @@ find_package(find_object_2d) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) -find_package(RTABMap 0.20.2 REQUIRED) +find_package(RTABMap 0.20.4 REQUIRED) find_package(OpenCV REQUIRED) @@ -122,6 +122,7 @@ add_message_files( GetPlan.srv AddLink.srv GetNodeData.srv + GetNodesInRadius.srv ) ## Generate added messages and services with any dependencies listed here diff --git a/include/rtabmap_ros/CoreWrapper.h b/include/rtabmap_ros/CoreWrapper.h index 5bfa0ed0..17bd8ba5 100644 --- a/include/rtabmap_ros/CoreWrapper.h +++ b/include/rtabmap_ros/CoreWrapper.h @@ -61,6 +61,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap_ros/CommonDataSubscriber.h" #include "rtabmap_ros/OdomInfo.h" #include "rtabmap_ros/AddLink.h" +#include "rtabmap_ros/GetNodesInRadius.h" #include "MapsManager.h" @@ -210,6 +211,7 @@ private: bool setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res); bool listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtabmap_ros::ListLabels::Response& res); bool addLinkCallback(rtabmap_ros::AddLink::Request&, rtabmap_ros::AddLink::Response&); + bool getNodesInRadiusCallback(rtabmap_ros::GetNodesInRadius::Request&, rtabmap_ros::GetNodesInRadius::Response&); #ifdef WITH_OCTOMAP_MSGS bool octomapBinaryCallback(octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res); bool octomapFullCallback(octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res); @@ -319,6 +321,7 @@ private: ros::ServiceServer setLabelSrv_; ros::ServiceServer listLabelsSrv_; ros::ServiceServer addLinkSrv_; + ros::ServiceServer getNodesInRadiusSrv_; #ifdef WITH_OCTOMAP_MSGS ros::ServiceServer octomapBinarySrv_; ros::ServiceServer octomapFullSrv_; diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 81d64e39..41b2bc9d 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -617,6 +617,7 @@ void CoreWrapper::onInit() setLabelSrv_ = nh.advertiseService("set_label", &CoreWrapper::setLabelCallback, this); listLabelsSrv_ = nh.advertiseService("list_labels", &CoreWrapper::listLabelsCallback, this); addLinkSrv_ = nh.advertiseService("add_link", &CoreWrapper::addLinkCallback, this); + getNodesInRadiusSrv_ = nh.advertiseService("get_nodes_in_radius", &CoreWrapper::getNodesInRadiusCallback, this); #ifdef WITH_OCTOMAP_MSGS #ifdef RTABMAP_OCTOMAP octomapBinarySrv_ = nh.advertiseService("octomap_binary", &CoreWrapper::octomapBinaryCallback, this); @@ -1992,10 +1993,10 @@ void CoreWrapper::process( if(maxMappingNodes_ > 0 && filteredPoses.size()>1) { std::map nearestPoses; - std::vector nodes = graph::findNearestNodes(filteredPoses, mapToOdom_*odom, maxMappingNodes_); - for(std::vector::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter) + std::map nodes = graph::findNearestNodes(filteredPoses, mapToOdom_*odom, maxMappingNodes_); + for(std::map::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter) { - std::map::iterator pter = filteredPoses.find(*iter); + std::map::iterator pter = filteredPoses.find(iter->first); if(pter != filteredPoses.end()) { nearestPoses.insert(*pter); @@ -3020,10 +3021,10 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab if(maxMappingNodes_ > 0 && filteredPoses.size()>1) { std::map nearestPoses; - std::vector nodes = graph::findNearestNodes(filteredPoses, filteredPoses.rbegin()->second, maxMappingNodes_); - for(std::vector::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter) + std::map nodes = graph::findNearestNodes(filteredPoses, filteredPoses.rbegin()->second, maxMappingNodes_); + for(std::map::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter) { - std::map::iterator pter = filteredPoses.find(*iter); + std::map::iterator pter = filteredPoses.find(iter->first); if(pter != filteredPoses.end()) { nearestPoses.insert(*pter); @@ -3420,6 +3421,35 @@ bool CoreWrapper::addLinkCallback(rtabmap_ros::AddLink::Request& req, rtabmap_ro return false; } +bool CoreWrapper::getNodesInRadiusCallback(rtabmap_ros::GetNodesInRadius::Request& req, rtabmap_ros::GetNodesInRadius::Response& res) +{ + ROS_INFO("Get nodes in radius (%f): node_id=%d pose=(%f,%f,%f)", req.radius, req.node_id, req.x, req.y, req.z); + std::map poses; + if(req.node_id != 0 || (req.x == 0.0f && req.y == 0.0f && req.z == 0.0f)) + { + poses = rtabmap_.getNodesInRadius(req.node_id, req.radius); + } + else + { + poses = rtabmap_.getNodesInRadius(Transform(req.x, req.y, req.z, 0,0,0), req.radius); + } + + //Optimized graph + res.ids.resize(poses.size()); + res.poses.resize(poses.size()); + int index = 0; + for(std::map::const_iterator iter = poses.begin(); + iter != poses.end(); + ++iter) + { + res.ids[index] = iter->first; + transformToPoseMsg(iter->second, res.poses[index]); + ++index; + } + + return true; +} + void CoreWrapper::publishStats(const ros::Time & stamp) { UDEBUG("Publishing stats..."); @@ -3868,10 +3898,10 @@ bool CoreWrapper::octomapBinaryCallback( if(maxMappingNodes_ > 0 && poses.size()>1) { std::map nearestPoses; - std::vector nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxMappingNodes_); - for(std::vector::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter) + std::map nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxMappingNodes_); + for(std::map::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter) { - std::map::iterator pter = poses.find(*iter); + std::map::iterator pter = poses.find(iter->first); if(pter != poses.end()) { nearestPoses.insert(*pter); @@ -3899,10 +3929,10 @@ bool CoreWrapper::octomapFullCallback( if(maxMappingNodes_ > 0 && poses.size()>1) { std::map nearestPoses; - std::vector nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxMappingNodes_); - for(std::vector::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter) + std::map nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxMappingNodes_); + for(std::map::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter) { - std::map::iterator pter = poses.find(*iter); + std::map::iterator pter = poses.find(iter->first); if(pter != poses.end()) { nearestPoses.insert(*pter); diff --git a/srv/GetNodesInRadius.srv b/srv/GetNodesInRadius.srv new file mode 100644 index 00000000..48b02c4f --- /dev/null +++ b/srv/GetNodesInRadius.srv @@ -0,0 +1,21 @@ +#request + +# If target pose and node_id are all zeros, poses +# around the latest node in the graph are returned. +# If node_id is not zero, target pose is ignored. + +# Node id +int32 node_id + +# Target pose: +float32 x +float32 y +float32 z + +# Radius, <=0 means that RGBD/LocalRadius will be used +float32 radius + +--- +#response +int32[] ids +geometry_msgs/Pose[] poses \ No newline at end of file