feat: add gnss/imu localizer

autoware_launch/config/localization/eagleye_config.param.yaml

@@ -0,0 +1,177 @@
+/**: #GNSS cycle 5Hz, IMU cycle 50Hz.
+  ros__parameters:
+    # Estimate mode
+    use_gnss_mode: rtklib
+    use_can_less_mode: false
+
+    # Topic
+    twist:
+      twist_type: 1 # TwistStamped : 0, TwistWithCovarianceStamped: 1
+      twist_topic: /sensing/vehicle_velocity_converter/twist_with_covariance
+    imu_topic: /sensing/imu/imu_data
+    gnss:
+      velocity_source_type: 2 # rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, ublox_msgs/NavPVT: 2, geometry_msgs/TwistWithCovarianceStamped: 3
+      velocity_source_topic: /sensing/gnss/ublox/navpvt
+      llh_source_type: 2 # rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, sensor_msgs/NavSatFix: 2
+      llh_source_topic: /sensing/gnss/ublox/nav_sat_fix
+    sub_gnss:
+      llh_source_type: 2 # nmea_msgs/Sentence: 1, sensor_msgs/NavSatFix: 2
+      llh_source_topic:  /sensing/sub_gnss/ublox/nav_sat_fix
+
+    # TF
+    tf_gnss_frame:
+      parent: "base_link"
+      child: "gnss_link"
+
+    # Origin of GNSS coordinates (ECEF to ENU)
+    ecef_base_pos:
+      x: 0.0
+      y: 0.0
+      z: 0.0
+      use_ecef_base_position: false
+
+    reverse_imu_wz: true
+
+    # Navigation Parameters
+    # Basic Navigation Functions
+    common:
+      imu_rate: 50
+      gnss_rate: 5
+      stop_judgment_threshold: 0.01
+      slow_judgment_threshold: 0.278
+      moving_judgment_threshold: 2.78
+
+    velocity_scale_factor:
+      estimated_minimum_interval: 20
+      estimated_maximum_interval: 400
+      gnss_receiving_threshold: 0.25
+      velocity_scale_factor_save_str: /config/velocity_scale_factor.txt
+      save_velocity_scale_factor: false
+      velocity_scale_factor_save_duration: 100.0
+      th_velocity_scale_factor_percent: 10.0
+
+    yaw_rate_offset_stop:
+      estimated_interval: 3
+      outlier_threshold: 0.002
+
+    yaw_rate_offset:
+      estimated_minimum_interval: 30
+      gnss_receiving_threshold: 0.25
+      outlier_threshold: 0.002
+      1st:
+        estimated_maximum_interval: 300
+      2nd:
+        estimated_maximum_interval: 500
+
+    heading:
+      estimated_minimum_interval: 10
+      estimated_maximum_interval: 30
+      gnss_receiving_threshold: 0.25
+      outlier_threshold: 0.0524
+      outlier_ratio_threshold: 0.5
+      curve_judgment_threshold: 0.0873
+      init_STD: 0.0035 #[rad] (= 0.2 [deg])
+
+    heading_interpolate:
+      sync_search_period: 2
+      proc_noise: 0.0005 #[rad] (= 0.03 [deg])
+
+    slip_angle:
+      manual_coefficient: 0.0
+
+    slip_coefficient:
+      estimated_minimum_interval: 2
+      estimated_maximum_interval: 100
+      curve_judgment_threshold: 0.017453
+      lever_arm: 0.0
+
+    rolling:
+      filter_process_noise: 0.01
+      filter_observation_noise: 1
+
+    trajectory:
+      curve_judgment_threshold: 0.017453
+      timer_update_rate: 10
+      deadlock_threshold: 1
+      sensor_noise_velocity: 0.05
+      sensor_scale_noise_velocity: 0.02
+      sensor_noise_yaw_rate: 0.01
+      sensor_bias_noise_yaw_rate: 0.1
+
+    smoothing:
+      moving_average_time: 3
+      moving_ratio_threshold: 0.1
+
+    height:
+      estimated_minimum_interval: 200
+      estimated_maximum_interval: 2000
+      update_distance: 0.1
+      gnss_receiving_threshold: 0.1
+      outlier_threshold: 0.3
+      outlier_ratio_threshold: 0.5
+      moving_average_time: 1
+
+    position:
+      estimated_interval: 300
+      update_distance: 0.1
+      outlier_threshold: 3.0
+      gnss_receiving_threshold: 0.25
+      outlier_ratio_threshold: 0.5
+
+    position_interpolate:
+      sync_search_period: 2
+
+    monitor:
+      print_status: true
+      log_output_status: false
+      use_compare_yaw_rate: false
+      comparison_twist_topic: /calculated_twist
+      th_diff_rad_per_sec: 0.17453
+      th_num_continuous_abnormal_yaw_rate: 25
+
+    # Optional Navigation Functions
+    angular_velocity_offset_stop:
+      estimated_interval: 4
+      outlier_threshold: 0.002
+
+    rtk_dead_reckoning:
+      rtk_fix_STD: 0.3 #[m]
+      proc_noise: 0.05 #[m]
+
+    rtk_heading:
+      update_distance: 0.3
+      estimated_minimum_interval: 10
+      estimated_maximum_interval: 30
+      gnss_receiving_threshold: 0.25
+      outlier_threshold: 0.0524
+      outlier_ratio_threshold: 0.5
+      curve_judgment_threshold: 0.0873
+
+    enable_additional_rolling:
+      update_distance: 0.3
+      moving_average_time: 1
+      sync_judgment_threshold: 0.01
+      sync_search_period: 1
+
+    velocity_estimator:
+      gga_downsample_time: 0.5
+      stop_judgment_velocity_threshold: 0.2
+      stop_judgment_interval: 1
+      variance_threshold: 0.000025
+      pitch_rate_offset:
+        estimated_interval: 8
+      pitching:
+        estimated_interval: 5
+        outlier_threshold: 0.0174
+        gnss_receiving_threshold: 0.2
+        outlier_ratio_threshold: 0.5
+      acceleration_offset:
+        estimated_minimum_interval: 30
+        estimated_maximum_interval: 500
+        filter_process_noise: 0.01
+        filter_observation_noise: 1
+      doppler_fusion:
+        estimated_interval: 4
+        gnss_receiving_threshold: 0.2
+        outlier_ratio_threshold: 0.5
+        outlier_threshold: 0.1

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

@@ -0,0 +1,6 @@
+/**:
+  ros__parameters:
+    gnss_enabled: true
+    ndt_enabled: false
+    ekf_enabled: true
+    stop_check_enabled: false

autoware_launch/launch/autoware.launch.xml

@@ -84,6 +84,7 @@
   <!-- Localization -->
   <group if="$(var launch_localization)">
     <include file="$(find-pkg-share autoware_launch)/launch/components/tier4_localization_component.launch.xml"/>
+    <!-- <include file="$(find-pkg-share autoware_launch)/launch/components/map4_localization_component.launch.xml"/> -->
   </group>
 
   <!-- Perception -->

autoware_launch/launch/components/map4_localization_component.launch.xml

@@ -0,0 +1,45 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="pose_estimator_mode" default="lidar" description="select pose_estimator mode. lidar, gnss"/>
+
+  <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;)"/>
+
+  <group if="$(eval &quot;'$(var pose_estimator_mode)'=='lidar'&quot;)">
+    <let
+      name="pose_initializer_param_path"
+      value="$(find-pkg-share autoware_launch)/config/localization/pose_initializer.logging_simulator.lidar.param.yaml"
+      if="$(eval &quot;'$(var system_run_mode)'=='logging_simulation'&quot;)"
+    />
+
+    <include file="$(find-pkg-share map4_localization_launch)/eagleye_twist_localization_launch/localization.launch.xml">
+      <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+      <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
+
+      <arg name="crop_box_filter_measurement_range_param_path" value="$(find-pkg-share autoware_launch)/config/localization/crop_box_filter_measurement_range.param.yaml"/>
+      <arg name="voxel_grid_downsample_filter_param_path" value="$(find-pkg-share autoware_launch)/config/localization/voxel_grid_filter.param.yaml"/>
+      <arg name="random_downsample_filter_param_path" value="$(find-pkg-share autoware_launch)/config/localization/random_downsample_filter.param.yaml"/>
+      <arg name="ndt_scan_matcher_param_path" value="$(find-pkg-share autoware_launch)/config/localization/ndt_scan_matcher.param.yaml"/>
+      <arg name="eagleye_param_path" value="$(find-pkg-share autoware_launch)/config/localization/eagleye_config.param.yaml"/>
+      <arg name="localization_error_monitor_param_path" value="$(find-pkg-share autoware_launch)/config/localization/localization_error_monitor.param.yaml"/>
+      <arg name="ekf_localizer_param_path" value="$(find-pkg-share autoware_launch)/config/localization/ekf_localizer.param.yaml"/>
+      <arg name="pose_initializer_common_param_path" value="$(find-pkg-share autoware_launch)/config/localization/pose_initializer_common.param.yaml"/>
+      <arg name="pose_initializer_param_path" value="$(var pose_initializer_param_path)"/>
+    </include>
+  </group>
+
+  <group if="$(eval &quot;'$(var pose_estimator_mode)'=='gnss'&quot;)">
+    <let
+      name="pose_initializer_param_path"
+      value="$(find-pkg-share autoware_launch)/config/localization/pose_initializer.logging_simulator.gnss.param.yaml"
+      if="$(eval &quot;'$(var system_run_mode)'=='logging_simulation'&quot;)"
+    />
+
+    <include file="$(find-pkg-share map4_localization_launch)/eagleye_pose_twist_localization_launch/localization.launch.xml">
+      <arg name="eagleye_param_path" value="$(find-pkg-share autoware_launch)/config/localization/eagleye_config.param.yaml"/>
+      <arg name="localization_error_monitor_param_path" value="$(find-pkg-share autoware_launch)/config/localization/localization_error_monitor.param.yaml"/>
+      <arg name="ekf_localizer_param_path" value="$(find-pkg-share autoware_launch)/config/localization/ekf_localizer.param.yaml"/>
+      <arg name="pose_initializer_common_param_path" value="$(find-pkg-share autoware_launch)/config/localization/pose_initializer_common.param.yaml"/>
+      <arg name="pose_initializer_param_path" value="$(var pose_initializer_param_path)"/>
+    </include>
+  </group>
+</launch>

autoware_launch/launch/components/tier4_localization_component.launch.xml

@@ -3,7 +3,7 @@
   <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;)"/>
   <let
     name="pose_initializer_param_path"
-    value="$(find-pkg-share autoware_launch)/config/localization/pose_initializer.logging_simulator.param.yaml"
+    value="$(find-pkg-share autoware_launch)/config/localization/pose_initializer.logging_simulator.lidar.param.yaml"
     if="$(eval &quot;'$(var system_run_mode)'=='logging_simulation'&quot;)"
   />