-
Notifications
You must be signed in to change notification settings - Fork 86
/
sick_nav_350.launch
87 lines (78 loc) · 9.57 KB
/
sick_nav_350.launch
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
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
<?xml version="1.0"?>
<!-- LAUNCH FILE FOR NAV350 -->
<!-- NAV350 support is currently experimental and under development. -->
<!-- Using node option required="true" will close roslaunch after node exits -->
<launch>
<arg name="hostname" default="192.168.0.1"/>
<arg name="cloud_topic" default="cloud"/>
<arg name="laserscan_topic" default="scan"/>
<arg name="frame_id" default="cloud"/>
<arg name="sw_pll_only_publish" default="true"/>
<arg name="add_transform_xyz_rpy" default="0,0,0,0,0,0"/>
<arg name="add_transform_check_dynamic_updates" default="false"/> <!-- Note: dynamical updates of parameter add_transform_xyz_rpy can decrease the performance and is therefor deactivated by default -->
<arg name="nav_do_initial_mapping" default="false"/> <!-- Run mapping during initialization. Requires at least 3 visible reflectors, vehicle not moving. If nav_do_initial_mapping is false (default), ensure NAV350 mapping is configured e.g. by SOPAS ET. -->
<arg name="nav_set_landmark_layout_by_imk_file" default=""/> <!-- Read and set NAV350 landmark layout from a text file (.imk-file). Do not apply if nav_set_landmark_layout_file is empty (default), otherwise the landmark layout is overwritten by a imk-file created+saved using SOPAS ET -->
<arg name="tf_publish_rate" default="10.0" /> <!-- Rate to publish TF messages in hz, use 0 to deactivate TF messages -->
<node name="sick_nav_350" pkg="sick_scan_xd" type="sick_generic_caller" respawn="false" output="screen" required="true">
<param name="scanner_type" type="string" value="sick_nav_350"/>
<param name="use_binary_protocol" type="bool" value="true"/>
<param name="hostname" type="string" value="$(arg hostname)"/>
<param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
<param name="laserscan_topic" type="string" value="$(arg laserscan_topic)"/>
<param name="frame_id" type="str" value="$(arg frame_id)"/>
<param name="port" type="string" value="2112"/>
<param name="timelimit" type="int" value="5"/>
<param name="sw_pll_only_publish" type="bool" value="$(arg sw_pll_only_publish)"/>
<param name="min_ang" type="double" value="0.0"/>
<param name="max_ang" type="double" value="6.28318530"/>
<param name="nav_operation_mode" type="int" value="4" /> <!-- Switch to operational mode after initialization: 0 = power down, 1 = standby, 2 = mapping, 3 = landmark detection, 4 = navigation -->
<param name="nav_start_polling" type="bool" value="true" /> <!-- Start to poll scan data, pose and landmark data after initialization -->
<param name="nav_tf_parent_frame_id" type="string" value="cloud" /> <!-- Parent (world) frame id of ros transform of the NAV pose (ROS only) -->
<param name="nav_tf_child_frame_id" type="string" value="nav" /> <!-- Child (sensor) frame id of ros transform of the NAV pose (ROS only) -->
<param name="nav_curr_layer" type="int" value="0" /> <!-- Set the current NAV Layer for Positioning and Mapping -->
<param name="nav_set_landmark_layout_by_imk_file" type="string" value="$(arg nav_set_landmark_layout_by_imk_file)"/> <!-- Run mapping during initialization. Requires at least 3 visible reflectors, vehicle not moving. If nav_do_initial_mapping is false (default), ensure NAV350 mapping is configured e.g. by SOPAS ET. -->
<param name="nav_do_initial_mapping" type="bool" value="$(arg nav_do_initial_mapping)" /> <!-- Run mapping during initialization. Requires at least 3 visible reflectors, vehicle not moving. If nav_do_initial_mapping is false (default), ensure NAV350 mapping is configured e.g. by SOPAS ET. -->
<param name="nav_map_cfg_mean" type="int" value="50" /> <!-- Configure Mapping if nav_do_initial_mapping is true: nav_map_cfg_mean := Mean Number of scans for averaging, 1...127, default: 50 -->
<param name="nav_map_cfg_neg" type="int" value="0" /> <!-- Configure Mapping if nav_do_initial_mapping is true: nav_map_cfg_neg := If the parameter is set to 0, the NAV350 responds all measured reflectors, If the parameter is set to 1, the NAV350 responds new reflectors, default: 0 -->
<param name="nav_map_cfg_x" type="int" value="0" /> <!-- Configure Mapping if nav_do_initial_mapping is true: nav_map_cfg_x := X-Position of the NAV350, -10000000 ... +10000000 mm, default: 0 -->
<param name="nav_map_cfg_y" type="int" value="0" /> <!-- Configure Mapping if nav_do_initial_mapping is true: nav_map_cfg_y := Y-Position of the NAV350, -10000000 ... +10000000 mm, default: 0 -->
<param name="nav_map_cfg_phi" type="int" value="0" /> <!-- Configure Mapping if nav_do_initial_mapping is true: nav_map_cfg_phi := Heading of the NAV350, -360000 ... +360000 mdeg, default: 0 -->
<param name="nav_map_cfg_reflector_size" type="int" value="80" /> <!-- Configure Mapping if nav_do_initial_mapping is true: nav_map_cfg_reflector_size := Reflector size, 1 ... 150 mm, default: 80 -->
<!-- Apply an additional transform to the cartesian pointcloud, default: "0,0,0,0,0,0" (i.e. no transform) -->
<!-- Note: add_transform_xyz_rpy is specified by 6D pose x, y, z, roll, pitch, yaw in [m] resp. [rad] -->
<!-- It transforms a 3D point in cloud coordinates to 3D point in user defined world coordinates: -->
<!-- add_transform_xyz_rpy := T[world,cloud] with parent "world" and child "cloud", i.e. P_world = T[world,cloud] * P_cloud -->
<!-- The additional transform applies to cartesian lidar pointclouds and visualization marker (fields) -->
<!-- It is NOT applied to polar pointclouds, radarscans, ldmrs objects or other messages -->
<param name="add_transform_xyz_rpy" type="string" value="$(arg add_transform_xyz_rpy)" />
<param name="add_transform_check_dynamic_updates" type="bool" value="$(arg add_transform_check_dynamic_updates)" />
<param name="start_services" type="bool" value="True" /> <!-- Start ros service for cola commands, default: true -->
<param name="message_monitoring_enabled" type="bool" value="True" /> <!-- Enable message monitoring with reconnect+reinit in case of timeouts, default: true -->
<param name="read_timeout_millisec_default" type="int" value="5000"/> <!-- 5 sec read timeout in operational mode (measurement mode), default: 5000 milliseconds -->
<param name="read_timeout_millisec_startup" type="int" value="120000"/> <!-- 120 sec read timeout during startup (sensor may be starting up, which can take up to 120 sec.), default: 120000 milliseconds -->
<param name="read_timeout_millisec_kill_node" type="int" value="150000"/> <!-- 150 sec pointcloud timeout, ros node will be killed if no point cloud published within the last 150 sec., default: 150000 milliseconds -->
<!-- Note: read_timeout_millisec_kill_node less or equal 0 deactivates pointcloud monitoring (not recommended) -->
<param name="client_authorization_pw" type="string" value="F4724744"/> <!-- Default password for client authorization -->
<!--
On ROS-1 and ROS-2, sick_scan_xd publishes TF messsages to map a given base frame (i.e. base coordinates system) to the lidar frame (i.e. lidar coordinates system) and vice versa.
The default base frame id is "map" (which is the default frame in rviz).
The default 6D pose is (x,y,z,roll,pitch,yaw) = (0,0,0,0,0,0) defined by position (x,y,z) in meter and (roll,pitch,yaw) in radians.
This 6D pose (x,y,z,roll,pitch,yaw) is the transform T[base,lidar] with parent "base" and child "lidar".
For lidars mounted on a carrier, the lidar pose T[base,lidar] and base frame can be configured in this launchfile using the following parameter.
The lidar frame id given by parameter "frame_id" resp. "publish_frame_id".
Note that the transform is specified using (x,y,z,roll,pitch,yaw). In contrast, the ROS static_transform_publisher uses commandline arguments in order (x,y,z,yaw,pitch,roll).
-->
<param name="tf_base_frame_id" type="string" value="map" /> <!-- Frame id of base coordinates system, e.g. "map" (default frame in rviz) -->
<param name="tf_base_lidar_xyz_rpy" type="string" value="0,0,0,0,0,0" /> <!-- T[base,lidar], 6D pose (x,y,z,roll,pitch,yaw) in meter resp. radians with parent "map" and child "cloud" -->
<param name="tf_publish_rate" type="double" value="$(arg tf_publish_rate)" /> <!-- Rate to publish TF messages in hz, use 0 to deactivate TF messages -->
<!--
Optional mode to convert lidar ticks to ros- resp. system-timestamps:
tick_to_timestamp_mode = 0 (default): convert lidar ticks in microseconds to system timestamp by software-pll
tick_to_timestamp_mode = 1 (optional tick-mode): convert lidar ticks in microseconds to timestamp by 1.0e-6*(curtick-firstTick)+firstSystemTimestamp
tick_to_timestamp_mode = 2 (optional tick-mode): convert lidar ticks in microseconds directly into a lidar timestamp by sec = tick/1000000, nsec = 1000*(tick%1000000)
Note: Using tick_to_timestamp_mode = 2, the timestamps in ROS message headers will be in lidar time, not in system time. Lidar and system time can be very different.
Using tick_to_timestamp_mode = 2 might cause unexpected results or error messages. We recommend using tick_to_timestamp_mode = 2 for special test cases only.
-->
<param name="tick_to_timestamp_mode" type="int" value="0"/>
</node>
</launch>