merged master->ros2

.github/workflows/ros1.yml

@@ -68,5 +68,5 @@ jobs:
         source /opt/ros/${{ matrix.ros_distro }}/setup.bash
         source ${{github.workspace}}/catkin_ws/devel/setup.bash
         cd ${{github.workspace}}/catkin_ws
-        catkin_make
+        catkin_make -DSETUPTOOLS_DEB_LAYOUT=OFF
         catkin_make install     

CMakeLists.txt

@@ -56,7 +56,7 @@ find_package(octomap_msgs)
 #find_package(fiducial_msgs)
 
 ## System dependencies are found with CMake's conventions
-find_package(RTABMap 0.20.16 REQUIRED)
+find_package(RTABMap 0.20.17 REQUIRED)
 find_package(Boost REQUIRED COMPONENTS system) # dependencies from PCL
 find_package(PCL 1.7 REQUIRED COMPONENTS kdtree) #This crashes idl generation if all components are found?! see https://github.com/ros2/rosidl/issues/402#issuecomment-565586908
 
@@ -149,6 +149,7 @@ ENDIF(RTABMAP_GUI OR rviz_default_plugins_FOUND)
    "srv/ResetPose.srv"
    "srv/SetGoal.srv"
    "srv/SetLabel.srv"
+   "srv/RemoveLabel.srv"
    "srv/GetPlan.srv"
    "srv/AddLink.srv"
    "srv/GetNodeData.srv"

include/rtabmap_ros/CoreWrapper.h

@@ -60,6 +60,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_ros/srv/publish_map.hpp"
 #include "rtabmap_ros/srv/set_goal.hpp"
 #include "rtabmap_ros/srv/set_label.hpp"
+#include "rtabmap_ros/srv/remove_label.hpp"
 #include "rtabmap_ros/msg/goal.hpp"
 #include "rtabmap_ros/srv/get_plan.hpp"
 #include "rtabmap_ros/CommonDataSubscriber.h"
@@ -231,6 +232,7 @@ private:
 	void cancelGoalCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
 	void setLabelCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_ros::srv::SetLabel::Request>, std::shared_ptr<rtabmap_ros::srv::SetLabel::Response>);
 	void listLabelsCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_ros::srv::ListLabels::Request>, std::shared_ptr<rtabmap_ros::srv::ListLabels::Response> res);
+	void removeLabelCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_ros::srv::RemoveLabel::Request>, std::shared_ptr<rtabmap_ros::srv::RemoveLabel::Response> res);
 	void addLinkCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_ros::srv::AddLink::Request>, std::shared_ptr<rtabmap_ros::srv::AddLink::Response> res);
 	void getNodesInRadiusCallback(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_ros::srv::GetNodesInRadius::Request>, std::shared_ptr<rtabmap_ros::srv::GetNodesInRadius::Response> res);
 #ifdef WITH_OCTOMAP_MSGS
@@ -356,6 +358,7 @@ private:
 	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr cancelGoalSrv_;
 	rclcpp::Service<rtabmap_ros::srv::SetLabel>::SharedPtr setLabelSrv_;
 	rclcpp::Service<rtabmap_ros::srv::ListLabels>::SharedPtr listLabelsSrv_;
+	rclcpp::Service<rtabmap_ros::srv::RemoveLabel>::SharedPtr removeLabelSrv_;
 	rclcpp::Service<rtabmap_ros::srv::AddLink>::SharedPtr addLinkSrv_;
 	rclcpp::Service<rtabmap_ros::srv::GetNodesInRadius>::SharedPtr getNodesInRadiusSrv_;
 #ifdef WITH_OCTOMAP_MSGS

package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <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

@@ -530,8 +530,10 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 					RCLCPP_INFO(this->get_logger(), "Subscribe to inter odom + info messages");
 					interOdomSync_ = new message_filters::Synchronizer<MyExactInterOdomSyncPolicy>(MyExactInterOdomSyncPolicy(100), interOdomSyncSub_, interOdomInfoSyncSub_);
 					interOdomSync_->registerCallback(std::bind(&CoreWrapper::interOdomInfoCallback, this, std::placeholders::_1, std::placeholders::_2));
-					interOdomSyncSub_.subscribe(this, "inter_odom");
+					rmw_qos_profile_t qos = rmw_qos_profile_default;
-					interOdomInfoSyncSub_.subscribe(this, "inter_odom_info");
+					qos.depth = 100;
+					interOdomSyncSub_.subscribe(this, "inter_odom", qos);
+					interOdomInfoSyncSub_.subscribe(this, "inter_odom_info", qos);
 				}
 				else
 				{
@@ -648,6 +650,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	cancelGoalSrv_ = this->create_service<std_srvs::srv::Empty>("cancel_goal", std::bind(&CoreWrapper::cancelGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 	setLabelSrv_ = this->create_service<rtabmap_ros::srv::SetLabel>("set_label", std::bind(&CoreWrapper::setLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 	listLabelsSrv_ = this->create_service<rtabmap_ros::srv::ListLabels>("list_labels", std::bind(&CoreWrapper::listLabelsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+	removeLabelSrv_ = this->create_service<rtabmap_ros::srv::RemoveLabel>("remove_label", std::bind(&CoreWrapper::removeLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 	addLinkSrv_ = this->create_service<rtabmap_ros::srv::AddLink>("add_link", std::bind(&CoreWrapper::addLinkCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 	getNodesInRadiusSrv_ = this->create_service<rtabmap_ros::srv::GetNodesInRadius>("get_nodes_in_radius", std::bind(&CoreWrapper::getNodesInRadiusCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
@@ -2322,7 +2325,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 ||
@@ -4006,6 +4009,29 @@ void CoreWrapper::listLabelsCallback(
 	}
 }
 
+void CoreWrapper::removeLabelCallback(
+		const std::shared_ptr<rmw_request_id_t>,
+		const std::shared_ptr<rtabmap_ros::srv::RemoveLabel::Request> req,
+		std::shared_ptr<rtabmap_ros::srv::RemoveLabel::Response>)
+{
+	if(rtabmap_.getMemory())
+	{
+		int id = rtabmap_.getMemory()->getSignatureIdByLabel(req->label, true);
+		if(id == 0)
+		{
+			RCLCPP_WARN(this->get_logger(), "Label \"%s\" not found in the map, cannot remove it!", req->label.c_str());
+		}
+		else if(!rtabmap_.labelLocation(id, ""))
+		{
+			RCLCPP_ERROR(this->get_logger(), "Failed removing label \"%s\".", req->label.c_str());
+		}
+		else
+		{
+			RCLCPP_INFO(this->get_logger(), "Removed label \"%s\".", req->label.c_str());
+		}
+	}
+}
+
 void CoreWrapper::addLinkCallback(const std::shared_ptr<rmw_request_id_t>,
 		const std::shared_ptr<rtabmap_ros::srv::AddLink::Request> req,
 		std::shared_ptr<rtabmap_ros::srv::AddLink::Response>)
@@ -4024,18 +4050,20 @@ void CoreWrapper::getNodesInRadiusCallback(
 {
 	RCLCPP_INFO(get_logger(), "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->dists_sqr.resize(poses.size());
 	int index = 0;
 	for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin();
 		iter != poses.end();
@@ -4043,6 +4071,8 @@ void CoreWrapper::getNodesInRadiusCallback(
 	{
 		res->ids[index] = iter->first;
 		transformToPoseMsg(iter->second, res->poses[index]);
+		UASSERT(dists.find(iter->first) != dists.end());
+		res->dists_sqr[index] = dists.at(iter->first);
 		++index;
 	}
 }

src/GuiWrapper.cpp

@@ -51,6 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_ros/MsgConversion.h"
 #include "rtabmap_ros/srv/set_goal.hpp"
 #include "rtabmap_ros/srv/set_label.hpp"
+#include "rtabmap_ros/srv/remove_label.hpp"
 #include "rtabmap_ros/PreferencesDialogROS.h"
 
 float max3( const float& a, const float& b, const float& c)
@@ -442,6 +443,22 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
 				RCLCPP_WARN(this->get_logger(), "Service \"set_label\" not available.");
 			}
 		}
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdRemoveLabel)
+		{
+			UASSERT(cmdEvent->value1().isStr());
+			auto client = this->create_client<rtabmap_ros::srv::RemoveLabel>("remove_label");
+			if(client->wait_for_service(std::chrono::seconds(1)))
+			{
+				auto request = std::make_shared<rtabmap_ros::srv::RemoveLabel::Request>();
+				request->label = cmdEvent->value1().toStr();
+				auto result_future = client->async_send_request(request);
+				result_future.wait();
+			}
+			else
+			{
+				RCLCPP_WARN(this->get_logger(), "Service \"remove_label\" not available.");
+			}
+		}
 		else
 		{
 			RCLCPP_WARN(this->get_logger(), "Not handled command (%d)...", cmd);

src/nodelets/icp_odometry.cpp

@@ -159,8 +159,8 @@ void ICPOdometry::updateParameters(ParametersMap & parameters)
 	iter = parameters.find(Parameters::kIcpRangeMin());
 	if(iter != parameters.end())
 	{
-		int value = uStr2Int(iter->second);
+		float value = uStr2Float(iter->second);
-		if(value > 1)
+		if(value != 0.0f)
 		{
 			if(!this->has_parameter("scan_range_min"))
 			{
@@ -177,8 +177,8 @@ void ICPOdometry::updateParameters(ParametersMap & parameters)
 	iter = parameters.find(Parameters::kIcpRangeMax());
 	if(iter != parameters.end())
 	{
-		int value = uStr2Int(iter->second);
+		float value = uStr2Float(iter->second);
-		if(value > 1)
+		if(value != 0.0f)
 		{
 			if(!this->has_parameter("scan_range_max"))
 			{
@@ -210,6 +210,11 @@ void ICPOdometry::updateParameters(ParametersMap & parameters)
 			}
 		}
 	}
+	else if(this->has_parameter("scan_voxel_size"))
+	{
+		RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_voxel_size is set (%f), setting %s to 0", scanVoxelSize_, Parameters::kIcpVoxelSize().c_str());
+		parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), "0"));
+	}
 	iter = parameters.find(Parameters::kIcpPointToPlaneK());
 	if(iter != parameters.end())
 	{
@@ -221,8 +226,18 @@ void ICPOdometry::updateParameters(ParametersMap & parameters)
 				RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_k\" for convenience.", iter->second.c_str(), iter->first.c_str());
 				scanNormalK_ = value;
 			}
+			else
+			{
+				RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_k is set (%d), setting %s to same value.", scanNormalK_, Parameters::kIcpPointToPlaneK().c_str());
+				iter->second = uNumber2Str(scanNormalK_);
+			}
 		}
 	}
+	else if(this->has_parameter("scan_normal_k"))
+	{
+		RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_k is set (%d), setting %s to same value.", scanNormalK_, Parameters::kIcpPointToPlaneK().c_str());
+		parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneK(), uNumber2Str(scanNormalK_)));
+	}
 	iter = parameters.find(Parameters::kIcpPointToPlaneRadius());
 	if(iter != parameters.end())
 	{
@@ -234,21 +249,41 @@ void ICPOdometry::updateParameters(ParametersMap & parameters)
 				RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_radius\" for convenience.", iter->second.c_str(), iter->first.c_str());
 				scanNormalRadius_ = value;
 			}
-		}
+			else
-		iter = parameters.find(Parameters::kIcpPointToPlaneGroundNormalsUp());
-		if(iter != parameters.end())
-		{
-			float value = uStr2Float(iter->second);
-			if(value != 0.0f)
 			{
-				if(!this->has_parameter("scan_normal_ground_up"))
+				RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_radius is set (%f), setting %s to same value.", scanNormalRadius_, Parameters::kIcpPointToPlaneRadius().c_str());
-				{
+				iter->second = uNumber2Str(scanNormalK_);
-					RCLCPP_WARN(get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_ground_up\" for convenience.", iter->second.c_str(), iter->first.c_str());
-					scanNormalGroundUp_ = value;
-				}
 			}
 		}
 	}
+	else if(this->has_parameter("scan_normal_radius"))
+	{
+		RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_radius is set (%f), setting %s to same value.", scanNormalRadius_, Parameters::kIcpPointToPlaneRadius().c_str());
+		parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneRadius(), uNumber2Str(scanNormalRadius_)));
+	}
+	iter = parameters.find(Parameters::kIcpPointToPlaneGroundNormalsUp());
+	if(iter != parameters.end())
+	{
+		float value = uStr2Float(iter->second);
+		if(value != 0.0f)
+		{
+			if(!this->has_parameter("scan_normal_ground_up"))
+			{
+				RCLCPP_WARN(get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_ground_up\" for convenience.", iter->second.c_str(), iter->first.c_str());
+				scanNormalGroundUp_ = value;
+			}
+			else
+			{
+				RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_ground_up is set (%f), setting %s to same value.", scanNormalGroundUp_, Parameters::kIcpPointToPlaneGroundNormalsUp().c_str());
+				iter->second = uNumber2Str(scanNormalK_);
+			}
+		}
+	}
+	else if(this->has_parameter("scan_normal_ground_up"))
+	{
+		RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_ground_up is set (%f), setting %s to same value.", scanNormalGroundUp_, Parameters::kIcpPointToPlaneGroundNormalsUp().c_str());
+		parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(scanNormalGroundUp_)));
+	}
 }
 
 void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scanMsg)

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[] dists_sqr

srv/RemoveLabel.srv

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