slam_velodyne.launch 3.98 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
<launch>

  <!-- General args -->
  <arg name="use_sim_time" default="true" doc="Use simulation time when replaying rosbags with '--clock' option."/>
  <arg name="outdoor" default="true" doc="Decide which set of parameters to use"/>
  <arg name="rviz" default="true" doc="Visualize results with RViz."/>
  <arg name="vlp16_driver" default="false" doc="If true, start Velodyne driver."/>
  <arg name="gps" default="false" doc="If true, use GPS data to calibrate SLAM output. Otherwise, provide calibration."/>
  <arg name="tags_topic" default="tag_detections" doc="topic from which to get the tag measurements"/>
  <!-- Velodyne driver parameters -->
  <arg name="device_ip" default="" />
  <arg name="port" default="2368" />
  <arg name="pcap" default="" />
  <!-- /!\ rpm and timestamp_first_packet are also used to generate approximate point-wise timestamps as 'time' field is not usable. -->
  <arg name="rpm" default="600." doc="Ouster sensor spinning speed."/>
  <arg name="timestamp_first_packet" default="false" doc="If Ouster timestamping is based on the first or last packet of each scan."/>

  <!-- Sim Time, used when replaying rosbag files (with mandatory option 'clock') -->
  <!-- /!\ if replaying pcap files use_sim_time must be false-->
  <param name="/use_sim_time" value="$(arg use_sim_time)"/>

  <!-- Rviz-->
  <group if="$(arg rviz)">
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find lidar_slam)/params/slam.rviz"/>
  </group>

  <!-- Decode raw Velodyne packets to 3D points -->
  <group if="$(arg vlp16_driver)">
    <!-- start nodelet manager and driver nodelets -->
    <include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
      <arg name="device_ip"              value="$(arg device_ip)"/>
      <arg name="frame_id"               value="velodyne"/>
      <arg name="manager"                value="velodyne_nodelet_manager" />
      <arg name="model"                  value="VLP16"/>
      <arg name="pcap"                   value="$(arg pcap)"/>
      <arg name="port"                   value="$(arg port)"/>
      <arg name="read_fast"              value="false"/>
      <arg name="read_once"              value="true"/>
      <arg name="repeat_delay"           value="0.0"/>
      <arg name="rpm"                    value="$(arg rpm)"/>
      <arg name="gps_time"               value="false"/>
      <arg name="cut_angle"              value="-0.01"/>
      <arg name="timestamp_first_packet" value="$(arg timestamp_first_packet)"/>
    </include>

    <!-- start transform nodelet -->
    <include file="$(find velodyne_pointcloud)/launch/transform_nodelet.launch">
      <arg name="model"          value="VLP16"/>
      <arg name="calibration"    value="$(find velodyne_pointcloud)/params/VLP16db.yaml"/>
      <arg name="manager"        value="velodyne_nodelet_manager" />
      <arg name="fixed_frame"    value="" />
      <arg name="target_frame"   value="" />
      <arg name="max_range"      value="130.0"/>
      <arg name="min_range"      value="0.4"/>
      <arg name="organize_cloud" value="false"/>
    </include>
  </group>

  <!-- Velodyne points conversion -->
  <node name="velodyne_conversion" pkg="lidar_conversions" type="velodyne_conversion_node" output="screen">
    <param name="rpm" value="$(arg rpm)"/>
    <param name="timestamp_first_packet" value="$(arg timestamp_first_packet)"/>
63
    <remap from="velodyne_points" to="/ct_icp/pointcloud"/>
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
  </node>

  <!-- LiDAR SLAM : compute TF slam_init -> velodyne -->
  <node name="lidar_slam" pkg="lidar_slam" type="lidar_slam_node" output="screen">
    <rosparam if="$(arg outdoor)" file="$(find lidar_slam)/params/slam_config_outdoor.yaml" command="load"/>
    <rosparam unless="$(arg outdoor)" file="$(find lidar_slam)/params/slam_config_indoor.yaml" command="load"/>
    <param name="gps/use_gps" value="$(arg gps)"/>
    <remap from="tag_detections" to="$(arg tags_topic)"/>
  </node>

  <!-- Launch GPS/UTM conversions nodes -->
  <group if="$(arg gps)">
    <include file="$(find lidar_slam)/launch/gps_conversions.launch"/>
  </group>

</launch>