Merge master->ros2 (fixed #674)

CMakeLists.txt

@@ -55,7 +55,7 @@ find_package(octomap_msgs)
 find_package(move_base_msgs)
 
 ## System dependencies are found with CMake's conventions
-find_package(RTABMap 0.20.14 REQUIRED)
+find_package(RTABMap 0.20.15 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
 

launch/tests/test_velodyne.launch

@@ -1,4 +1,4 @@
-
+<!-- -->
 <launch>
   
     <!-- 
@@ -18,15 +18,25 @@
     <arg name="scan_topic"    default="/velodyne_points"/>
     <arg name="use_sim_time"  default="false"/>
     <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
+
+    <arg name="frame_id"        default="velodyne"/>
+    <arg name="queue_size"      default="10"/> <!-- Set to 100 for kitti dataset to make sure all scans are processed -->
+    <arg name="queue_size_odom" default="1"/>  <!-- Set to 100 for kitti dataset to make sure all scans are processed -->
+    <arg name="loop_ratio"      default="0.2"/>
+
+    <arg name="resolution"         default="0.1"/> <!-- set 0.1-0.3 for indoor, set 0.3-0.5 for outdoor (0.4 for kitti) -->
+    <arg name="iterations"         default="10"/>
     
-    <arg name="frame_id" default="velodyne"/>
+    <!-- Grid parameters -->
-    <arg name="queue_size" default="1"/> <!-- Set to 100 for kitti dataset to make sure all scans are processed -->
+    <arg name="ground_is_obstacle" default="true"/>
-    <arg name="loop_ratio" default="0.4"/> <!-- Set to 0.2 for kitti -->
+    <arg name="grid_max_range"     default="20"/>
     
-    <arg name="resolution" default="0.1"/> <!-- set 0.1-0.3 for indoor, set 0.3-0.5 for outdoor (0.4 for kitti) -->
+    <!-- For F2M Odometry -->
-    <arg name="iterations" default="10"/>
+    <arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car, kitti) -->
-    <arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (car, kitti) -->
+    <arg name="local_map_size"    default="15000"/>
+    <arg name="key_frame_thr"     default="0.8"/>
     
+    <!-- For FLOAM Odometry -->
     <arg name="floam"    default="false"/> <!-- RTAB-Map should be built with FLOAM http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261p8563.html -->
     <arg name="floam_sensor" default="0"/> <!-- 0=16 rings (VLP16), 1=32 rings, 2=64 rings (kitti dataset) -->
     
@@ -48,7 +58,7 @@
         <remap from="scan_cloud" to="$(arg scan_topic)"/>
         <param name="frame_id"        type="string" value="$(arg frame_id)"/>  
         <param name="odom_frame_id"   type="string" value="odom"/>
-        <param name="queue_size"      type="int"    value="$(arg queue_size)"/>
+        <param name="queue_size"      type="int"    value="$(arg queue_size_odom)"/>
         <param name="wait_for_transform_duration" type="double" value="0.2"/>
         <param     if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
         <param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
@@ -75,13 +85,15 @@
         <param if="$(arg ground_normals_up)" name="Icp/PointToPlaneGroundNormalsUp"  type="string" value="0.8"/>
 
         <!-- Odom parameters -->       
-        <param name="Odom/ScanKeyFrameThr"       type="string" value="0.9"/>
+        <param name="Odom/ScanKeyFrameThr"       type="string" value="$(arg key_frame_thr)"/>
         <param     if="$(arg floam)" name="Odom/Strategy" type="string" value="11"/>
         <param unless="$(arg floam)" name="Odom/Strategy" type="string" value="0"/>
         <param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg resolution)"/>
-        <param name="OdomF2M/ScanMaxSize"        type="string" value="15000"/>
+        <param name="OdomF2M/ScanMaxSize"        type="string" value="$(arg local_map_size)"/>
         <param name="OdomLOAM/Sensor"            type="string" value="$(arg floam_sensor)"/> 
         <param name="OdomLOAM/Resolution"        type="string" value="$(arg resolution)"/>
+        <param     if="$(arg scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
+        <param unless="$(arg scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
       </node>
 
       <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">	  
@@ -109,9 +121,9 @@
         
         <param name="Reg/Strategy"                   type="string" value="1"/> 
         <param name="Grid/CellSize"                  type="string" value="$(arg resolution)"/>
-        <param name="Grid/RangeMax"                  type="string" value="20"/>
+        <param name="Grid/RangeMax"                  type="string" value="$(arg grid_max_range)"/>
         <param name="Grid/ClusterRadius"             type="string" value="1"/>
-        <param name="Grid/GroundIsObstacle"          type="string" value="true"/>
+        <param name="Grid/GroundIsObstacle"          type="string" value="$(arg ground_is_obstacle)"/>
         <param name="Optimizer/GravitySigma"         type="string" value="0.3"/>
 
         <!-- ICP parameters -->

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.14</version>
+  <version>0.20.15</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

@@ -377,7 +377,12 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 		}
 		if(!paramValue.empty())
 		{
-			if(iter->second.first)
+			if(!iter->second.second.empty() && parameters_.find(iter->second.second)!=parameters_.end())
+			{
+				RCLCPP_WARN(this->get_logger(), "Rtabmap: Parameter name changed: \"%s\" -> \"%s\". The new parameter is already used with value \"%s\", ignoring the old one with value \"%s\".",
+						iter->first.c_str(), iter->second.second.c_str(), parameters_.find(iter->second.second)->second.c_str(), paramValue.c_str());
+			}
+			else if(iter->second.first)
 			{
 				// can be migrated
 				uInsert(parameters_, ParametersPair(iter->second.second, paramValue));
@@ -403,26 +408,26 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	// Backward compatibility (MapsManager)
 	mapsManager_.backwardCompatibilityParameters(*this, parameters_);
 
-	bool gridFromDepth = Parameters::defaultGridFromDepth();
+	int gridSensor = Parameters::defaultGridSensor();
-	if((this->isSubscribedToScan2d() || this->isSubscribedToScan3d() || genScan_) && parameters_.find(Parameters::kGridFromDepth()) == parameters_.end())
+	if((this->isSubscribedToScan2d() || this->isSubscribedToScan3d() || genScan_) && parameters_.find(Parameters::kGridSensor()) == parameters_.end())
 	{
-		RCLCPP_WARN(this->get_logger(), "Setting \"%s\" parameter to false (default true) as \"subscribe_scan\" or \"subscribe_scan_cloud\" or \"gen_scan\" is "
+		RCLCPP_WARN(this->get_logger(), "Setting \"%s\" parameter to 0 (default 1) as \"subscribe_scan\" or \"subscribe_scan_cloud\" or \"gen_scan\" is "
 				"true. The occupancy grid map will be constructed from "
 				"laser scans. To get occupancy grid map from cloud projection, set \"%s\" "
 				"to true. To suppress this warning, "
-				"add <param name=\"%s\" type=\"string\" value=\"false\"/>",
+				"add <param name=\"%s\" type=\"string\" value=\"0\"/>",
-				Parameters::kGridFromDepth().c_str(),
+				Parameters::kGridSensor().c_str(),
-				Parameters::kGridFromDepth().c_str(),
+				Parameters::kGridSensor().c_str(),
-				Parameters::kGridFromDepth().c_str());
+				Parameters::kGridSensor().c_str());
-		parameters_.insert(ParametersPair(Parameters::kGridFromDepth(), "false"));
+		parameters_.insert(ParametersPair(Parameters::kGridSensor(), "0"));
 	}
-	Parameters::parse(parameters_, Parameters::kGridFromDepth(), gridFromDepth);
+	Parameters::parse(parameters_, Parameters::kGridSensor(), gridSensor);
-	if((this->isSubscribedToScan2d() || this->isSubscribedToScan3d() || genScan_) && parameters_.find(Parameters::kGridRangeMax()) == parameters_.end() && !gridFromDepth)
+	if((this->isSubscribedToScan2d() || this->isSubscribedToScan3d() || genScan_) && parameters_.find(Parameters::kGridRangeMax()) == parameters_.end() && gridSensor==0)
 	{
-		RCLCPP_INFO(this->get_logger(), "Setting \"%s\" parameter to 0 (default %f) as \"subscribe_scan\" or \"subscribe_scan_cloud\" or \"gen_scan\" is true.",
+		RCLCPP_INFO(this->get_logger(), "Setting \"%s\" parameter to 0 (default %f) as \"subscribe_scan\" or \"subscribe_scan_cloud\" or \"gen_scan\" is true and %s is 0.",
 				Parameters::kGridRangeMax().c_str(),
 				Parameters::defaultGridRangeMax(),
-				Parameters::kGridFromDepth().c_str());
+				Parameters::kGridSensor().c_str());
 		parameters_.insert(ParametersPair(Parameters::kGridRangeMax(), "0"));
 	}
 	if(this->isSubscribedToScan3d() && parameters_.find(Parameters::kIcpPointToPlaneRadius()) == parameters_.end())

src/MapsManager.cpp

@@ -1209,10 +1209,10 @@ void MapsManager::publishMaps(
 			else if(poses.size())
 			{
 				UWARN("Octomap projection map is empty! (poses=%d octomap nodes=%d). "
-						"Make sure you activated \"%s\" and \"%s\" to true. "
+						"Make sure you enabled \"%s\" and set \"%s\"=1. "
 						"See \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" for more info.",
 						(int)poses.size(), (int)octomap_->octree()->size(),
-						Parameters::kGrid3D().c_str(), Parameters::kGridFromDepth().c_str());
+						Parameters::kGrid3D().c_str(), Parameters::kGridSensor().c_str());
 			}
 		}
 	}

src/MsgConversion.cpp

@@ -1074,7 +1074,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::msg::NodeData & msg)
 	{
 		UERROR("Word IDs and 3D points should be the same size (%d, %d)!", (int)msg.word_ids.size(), (int)msg.word_pts.size());
 	}
-	if(wordsDescriptors.rows != (int)msg.word_ids.size())
+	if(!wordsDescriptors.empty() && wordsDescriptors.rows != (int)msg.word_ids.size())
 	{
 		UERROR("Word IDs and descriptors should be the same size (%d, %d)!", (int)msg.word_ids.size(), wordsDescriptors.rows);
 		wordsDescriptors = cv::Mat();

src/OdometryROS.cpp

@@ -291,7 +291,12 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams)
 		if(get_parameter(iter->first, parameter))
 		{
 			std::string vStr = parameter.as_string();
-			if(iter->second.first && parameters_.find(iter->second.second) != parameters_.end())
+			if(!iter->second.second.empty() && parameters_.find(iter->second.second)!=parameters_.end())
+			{
+				RCLCPP_WARN(this->get_logger(), "Odometry: Parameter name changed: \"%s\" -> \"%s\". The new parameter is already used with value \"%s\", ignoring the old one with value \"%s\".",
+						iter->first.c_str(), iter->second.second.c_str(), parameters_.find(iter->second.second)->second.c_str(), vStr.c_str());
+			}
+			else if(iter->second.first && parameters_.find(iter->second.second) != parameters_.end())
 			{
 				// can be migrated
 				parameters_.at(iter->second.second)= vStr;

src/nodelets/stereo_odometry.cpp

@@ -180,7 +180,20 @@ void StereoOdometry::callback(
 						waitForTransform());
 				if(stereoTransform.isNull())
 				{
-					RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get TF between the two cameras!", Parameters::kRtabmapImagesAlreadyRectified().c_str());
+					RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get TF between the two cameras! (between frames %s and %s)",
+							Parameters::kRtabmapImagesAlreadyRectified().c_str(),
+							cameraInfoRight->header.frame_id.c_str(),
+							cameraInfoLeft->header.frame_id.c_str());
+					return;
+				}
+				else if(stereoTransform.isIdentity())
+				{
+					RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get a valid TF between the two cameras! "
+							"Identity transform returned between left and right cameras. Verify that if TF between "
+							"the cameras is valid: \"rosrun tf tf_echo %s %s\".",
+							Parameters::kRtabmapImagesAlreadyRectified().c_str(),
+							cameraInfoRight->header.frame_id.c_str(),
+							cameraInfoLeft->header.frame_id.c_str());
 					return;
 				}
 			}