Merge branch 'main' into autoware_msg

autoware_launch/config/control/control_validator/control_validator.param.yaml

@@ -1,8 +1,6 @@
 /**:
   ros__parameters:
 
-    publish_diag: false  # if true, diagnostic msg is published
-
     # If the number of consecutive invalid predicted_path exceeds this threshold, the Diag will be set to ERROR.
     # (For example, threshold = 1 means, even if the predicted_path is invalid, Diag will not be ERROR if
     #  the next predicted_path is valid.)

autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml

@@ -13,7 +13,7 @@
     curvature_smoothing_num_ref_steer: 15   # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num)
 
     # -- trajectory extending --
-    extend_trajectory_for_end_yaw_control: true  # flag of trajectory extending for terminal yaw control
+    extend_trajectory_for_end_yaw_control: false  # flag of trajectory extending for terminal yaw control
 
     # -- mpc optimization --
     qp_solver_type: "osqp"                       # optimization solver option (unconstraint_fast or osqp)

autoware_launch/config/localization/ar_tag_based_localizer.param.yaml

@@ -0,0 +1,40 @@
+/**:
+  ros__parameters:
+    # marker_size
+    marker_size: 0.6
+
+    # target_tag_ids
+    target_tag_ids: ['0','1','2','3','4','5','6']
+
+    # base_covariance
+    #   This value is dynamically scaled according to the distance at which AR tags are detected.
+    base_covariance: [0.2, 0.0, 0.0, 0.0,  0.0,  0.0,
+                      0.0, 0.2, 0.0, 0.0,  0.0,  0.0,
+                      0.0, 0.0, 0.2, 0.0,  0.0,  0.0,
+                      0.0, 0.0, 0.0, 0.02, 0.0,  0.0,
+                      0.0, 0.0, 0.0, 0.0,  0.02, 0.0,
+                      0.0, 0.0, 0.0, 0.0,  0.0,  0.02]
+
+    # Detect AR-Tags within this range and publish the pose of ego vehicle
+    distance_threshold: 13.0  # [m]
+
+    # consider_orientation
+    consider_orientation: false
+
+    # Detector parameters
+    # See https://github.com/pal-robotics/aruco_ros/blob/7787a6794d30c248bc546d1582e65dd47bc40c12/aruco/include/aruco/markerdetector.h#L106-L126
+    detection_mode: "DM_NORMAL"  # select from [DM_NORMAL, DM_FAST, DM_VIDEO_FAST]
+    min_marker_size: 0.02
+
+    # Parameters for comparison with EKF Pose
+    # If the difference between the EKF pose and the current pose is within the range of values set below, the current pose is published.
+    # [How to determine the value]
+    # * ekf_time_tolerance: Since it is abnormal if the data comes too old from EKF, the tentative tolerance value is set at 5 seconds.
+    #                       This value is assumed to be unaffected even if it is increased or decreased by some amount.
+    # * ekf_position_tolerance: Since it is possible that multiple AR tags with the same ID could be placed, the tolerance should be as small as possible.
+    #                           And if the vehicle is running only on odometry in a section without AR tags,
+    #                           it is possible that self-position estimation could be off by a few meters.
+    #                           it should be fixed by AR tag detection, so tolerance should not be smaller than 10 meters.
+    #                           Therefore, the tolerance is set at 10 meters.
+    ekf_time_tolerance: 5.0 # [s]
+    ekf_position_tolerance: 10.0 # [m]

autoware_launch/config/perception/object_recognition/detection/lidar_model/pointpainting.param.yaml

@@ -26,7 +26,7 @@
       iou_nms_target_class_names: ["CAR"]
       iou_nms_search_distance_2d: 10.0
       iou_nms_threshold: 0.1
-      score_threshold: 0.4
+      score_threshold: 0.35
     omp_params:
       # omp params
       num_threads: 1

autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/radar_object_tracker.param.yaml

@@ -24,3 +24,6 @@
     max_distance_from_lane: 5.0 # [m]
     max_angle_diff_from_lane: 0.785398 # [rad] (45 deg)
     max_lateral_velocity: 7.0 # [m/s]
+
+    # tracking model parameters
+    tracking_config_directory: $(find-pkg-share autoware_launch)/config/perception/object_recognition/tracking/radar_object_tracker/tracking/

autoware_launch/config/perception/traffic_light_arbiter/traffic_light_arbiter.param.yaml

@@ -3,3 +3,4 @@
     external_time_tolerance: 5.0
     perception_time_tolerance: 1.0
     external_priority: false
+    enable_signal_matching: false

autoware_launch/config/planning/scenario_planning/common/common.param.yaml

@@ -1,5 +1,7 @@
 /**:
   ros__parameters:
+    max_vel: 11.1           # max velocity limit [m/s]
+
     # constraints param for normal driving
     normal:
       min_acc: -1.0         # min deceleration [m/ss]

autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml

@@ -1,7 +1,6 @@
 /**:
   ros__parameters:
     # motion state constraints
-    max_velocity: 20.0      # max velocity limit [m/s]
     stop_decel: 0.0         # deceleration at a stop point[m/ss]
 
     # external velocity limit parameter
@@ -26,7 +25,7 @@
     # engage & replan parameters
     replan_vel_deviation: 5.53         # velocity deviation to replan initial velocity [m/s]
     engage_velocity: 0.25              # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed)
-    engage_acceleration: 0.1           # engage acceleration [m/ss] (use this acceleration when engagement)
+    engage_acceleration: 0.5           # engage acceleration [m/ss] (use this acceleration when engagement)
     engage_exit_ratio: 0.5             # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity.
     stop_dist_to_prohibit_engage: 0.5  # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m]
 

autoware_launch/config/planning/scenario_planning/common/planning_validator/planning_validator.param.yaml

@@ -26,3 +26,14 @@
       steering_rate: 10.0
       velocity_deviation: 100.0
       distance_deviation: 100.0
+      longitudinal_distance_deviation: 1.0
+
+    parameters:
+      # The required trajectory length is calculated as the distance needed
+      # to stop from the current speed at this deceleration.
+      forward_trajectory_length_acceleration: -3.0
+
+      # An error is raised if the required trajectory length is less than this distance.
+      # Setting it to 0 means an error will occur if even slightly exceeding the end of the path,
+      # therefore, a certain margin is necessary.
+      forward_trajectory_length_margin: 2.0

autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml

@@ -127,7 +127,7 @@
         object_check_return_pose_distance: 20.0          # [m]
         # filtering parking objects
         threshold_distance_object_is_on_center: 1.0     # [m]
-        object_check_shiftable_ratio: 0.6               # [-]
+        object_check_shiftable_ratio: 0.8               # [-]
         object_check_min_road_shoulder_width: 0.5       # [m]
         # lost object compensation
         object_last_seen_threshold: 2.0
@@ -187,6 +187,7 @@
         time_horizon_for_rear_object: 10.0               # [s]
         delay_until_departure: 0.0                       # [s]
         # rss parameters
+        extended_polygon_policy: "along_path"            # [-] select "rectangle" or "along_path"
         expected_front_deceleration: -1.0                # [m/ss]
         expected_rear_deceleration: -1.0                 # [m/ss]
         rear_vehicle_reaction_time: 2.0                  # [s]
@@ -206,7 +207,7 @@
           hard_road_shoulder_margin: 0.3                # [m]
           max_right_shift_length: 5.0
           max_left_shift_length: 5.0
-          max_deviation_from_lane: 0.5                  # [m]
+          max_deviation_from_lane: 0.2                  # [m]
         # avoidance distance parameters
         longitudinal:
           min_prepare_time: 1.0                         # [s]

autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml

@@ -35,10 +35,14 @@
       # object recognition
       object_recognition:
         use_object_recognition: true
-        object_recognition_collision_check_margin: 0.6 # this should be larger than `surround_check_distance` of surround_obstacle_checker
+        collision_check_soft_margins: [2.0, 1.5, 1.0]
+        collision_check_hard_margins: [0.6]  # this should be larger than `surround_check_distance` of surround_obstacle_checker
         object_recognition_collision_check_max_extra_stopping_margin: 1.0
         th_moving_object_velocity: 1.0
         detection_bound_offset: 15.0
+        outer_road_detection_offset: 1.0
+        inner_road_detection_offset: 0.0
+
 
       # pull over
       pull_over:

autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml

@@ -103,6 +103,7 @@
         duration: 5.0                       # [s]
         max_lateral_jerk: 100.0             # [m/s3]
         overhang_tolerance: 0.0             # [m]
+        unsafe_hysteresis_threshold: 5      # [/]
 
       finish_judge_lateral_threshold: 0.2        # [m]
 

autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml

@@ -6,7 +6,6 @@
       th_stopped_velocity: 0.01
       th_stopped_time: 1.0
       collision_check_margins: [2.0, 1.0, 0.5, 0.1]
-      collision_check_distance_from_end: -10.0
       collision_check_margin_from_front_object: 5.0
       th_moving_object_velocity: 1.0
       th_distance_to_middle_of_the_road: 0.5
@@ -39,6 +38,7 @@
       backward_path_update_duration: 3.0
       ignore_distance_from_lane_end: 15.0
       # turns signal
+      prepare_time_before_start: 0.0
       th_turn_signal_on_lateral_offset: 1.0
       intersection_search_length: 30.0
       length_ratio_for_turn_signal_deactivation_near_intersection: 0.5
@@ -93,7 +93,7 @@
           delay_until_departure: 1.0
         # For target object filtering
         target_filtering:
-          safety_check_time_horizon: 5.0
+          safety_check_time_horizon: 10.0
           safety_check_time_resolution: 1.0
           # detection range
           object_check_forward_distance: 10.0

autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml

@@ -4,7 +4,7 @@
       use_pass_judge_line: true
       stop_line_margin: 1.0 # [m]
       backward_length: 50.0 # [m]
-      ignore_width_from_center_line: 0.7 # [m]
+      ignore_width_from_center_line: 0.0 # [m]
       max_future_movement_time: 10.0 # [second]
       threshold_yaw_diff: 0.523 # [rad]
       adjacent_extend_width: 1.5 # [m]

autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml

@@ -41,7 +41,7 @@
         ego_pass_first_margin_y: [3.0] # [[s]] time to vehicle margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition)
         ego_pass_first_additional_margin: 0.5 # [s] additional time margin for ego pass first situation to suppress chattering
         ego_pass_later_margin_x: [0.0] # [[s]] time to vehicle margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition)
-        ego_pass_later_margin_y: [10.0] # [[s]] time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition)
+        ego_pass_later_margin_y: [13.0] # [[s]] time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition)
         ego_pass_later_additional_margin: 0.5 # [s] additional time margin for object pass first situation to suppress chattering
 
         no_stop_decision: # parameters to determine if it is safe to attempt stopping before the crosswalk
@@ -68,3 +68,20 @@
           bicycle: true # [-] whether to look and stop by BICYCLE objects
           motorcycle: true # [-] whether to look and stop by MOTORCYCLE objects (tmp: currently it is difficult for perception modules to detect bicycles and motorcycles separately.)
           pedestrian: true # [-] whether to look and stop by PEDESTRIAN objects
+
+      # param for occlusions
+      occlusion:
+        enable: true  # if true, ego will slowdown around crosswalks that are occluded
+        occluded_object_velocity: 1.0 # [m/s] assumed velocity of objects that may come out of the occluded space
+        slow_down_velocity: 1.0  # [m/s]
+        time_buffer: 0.5  # [s] consecutive time with/without an occlusion to add/remove the slowdown
+        min_size: 0.5  # [m] minimum size of an occlusion (square side size)
+        free_space_max: 43  # [-] maximum value of a free space cell in the occupancy grid
+        occupied_min: 58    # [-] minimum value of an occupied cell in the occupancy grid
+        ignore_with_red_traffic_light: true  # [-] if true, occlusions at crosswalks with traffic lights are ignored
+        ignore_behind_predicted_objects: true  # [-] if true, occlusions behind predicted objects are ignored
+        ignore_velocity_thresholds:
+          default: 0.5  # [m/s] occlusions are only ignored behind objects with a higher or equal velocity
+          custom_labels: ["PEDESTRIAN"]  # labels for which to define a non-default velocity threshold (see autoware_auto_perception_msgs::msg::ObjectClassification for all the labels)
+          custom_thresholds: [0.0]  # velocities of the custom labels
+        extra_predicted_objects_size: 0.5  # [m] extra size added to the objects for masking the occlusions

autoware_launch/config/system/system_error_monitor/system_error_monitor.awsim.param.yaml

@@ -24,6 +24,8 @@
         /autoware/localization/node_alive_monitoring: default
         /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" }
         /autoware/localization/performance_monitoring/localization_error_ellipse: { sf_at: "warn", lf_at: "none", spf_at: "none" }
+        /autoware/localization/performance_monitoring/localization_stability: { sf_at: "warn", lf_at: "none", spf_at: "none" }
+        /autoware/localization/performance_monitoring/sensor_fusion_status: { sf_at: "warn", lf_at: "none", spf_at: "none" }
 
         /autoware/map/node_alive_monitoring: default
 

autoware_launch/config/system/system_error_monitor/system_error_monitor.param.yaml

@@ -24,6 +24,7 @@
         /autoware/localization/node_alive_monitoring: default
         /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" }
         /autoware/localization/performance_monitoring/localization_error_ellipse: default
+        /autoware/localization/performance_monitoring/localization_stability: default
         /autoware/localization/performance_monitoring/sensor_fusion_status: { sf_at: "error", lf_at: "none", spf_at: "none" }
 
         /autoware/map/node_alive_monitoring: default

autoware_launch/launch/components/tier4_localization_component.launch.xml

@@ -52,6 +52,9 @@
 
       <!-- parameter paths for eagleye -->
       <arg name="eagleye_param_path" value="$(find-pkg-share autoware_launch)/config/localization/eagleye_config.param.yaml"/>
+
+      <!-- parameter path for ar_tag_based_localizer -->
+      <arg name="ar_tag_based_localizer_param_path" value="$(var loc_config_path)/ar_tag_based_localizer.param.yaml"/>
     </include>
   </group>
 </launch>

autoware_launch/launch/components/tier4_perception_component.launch.xml

@@ -127,10 +127,6 @@
       name="object_recognition_tracking_radar_object_tracker_node_param_path"
       value="$(find-pkg-share autoware_launch)/config/perception/object_recognition/tracking/radar_object_tracker/radar_object_tracker.param.yaml"
     />
-    <arg
-      name="object_recognition_tracking_radar_object_tracker_tracking_config_directory_path"
-      value="$(find-pkg-share autoware_launch)/config/perception/object_recognition/tracking/radar_object_tracker/tracking/"
-    />
     <arg
       name="object_recognition_tracking_object_merger_data_association_matrix_param_path"
       value="$(find-pkg-share autoware_launch)/config/perception/object_recognition/tracking/tracking_object_merger/data_association_matrix.param.yaml"

autoware_launch/launch/planning_simulator.launch.xml

@@ -13,6 +13,8 @@
   <!-- Map -->
   <arg name="lanelet2_map_file" default="lanelet2_map.osm" description="lanelet2 map file name"/>
   <arg name="pointcloud_map_file" default="pointcloud_map.pcd" description="pointcloud map file name"/>
+  <let name="rviz_initial_pose_auto_fix_target" value="vector_map"/>
+  <let name="gnss_initial_pose_auto_fix_target" value="vector_map"/>
   <!-- System -->
   <arg name="launch_system_monitor" default="false" description="launch system monitor"/>
   <arg name="launch_dummy_diag_publisher" default="false" description="launch dummy diag publisher"/>
@@ -39,6 +41,8 @@
     <let name="launch_vehicle_interface" value="true" unless="$(var vehicle_simulation)"/>
 
     <include file="$(find-pkg-share autoware_launch)/launch/autoware.launch.xml">
+      <!-- Global parameters -->
+      <arg name="use_sim_time" value="$(var scenario_simulation)"/>
       <!-- Common -->
       <arg name="map_path" value="$(var map_path)"/>
       <arg name="vehicle_model" value="$(var vehicle_model)"/>

autoware_launch/rviz/autoware.rviz

@@ -3613,7 +3613,7 @@ Visualization Manager:
       Name: Debug
   Enabled: true
   Global Options:
-    Background Color: 10; 10; 10
+    Background Color: 42; 42; 42
     Default Light: true
     Fixed Frame: map
     Frame Rate: 30