Added remove_label service. Updated get_nodes_in_radius service (added parameter k, return also distances of nodes found)

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.16 REQUIRED)
+find_package(RTABMap 0.20.17 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 
@@ -134,6 +134,7 @@ add_message_files(
    ResetPose.srv
    SetGoal.srv
    SetLabel.srv
+   RemoveLabel.srv
    GetPlan.srv
    AddLink.srv
    GetNodeData.srv

include/rtabmap_ros/CoreWrapper.h

@@ -57,6 +57,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_ros/PublishMap.h"
 #include "rtabmap_ros/SetGoal.h"
 #include "rtabmap_ros/SetLabel.h"
+#include "rtabmap_ros/RemoveLabel.h"
 #include "rtabmap_ros/Goal.h"
 #include "rtabmap_ros/GetPlan.h"
 #include "rtabmap_ros/CommonDataSubscriber.h"
@@ -232,6 +233,7 @@ private:
 	bool cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
 	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 removeLabelCallback(rtabmap_ros::RemoveLabel::Request& req, rtabmap_ros::RemoveLabel::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
@@ -353,6 +355,7 @@ private:
 	ros::ServiceServer cancelGoalSrv_;
 	ros::ServiceServer setLabelSrv_;
 	ros::ServiceServer listLabelsSrv_;
+	ros::ServiceServer removeLabelSrv_;
 	ros::ServiceServer addLinkSrv_;
 	ros::ServiceServer getNodesInRadiusSrv_;
 #ifdef WITH_OCTOMAP_MSGS

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>rtabmap_ros</name>
-  <version>0.20.16</version>
+  <version>0.20.17</version>
   <description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

src/CoreWrapper.cpp

@@ -709,6 +709,7 @@ void CoreWrapper::onInit()
 	cancelGoalSrv_ = nh.advertiseService("cancel_goal", &CoreWrapper::cancelGoalCallback, this);
 	setLabelSrv_ = nh.advertiseService("set_label", &CoreWrapper::setLabelCallback, this);
 	listLabelsSrv_ = nh.advertiseService("list_labels", &CoreWrapper::listLabelsCallback, this);
+	removeLabelSrv_ = nh.advertiseService("remove_label", &CoreWrapper::removeLabelCallback, this);
 	addLinkSrv_ = nh.advertiseService("add_link", &CoreWrapper::addLinkCallback, this);
 	getNodesInRadiusSrv_ = nh.advertiseService("get_nodes_in_radius", &CoreWrapper::getNodesInRadiusCallback, this);
 #ifdef WITH_OCTOMAP_MSGS
@@ -2352,7 +2353,7 @@ std::map<int, Transform> CoreWrapper::filterNodesToAssemble(
 	std::map<int, Transform> output;
 	if(mappingMaxNodes_ > 0)
 	{
-		std::map<int, float> nodesDist = graph::findNearestNodes(nodes, currentPose, mappingMaxNodes_);
+		std::map<int, float> nodesDist = graph::findNearestNodes(currentPose, nodes, 0, 0, mappingMaxNodes_);
 		for(std::map<int, float>::iterator iter=nodesDist.begin(); iter!=nodesDist.end(); ++iter)
 		{
 			if(mappingAltitudeDelta_<=0.0 ||
@@ -4028,6 +4029,27 @@ bool CoreWrapper::listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtab
 	return true;
 }
 
+bool CoreWrapper::removeLabelCallback(rtabmap_ros::RemoveLabel::Request& req, rtabmap_ros::RemoveLabel::Response& res)
+{
+	if(rtabmap_.getMemory())
+	{
+		int id = rtabmap_.getMemory()->getSignatureIdByLabel(req.label, true);
+		if(id == 0)
+		{
+			NODELET_WARN("Label \"%s\" not found in the map, cannot remove it!", req.label.c_str());
+		}
+		else if(!rtabmap_.labelLocation(id, ""))
+		{
+			NODELET_ERROR("Failed removing label \"%s\".", req.label.c_str());
+		}
+		else
+		{
+			NODELET_INFO("Removed label \"%s\".", req.label.c_str());
+		}
+	}
+	return true;
+}
+
 bool CoreWrapper::addLinkCallback(rtabmap_ros::AddLink::Request& req, rtabmap_ros::AddLink::Response&)
 {
 	if(rtabmap_.getMemory())
@@ -4043,25 +4065,30 @@ bool CoreWrapper::getNodesInRadiusCallback(rtabmap_ros::GetNodesInRadius::Reques
 {
 	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;
+	std::map<int, float> dists;
 	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);
+		poses = rtabmap_.getNodesInRadius(req.node_id, req.radius, req.k, &dists);
 	}
 	else
 	{
-		poses = rtabmap_.getNodesInRadius(Transform(req.x, req.y, req.z, 0,0,0), req.radius);
+		poses = rtabmap_.getNodesInRadius(Transform(req.x, req.y, req.z, 0,0,0), req.radius, req.k, &dists);
 	}
 
 	//Optimized graph
 	res.ids.resize(poses.size());
 	res.poses.resize(poses.size());
+	res.distsSqr.resize(poses.size());
 	int index = 0;
 	for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin();
 		iter != poses.end();
 		++iter)
 	{
+		UERROR("add %d", iter->first);
 		res.ids[index] = iter->first;
 		transformToPoseMsg(iter->second, res.poses[index]);
+		UASSERT(dists.find(iter->first) != dists.end());
+		res.distsSqr[index] = dists.at(iter->first);
 		++index;
 	}
 

src/GuiWrapper.cpp

@@ -52,6 +52,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_ros/GetMap.h"
 #include "rtabmap_ros/SetGoal.h"
 #include "rtabmap_ros/SetLabel.h"
+#include "rtabmap_ros/RemoveLabel.h"
 #include "rtabmap_ros/PreferencesDialogROS.h"
 
 float max3( const float& a, const float& b, const float& c)
@@ -400,6 +401,16 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
 				ROS_ERROR("Can't call \"set_label\" service");
 			}
 		}
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdRemoveLabel)
+		{
+			UASSERT(cmdEvent->value1().isStr());
+			rtabmap_ros::RemoveLabel removeLabelSrv;
+			removeLabelSrv.request.label = cmdEvent->value1().toStr();
+			if(!ros::service::call("remove_label", removeLabelSrv))
+			{
+				ROS_ERROR("Can't call \"remove_label\" service");
+			}
+		}
 		else
 		{
 			ROS_WARN("Not handled command (%d)...", cmd);

srv/GetNodesInRadius.srv

@@ -1,7 +1,11 @@
 #request
 
-# If target pose and node_id are all zeros, poses 
+# In mapping mode (Mem/IncrementalMemory=true), if target pose 
-# around the latest node in the graph are returned.
+#   and node_id are all zeros, poses around the latest node 
+#   in the graph are returned.
+# In localization mode (Mem/IncrementalMemory=false), if target pose 
+#   and node_id are all zeros, poses around the latest localization 
+#   pose are returned.
 # If node_id is not zero, target pose is ignored.
 
 # Node id
@@ -12,10 +16,16 @@ float32 x
 float32 y
 float32 z
 
-# Radius, <=0 means that RGBD/LocalRadius will be used
+# Radius, <=0 means that RGBD/LocalRadius will be used 
+# if k is also 0. If k>0 and a radius of 0 means all nearest 
+# poses up to k.
 float32 radius
 
+# Maximum number of nearest poses
+int32 k
+
 ---
 #response
 int32[] ids
-geometry_msgs/Pose[] poses
+geometry_msgs/Pose[] poses
+float32[] distsSqr

srv/RemoveLabel.srv

@@ -0,0 +1,4 @@
+#request
+string label
+---
+#response