Skip to content

Commit a86f7a3

Browse files
fmricofmrico
authored
Merge pull request #59 from midemig/jazzy
Port from rolling to jazzy of fusion_localizer
2 parents 3ceec0b + 76f1f93 commit a86f7a3

13 files changed

Lines changed: 833 additions & 103 deletions

File tree

‎localizers/easynav_fusion_localizer/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -72,6 +72,10 @@ install(TARGETS
7272
RUNTIME DESTINATION lib/${PROJECT_NAME}
7373
)
7474

75+
install(DIRECTORY maps/
76+
DESTINATION share/${PROJECT_NAME}/maps
77+
)
78+
7579
if(BUILD_TESTING)
7680
find_package(ament_lint_auto REQUIRED)
7781
set(ament_cmake_copyright_FOUND TRUE)

‎localizers/easynav_fusion_localizer/config/example_params.yaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ localizer_node:
2525
latitude_origin: 40.2834931
2626
longitude_origin: -3.8207253999999997
2727
altitude_origin: 746.184
28-
local_filter:
28+
global_filter:
2929
two_d_mode: true
3030
publish_tf: true
3131

‎localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml‎

Lines changed: 280 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,280 @@
1+
controller_node:
2+
ros__parameters:
3+
use_sim_time: true
4+
controller_types: [serest]
5+
dummy:
6+
rt_freq: 30.0
7+
plugin: easynav_controller/DummyController
8+
cycle_time_nort: 0.01
9+
cycle_time_rt: 0.0001
10+
mppi:
11+
rt_freq: 30.0
12+
plugin: easynav_mppi_controller/MPPIController
13+
num_samples: 200
14+
horizon_steps: 5
15+
dt: 0.15
16+
lambda: 1.0
17+
fov: 1.57
18+
max_linear_velocity: 1.5
19+
max_angular_velocity: 1.0
20+
max_linear_acceleration: 0.5
21+
max_angular_acceleration: 0.5
22+
safety_radius: 0.3
23+
serest:
24+
rt_freq: 30.0
25+
plugin: easynav_serest_controller/SerestController
26+
allow_reverse: true
27+
max_linear_speed: 0.8
28+
max_angular_speed: 1.2
29+
v_progress_min: 0.08 # 8 cm/s de crucero mínimo cuando alineado
30+
k_s_share_max: 0.5 # succión lateral no cancela >50% del avance
31+
k_theta: 2.5 # sube un poco
32+
k_y: 1.5
33+
goal_pos_tol: 0.1 # 7 cm
34+
goal_yaw_tol_deg: 6.0 # 6 grados
35+
slow_radius: 0.60
36+
slow_min_speed: 0.03
37+
final_align_k: 2.0
38+
final_align_wmax: 0.6
39+
corner_guard_enable: true
40+
corner_gain_ey: 1.8
41+
corner_gain_eth: 0.7
42+
corner_gain_kappa: 0.4
43+
corner_min_alpha: 0.35
44+
corner_boost_omega: 1.0
45+
a_lat_soft: 0.9
46+
apex_ey_des: 0.05
47+
48+
localizer_node:
49+
ros__parameters:
50+
use_sim_time: true
51+
localizer_types: [Ukf]
52+
dummy:
53+
rt_freq: 30.0
54+
plugin: easynav_localizer/DummyLocalizer
55+
cycle_time_nort: 0.01
56+
cycle_time_rt: 0.001
57+
Ukf:
58+
rt_freq: 50.0
59+
freq: 5.0
60+
reseed_freq: 0.1
61+
plugin: easynav_fusion_localizer/FusionLocalizer
62+
latitude_origin: 40.2834931
63+
longitude_origin: -3.8207253999999997
64+
altitude_origin: 746.184
65+
local_filter:
66+
two_d_mode: true
67+
publish_tf: true
68+
69+
# --- Reference Frames ---
70+
map_frame: map
71+
odom_frame: odom
72+
base_link_frame: base_link
73+
world_frame: odom
74+
75+
dynamic_process_noise_covariance: true
76+
77+
#--- INPUT 2: Wheel Odometry ---
78+
odom0: /robotnik_base_control/odom
79+
odom0_config:
80+
[true, true, false,
81+
false, false, true,
82+
true, true, false,
83+
false, false, true,
84+
false, false, false]
85+
odom0_differential: false
86+
87+
# --- INPUT 3: IMU ---
88+
# imu0: /imu/data
89+
# imu0_config:
90+
# [false, false, false,
91+
# false, false, false,
92+
# false, false, false,
93+
# false, false, true,
94+
# true, true, true]
95+
# imu0_differential: false
96+
# imu0_remove_gravitational_acceleration: true
97+
98+
# twist0: /gps/fix_velocity
99+
# twist0_config:
100+
# [false, false, false,
101+
# false, false, false,
102+
# true, false, false,
103+
# false, false, false,
104+
# false, false, false]
105+
# twist0_differential: false
106+
107+
# --- Process Variances ---
108+
process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
109+
0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
110+
0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
111+
0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
112+
0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
113+
0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
114+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
115+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
116+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
117+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
118+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0,
119+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0,
120+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
121+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
122+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015]
123+
global_filter:
124+
two_d_mode: true
125+
publish_tf: true
126+
debug: true
127+
debug_out_file: /tmp/ukf_global_debug.txt
128+
transform_timeout: 0.1
129+
smooth_lagged_data: true
130+
# Defines the time window in seconds to keep past states in memory.
131+
# history_length: 5.0
132+
dynamic_process_noise_covariance: true
133+
134+
# --- Reference Frames ---
135+
map_frame: map
136+
odom_frame: odom
137+
base_link_frame: base_link
138+
world_frame: map
139+
140+
# --- INPUT 1: GPS Odometry ---
141+
gps0: /gps/fix
142+
gps0_config:
143+
[true, true, false,
144+
false, false, false,
145+
false, false, false,
146+
false, false, false,
147+
false, false, false]
148+
gps0_differential: false
149+
150+
# --- INPUT 2: Wheel Odometry ---
151+
odom0: /robotnik_base_control/odom
152+
odom0_config:
153+
[false, false, false,
154+
false, false, false,
155+
true, true, false,
156+
false, false, true,
157+
false, false, false]
158+
odom0_differential: false
159+
160+
# --- INPUT 3: IMU ---
161+
imu0: /imu/data
162+
imu0_config:
163+
[false, false, false,
164+
false, false, false,
165+
false, false, false,
166+
false, false, true,
167+
false, false, false]
168+
imu0_differential: false
169+
imu0_remove_gravitational_acceleration: true
170+
171+
# twist0: /gps/fix_velocity
172+
# twist0_config:
173+
# [false, false, false,
174+
# false, false, false,
175+
# true, true, false,
176+
# false, false, false,
177+
# false, false, false]
178+
# twist0_differential: false
179+
180+
# --- Process Variances ---
181+
process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
182+
0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
183+
0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
184+
0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
185+
0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
186+
0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
187+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
188+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
189+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
190+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
191+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0,
192+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0,
193+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
194+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
195+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015]
196+
197+
initial_estimate_covariance: [
198+
5.0, 5.0, 1e-3, 1e-3, 1e-3, 1.0,
199+
0.5, 0.5, 1e-3, 1e-3, 1e-3, 0.1,
200+
0.1, 0.1, 0.1
201+
]
202+
203+
204+
maps_manager_node:
205+
ros__parameters:
206+
use_sim_time: true
207+
map_types: [costmap]
208+
routes:
209+
freq: 10.0
210+
plugin: easynav_routes_maps_manager/RoutesMapsManager
211+
filters: [routes_costmap]
212+
package: easynav_fusion_localizer
213+
map_path_file: routes/simple_square.yaml
214+
routes_costmap:
215+
plugin: easynav_routes_maps_manager/RoutesCostmapFilter
216+
min_cost: 50
217+
route_width: 0.5
218+
cycle_time_nort: 0.001
219+
cycle_time_rt: 0.001
220+
costmap:
221+
freq: 10.0
222+
plugin: easynav_costmap_maps_manager/CostmapMapsManager
223+
package: easynav_fusion_localizer
224+
map_path_file: maps/square.yaml
225+
filters: [obstacles, inflation]
226+
obstacles:
227+
plugin: easynav_costmap_maps_manager/CostmapMapsManager/ObstaclesFilter
228+
inflation:
229+
plugin: easynav_costmap_maps_manager/CostmapMapsManager/InflationFilter
230+
inflation_radius: 1.3
231+
inscribed_radius: 0.25
232+
cost_scaling_factor: 3.0
233+
dummy:
234+
freq: 10.0
235+
plugin: easynav_maps_manager/DummyMapsManager
236+
cycle_time_nort: 0.1
237+
cycle_time_rt: 0.0001
238+
239+
planner_node:
240+
ros__parameters:
241+
use_sim_time: true
242+
planner_types: [costmap]
243+
dummy:
244+
freq: 1.0
245+
plugin: easynav_planner/DummyPlanner
246+
cycle_time_nort: 0.2
247+
cycle_time_rt: 0.0001
248+
costmap:
249+
freq: 10.0
250+
plugin: easynav_costmap_planner/CostmapPlanner
251+
cost_factor: 10.0
252+
continuous_replan: true
253+
simple:
254+
freq: 0.5
255+
plugin: easynav_simple_planner/SimplePlanner
256+
robot_radius: 0.3
257+
258+
sensors_node:
259+
ros__parameters:
260+
use_sim_time: true
261+
forget_time: 0.5
262+
sensors: [imu, gps]
263+
perception_default_frame: odom
264+
imu:
265+
topic: /imu/data
266+
type: sensor_msgs/msg/Imu
267+
gps:
268+
topic: /gps/fix
269+
type: sensor_msgs/msg/NavSatFix
270+
laser1:
271+
topic: /front_laser/points
272+
type: sensor_msgs/msg/PointCloud2
273+
group: points
274+
275+
276+
system_node:
277+
ros__parameters:
278+
use_sim_time: true
279+
position_tolerance: 0.75
280+
angle_tolerance: 0.25

0 commit comments

Comments
 (0)