-
Notifications
You must be signed in to change notification settings - Fork 15
/
localiser.launch
158 lines (135 loc) · 6.47 KB
/
localiser.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
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
<?xml version="1.0"?>
<!--
Start the localisation node
All the values in this file are the same as the default values.
-->
<launch>
<!-- Overridable arguments for some common arguments -->
<arg name="publish_particles" default="false" />
<arg name="internal_laser" default="false" />
<arg name="motion_model_alpha_1" default="0.05" />
<arg name="motion_model_alpha_2" default="0.1" />
<arg name="motion_model_alpha_3" default="0.02" />
<arg name="motion_model_alpha_4" default="0.05" />
<arg name="particle_filter_kld_epsilon" default="0.05" />
<arg name="particle_filter_kld_z" default="0.95" />
<arg name="likelihood_sigma_hit" default="0.1" />
<arg name="fixed_frame" default="map" />
<arg name="localised_frame" default="base_link" />
<arg name="odom_frame" default="odom" />
<arg name="post_date_transform" default="0.1" />
<arg name="output" default="screen" />
<node name="quickmcl_node"
pkg="quickmcl"
type="node"
output="$(arg output)">
<!-- TF frame to work in, map expected to be in this frame. -->
<param name="fixed_frame" type="string" value="$(arg fixed_frame)" />
<!-- TF frame to localise. -->
<param name="localised_frame" type="string" value="$(arg localised_frame)" />
<!-- TF frame to publish transform for (from fixed frame). -->
<param name="odom_frame" type="string" value="$(arg odom_frame)" />
<!-- How much to post-date transform, should be similar to delta between
scans. Helps rviz get less glitchy. -->
<param name="post_date_transform" type="double" value="$(arg post_date_transform)" />
<!-- If true, publish markers as a particle cloud to -->
<param name="publish_particles" type="bool" value="$(arg publish_particles)" />
<!-- If true, process /scan internally instead of relying on /cloud -->
<param name="internal_laser_processing"
type="bool"
value="$(arg internal_laser)" />
<!-- Example remaps, commented out so they can be remapped on a group level
when including this file. -->
<!-- Topics used for inputs -->
<!-- Laser point cloud (sensor_msgs/PointCloud2)
(used if internal_laser_processing = False) -->
<!-- <remap from="cloud" to="cloud" /> -->
<!-- Laser scan (sensor_msgs/LaserScan)
(used if internal_laser_processing = True) -->
<!-- <remap from="scan" to="scan" /> -->
<!-- Map (nav_msgs/OccupancyGrid) -->
<!-- <remap from="map" to="map" /> -->
<!-- Pose reset from rviz (geometry_msgs/PoseWithCovarianceStamped) -->
<!-- <remap from="initialpose" to="initialpose" /> -->
<!-- Name of topics to publish to, for visualisation & debugging-->
<!-- <remap from="particles" to="particles" />
<remap from="likelihood_map" to="likelihood_map" />
<remap from="quickmcl_pose" to="quickmcl_pose" /> -->
<!-- Parameters for motion model -->
<!-- Expected noise in odometry: Rotation noise from rotation -->
<param name="motion_model_alpha_1"
type="double"
value="$(arg motion_model_alpha_1)" />
<!-- Expected noise in odometry: Rotation noise from translation -->
<param name="motion_model_alpha_2"
type="double"
value="$(arg motion_model_alpha_2)" />
<!-- Expected noise in odometry: Translation noise from translation -->
<param name="motion_model_alpha_3"
type="double"
value="$(arg motion_model_alpha_3)" />
<!-- Expected noise in odometry: Translation noise from rotation -->
<param name="motion_model_alpha_4"
type="double"
value="$(arg motion_model_alpha_4)" />
<!-- Minimum translation for filter to update [m] -->
<param name="motion_model_min_trans" type="double" value="0.2" />
<!-- Minimum rotation for filter to update [rad] -->
<param name="motion_model_min_rot"
type="double"
value="0.5235987755982988" />
<!-- Parameters for particle filter -->
<!-- Number of particles (min) -->
<param name="particle_filter_particle_count_min" type="int" value="100" />
<!-- Number of particles (max) -->
<param name="particle_filter_particle_count_max" type="int" value="5000" />
<!-- How often (in filter updates) to resample -->
<param name="particle_filter_resample_count" type="int" value="2" />
<!-- Resample type: low_variance, adaptive, kld are valid
Note:
When using low_variance, only the min number of particles are used,
and alpha_fast/alpha_slow are not used.
-->
<param name="particle_filter_resample_type"
type="string"
value="kld" />
<!-- Low pass constant for adaptive/kld.
Set both fast and slow to 0 to disable adaptive sampling. -->
<param name="particle_filter_alpha_fast" type="double" value="0.0" />
<!-- Low pass constant for adaptive/kld.
Set both fast and slow to 0 to disable adaptive sampling. -->
<param name="particle_filter_alpha_slow" type="double" value="0.0" />
<!-- KLD epsilon parameter -->
<param name="particle_filter_kld_epsilon"
type="double"
value="$(arg particle_filter_kld_epsilon)" />
<!-- KLD z_(1-\delta) parameter -->
<param name="particle_filter_kld_z"
type="double"
value="$(arg particle_filter_kld_z)" />
<!-- KLD/clustering bucket resolution x and y [m] -->
<param name="space_partitioning_resolution_xy"
type="double"
value="0.5" />
<!-- KLD/clustering bucket resolution rotation [rad] -->
<param name="space_partitioning_resolution_theta"
type="double"
value="0.17453292519943295" />
<!-- Parameters for sensor model (likelihood map) -->
<!-- These z-values should add up to 1, being "mixing factors" -->
<param name="likelihood_z_hit" type="double" value="0.9" />
<param name="likelihood_z_rand" type="double" value="0.1" />
<!-- Maximum laser distance [m] -->
<param name="likelihood_max_laser_distance" type="double" value="14.0" />
<!-- Maximum distance to compute obstacle distances for [m] -->
<param name="likelihood_max_obstacle_distance" type="double" value="2.0" />
<!-- Number of laser beams to use from the scan. This is a sub-sampling.
Set to 0 to use all (not recommended).
-->
<param name="likelihood_num_beams" type="int" value="30" />
<!-- Sigma of hit distribution [m] -->
<param name="likelihood_sigma_hit"
type="double"
value="$(arg likelihood_sigma_hit)" />
</node>
</launch>