Updated velodyne and ouster example launch files

2026-10-03 16:27:46 +08:00 · 2021-05-24 10:10:40 -04:00
parent 761d8d6129
commit 322c6a223a
2 changed files with 52 additions and 14 deletions
@@ -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)"/>
@@ -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 -->