Merge pull request #86 from tier4/sync-awf-latest-xx1

chore: sync awf-latest-xx1
tier4 · Apr 27, 2023 · 5c85be7 · 5c85be7
2 parents 2761044 + 5910b08
commit 5c85be7
Show file tree

Hide file tree

Showing 19 changed files with 363 additions and 38 deletions.
diff --git a/autoware_launch/config/localization/eagleye_config.param.yaml b/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
diff --git a/autoware_launch/config/localization/pose_initializer.logging_simulator.gnss.param.yaml b/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
diff --git a/..._initializer.logging_simulator.param.yaml → ...alizer.logging_simulator.lidar.param.yaml b/..._initializer.logging_simulator.param.yaml → ...alizer.logging_simulator.lidar.param.yaml
diff --git a/autoware_launch/config/map/lanelet2_map_loader.param.yaml b/autoware_launch/config/map/lanelet2_map_loader.param.yaml
@@ -1,6 +1,6 @@
 /**:
   ros__parameters:
-    lanelet2_map_projector_type: MGRS   # Options: MGRS, UTM
+    lanelet2_map_projector_type: MGRS   # Options: MGRS, UTM, local
     latitude: 40.81187906        # Latitude of map_origin, using in UTM
     longitude: 29.35810110          # Longitude of map_origin, using in UTM
 

diff --git a/...launch/config/perception/occupancy_grid_map/laserscan_based_occupancy_grid_map.param.yaml b/...launch/config/perception/occupancy_grid_map/laserscan_based_occupancy_grid_map.param.yaml
@@ -9,6 +9,6 @@
 
     use_height_filter: true
     enable_single_frame_mode: false
-    map_length: 100.0
-    map_width: 100.0
+    map_length: 150.0
+    map_width: 150.0
     map_resolution: 0.5
diff --git a/...aunch/config/perception/occupancy_grid_map/pointcloud_based_occupancy_grid_map.param.yaml b/...aunch/config/perception/occupancy_grid_map/pointcloud_based_occupancy_grid_map.param.yaml
@@ -1,6 +1,6 @@
 /**:
   ros__parameters:
-    map_length: 100.0     # [m]
+    map_length: 150.0     # [m]
     map_resolution: 0.5 # [m]
 
     use_height_filter: true

diff --git a/...ing/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml b/...ing/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml
@@ -45,6 +45,3 @@
       drivable_area_right_bound_offset: 0.0
       drivable_area_left_bound_offset: 0.0
       drivable_area_types_to_skip: [road_border]
-
-    lane_change:
-      lane_change_prepare_duration: 4.0 # [s]
diff --git a/...nning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree.xml b/...nning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree.xml
@@ -4,8 +4,8 @@
     <BehaviorTree ID="BehaviorTree">
         <Fallback>
             <ReactiveSequence>
-                <Condition ID="PullOver_Request"/>
-                <Action ID="PullOver_Plan" output="{output}"/>
+                <Condition ID="GoalPlanner_Request"/>
+                <Action ID="GoalPlanner_Plan" output="{output}"/>
             </ReactiveSequence>
             <ReactiveSequence>
                 <Condition ID="PullOut_Request"/>
@@ -74,10 +74,10 @@
             <output_port name="output"/>
         </Action>
         <Condition ID="PullOut_Request"/>
-        <Action ID="PullOver_Plan">
+        <Action ID="GoalPlanner_Plan">
             <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
         </Action>
-        <Condition ID="PullOver_Request"/>
+        <Condition ID="GoalPlanner_Request"/>
         <Condition ID="SideShift_CheckApproval"/>
         <Action ID="SideShift_Plan">
             <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>

diff --git a/...g/behavior_planning/behavior_path_planner/behavior_path_planner_tree_lane_change_only.xml b/...g/behavior_planning/behavior_path_planner/behavior_path_planner_tree_lane_change_only.xml
@@ -4,8 +4,8 @@
     <BehaviorTree ID="BehaviorTree">
         <Fallback>
             <ReactiveSequence>
-                <Condition ID="PullOver_Request"/>
-                <Action ID="PullOver_Plan" output="{output}"/>
+                <Condition ID="GoalPlanner_Request"/>
+                <Action ID="GoalPlanner_Plan" output="{output}"/>
             </ReactiveSequence>
             <ReactiveSequence>
                 <Condition ID="PullOut_Request"/>
@@ -66,10 +66,10 @@
             <output_port name="output"/>
         </Action>
         <Condition ID="PullOut_Request"/>
-        <Action ID="PullOver_Plan">
+        <Action ID="GoalPlanner_Plan">
             <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
         </Action>
-        <Condition ID="PullOver_Request"/>
+        <Condition ID="GoalPlanner_Request"/>
         <Condition ID="SideShift_CheckApproval"/>
         <Action ID="SideShift_Plan">
             <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>

diff --git a/...e_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree_pull_out.xml b/...e_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree_pull_out.xml
@@ -4,9 +4,9 @@
     <BehaviorTree ID="BehaviorTree">
         <Fallback>
             <ReactiveSequence>
-                <Condition ID="PullOver_Request"/>
-                <Condition ID="PullOver_Ready"/>
-                <Action ID="PullOver_Plan" output="{output}"/>
+                <Condition ID="GoalPlanner_Request"/>
+                <Condition ID="GoalPlanner_Ready"/>
+                <Action ID="GoalPlanner_Plan" output="{output}"/>
             </ReactiveSequence>
             <ReactiveSequence>
                 <Condition ID="PullOut_Request"/>
@@ -78,11 +78,11 @@
         </Action>
         <Condition ID="PullOut_Ready"/>
         <Condition ID="PullOut_Request"/>
-        <Action ID="PullOver_Plan">
+        <Action ID="GoalPlanner_Plan">
             <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
         </Action>
-        <Condition ID="PullOver_Ready"/>
-        <Condition ID="PullOver_Request"/>
+        <Condition ID="GoalPlanner_Ready"/>
+        <Condition ID="GoalPlanner_Request"/>
         <Condition ID="SideShift_CheckApproval"/>
         <Action ID="SideShift_Plan">
             <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>

diff --git a/...or_planning/behavior_path_planner/behavior_path_planner_tree_with_external_request_LC.xml b/...or_planning/behavior_path_planner/behavior_path_planner_tree_with_external_request_LC.xml
@@ -4,8 +4,8 @@
     <BehaviorTree ID="BehaviorTree">
         <Fallback>
             <ReactiveSequence>
-                <Condition ID="PullOver_Request"/>
-                <Action ID="PullOver_Plan" output="{output}"/>
+                <Condition ID="GoalPlanner_Request"/>
+                <Action ID="GoalPlanner_Plan" output="{output}"/>
             </ReactiveSequence>
             <ReactiveSequence>
                 <Condition ID="PullOut_Request"/>
@@ -61,10 +61,10 @@
             <output_port name="output"/>
         </Action>
         <Condition ID="PullOut_Request"/>
-        <Action ID="PullOver_Plan">
+        <Action ID="GoalPlanner_Plan">
             <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
         </Action>
-        <Condition ID="PullOver_Request"/>
+        <Condition ID="GoalPlanner_Request"/>
         <Action ID="SideShift_Plan">
             <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
         </Action>

diff --git a/...th_planner/pull_over/pull_over.param.yaml → ...nner/goal_planner/goal_planner.param.yaml b/...th_planner/pull_over/pull_over.param.yaml → ...nner/goal_planner/goal_planner.param.yaml
@@ -1,6 +1,6 @@
 /**:
   ros__parameters:
-    pull_over:
+    goal_planner:
       minimum_request_length: 100.0
       th_arrived_distance: 1.0
       th_stopped_velocity: 0.01

diff --git a/...ning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml b/...ning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml
@@ -45,7 +45,7 @@
       priority: 2
       max_module_size: 1
 
-    pull_over:
+    goal_planner:
       enable_module: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false

diff --git a/...ing/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml b/...ing/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml
@@ -13,7 +13,7 @@
       - "lane_change_right"
       - "avoidance_left"
       - "avoidance_right"
-      - "pull_over"
+      - "goal_planner"
       - "pull_out"
       - "intersection_occlusion"
 
@@ -28,6 +28,6 @@
       - "lane_change_right"
       - "avoidance_left"
       - "avoidance_right"
-      - "pull_over"
+      - "goal_planner"
       - "pull_out"
       - "intersection_occlusion"
diff --git a/autoware_launch/launch/autoware.launch.xml b/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 -->