Added get_nodes_in_radius service (https://github.com/introlab/rtabmap/issues/595)

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

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

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<int, Transform> nearestPoses;
-					std::vector<int> nodes = graph::findNearestNodes(filteredPoses, mapToOdom_*odom, maxMappingNodes_);
+					std::map<int, float> nodes = graph::findNearestNodes(filteredPoses, mapToOdom_*odom, maxMappingNodes_);
-					for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
+					for(std::map<int, float>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
 					{
-						std::map<int, Transform>::iterator pter = filteredPoses.find(*iter);
+						std::map<int, Transform>::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<int, Transform> nearestPoses;
-					std::vector<int> nodes = graph::findNearestNodes(filteredPoses, filteredPoses.rbegin()->second, maxMappingNodes_);
+					std::map<int, float> nodes = graph::findNearestNodes(filteredPoses, filteredPoses.rbegin()->second, maxMappingNodes_);
-					for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
+					for(std::map<int, float>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
 					{
-						std::map<int, Transform>::iterator pter = filteredPoses.find(*iter);
+						std::map<int, Transform>::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<int, Transform> 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<int, rtabmap::Transform>::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<int, Transform> nearestPoses;
-		std::vector<int> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxMappingNodes_);
+		std::map<int, float> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxMappingNodes_);
-		for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
+		for(std::map<int, float>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
 		{
-			std::map<int, Transform>::iterator pter = poses.find(*iter);
+			std::map<int, Transform>::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<int, Transform> nearestPoses;
-		std::vector<int> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxMappingNodes_);
+		std::map<int, float> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxMappingNodes_);
-		for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
+		for(std::map<int, float>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
 		{
-			std::map<int, Transform>::iterator pter = poses.find(*iter);
+			std::map<int, Transform>::iterator pter = poses.find(iter->first);
 			if(pter != poses.end())
 			{
 				nearestPoses.insert(*pter);

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