-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy patharg_demo_localization.launch
131 lines (94 loc) · 4.64 KB
/
arg_demo_localization.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
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
<?xml version="1.0"?>
<launch>
<!-- change topic in rosbag -->
<node pkg="topic_tools" type="relay" name="relay_pose" args="/devbot/pose /gps_local/pose" />
<!-- general configuration parameters -->
<arg name="lidar_localization" default="false"/> <!-- true: lidar for localization, false: gps_local/pose for localization -->
<arg name="map_path" default="$(find arg_data_croix_en_ternois)" />
<arg name="pcd_activated" default="true" />
<!-- Option 1: GPS based localization -->
<group unless="$(arg lidar_localization)" >
<!-- add a noise to the gps ground truth -->
<include file="$(find arg_localization)/launch/gps_pose_estimator.launch">
<arg name="pose_stddev_x_y" value="0.0" />
<arg name="pose_mu_x_y" value="0.0" />
<arg name="pose_stddev_yaw" value="0.0" />
<arg name="pose_mu_yaw" value="0.0" />
</include>
</group>
<!-- Option 2: Lidar based localization -->
<group if="$(arg lidar_localization)" >
<!-- Run a VoxelGrid filter to clean NaNs and downsample the data -->
<node pkg="pointcloud_preprocessor" type="voxel_grid_downsample_filter_node" name="voxel_grid_filter" output="log">
<remap from="~input" to="/sensing/lidar/top/points" />
<remap from="~output" to="/localization/downsample/pointcloud" />
<rosparam>
voxel_size_x: 2.0
voxel_size_y: 2.0
voxel_size_z: 2.0
</rosparam>
</node>
<!-- localization module -->
<group ns="localization">
<!-- pose_estimator module -->
<group ns="pose_estimator">
<include file="$(find arg_localization)/launch/lidar_pose_estimator.launch">
<arg name="publish_tf" value="false"/>
</include>
</group>
<!-- Publish TF map -> base_link -->
<include file="$(find arg_localization)/launch/map_base_link_tf_broadcaster.launch">
<arg name="topic_name" value="/localization/pose_estimator/pose"/>
</include>
</group>
</group>
<!-- DevBot2.0 -->
<group ns="devbot">
<!-- DevBot description -->
<include file="$(find arg_devbot_description)/launch/DevBot2.launch" />
</group>
<!-- localization module -->
<group ns="localization">
<!-- pose_twist_fusion_filter module -->
<group ns="pose_twist_fusion_filter">
<include file="$(find ekf_localizer)/launch/ekf_localizer.launch" >
<!-- for Pose measurement -->
<arg name="input_pose_name" value="/localization/pose_estimator/pose"/> <!-- possibilities: /not_used /localization/pose_estimator/pose -->
<arg name="pose_rate" value="20.0"/>
<arg name="pose_stddev_x" value="1.025"/>
<arg name="pose_stddev_y" value="1.025"/>
<arg name="pose_stddev_yaw" value="0.25"/>
<!-- for twist measurement -->
<arg name="input_twist_name" value="/devbot/twist"/> <!-- possibilities: /not_used /devbot/twist -->
<arg name="twist_rate" value="200.0"/>
<arg name="twist_stddev_vx" value="0.1"/>
<arg name="twist_stddev_wz" value="0.1"/>
</include>
</group>
</group>
<!-- Pointcloud loader -->
<group ns="map">
<node if="$(eval pcd_activated)" pkg="map_loader" type="pointcloud_map_loader" name="pointcloud_map_loader" args="$(arg map_path)/pcd/pointcloud_map.pcd" >
<remap from="~output/pointcloud_map" to="/map/pointcloud_map" />
</node>
</group>
<!-- RVIZ -->
<node pkg="rviz" type="rviz" name="rviz_arg" respawn="true" output="log" args="-d $(find arg_demos)/config/rviz_lidar_localization.rviz" />
<!-- Set start position based on GPS data -->
<include file="$(find arg_localization)/launch/init_position.launch">
<arg name="set_initial_pose_manually" value="False"/>
</include>
<!-- Publish odom topic for (gps_pose, localization_pose and ekf_pose -->
<node pkg="arg_localization" type="odom_publisher.py" name="odom_publisher_gps" output="log" respawn="true">
<param name="input_pose" value="/gps_local/pose"/>
<param name="output_odom" value="/gps_local/odom"/>
</node>
<node pkg="arg_localization" type="odom_publisher.py" name="odom_publisher_localization" output="log" respawn="true">
<param name="input_pose" value="/localization/pose_estimator/pose"/>
<param name="output_odom" value="/localization/pose_estimator/odom"/>
</node>
<node pkg="arg_localization" type="odom_publisher.py" name="odom_publisher_ekf" output="log" respawn="true">
<param name="input_pose" value="/localization/pose_twist_fusion_filter/pose"/>
<param name="output_odom" value="/localization/pose_twist_fusion_filter/odom"/>
</node>
</launch>