Skip to content

Commit 61ba0b4

Browse files
rsasaki0109rsasaki0109
authored
feat: add gnss/imu localizer (#200)
* Add gnss_imu_localizar Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp> * Eagleye parameter update fot sample data Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp> * Restore perception/planning/control parameters in lsim Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp> * Change use_gnss_mode Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp> * Fix spell Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp> * Fix spell Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp> * Delete unnecessary white spaces Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp> * Remove unnecessary trailing spaces Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp> * Prettier format Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp> * prettier format Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp> * clang-format Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp> * Revert "clang-format" This reverts commit 46cd907. Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp> * Rename GNSS/Lidar localization switching parameters Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp> * Remove conditional branching by pose_estimatar_mode in system_error_monitor Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp> * Change launch directory structure Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp> * Delete unnecessary parameters and files Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp> * Integrate map4_localization_component1,2 Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp> * Fix comment out in localization launch Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp> --------- Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>
1 parent 40568d9 commit 61ba0b4

6 files changed

+230
-1
lines changed

‎autoware_launch/config/localization/eagleye_config.param.yaml

+177
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,177 @@
1+
/**: #GNSS cycle 5Hz, IMU cycle 50Hz.
2+
ros__parameters:
3+
# Estimate mode
4+
use_gnss_mode: rtklib
5+
use_can_less_mode: false
6+
7+
# Topic
8+
twist:
9+
twist_type: 1 # TwistStamped : 0, TwistWithCovarianceStamped: 1
10+
twist_topic: /sensing/vehicle_velocity_converter/twist_with_covariance
11+
imu_topic: /sensing/imu/imu_data
12+
gnss:
13+
velocity_source_type: 2 # rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, ublox_msgs/NavPVT: 2, geometry_msgs/TwistWithCovarianceStamped: 3
14+
velocity_source_topic: /sensing/gnss/ublox/navpvt
15+
llh_source_type: 2 # rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, sensor_msgs/NavSatFix: 2
16+
llh_source_topic: /sensing/gnss/ublox/nav_sat_fix
17+
sub_gnss:
18+
llh_source_type: 2 # nmea_msgs/Sentence: 1, sensor_msgs/NavSatFix: 2
19+
llh_source_topic: /sensing/sub_gnss/ublox/nav_sat_fix
20+
21+
# TF
22+
tf_gnss_frame:
23+
parent: "base_link"
24+
child: "gnss_link"
25+
26+
# Origin of GNSS coordinates (ECEF to ENU)
27+
ecef_base_pos:
28+
x: 0.0
29+
y: 0.0
30+
z: 0.0
31+
use_ecef_base_position: false
32+
33+
reverse_imu_wz: true
34+
35+
# Navigation Parameters
36+
# Basic Navigation Functions
37+
common:
38+
imu_rate: 50
39+
gnss_rate: 5
40+
stop_judgment_threshold: 0.01
41+
slow_judgment_threshold: 0.278
42+
moving_judgment_threshold: 2.78
43+
44+
velocity_scale_factor:
45+
estimated_minimum_interval: 20
46+
estimated_maximum_interval: 400
47+
gnss_receiving_threshold: 0.25
48+
velocity_scale_factor_save_str: /config/velocity_scale_factor.txt
49+
save_velocity_scale_factor: false
50+
velocity_scale_factor_save_duration: 100.0
51+
th_velocity_scale_factor_percent: 10.0
52+
53+
yaw_rate_offset_stop:
54+
estimated_interval: 3
55+
outlier_threshold: 0.002
56+
57+
yaw_rate_offset:
58+
estimated_minimum_interval: 30
59+
gnss_receiving_threshold: 0.25
60+
outlier_threshold: 0.002
61+
1st:
62+
estimated_maximum_interval: 300
63+
2nd:
64+
estimated_maximum_interval: 500
65+
66+
heading:
67+
estimated_minimum_interval: 10
68+
estimated_maximum_interval: 30
69+
gnss_receiving_threshold: 0.25
70+
outlier_threshold: 0.0524
71+
outlier_ratio_threshold: 0.5
72+
curve_judgment_threshold: 0.0873
73+
init_STD: 0.0035 #[rad] (= 0.2 [deg])
74+
75+
heading_interpolate:
76+
sync_search_period: 2
77+
proc_noise: 0.0005 #[rad] (= 0.03 [deg])
78+
79+
slip_angle:
80+
manual_coefficient: 0.0
81+
82+
slip_coefficient:
83+
estimated_minimum_interval: 2
84+
estimated_maximum_interval: 100
85+
curve_judgment_threshold: 0.017453
86+
lever_arm: 0.0
87+
88+
rolling:
89+
filter_process_noise: 0.01
90+
filter_observation_noise: 1
91+
92+
trajectory:
93+
curve_judgment_threshold: 0.017453
94+
timer_update_rate: 10
95+
deadlock_threshold: 1
96+
sensor_noise_velocity: 0.05
97+
sensor_scale_noise_velocity: 0.02
98+
sensor_noise_yaw_rate: 0.01
99+
sensor_bias_noise_yaw_rate: 0.1
100+
101+
smoothing:
102+
moving_average_time: 3
103+
moving_ratio_threshold: 0.1
104+
105+
height:
106+
estimated_minimum_interval: 200
107+
estimated_maximum_interval: 2000
108+
update_distance: 0.1
109+
gnss_receiving_threshold: 0.1
110+
outlier_threshold: 0.3
111+
outlier_ratio_threshold: 0.5
112+
moving_average_time: 1
113+
114+
position:
115+
estimated_interval: 300
116+
update_distance: 0.1
117+
outlier_threshold: 3.0
118+
gnss_receiving_threshold: 0.25
119+
outlier_ratio_threshold: 0.5
120+
121+
position_interpolate:
122+
sync_search_period: 2
123+
124+
monitor:
125+
print_status: true
126+
log_output_status: false
127+
use_compare_yaw_rate: false
128+
comparison_twist_topic: /calculated_twist
129+
th_diff_rad_per_sec: 0.17453
130+
th_num_continuous_abnormal_yaw_rate: 25
131+
132+
# Optional Navigation Functions
133+
angular_velocity_offset_stop:
134+
estimated_interval: 4
135+
outlier_threshold: 0.002
136+
137+
rtk_dead_reckoning:
138+
rtk_fix_STD: 0.3 #[m]
139+
proc_noise: 0.05 #[m]
140+
141+
rtk_heading:
142+
update_distance: 0.3
143+
estimated_minimum_interval: 10
144+
estimated_maximum_interval: 30
145+
gnss_receiving_threshold: 0.25
146+
outlier_threshold: 0.0524
147+
outlier_ratio_threshold: 0.5
148+
curve_judgment_threshold: 0.0873
149+
150+
enable_additional_rolling:
151+
update_distance: 0.3
152+
moving_average_time: 1
153+
sync_judgment_threshold: 0.01
154+
sync_search_period: 1
155+
156+
velocity_estimator:
157+
gga_downsample_time: 0.5
158+
stop_judgment_velocity_threshold: 0.2
159+
stop_judgment_interval: 1
160+
variance_threshold: 0.000025
161+
pitch_rate_offset:
162+
estimated_interval: 8
163+
pitching:
164+
estimated_interval: 5
165+
outlier_threshold: 0.0174
166+
gnss_receiving_threshold: 0.2
167+
outlier_ratio_threshold: 0.5
168+
acceleration_offset:
169+
estimated_minimum_interval: 30
170+
estimated_maximum_interval: 500
171+
filter_process_noise: 0.01
172+
filter_observation_noise: 1
173+
doppler_fusion:
174+
estimated_interval: 4
175+
gnss_receiving_threshold: 0.2
176+
outlier_ratio_threshold: 0.5
177+
outlier_threshold: 0.1

‎autoware_launch/config/localization/pose_initializer.logging_simulator.gnss.param.yaml

+6
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
/**:
2+
ros__parameters:
3+
gnss_enabled: true
4+
ndt_enabled: false
5+
ekf_enabled: true
6+
stop_check_enabled: false

‎autoware_launch/config/localization/pose_initializer.logging_simulator.param.yaml ‎autoware_launch/config/localization/pose_initializer.logging_simulator.lidar.param.yaml

File renamed without changes.

‎autoware_launch/launch/autoware.launch.xml

+1
Original file line numberDiff line numberDiff line change
@@ -84,6 +84,7 @@
8484
<!-- Localization -->
8585
<group if="$(var launch_localization)">
8686
<include file="$(find-pkg-share autoware_launch)/launch/components/tier4_localization_component.launch.xml"/>
87+
<!-- <include file="$(find-pkg-share autoware_launch)/launch/components/map4_localization_component.launch.xml"/> -->
8788
</group>
8889

8990
<!-- Perception -->

‎autoware_launch/launch/components/map4_localization_component.launch.xml

+45
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
<?xml version="1.0"?>
2+
<launch>
3+
<arg name="pose_estimator_mode" default="lidar" description="select pose_estimator mode. lidar, gnss"/>
4+
5+
<let name="pose_initializer_param_path" value="$(find-pkg-share autoware_launch)/config/localization/pose_initializer.param.yaml" if="$(eval &quot;'$(var system_run_mode)'=='online'&quot;)"/>
6+
7+
<group if="$(eval &quot;'$(var pose_estimator_mode)'=='lidar'&quot;)">
8+
<let
9+
name="pose_initializer_param_path"
10+
value="$(find-pkg-share autoware_launch)/config/localization/pose_initializer.logging_simulator.lidar.param.yaml"
11+
if="$(eval &quot;'$(var system_run_mode)'=='logging_simulation'&quot;)"
12+
/>
13+
14+
<include file="$(find-pkg-share map4_localization_launch)/eagleye_twist_localization_launch/localization.launch.xml">
15+
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
16+
<arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
17+
18+
<arg name="crop_box_filter_measurement_range_param_path" value="$(find-pkg-share autoware_launch)/config/localization/crop_box_filter_measurement_range.param.yaml"/>
19+
<arg name="voxel_grid_downsample_filter_param_path" value="$(find-pkg-share autoware_launch)/config/localization/voxel_grid_filter.param.yaml"/>
20+
<arg name="random_downsample_filter_param_path" value="$(find-pkg-share autoware_launch)/config/localization/random_downsample_filter.param.yaml"/>
21+
<arg name="ndt_scan_matcher_param_path" value="$(find-pkg-share autoware_launch)/config/localization/ndt_scan_matcher.param.yaml"/>
22+
<arg name="eagleye_param_path" value="$(find-pkg-share autoware_launch)/config/localization/eagleye_config.param.yaml"/>
23+
<arg name="localization_error_monitor_param_path" value="$(find-pkg-share autoware_launch)/config/localization/localization_error_monitor.param.yaml"/>
24+
<arg name="ekf_localizer_param_path" value="$(find-pkg-share autoware_launch)/config/localization/ekf_localizer.param.yaml"/>
25+
<arg name="pose_initializer_common_param_path" value="$(find-pkg-share autoware_launch)/config/localization/pose_initializer_common.param.yaml"/>
26+
<arg name="pose_initializer_param_path" value="$(var pose_initializer_param_path)"/>
27+
</include>
28+
</group>
29+
30+
<group if="$(eval &quot;'$(var pose_estimator_mode)'=='gnss'&quot;)">
31+
<let
32+
name="pose_initializer_param_path"
33+
value="$(find-pkg-share autoware_launch)/config/localization/pose_initializer.logging_simulator.gnss.param.yaml"
34+
if="$(eval &quot;'$(var system_run_mode)'=='logging_simulation'&quot;)"
35+
/>
36+
37+
<include file="$(find-pkg-share map4_localization_launch)/eagleye_pose_twist_localization_launch/localization.launch.xml">
38+
<arg name="eagleye_param_path" value="$(find-pkg-share autoware_launch)/config/localization/eagleye_config.param.yaml"/>
39+
<arg name="localization_error_monitor_param_path" value="$(find-pkg-share autoware_launch)/config/localization/localization_error_monitor.param.yaml"/>
40+
<arg name="ekf_localizer_param_path" value="$(find-pkg-share autoware_launch)/config/localization/ekf_localizer.param.yaml"/>
41+
<arg name="pose_initializer_common_param_path" value="$(find-pkg-share autoware_launch)/config/localization/pose_initializer_common.param.yaml"/>
42+
<arg name="pose_initializer_param_path" value="$(var pose_initializer_param_path)"/>
43+
</include>
44+
</group>
45+
</launch>

‎autoware_launch/launch/components/tier4_localization_component.launch.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
<let name="pose_initializer_param_path" value="$(find-pkg-share autoware_launch)/config/localization/pose_initializer.param.yaml" if="$(eval &quot;'$(var system_run_mode)'=='online'&quot;)"/>
44
<let
55
name="pose_initializer_param_path"
6-
value="$(find-pkg-share autoware_launch)/config/localization/pose_initializer.logging_simulator.param.yaml"
6+
value="$(find-pkg-share autoware_launch)/config/localization/pose_initializer.logging_simulator.lidar.param.yaml"
77
if="$(eval &quot;'$(var system_run_mode)'=='logging_simulation'&quot;)"
88
/>
99

0 commit comments

Comments
 (0)