OdomInfo msg: changed type of localScanMap (to fix long compression issue). Updated test_velodyne.launch to make floam working with kitti bags

include/rtabmap_ros/MsgConversion.h

@@ -170,7 +170,7 @@ void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 
 std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo & info);
 rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg, bool ignoreData = false);
-void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg);
+void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg, bool ignoreData = false);
 
 cv::Mat userDataFromROS(const rtabmap_ros::UserData & dataMsg);
 void userDataToROS(const cv::Mat & data, rtabmap_ros::UserData & dataMsg, bool compress);

launch/tests/test_velodyne.launch

@@ -15,16 +15,19 @@
     <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="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="1"/> <!-- Set to 100 for kitti dataset to make sure all scans are processed -->
     
-    <arg name="resolution" default="0.1"/> <!-- set 0.1-0.3 for indoor, set 0.3-0.5 for outdoor -->
+    <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="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) -->
     
-    <include file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
+    <include unless="$(arg use_sim_time)" 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)"/>
@@ -39,9 +42,10 @@
 
     <group ns="rtabmap">
       <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
-        <remap from="scan_cloud" to="/velodyne_points"/>
+        <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     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"/>
 
@@ -56,7 +60,8 @@
         <param unless="$(arg floam)" name="Icp/VoxelSize" type="string" value="$(arg resolution)"/>
         <param name="Icp/DownsamplingStep"    type="string" value="1"/> <!-- cannot be increased with ring-like lidar -->
         <param name="Icp/Epsilon"             type="string" value="0.001"/>
-        <param name="Icp/PointToPlaneK"       type="string" value="20"/>
+        <param     if="$(arg floam)" name="Icp/PointToPlaneK" type="string" value="0"/>
+        <param unless="$(arg floam)" name="Icp/PointToPlaneK" type="string" value="20"/>
         <param name="Icp/PointToPlaneRadius"  type="string" value="0"/>
         <param name="Icp/MaxTranslation"      type="string" value="2"/>
         <param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
@@ -70,8 +75,8 @@
         <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="OdomLOAM/Sensor" type="string" value="0"/> <!-- select VLP16 format -->
+        <param name="OdomLOAM/Sensor"            type="string" value="$(arg floam_sensor)"/> 
-        <param name="OdomLOAM/Resolution" type="string" value="$(arg resolution)"/>
+        <param name="OdomLOAM/Resolution"        type="string" value="$(arg resolution)"/>
       </node>
 
       <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">	  
@@ -94,19 +99,17 @@
         <param name="RGBD/LinearUpdate"              type="string" value="0.05"/>
         <param name="Mem/NotLinkedNodesKept"         type="string" value="false"/>
         <param name="Mem/STMSize"                    type="string" value="30"/>
-        <!-- param name="Mem/LaserScanVoxelSize"     type="string" value="0.1"/ -->
+        <param name="Mem/LaserScanNormalK"           type="string" value="20"/>
-        <!-- param name="Mem/LaserScanNormalK"       type="string" value="10"/ -->
-        <!-- param name="Mem/LaserScanRadius"        type="string" value="0"/ -->
         
         <param name="Reg/Strategy"                   type="string" value="1"/> 
-        <param name="Grid/CellSize"                  type="string" value="0.1"/>
+        <param name="Grid/CellSize"                  type="string" value="$(arg resolution)"/>
         <param name="Grid/RangeMax"                  type="string" value="20"/>
         <param name="Grid/ClusterRadius"             type="string" value="1"/>
         <param name="Grid/GroundIsObstacle"          type="string" value="true"/>
         <param name="Optimizer/GravitySigma"         type="string" value="0.3"/>
 
         <!-- ICP parameters -->
-        <param name="Icp/VoxelSize"                  type="string" value="$(arg resolution)"/>
+        <param name="Icp/VoxelSize"                  type="string" value="0"/> <!-- already voxelized by point_cloud_assembler below -->
         <param name="Icp/PointToPlaneK"              type="string" value="20"/>
         <param name="Icp/PointToPlaneRadius"         type="string" value="0"/>
         <param name="Icp/PointToPlane"               type="string" value="true"/>
@@ -126,14 +129,17 @@
         <param name="subscribe_scan_cloud" type="bool" value="true"/>
         <param name="approx_sync" type="bool" value="false"/>
         <remap from="scan_cloud" to="/velodyne_points"/>
+        <remap from="odom_info" to="odom_info"/>
       </node>
 
       <node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_ros/point_cloud_assembler" output="screen">
-        <remap from="cloud"           to="/velodyne_points"/>
+        <remap from="cloud"           to="$(arg scan_topic)"/>
         <remap from="odom"            to="odom"/>
         <param     if="$(arg scan_20_hz)" name="max_clouds"      type="int"    value="20" />
         <param unless="$(arg scan_20_hz)" name="max_clouds"      type="int"    value="10" />
         <param name="fixed_frame_id"  type="string" value="" />
+        <param name="voxel_size"      type="double" value="$(arg resolution)" />
+        <param name="queue_size"      type="int"    value="$(arg queue_size)" />
       </node>
   </group>
 

msg/OdomInfo.msg

@@ -47,10 +47,8 @@ int32[] wordInliers
 int32[] localMapKeys
 Point3f[] localMapValues
 
-# compressed local scan map data
+# local scan map data
-# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+sensor_msgs/PointCloud2 localScanMap
-uint8[] localScanMap
-int32 localScanMapFormat
 
 # F2F odometry
 #   std::vector<cv::Point2f> refCorners;

src/MsgConversion.cpp

@@ -1477,12 +1477,14 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg, bool ig
 			info.localMap.insert(std::make_pair(msg.localMapKeys[i], point3fFromROS(msg.localMapValues[i])));
 		}
 
-		info.localScanMap = rtabmap::LaserScan(rtabmap::uncompressData(msg.localScanMap), 0, 0, (rtabmap::LaserScan::Format)msg.localScanMapFormat);
+		pcl::PCLPointCloud2 cloud;
+		pcl_conversions::toPCL(msg.localScanMap, cloud);
+		info.localScanMap = rtabmap::util3d::laserScanFromPointCloud(cloud);
 	}
 	return info;
 }
 
-void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg)
+void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg, bool ignoreData)
 {
 	msg.lost = info.lost;
 	msg.matches = info.reg.matches;
@@ -1516,26 +1518,28 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
 
 	msg.type = info.type;
 
-	msg.wordsKeys = uKeys(info.words);
-	keypointsToROS(uValues(info.words), msg.wordsValues);
-
-	msg.wordMatches = info.reg.matchesIDs;
-	msg.wordInliers = info.reg.inliersIDs;
-
-	points2fToROS(info.refCorners, msg.refCorners);
-	points2fToROS(info.newCorners, msg.newCorners);
-	msg.cornerInliers = info.cornerInliers;
-
 	transformToGeometryMsg(info.transform, msg.transform);
 	transformToGeometryMsg(info.transformFiltered, msg.transformFiltered);
 	transformToGeometryMsg(info.transformGroundTruth, msg.transformGroundTruth);
 	transformToGeometryMsg(info.guess, msg.guess);
 
-	msg.localMapKeys = uKeys(info.localMap);
+	if(!ignoreData)
-	points3fToROS(uValues(info.localMap), msg.localMapValues);
+	{
+		msg.wordsKeys = uKeys(info.words);
+		keypointsToROS(uValues(info.words), msg.wordsValues);
 
-	msg.localScanMap = rtabmap::compressData(rtabmap::util3d::transformLaserScan(info.localScanMap, info.localScanMap.localTransform()).data());
+		msg.wordMatches = info.reg.matchesIDs;
-	msg.localScanMapFormat = info.localScanMap.format();
+		msg.wordInliers = info.reg.inliersIDs;
+
+		points2fToROS(info.refCorners, msg.refCorners);
+		points2fToROS(info.newCorners, msg.newCorners);
+		msg.cornerInliers = info.cornerInliers;
+
+		msg.localMapKeys = uKeys(info.localMap);
+		points3fToROS(uValues(info.localMap), msg.localMapValues);
+
+		pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(info.localScanMap, info.localScanMap.localTransform()), msg.localScanMap);
+	}
 }
 
 cv::Mat userDataFromROS(const rtabmap_ros::UserData & dataMsg)

src/OdometryROS.cpp

@@ -859,22 +859,27 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
 	if(odomInfoPub_.getNumSubscribers() || odomInfoLitePub_.getNumSubscribers())
 	{
 		rtabmap_ros::OdomInfo infoMsg;
-		odomInfoToROS(info, infoMsg);
+		odomInfoToROS(info, infoMsg, odomInfoPub_.getNumSubscribers()==0);
 		infoMsg.header.stamp = header.stamp; // use corresponding time stamp to image
 		infoMsg.header.frame_id = odomFrameId_;
-		odomInfoPub_.publish(infoMsg);
+		if(odomInfoPub_.getNumSubscribers()>0) {
+			odomInfoPub_.publish(infoMsg);
+		}
 
-		infoMsg.wordInliers.clear();
+		if(odomInfoLitePub_.getNumSubscribers()>0)
-		infoMsg.wordMatches.clear();
+		{
-		infoMsg.wordsKeys.clear();
+			infoMsg.wordInliers.clear();
-		infoMsg.wordsValues.clear();
+			infoMsg.wordMatches.clear();
-		infoMsg.refCorners.clear();
+			infoMsg.wordsKeys.clear();
-		infoMsg.newCorners.clear();
+			infoMsg.wordsValues.clear();
-		infoMsg.cornerInliers.clear();
+			infoMsg.refCorners.clear();
-		infoMsg.localMapKeys.clear();
+			infoMsg.newCorners.clear();
-		infoMsg.localMapValues.clear();
+			infoMsg.cornerInliers.clear();
-		infoMsg.localScanMap.clear();
+			infoMsg.localMapKeys.clear();
-		odomInfoLitePub_.publish(infoMsg);
+			infoMsg.localMapValues.clear();
+			infoMsg.localScanMap = sensor_msgs::PointCloud2();
+			odomInfoLitePub_.publish(infoMsg);
+		}
 	}
 
 	if(!data.imageRaw().empty() && odomRgbdImagePub_.getNumSubscribers())

src/nodelets/icp_odometry.cpp

@@ -86,6 +86,8 @@ private:
 		ros::NodeHandle & nh = getNodeHandle();
 		ros::NodeHandle & pnh = getPrivateNodeHandle();
 
+		int queueSize = 1;
+		pnh.param("queue_size",  queueSize, queueSize);
 		pnh.param("scan_cloud_max_points",  scanCloudMaxPoints_, scanCloudMaxPoints_);
 		pnh.param("scan_downsampling_step", scanDownsamplingStep_, scanDownsamplingStep_);
 		pnh.param("scan_range_min",  scanRangeMin_, scanRangeMin_);
@@ -131,6 +133,7 @@ private:
 			pnh.param("scan_cloud_normal_k", scanNormalK_, scanNormalK_);
 		}
 
+		NODELET_INFO("IcpOdometry: queue_size             = %d", queueSize);
 		NODELET_INFO("IcpOdometry: scan_cloud_max_points  = %d", scanCloudMaxPoints_);
 		NODELET_INFO("IcpOdometry: scan_downsampling_step = %d", scanDownsamplingStep_);
 		NODELET_INFO("IcpOdometry: scan_range_min         = %f m", scanRangeMin_);
@@ -140,8 +143,8 @@ private:
 		NODELET_INFO("IcpOdometry: scan_normal_radius     = %f m", scanNormalRadius_);
 		NODELET_INFO("IcpOdometry: scan_normal_ground_up  = %f", scanNormalGroundUp_);
 
-		scan_sub_ = nh.subscribe("scan", 1, &ICPOdometry::callbackScan, this);
+		scan_sub_ = nh.subscribe("scan", queueSize, &ICPOdometry::callbackScan, this);
-		cloud_sub_ = nh.subscribe("scan_cloud", 1, &ICPOdometry::callbackCloud, this);
+		cloud_sub_ = nh.subscribe("scan_cloud", queueSize, &ICPOdometry::callbackCloud, this);
 
 		filtered_scan_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odom_filtered_input_scan", 1);
 	}