From b397d277e0843c88341b082fa5f939c215f445d0 Mon Sep 17 00:00:00 2001 From: taikitanaka3 Date: Tue, 20 Aug 2024 18:29:27 +0900 Subject: [PATCH 1/8] feat(aichallenge_submit_launch): replace from autoware launch to aichallenge submit launch --- .../mission_planner.param.yaml | 8 ++ .../common/nearest_search.param.yaml | 5 + .../avoidance/avoidance.param.yaml | 116 ++++++++++++++++++ .../behavior_path_planner.param.yaml | 69 +++++++++++ .../drivable_area_expansion.param.yaml | 58 +++++++++ .../lane_change/lane_change.param.yaml | 34 +++++ .../lane_following/lane_following.param.yaml | 4 + .../pull_out/pull_out.param.yaml | 31 +++++ .../pull_over/pull_over.param.yaml | 95 ++++++++++++++ .../side_shift/side_shift.param.yaml | 8 ++ .../lane_change_planner.param.yaml | 22 ++++ .../launch/reference.launch.xml | 22 ++-- 12 files changed, 461 insertions(+), 11 deletions(-) create mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/common/nearest_search.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml new file mode 100644 index 00000000..a801eca3 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + map_frame: map + arrival_check_angle_deg: 45.0 + arrival_check_distance: 1.0 + arrival_check_duration: 1.0 + goal_angle_threshold_deg: 45.0 + enable_correct_goal_pose: false diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/common/nearest_search.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/common/nearest_search.param.yaml new file mode 100644 index 00000000..eb670976 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/common/nearest_search.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + # ego + ego_nearest_dist_threshold: 3.0 # [m] + ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml new file mode 100644 index 00000000..c14f59f6 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -0,0 +1,116 @@ +# see AvoidanceParameters description in avoidance_module_data.hpp for description. +/**: + ros__parameters: + avoidance: + resample_interval_for_planning: 0.3 # [m] + resample_interval_for_output: 4.0 # [m] + detection_area_right_expand_dist: 0.0 # [m] + detection_area_left_expand_dist: 1.0 # [m] + drivable_area_right_bound_offset: 0.0 # [m] + drivable_area_left_bound_offset: 0.0 # [m] + object_envelope_buffer: 0.3 # [m] + + # avoidance module common setting + enable_bound_clipping: false + enable_avoidance_over_same_direction: true + enable_avoidance_over_opposite_direction: true + enable_update_path_when_object_is_gone: false + enable_force_avoidance_for_stopped_vehicle: false + enable_safety_check: false + enable_yield_maneuver: false + disable_path_update: false + + # for debug + publish_debug_marker: false + print_debug_info: false + + # avoidance is performed for the object type with true + target_object: + car: true + truck: true + bus: true + trailer: true + unknown: false + bicycle: false + motorcycle: false + pedestrian: false + + # For target object filtering + target_filtering: + # filtering moving objects + threshold_speed_object_is_stopped: 1.0 # [m/s] + threshold_time_object_is_moving: 1.0 # [s] + threshold_time_force_avoidance_for_stopped_vehicle: 10.0 # [s] + # detection range + object_check_force_avoidance_clearance: 30.0 # [m] + object_check_forward_distance: 150.0 # [m] + object_check_backward_distance: 2.0 # [m] + object_check_goal_distance: 20.0 # [m] + # filtering parking objects + threshold_distance_object_is_on_center: 1.0 # [m] + object_check_shiftable_ratio: 0.6 # [-] + object_check_min_road_shoulder_width: 0.5 # [m] + # lost object compensation + object_last_seen_threshold: 2.0 + + # For safety check + safety_check: + safety_check_backward_distance: 100.0 # [m] + safety_check_time_horizon: 10.0 # [s] + safety_check_idling_time: 1.5 # [s] + safety_check_accel_for_rss: 2.5 # [m/ss] + safety_check_hysteresis_factor: 2.0 # [-] + + # For avoidance maneuver + avoidance: + # avoidance lateral parameters + lateral: + lateral_collision_margin: 1.0 # [m] + lateral_collision_safety_buffer: 0.7 # [m] + lateral_passable_safety_buffer: 0.0 # [m] + road_shoulder_safety_margin: 0.3 # [m] + avoidance_execution_lateral_threshold: 0.499 + max_right_shift_length: 5.0 + max_left_shift_length: 5.0 + # avoidance distance parameters + longitudinal: + prepare_time: 2.0 # [s] + longitudinal_collision_safety_buffer: 0.0 # [m] + min_prepare_distance: 1.0 # [m] + min_avoidance_distance: 10.0 # [m] + min_nominal_avoidance_speed: 7.0 # [m/s] + min_sharp_avoidance_speed: 1.0 # [m/s] + + # For yield maneuver + yield: + yield_velocity: 2.78 # [m/s] + + # For stop maneuver + stop: + min_distance: 10.0 # [m] + max_distance: 20.0 # [m] + + constraints: + # vehicle slows down under longitudinal constraints + use_constraints_for_decel: false # [-] + + # lateral constraints + lateral: + nominal_lateral_jerk: 0.2 # [m/s3] + max_lateral_jerk: 1.0 # [m/s3] + + # longitudinal constraints + longitudinal: + nominal_deceleration: -1.0 # [m/ss] + nominal_jerk: 0.5 # [m/sss] + max_deceleration: -2.0 # [m/ss] + max_jerk: 1.0 + # For prevention of large acceleration while avoidance + min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] + max_avoidance_acceleration: 0.5 # [m/ss] + + target_velocity_matrix: + col_size: 2 + matrix: + [2.78, 13.9, # velocity [m/s] + 0.50, 1.00] # margin [m] diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml new file mode 100644 index 00000000..6ad2b31d --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml @@ -0,0 +1,69 @@ +/**: + ros__parameters: + verbose: false + + planning_hz: 10.0 + backward_path_length: 5.0 + forward_path_length: 300.0 + backward_length_buffer_for_end_of_pull_over: 5.0 + backward_length_buffer_for_end_of_pull_out: 5.0 + minimum_pull_over_length: 16.0 + + refine_goal_search_radius_range: 7.5 + + # parameters for turn signal decider + turn_signal_intersection_search_distance: 30.0 + turn_signal_intersection_angle_threshold_deg: 15.0 + turn_signal_minimum_search_distance: 10.0 + turn_signal_search_time: 3.0 + turn_signal_shift_length_threshold: 0.3 + turn_signal_on_swerving: true + + path_interval: 2.0 + + visualize_maximum_drivable_area: true + + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + + expected_front_deceleration_for_abort: -1.0 + expected_rear_deceleration_for_abort: -2.0 + + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + + # USE ONLY WHEN THE OPTION COMPILE_WITH_OLD_ARCHITECTURE IS SET TO FALSE. + # https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/CMakeLists.txt + # NOTE: The smaller the priority number is, the higher the module priority is. + lane_change: + enable_module: true + enable_simultaneous_execution: false + priority: 4 + max_module_size: 1 + + pull_out: + enable_module: true + enable_simultaneous_execution: false + priority: 0 + max_module_size: 1 + + side_shift: + enable_module: true + enable_simultaneous_execution: false + priority: 2 + max_module_size: 1 + + pull_over: + enable_module: true + enable_simultaneous_execution: false + priority: 1 + max_module_size: 1 + + avoidance: + enable_module: true + enable_simultaneous_execution: false + priority: 3 + max_module_size: 1 diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml new file mode 100644 index 00000000..902a93ce --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml @@ -0,0 +1,58 @@ +/**: + ros__parameters: + # Static expansion + avoidance: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + lane_change: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + lane_following: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + pull_out: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + pull_over: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + side_shift: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + + # Dynamic expansion by projecting the ego footprint along the path + dynamic_expansion: + enabled: false + ego: + extra_footprint_offset: + front: 0.5 # [m] extra length to add to the front of the ego footprint + rear: 0.5 # [m] extra length to add to the rear of the ego footprint + left: 0.5 # [m] extra length to add to the left of the ego footprint + right: 0.5 # [m] extra length to add to the rear of the ego footprint + dynamic_objects: + avoid: true # if true, the drivable area is not expanded in the predicted path of dynamic objects + extra_footprint_offset: + front: 0.5 # [m] extra length to add to the front of the dynamic object footprint + rear: 0.5 # [m] extra length to add to the rear of the dynamic object footprint + left: 0.5 # [m] extra length to add to the left of the dynamic object footprint + right: 0.5 # [m] extra length to add to the rear of the dynamic object footprint + expansion: + method: polygon # method used to expand the drivable area. Either 'lanelet' or 'polygon'. + # 'lanelet': add lanelets overlapped by the ego footprints + # 'polygon': add polygons built around sections of the ego footprint that go out of the drivable area + max_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) + max_path_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) + extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint + avoid_linestring: + types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area + - road_border + distance: 0.0 # [m] distance to keep between the drivable area and the linestrings to avoid + compensate: + enable: true # if true, when the drivable area cannot be expanded in one direction to completely include the ego footprint, it is expanded in the opposite direction + extra_distance: 3.0 # [m] extra distance to add to the compensation diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml new file mode 100644 index 00000000..295769ac --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml @@ -0,0 +1,34 @@ +/**: + ros__parameters: + lane_change: + lane_change_prepare_duration: 4.0 # [s] + lane_changing_safety_check_duration: 8.0 # [s] + + minimum_lane_change_prepare_distance: 2.0 # [m] + minimum_lane_change_length: 16.5 # [m] + backward_length_buffer_for_end_of_lane: 3.0 # [m] + lane_change_finish_judge_buffer: 2.0 # [m] + + lane_changing_lateral_jerk: 0.5 # [m/s3] + lane_changing_lateral_acc: 0.5 # [m/s2] + + minimum_lane_change_velocity: 2.78 # [m/s] + prediction_time_resolution: 0.5 # [s] + maximum_deceleration: 1.0 # [m/s2] + lane_change_sampling_num: 3 + + # collision check + enable_collision_check_at_prepare_phase: false + prepare_phase_ignore_target_speed_thresh: 0.1 # [m/s] + use_predicted_path_outside_lanelet: false + use_all_predicted_path: false + + # abort + enable_cancel_lane_change: true + enable_abort_lane_change: false + + abort_delta_time: 3.0 # [s] + abort_max_lateral_jerk: 1000.0 # [m/s3] + + # debug + publish_debug_marker: false diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml new file mode 100644 index 00000000..b6a9574b --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + lane_following: + lane_change_prepare_duration: 2.0 diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml new file mode 100644 index 00000000..68000747 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml @@ -0,0 +1,31 @@ +/**: + ros__parameters: + pull_out: + th_arrived_distance: 1.0 + th_stopped_velocity: 0.01 + th_stopped_time: 1.0 + collision_check_margin: 1.0 + collision_check_distance_from_end: 1.0 + # shift pull out + enable_shift_pull_out: true + shift_pull_out_velocity: 2.0 + pull_out_sampling_num: 4 + minimum_shift_pull_out_distance: 20.0 + maximum_lateral_jerk: 2.0 + minimum_lateral_jerk: 0.5 + deceleration_interval: 15.0 + # geometric pull out + enable_geometric_pull_out: true + divide_pull_out_path: false + geometric_pull_out_velocity: 1.0 + arc_path_interval: 1.0 + lane_departure_margin: 0.2 + backward_velocity: -1.0 + pull_out_max_steer_angle: 0.26 # 15deg + # search start pose backward + enable_back: true + search_priority: "efficient_path" # "efficient_path" or "short_back_distance" + max_back_distance: 30.0 + backward_search_resolution: 2.0 + backward_path_update_duration: 3.0 + ignore_distance_from_lane_end: 15.0 diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml new file mode 100644 index 00000000..ca1eb740 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml @@ -0,0 +1,95 @@ +/**: + ros__parameters: + pull_over: + request_length: 100.0 + th_arrived_distance: 1.0 + th_stopped_velocity: 0.01 + th_stopped_time: 2.0 # It must be greater than the state_machine's. + pull_over_velocity: 3.0 + pull_over_minimum_velocity: 1.38 + margin_from_boundary: 0.5 + decide_path_distance: 10.0 + maximum_deceleration: 0.5 + # goal research + enable_goal_research: true + search_priority: "efficient_path" # "efficient_path" or "close_goal" + forward_goal_search_length: 20.0 + backward_goal_search_length: 20.0 + goal_search_interval: 2.0 + longitudinal_margin: 3.0 + max_lateral_offset: 0.5 + lateral_offset_interval: 0.25 + ignore_distance_from_lane_start: 15.0 + # occupancy grid map + use_occupancy_grid: true + use_occupancy_grid_for_longitudinal_margin: false + occupancy_grid_collision_check_margin: 0.0 + theta_size: 360 + obstacle_threshold: 60 + # object recognition + use_object_recognition: true + object_recognition_collision_check_margin: 1.0 + # shift path + enable_shift_parking: true + pull_over_sampling_num: 4 + maximum_lateral_jerk: 2.0 + minimum_lateral_jerk: 0.5 + deceleration_interval: 15.0 + after_pull_over_straight_distance: 1.0 + # freespace parking + enable_freespace_parking: true + freespace_parking: + planning_algorithm: "astar" # options: astar, rrtstar + velocity: 1.0 + vehicle_shape_margin: 1.0 + time_limit: 3000.0 + minimum_turning_radius: 5.0 + maximum_turning_radius: 5.0 + turning_radius_size: 1 + # search configs + theta_size: 144 + angle_goal_range: 6.0 + curve_weight: 1.2 + reverse_weight: 1.0 + lateral_goal_range: 0.5 + longitudinal_goal_range: 2.0 + # costmap configs + obstacle_threshold: 30 + # -- A* search Configurations -- + astar: + only_behind_solutions: false + use_back: false + distance_heuristic_weight: 1.0 + # -- RRT* search Configurations -- + rrtstar: + enable_update: true + use_informed_sampling: true + max_planning_time: 150.0 + neighbor_radius: 8.0 + margin: 1.0 + # parallel parking path + enable_arc_forward_parking: true + enable_arc_backward_parking: true + after_forward_parking_straight_distance: 2.0 + after_backward_parking_straight_distance: 2.0 + forward_parking_velocity: 1.38 + backward_parking_velocity: -1.38 + forward_parking_lane_departure_margin: 0.0 + backward_parking_lane_departure_margin: 0.0 + arc_path_interval: 1.0 + pull_over_max_steer_angle: 0.35 # 20deg + # hazard on when parked + hazard_on_threshold_distance: 1.0 + hazard_on_threshold_velocity: 0.5 + # check safety with dynamic objects. Not used now. + pull_over_duration: 2.0 + pull_over_prepare_duration: 4.0 + min_stop_distance: 5.0 + stop_time: 2.0 + hysteresis_buffer_distance: 2.0 + prediction_time_resolution: 0.5 + enable_collision_check_at_prepare_phase: false + use_predicted_path_outside_lanelet: false + use_all_predicted_path: false + # debug + print_debug_info: false diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml new file mode 100644 index 00000000..79044041 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + side_shift: + min_distance_to_start_shifting: 5.0 + time_to_start_shifting: 1.0 + shifting_lateral_jerk: 0.2 + min_shifting_distance: 5.0 + min_shifting_speed: 5.56 diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml new file mode 100644 index 00000000..0bf42387 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml @@ -0,0 +1,22 @@ +/**: + ros__parameters: + min_stop_distance: 5.0 + stop_time: 2.0 + hysteresis_buffer_distance: 2.0 + backward_path_length: 5.0 + forward_path_length: 300.0 + max_accel: -5.0 + lane_change_prepare_duration: 4.0 + lane_changing_duration: 8.0 + backward_length_buffer_for_end_of_lane: 5.0 + lane_change_finish_judge_buffer: 3.0 + minimum_lane_change_length: 12.0 + minimum_lane_change_velocity: 5.6 + prediction_duration: 8.0 + prediction_time_resolution: 0.5 + drivable_area_resolution: 0.1 + drivable_area_width: 100.0 + drivable_area_height: 50.0 + static_obstacle_velocity_thresh: 1.5 + maximum_deceleration: 1.0 + refine_goal_search_radius_range: 7.5 diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml index 5ab2d4b3..dc6ba8ec 100644 --- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml @@ -114,7 +114,7 @@ - + @@ -179,14 +179,14 @@ - - - - - - - - + + + + + + + + @@ -226,7 +226,7 @@ - + @@ -235,7 +235,7 @@ - + From 3f4a63f7dc9e590c809f26fe295894c6c47c6cb0 Mon Sep 17 00:00:00 2001 From: taikitanaka3 Date: Tue, 20 Aug 2024 18:33:24 +0900 Subject: [PATCH 2/8] revert: this line to original --- .../aichallenge_submit_launch/launch/reference.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml index dc6ba8ec..4302157b 100644 --- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml @@ -185,7 +185,7 @@ - + From 4d1450b8c25f6f458b8698210a8c2566c596aef3 Mon Sep 17 00:00:00 2001 From: taikitanaka3 Date: Tue, 20 Aug 2024 18:38:41 +0900 Subject: [PATCH 3/8] chore: remove unused --- .../behavior_path_planner.param.yaml | 69 ------------------- .../lane_change_planner.param.yaml | 22 ------ 2 files changed, 91 deletions(-) delete mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml delete mode 100644 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml deleted file mode 100644 index 6ad2b31d..00000000 --- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml +++ /dev/null @@ -1,69 +0,0 @@ -/**: - ros__parameters: - verbose: false - - planning_hz: 10.0 - backward_path_length: 5.0 - forward_path_length: 300.0 - backward_length_buffer_for_end_of_pull_over: 5.0 - backward_length_buffer_for_end_of_pull_out: 5.0 - minimum_pull_over_length: 16.0 - - refine_goal_search_radius_range: 7.5 - - # parameters for turn signal decider - turn_signal_intersection_search_distance: 30.0 - turn_signal_intersection_angle_threshold_deg: 15.0 - turn_signal_minimum_search_distance: 10.0 - turn_signal_search_time: 3.0 - turn_signal_shift_length_threshold: 0.3 - turn_signal_on_swerving: true - - path_interval: 2.0 - - visualize_maximum_drivable_area: true - - lateral_distance_max_threshold: 2.0 - longitudinal_distance_min_threshold: 3.0 - - expected_front_deceleration: -1.0 - expected_rear_deceleration: -1.0 - - expected_front_deceleration_for_abort: -1.0 - expected_rear_deceleration_for_abort: -2.0 - - rear_vehicle_reaction_time: 2.0 - rear_vehicle_safety_time_margin: 1.0 - - # USE ONLY WHEN THE OPTION COMPILE_WITH_OLD_ARCHITECTURE IS SET TO FALSE. - # https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/CMakeLists.txt - # NOTE: The smaller the priority number is, the higher the module priority is. - lane_change: - enable_module: true - enable_simultaneous_execution: false - priority: 4 - max_module_size: 1 - - pull_out: - enable_module: true - enable_simultaneous_execution: false - priority: 0 - max_module_size: 1 - - side_shift: - enable_module: true - enable_simultaneous_execution: false - priority: 2 - max_module_size: 1 - - pull_over: - enable_module: true - enable_simultaneous_execution: false - priority: 1 - max_module_size: 1 - - avoidance: - enable_module: true - enable_simultaneous_execution: false - priority: 3 - max_module_size: 1 diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml deleted file mode 100644 index 0bf42387..00000000 --- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml +++ /dev/null @@ -1,22 +0,0 @@ -/**: - ros__parameters: - min_stop_distance: 5.0 - stop_time: 2.0 - hysteresis_buffer_distance: 2.0 - backward_path_length: 5.0 - forward_path_length: 300.0 - max_accel: -5.0 - lane_change_prepare_duration: 4.0 - lane_changing_duration: 8.0 - backward_length_buffer_for_end_of_lane: 5.0 - lane_change_finish_judge_buffer: 3.0 - minimum_lane_change_length: 12.0 - minimum_lane_change_velocity: 5.6 - prediction_duration: 8.0 - prediction_time_resolution: 0.5 - drivable_area_resolution: 0.1 - drivable_area_width: 100.0 - drivable_area_height: 50.0 - static_obstacle_velocity_thresh: 1.5 - maximum_deceleration: 1.0 - refine_goal_search_radius_range: 7.5 From bf9594756a95f3bc16cd08c7544c77411602388b Mon Sep 17 00:00:00 2001 From: taikitanaka3 Date: Tue, 20 Aug 2024 18:38:56 +0900 Subject: [PATCH 4/8] chore: move map dir according to launch --- .../config/{ => map}/lanelet2_map_loader.param.yaml | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/{ => map}/lanelet2_map_loader.param.yaml (100%) diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/lanelet2_map_loader.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/map/lanelet2_map_loader.param.yaml similarity index 100% rename from aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/lanelet2_map_loader.param.yaml rename to aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/map/lanelet2_map_loader.param.yaml From 9ccfe50b7f6048518675471e72f1c4922f481508 Mon Sep 17 00:00:00 2001 From: Masahiro Kubota Date: Fri, 23 Aug 2024 00:55:26 +0900 Subject: [PATCH 5/8] feat: add planning and control Signed-off-by: Masahiro Kubota --- .../launch/aichallenge.launch.xml | 130 ++++++++++++++++++ .../src/aichallenge_launch/package.xml | 9 ++ .../config/lanelet2_map_loader.param.yaml | 7 + 3 files changed, 146 insertions(+) create mode 100755 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/lanelet2_map_loader.param.yaml diff --git a/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml index 2b5c2078..24d2722d 100644 --- a/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml +++ b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml @@ -92,6 +92,127 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -146,4 +267,13 @@ + + + + + + + + + diff --git a/aichallenge/workspace/src/aichallenge_launch/package.xml b/aichallenge/workspace/src/aichallenge_launch/package.xml index e67ecbee..a66b3510 100644 --- a/aichallenge/workspace/src/aichallenge_launch/package.xml +++ b/aichallenge/workspace/src/aichallenge_launch/package.xml @@ -26,6 +26,15 @@ racing_kart_description racing_kart_sensor_kit_description + dummy_perception_publisher + mission_planner + behavior_path_planner + path_to_trajectory + default_ad_api + ad_api_adaptors + simple_pure_pursuit + + ament_lint_auto ament_lint_common diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/lanelet2_map_loader.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/lanelet2_map_loader.param.yaml new file mode 100755 index 00000000..e3a4fdf8 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/lanelet2_map_loader.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + lanelet2_map_projector_type: MGRS # Options: MGRS, UTM + latitude: 40.81187906 # Latitude of map_origin, using in UTM + longitude: 29.35810110 # Longitude of map_origin, using in UTM + + center_line_resolution: 5.0 # [m] From ebd4100c4ee0083c7b02f965f537a7adc70ef2f8 Mon Sep 17 00:00:00 2001 From: Masahiro Kubota Date: Fri, 23 Aug 2024 01:33:43 +0900 Subject: [PATCH 6/8] fix: modify accel map based on rosba Signed-off-by: Masahiro Kubota --- .../src/aichallenge_launch/data/accel_map.csv | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv b/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv index 32e639ca..a876b0df 100644 --- a/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv +++ b/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv @@ -1,7 +1,8 @@ -default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89 -0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5 -0.1,0.6,0.42,0.24,0.18,0.12,0.05,-0.08,-0.16,-0.2,-0.24,-0.28 -0.2,1.15,0.98,0.78,0.6,0.48,0.34,0.26,0.2,0.1,0.05,-0.03 -0.3,1.75,1.6,1.42,1.3,1.14,1,0.9,0.8,0.72,0.64,0.58 -0.4,2.65,2.48,2.3,2.13,1.95,1.75,1.58,1.45,1.32,1.2,1.1 -0.5,3.3,3.25,3.12,2.92,2.68,2.35,2.17,1.98,1.88,1.73,1.61 +default,0.0,1.0,2.0,3.0 +0.0,0.0,0.0,0.0,0.0 +0.1,0.0,0.0,0.0,0.0 +0.2,0.0,0.0,0.0,0.0 +0.3,0.05,0.05,0.05,0.05 +0.4,0.1,0.1,0.1,0.1 +0.5,0.175,0.175,0.175,0.175 +0.6,0.25,0.25,0.25,0.25 From 750da07de16e2f114c21ace678a6a1c7e430e1e9 Mon Sep 17 00:00:00 2001 From: Masahiro Kubota Date: Fri, 23 Aug 2024 01:37:41 +0900 Subject: [PATCH 7/8] fix: modify rosbag Signed-off-by: Masahiro Kubota --- .../src/aichallenge_launch/data/accel_map.csv | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv b/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv index a876b0df..165e7ee3 100644 --- a/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv +++ b/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv @@ -1,8 +1,8 @@ -default,0.0,1.0,2.0,3.0 -0.0,0.0,0.0,0.0,0.0 -0.1,0.0,0.0,0.0,0.0 -0.2,0.0,0.0,0.0,0.0 -0.3,0.05,0.05,0.05,0.05 -0.4,0.1,0.1,0.1,0.1 -0.5,0.175,0.175,0.175,0.175 -0.6,0.25,0.25,0.25,0.25 +default,0.0,1.0,2.0,3.0,4.0,5.0 +0.0,0.0,0.0,0.0,0.0,0.0,0.0 +0.1,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.3,0.05,0.05,0.05,0.05,0.05,0.05 +0.4,0.1,0.1,0.1,0.1,0.1,0.1 +0.5,0.175,0.175,0.175,0.175,0.175,0.175 +0.6,0.25,0.25,0.25,0.25,0.25,0.25 From 78eb55c27218ac1dfe78d8bf4877dfe3881e00d7 Mon Sep 17 00:00:00 2001 From: Masahiro Kubota Date: Sat, 24 Aug 2024 00:40:47 +0900 Subject: [PATCH 8/8] chore: delete redundant file Signed-off-by: Masahiro Kubota --- .../config/lanelet2_map_loader.param.yaml | 7 ------- 1 file changed, 7 deletions(-) delete mode 100755 aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/lanelet2_map_loader.param.yaml diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/lanelet2_map_loader.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/lanelet2_map_loader.param.yaml deleted file mode 100755 index e3a4fdf8..00000000 --- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/lanelet2_map_loader.param.yaml +++ /dev/null @@ -1,7 +0,0 @@ -/**: - ros__parameters: - lanelet2_map_projector_type: MGRS # Options: MGRS, UTM - latitude: 40.81187906 # Latitude of map_origin, using in UTM - longitude: 29.35810110 # Longitude of map_origin, using in UTM - - center_line_resolution: 5.0 # [m]