Merge branch 'master' of https://github.com/introlab/rtabmap_ros into addFloodFill

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.9 REQUIRED)
+find_package(RTABMap 0.20.10 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 

docker/melodic/latest/Dockerfile

@@ -8,6 +8,6 @@ RUN source /ros_entrypoint.sh && \
     catkin_init_workspace && \
     git clone https://github.com/introlab/rtabmap_ros.git && \
     cd .. && \
-    catkin_make -j1 -DCMAKE_INSTALL_PREFIX=/opt/ros/melodic install && \
+    catkin_make -j1 -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_INSTALL_PREFIX=/opt/ros/melodic install && \
     cd && \
     rm -rf catkin_ws

docker/noetic/latest/Dockerfile

@@ -8,6 +8,6 @@ RUN source /ros_entrypoint.sh && \
     catkin_init_workspace && \
     git clone https://github.com/introlab/rtabmap_ros.git && \
     cd .. && \
-    catkin_make -j1 -DCMAKE_INSTALL_PREFIX=/opt/ros/noetic install && \
+    catkin_make -j1 -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_INSTALL_PREFIX=/opt/ros/noetic install && \
     cd && \
     rm -rf catkin_ws

launch/rtabmap.launch

@@ -37,6 +37,7 @@
   
   <arg name="frame_id"                default="camera_link"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
   <arg name="odom_frame_id"           default=""/>                <!-- If set, TF is used to get odometry instead of the topic -->
+  <arg name="odom_frame_id_init"      default=""/>                <!-- If set, TF map->odom is published even if no odometry topic has been received yet. The frame id should match the one in the topic. -->
   <arg name="map_frame_id"            default="map"/>
   <arg name="ground_truth_frame_id"   default=""/>     <!-- e.g., "world" -->
   <arg name="ground_truth_base_frame_id" default=""/>  <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
@@ -310,6 +311,7 @@
       <param name="frame_id"             type="string" value="$(arg frame_id)"/>
       <param name="map_frame_id"         type="string" value="$(arg map_frame_id)"/>
       <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
+      <param name="odom_frame_id_init"   type="string" value="$(arg odom_frame_id_init)"/>
       <param name="publish_tf"           type="bool"   value="$(arg publish_tf_map)"/>
       <param name="gen_scan"             type="bool"   value="$(arg gen_scan)"/>
       <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>

launch/tests/test_ouster_gen2.launch

@@ -4,23 +4,48 @@
     <!-- 
     Hand-held 3D lidar mapping example using only a Ouster GEN2 (no camera).
     Prerequisities: rtabmap should be built with libpointmatcher
+    
     Example:
-     $ roslaunch rtabmap_ros test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX
+    
-     $ rosrun rviz rviz -f map
+      $ roslaunch rtabmap_ros test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX
-     $ Show TF and /rtabmap/cloud_map topics
+      $ rosrun rviz rviz -f map
+      RVIZ: Show TF and /rtabmap/cloud_map topics
+    
     ISSUE: You may have to reset odometry after receiving the first cloud if the map looks tilted. The problem seems 
            coming from the first cloud sent by os_cloud_node, which may be poorly synchronized with IMU data.
+           
+    PTP mode (synchronize timestamp with host computer time)
+    
+      * Install:
+      
+        $ sudo apt install linuxptp httpie
+        $ printf "[global]\ntx_timestamp_timeout 10\n" >> ~/os.conf
+      
+      * Running:
+     
+        (replace "XXXXXXXXXXXX" by your ouster serial, as well as XXX by its IP address)
+        (replace "eth0" by the network interface used to communicate with ouster)
+       
+        $ http PUT http://os-XXXXXXXXXXXX.local/api/v1/time/ptp/profile <<< '"default-relaxed"'
+        $ sudo ptp4l -i eth0 -m -f ~/os.conf -S
+        $ roslaunch rtabmap_ros test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX ptp:=true
+       
     -->
 
-    <!-- Required: -->
+    <arg name="use_sim_time"  default="false"/>
-    <arg name="sensor_hostname"/>
-    <arg name="udp_dest"/>
 
-    <arg name="frame_id" default="os_sensor"/>
+    <!-- Required: -->
+    <arg unless="$(arg use_sim_time)" name="sensor_hostname"/>
+    <arg unless="$(arg use_sim_time)" name="udp_dest"/>
+
+    <arg name="frame_id"      default="os_sensor"/>
     <arg name="rtabmapviz"    default="true"/>
     <arg name="scan_20_hz"    default="true"/>
-    <arg name="voxel_size"    default="0.15"/> <!-- indoor: 0.1 to 0.3, outdoor: 0.3 to 0.5 --> 
+    <arg name="voxel_size"    default="0.15"/>           <!-- indoor: 0.1 to 0.3, outdoor: 0.3 to 0.5 --> 
-    <arg name="use_sim_time"  default="false"/>
+    <arg name="assemble"      default="false"/>
+    <arg name="ptp"           default="false"/>          <!-- See comments in header to start before launching the launch -->
+    <arg name="distortion_correction"  default="false"/> <!-- Requires this pull request: https://github.com/ouster-lidar/ouster_example/pull/245 -->
+    
     <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
     
     <!-- Ouster -->
@@ -30,6 +55,8 @@
       <arg name="image" value="true"/>
       <arg     if="$(arg scan_20_hz)" name="lidar_mode" value="1024x20"/>
       <arg unless="$(arg scan_20_hz)" name="lidar_mode" value="1024x10"/>
+      <arg if="$(arg ptp)" name="timestamp_mode" value="TIME_FROM_PTP_1588"/>
+      <arg if="$(arg distortion_correction)" name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
     </include>
 
     <!-- IMU orientation estimation and publish tf accordingly to os_sensor frame -->
@@ -41,7 +68,7 @@
       <param name="use_mag" value="false"/>
       <param name="world_frame" value="enu"/>
       <param name="publish_tf" value="false"/>
-    </node> 
+    </node>
     <node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager">
       <remap from="imu/data" to="/os_cloud_node/imu/data"/>
       <param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
@@ -51,12 +78,12 @@
     <group ns="rtabmap">
       <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
         <remap from="scan_cloud" to="/os_cloud_node/points"/>
+        <remap from="imu"        to="/os_cloud_node/imu/data"/>
         <param name="frame_id"        type="string" value="$(arg frame_id)"/>  
         <param name="odom_frame_id"   type="string" value="odom"/>
         <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"/>
 
-        <remap from="imu" to="/os_cloud_node/imu/data"/>
         <param name="guess_frame_id"   type="string" value="$(arg frame_id)_stabilized"/>
         <param name="wait_imu_to_init" type="bool" value="true"/>
      
@@ -72,7 +99,7 @@
         <param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
         <param name="Icp/PM"                  type="string" value="true"/> 
         <param name="Icp/PMOutlierRatio"      type="string" value="0.1"/>
-        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>  
+        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
 
         <!-- Odom parameters -->       
         <param name="Odom/ScanKeyFrameThr"       type="string" value="0.95"/>
@@ -88,11 +115,13 @@
         <param name="subscribe_scan_cloud" type="bool" value="true"/>
         <param name="approx_sync"          type="bool" value="false"/>
         
-        <remap from="scan_cloud" to="/os_cloud_node/points"/>
+        <remap     if="$(arg assemble)" from="scan_cloud" to="assembled_cloud"/>
+        <remap unless="$(arg assemble)" from="scan_cloud" to="/os_cloud_node/points"/>
         <remap from="imu"        to="/os_cloud_node/imu/data"/>
      
         <!-- RTAB-Map's parameters -->
-        <param name="Rtabmap/DetectionRate"          type="string" value="1"/>  
+        <param     if="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="0"/> <!-- already set 1 Hz in point_cloud_assembler -->
+        <param unless="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="1"/>  
         <param name="RGBD/NeighborLinkRefining"      type="string" value="false"/>
         <param name="RGBD/ProximityBySpace"          type="string" value="true"/>
         <param name="RGBD/ProximityMaxGraphDepth"    type="string" value="0"/>
@@ -127,6 +156,13 @@
         <param name="Icp/PMOutlierRatio"             type="string" value="0.7"/>
         <param name="Icp/CorrespondenceRatio"        type="string" value="0.2"/>
       </node>
+      
+      <node if="$(arg assemble)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
+        <remap from="cloud"           to="/os_cloud_node/points"/>
+        <remap from="odom"            to="odom"/>
+        <param name="assembling_time" type="double" value="1" />
+        <param name="fixed_frame_id"  type="string" value="" />
+      </node>
 
       <node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
         <param name="frame_id" type="string" value="$(arg frame_id)"/>

launch/tests/test_velodyne.launch

@@ -14,6 +14,7 @@
     <arg name="use_imu"       default="false"/> <!-- Assuming IMU fixed to lidar with /velodyne -> /imu_link TF -->
     <arg name="imu_topic"     default="/imu/data"/>
     <arg name="scan_20_hz"    default="false"/> <!-- If we launch the velodyne with "rpm:=1200" argument -->
+    <arg name="organize_cloud" default="false"/>
     <arg name="use_sim_time"  default="false"/>
     <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
     
@@ -22,6 +23,7 @@
     <include file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
        <arg     if="$(arg scan_20_hz)" name="rpm" value="1200"/>
        <arg unless="$(arg scan_20_hz)" name="rpm" value="600"/>
+       <arg name="organize_cloud" value="$(arg organize_cloud)"/>
     </include>
     
     <!-- IMU orientation estimation and publish tf accordingly to os1_sensor frame -->

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap_ros</name>
-  <version>0.20.9</version>
+  <version>0.20.10</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

@@ -142,12 +142,14 @@ void CoreWrapper::onInit()
 	bool publishTf = true;
 	double tfDelay = 0.05; // 20 Hz
 	double tfTolerance = 0.1; // 100 ms
+	std::string odomFrameIdInit;
 
 	pnh.param("config_path",         configPath_, configPath_);
 	pnh.param("database_path",       databasePath_, databasePath_);
 
 	pnh.param("frame_id",            frameId_, frameId_);
 	pnh.param("odom_frame_id",       odomFrameId_, odomFrameId_); // set to use odom from TF
+	pnh.param("odom_frame_id_init",  odomFrameIdInit, odomFrameIdInit); // set to publish map->odom TF before receiving odom topic
 	pnh.param("map_frame_id",        mapFrameId_, mapFrameId_);
 	pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
 	pnh.param("ground_truth_base_frame_id", groundTruthBaseFrameId_, frameId_);
@@ -157,6 +159,18 @@ void CoreWrapper::onInit()
 				"anymore! It is replaced by \"rgbd_cameras\" parameter "
 				"used when \"subscribe_rgbd\" is true");
 	}
+	if(!odomFrameIdInit.empty())
+	{
+		if(odomFrameId_.empty())
+		{
+			ROS_INFO("rtabmap: odom_frame_id_init = %s", odomFrameIdInit.c_str());
+			odomFrameId_ = odomFrameIdInit;
+		}
+		else
+		{
+			ROS_WARN("odom_frame_id_init (%s) is ignored if odom_frame_id (%s) is set.", odomFrameIdInit.c_str(), odomFrameId_.c_str());
+		}
+	}
 
 	pnh.param("publish_tf",          publishTf, publishTf);
 	pnh.param("tf_delay",            tfDelay, tfDelay);
@@ -1859,7 +1873,12 @@ void CoreWrapper::process(
 		{
 			if(iter->first.header.stamp < lastPoseStamp_)
 			{
-				Transform interOdom = rtabmap_ros::transformFromPoseMsg(iter->first.pose.pose);
+				Transform interOdom;
+				if(!rtabmap_.getLocalOptimizedPoses().empty())
+				{
+					// add intermediate poses only if the current local graph is not empty
+					interOdom = rtabmap_ros::transformFromPoseMsg(iter->first.pose.pose);
+				}
 				if(!interOdom.isNull())
 				{
 					cv::Mat covariance;
@@ -2105,6 +2124,15 @@ void CoreWrapper::process(
 			timeRtabmap = timer.ticks();
 			mapToOdomMutex_.lock();
 			mapToOdom_ = rtabmap_.getMapCorrection();
+
+			if(!odomFrameId.empty() && !odomFrameId_.empty() && odomFrameId_.compare(odomFrameId)!=0)
+			{
+				ROS_ERROR("Odometry received doesn't have same frame_id "
+						  "than the one previously set (old=%s, new=%s). "
+						  "Are there multiple nodes publishing on same odometry topic name? "
+						  "The new frame_id is now used.", odomFrameId_.c_str(), odomFrameId.c_str());
+			}
+
 			odomFrameId_ = odomFrameId;
 			mapToOdomMutex_.unlock();
 

src/MsgConversion.cpp

@@ -369,7 +369,6 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_ros::RGBDImageConstPtr & imag
 		int depthHeight = depthMsg->image.rows;
 
 		UASSERT_MSG(
-			imageWidth % depthWidth == 0 && imageHeight % depthHeight == 0 &&
 			imageWidth/depthWidth == imageHeight/depthHeight,
 			uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
 
@@ -1731,7 +1730,6 @@ bool convertRGBDMsgs(
 	if(depthMsgs.size())
 	{
 		UASSERT_MSG(
-				imageWidth % depthWidth == 0 && imageHeight % depthHeight == 0 &&
 				imageWidth/depthWidth == imageHeight/depthHeight,
 				uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
 	}

src/nodelets/rgbd_odometry.cpp

@@ -331,7 +331,6 @@ private:
 		int depthHeight = depthImages[0]->image.rows;
 
 		UASSERT_MSG(
-			imageWidth % depthWidth == 0 && imageHeight % depthHeight == 0 &&
 			imageWidth/depthWidth == imageHeight/depthHeight,
 			uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());