From 322c6a223a320f19b6fd0d066852e206505f8244 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Mon, 24 May 2021 10:10:40 -0400
Subject: [PATCH] Updated velodyne and ouster example launch files

---
 launch/tests/test_ouster_gen2.launch | 64 ++++++++++++++++++++++------
 launch/tests/test_velodyne.launch    |  2 +
 2 files changed, 52 insertions(+), 14 deletions(-)

diff --git a/launch/tests/test_ouster_gen2.launch b/launch/tests/test_ouster_gen2.launch
index 8219e7f6..e5650408 100644
--- a/launch/tests/test_ouster_gen2.launch
+++ b/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
-     $ Show TF and /rtabmap/cloud_map topics
+    
+      $ roslaunch rtabmap_ros test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX
+      $ 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="sensor_hostname"/>
-    <arg name="udp_dest"/>
+    <arg name="use_sim_time"  default="false"/>
 
-    <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="use_sim_time"  default="false"/>
+    <arg name="voxel_size"    default="0.15"/>           <!-- indoor: 0.1 to 0.3, outdoor: 0.3 to 0.5 --> 
+    <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)"/>
diff --git a/launch/tests/test_velodyne.launch b/launch/tests/test_velodyne.launch
index 14734fa9..d2c25d2d 100644
--- a/launch/tests/test_velodyne.launch
+++ b/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 -->