From b5dd624ae24079a2b2c40dc2c38b0041e012729d Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Thu, 30 Sep 2021 09:55:29 -0400
Subject: [PATCH] test_velodyne.launch: added more options

---
 launch/tests/test_velodyne.launch | 15 ++++++++++-----
 1 file changed, 10 insertions(+), 5 deletions(-)

diff --git a/launch/tests/test_velodyne.launch b/launch/tests/test_velodyne.launch
index b9290985..634b70a5 100644
--- a/launch/tests/test_velodyne.launch
+++ b/launch/tests/test_velodyne.launch
@@ -24,6 +24,8 @@
     <arg name="loop_ratio" default="0.4"/> <!-- Set to 0.2 for kitti -->
     
     <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="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (car, 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) -->
@@ -47,6 +49,7 @@
         <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="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"/>
 
@@ -56,7 +59,7 @@
      
         <!-- ICP parameters -->
         <param name="Icp/PointToPlane"        type="string" value="true"/>
-        <param name="Icp/Iterations"          type="string" value="10"/>
+        <param name="Icp/Iterations"          type="string" value="$(arg iterations)"/>
         <param     if="$(arg floam)" name="Icp/VoxelSize" type="string" value="0"/>
         <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 -->
@@ -65,10 +68,11 @@
         <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"/>
+        <param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
         <param name="Icp/PM"                  type="string" value="true"/> 
         <param name="Icp/PMOutlierRatio"      type="string" value="0.7"/>
         <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>  
+        <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"/>
@@ -86,6 +90,7 @@
         <param name="subscribe_rgb"        type="bool" value="false"/>
         <param name="subscribe_scan_cloud" type="bool" value="true"/>
         <param name="approx_sync"          type="bool" value="false"/>
+        <param name="wait_for_transform_duration" type="double" value="0.2"/>
         
         <remap from="scan_cloud" to="assembled_cloud"/>
         <remap from="imu" to="$(arg imu_topic)"/>
@@ -114,10 +119,10 @@
         <param name="Icp/PointToPlaneK"              type="string" value="20"/>
         <param name="Icp/PointToPlaneRadius"         type="string" value="0"/>
         <param name="Icp/PointToPlane"               type="string" value="true"/>
-        <param name="Icp/Iterations"                 type="string" value="10"/>
+        <param name="Icp/Iterations"                 type="string" value="$(arg iterations)"/>
         <param name="Icp/Epsilon"                    type="string" value="0.001"/>
         <param name="Icp/MaxTranslation"             type="string" value="3"/>
-        <param name="Icp/MaxCorrespondenceDistance"  type="string" value="1"/>
+        <param name="Icp/MaxCorrespondenceDistance"  type="string" value="$(eval resolution*10)"/>
         <param name="Icp/PM"                         type="string" value="true"/> 
         <param name="Icp/PMOutlierRatio"             type="string" value="0.7"/>
         <param name="Icp/CorrespondenceRatio"        type="string" value="$(arg loop_ratio)"/>
@@ -129,7 +134,7 @@
         <param name="subscribe_odom_info" type="bool" value="true"/>
         <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="scan_cloud" to="odom_filtered_input_scan"/>
         <remap from="odom_info" to="odom_info"/>
       </node>