rtabmap_ros/rtabmap_examples/launch/test_velodyne.launch

<?xml version="1.0"?>
<!-- -->
<launch>

    <!--
    Hand-held 3D lidar mapping example using only a Velodyne PUCK (no camera).
    Prerequisities: rtabmap should be built with libpointmatcher
    Example:
     $ roslaunch rtabmap_examples test_velodyne.launch
     $ rosrun rviz rviz -f map
     $ Show TF and /rtabmap/cloud_map topics
    -->

    <arg name="rtabmap_viz"   default="true"/>
    <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="deskewing"     default="true"/>
    <arg name="slerp"         default="false"/> <!-- If true, a slerp between the first and last time will be used to deskew each point, which is faster than using tf for every point but less accurate -->
    <arg name="organize_cloud" default="$(arg deskewing)"/> <!-- Should be organized if deskewing is enabled -->
    <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="10"/> <!-- Set to 100 for kitti dataset to make sure all scans are processed -->
    <arg name="queue_size_odom" default="1"/>  <!-- Set to 100 for kitti dataset to make sure all scans are processed -->
    <arg name="loop_ratio"      default="0.2"/>

    <arg name="resolution"         default="0.05"/> <!-- set 0.05-0.3 for indoor, set 0.3-0.5 for outdoor (0.4 for kitti) -->
    <arg name="iterations"         default="10"/>

    <!-- Grid parameters -->
    <arg name="ground_is_obstacle" default="true"/>
    <arg name="grid_max_range"     default="20"/>

    <!-- For F2M Odometry -->
    <arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car, kitti) -->
    <arg name="local_map_size"    default="15000"/>
    <arg name="key_frame_thr"     default="0.4"/>

    <!-- For FLOAM Odometry -->
    <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 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)"/>
    </include>

    <!-- IMU orientation estimation and publish tf -->
    <node if="$(arg use_imu)" pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
      <remap from="imu/data_raw" to="$(arg imu_topic)"/>
      <remap from="imu/data" to="$(arg imu_topic)/filtered"/>
    </node>
    <node if="$(arg use_imu)" pkg="nodelet" type="nodelet" name="imu_filter" args="load imu_filter_madgwick/ImuFilterNodelet imu_nodelet_manager">
      <param name="use_mag" value="false"/>
      <param name="world_frame" value="enu"/>
      <param name="publish_tf" value="false"/>
    </node>
    <node if="$(arg use_imu)" pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_util/imu_to_tf imu_nodelet_manager">
      <remap from="imu/data" to="$(arg imu_topic)/filtered"/>
      <param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
      <param name="base_frame_id" value="$(arg frame_id)"/>
    </node>

    <group ns="rtabmap">
      <node pkg="rtabmap_odom" type="icp_odometry" name="icp_odometry" output="screen">
        <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="deskewing"       type="bool"   value="$(arg deskewing)"/>
        <param name="deskewing_slerp" type="bool"   value="$(arg slerp)"/>
        <param name="queue_size"      type="int"    value="$(arg queue_size_odom)"/>
        <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"/>

        <remap if="$(arg use_imu)" from="imu" to="$(arg imu_topic)/filtered"/>
        <param if="$(arg use_imu)" name="guess_frame_id"   type="string" value="$(arg frame_id)_stabilized"/>
        <param if="$(arg use_imu)" name="wait_imu_to_init" type="bool" value="true"/>

        <!-- ICP parameters -->
        <param name="Icp/PointToPlane"        type="string" value="true"/>
        <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 -->
        <param name="Icp/Epsilon"             type="string" value="0.001"/>
        <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="$(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="$(arg key_frame_thr)"/>
        <param     if="$(arg floam)" name="Odom/Strategy" type="string" value="11"/>
        <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="$(arg local_map_size)"/>
        <param name="OdomLOAM/Sensor"            type="string" value="$(arg floam_sensor)"/>
        <param name="OdomLOAM/Resolution"        type="string" value="$(arg resolution)"/>
        <param if="$(eval not deskewing and scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
        <param if="$(eval not deskewing and not scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
        <param if="$(arg deskewing)" name="OdomLOAM/ScanPeriod" type="string" value="0"/>
      </node>

      <node pkg="rtabmap_slam" type="rtabmap" name="rtabmap" output="screen" args="-d">
        <param name="frame_id"             type="string" value="$(arg frame_id)"/>
        <param name="subscribe_depth"      type="bool" value="false"/>
        <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)/filtered"/>

        <!-- RTAB-Map's parameters -->
        <param 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"/>
        <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/>
        <param name="RGBD/AngularUpdate"             type="string" value="0.05"/>
        <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/LaserScanNormalK"           type="string" value="20"/>

        <param name="Reg/Strategy"                   type="string" value="1"/>
        <param name="Grid/CellSize"                  type="string" value="$(arg resolution)"/>
        <param name="Grid/RangeMax"                  type="string" value="$(arg grid_max_range)"/>
        <param name="Grid/ClusterRadius"             type="string" value="1"/>
        <param name="Grid/GroundIsObstacle"          type="string" value="$(arg ground_is_obstacle)"/>
        <param name="Optimizer/GravitySigma"         type="string" value="0.3"/>

        <!-- ICP parameters -->
        <param name="Icp/VoxelSize"                  type="string" value="$(arg resolution)"/>
        <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="$(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="$(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)"/>
      </node>

      <node if="$(arg rtabmap_viz)" name="rtabmap_viz" pkg="rtabmap_viz" type="rtabmap_viz" output="screen">
        <param name="frame_id" type="string" value="$(arg frame_id)"/>
        <param name="odom_frame_id" type="string" value="odom"/>
        <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="odom_filtered_input_scan"/>
        <remap from="odom_info" to="odom_info"/>
      </node>

      <node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_util/point_cloud_assembler" output="screen">
        <remap     if="$(arg deskewing)" from="cloud" to="odom_filtered_input_scan"/>
        <remap unless="$(arg deskewing)" 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="queue_size"      type="int"    value="$(arg queue_size)" />
      </node>
  </group>

</launch>