diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index a8bd3f4796..e33089a5fd 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -119,9 +119,12 @@ - - - + + + + + + diff --git a/control_launch/config/trajectory_follower/mpc_follower.param.yaml b/control_launch/config/trajectory_follower/lateral/mpc.param.yaml similarity index 100% rename from control_launch/config/trajectory_follower/mpc_follower.param.yaml rename to control_launch/config/trajectory_follower/lateral/mpc.param.yaml diff --git a/control_launch/config/trajectory_follower/pure_pursuit.param.yaml b/control_launch/config/trajectory_follower/lateral/pure_pursuit.param.yaml similarity index 100% rename from control_launch/config/trajectory_follower/pure_pursuit.param.yaml rename to control_launch/config/trajectory_follower/lateral/pure_pursuit.param.yaml diff --git a/control_launch/config/trajectory_follower/longitudinal_controller.param.yaml b/control_launch/config/trajectory_follower/longitudinal/pid.param.yaml similarity index 100% rename from control_launch/config/trajectory_follower/longitudinal_controller.param.yaml rename to control_launch/config/trajectory_follower/longitudinal/pid.param.yaml diff --git a/control_launch/config/trajectory_follower/trajectory_follower_node.param.yaml b/control_launch/config/trajectory_follower/trajectory_follower_node.param.yaml new file mode 100644 index 0000000000..dcd985bb4a --- /dev/null +++ b/control_launch/config/trajectory_follower/trajectory_follower_node.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + ctrl_period: 0.03 + timeout_thr_sec: 0.5 diff --git a/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml b/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml index 4876699351..351ad877e1 100644 --- a/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml +++ b/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml @@ -12,9 +12,11 @@ lon_jerk_lim: 5.0 lat_acc_lim: 5.0 lat_jerk_lim: 5.0 + actual_steer_diff_lim: 1.0 on_transition: vel_lim: 50.0 lon_acc_lim: 1.0 lon_jerk_lim: 0.5 lat_acc_lim: 1.2 lat_jerk_lim: 0.75 + actual_steer_diff_lim: 0.05 diff --git a/control_launch/launch/control.launch.py b/control_launch/launch/control.launch.py index e2c69f208d..0dc4d1e836 100644 --- a/control_launch/launch/control.launch.py +++ b/control_launch/launch/control.launch.py @@ -39,6 +39,11 @@ def launch_setup(context, *args, **kwargs): lon_controller_param_path = LaunchConfiguration("lon_controller_param_path").perform(context) with open(lon_controller_param_path, "r") as f: lon_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] + trajectory_follower_node_param_path = LaunchConfiguration( + "trajectory_follower_node_param_path" + ).perform(context) + with open(trajectory_follower_node_param_path, "r") as f: + trajectory_follower_node_param = yaml.safe_load(f)["/**"]["ros__parameters"] vehicle_cmd_gate_param_path = LaunchConfiguration("vehicle_cmd_gate_param_path").perform( context ) @@ -61,8 +66,8 @@ def launch_setup(context, *args, **kwargs): shift_decider_param = yaml.safe_load(f)["/**"]["ros__parameters"] controller_component = ComposableNode( - package="trajectory_follower_nodes", - plugin="autoware::motion::control::trajectory_follower_nodes::Controller", + package="trajectory_follower_node", + plugin="autoware::motion::control::trajectory_follower_node::Controller", name="controller_node_exe", namespace="trajectory_follower", remappings=[ @@ -83,6 +88,7 @@ def launch_setup(context, *args, **kwargs): "longitudinal_controller_mode": LaunchConfiguration("longitudinal_controller_mode"), }, nearest_search_param, + trajectory_follower_node_param, lon_controller_param, lat_controller_param, ], @@ -208,7 +214,12 @@ def launch_setup(context, *args, **kwargs): launch_arguments=[ ("use_intra_process", LaunchConfiguration("use_intra_process")), ("target_container", "/control/control_container"), - ("initial_selector_mode", LaunchConfiguration("initial_selector_mode")), + # This is temporary uncomment. I will replace control_launch with tier4_control_launch soon. + # ("initial_selector_mode", LaunchConfiguration("initial_selector_mode")), + ( + "external_cmd_selector_param_path", + LaunchConfiguration("external_cmd_selector_param_path"), + ), ], ) @@ -260,8 +271,8 @@ def add_launch_arg(name: str, default_value=None, description=None): # lateral controller mode add_launch_arg( "lateral_controller_mode", - "mpc_follower", - "lateral controller mode: `mpc_follower` or `pure_pursuit`", + "mpc", + "lateral controller mode: `mpc` or `pure_pursuit`", ) # longitudinal controller mode @@ -284,7 +295,7 @@ def add_launch_arg(name: str, default_value=None, description=None): "lat_controller_param_path", [ FindPackageShare("control_launch"), - "/config/trajectory_follower/mpc_follower.param.yaml", + "/config/trajectory_follower/lateral/mpc.param.yaml", ], "path to the parameter file of lateral controller. default is `mpc_follower`", ) @@ -292,7 +303,15 @@ def add_launch_arg(name: str, default_value=None, description=None): "lon_controller_param_path", [ FindPackageShare("control_launch"), - "/config/trajectory_follower/longitudinal_controller.param.yaml", + "/config/trajectory_follower/longitudinal/pid.param.yaml", + ], + "path to the parameter file of longitudinal controller", + ) + add_launch_arg( + "trajectory_follower_node_param_path", + [ + FindPackageShare("control_launch"), + "/config/trajectory_follower/trajectory_follower_node.param.yaml", ], "path to the parameter file of longitudinal controller", ) @@ -332,6 +351,13 @@ def add_launch_arg(name: str, default_value=None, description=None): # external cmd selector add_launch_arg("initial_selector_mode", "remote", "local or remote") + add_launch_arg( + "external_cmd_selector_param_path", + [ + FindPackageShare("external_cmd_selector"), + "/config/external_cmd_selector.param.yaml", + ], + ) # component add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") diff --git a/control_launch/package.xml b/control_launch/package.xml index ed1180a06e..ebf76a49f8 100644 --- a/control_launch/package.xml +++ b/control_launch/package.xml @@ -14,8 +14,7 @@ external_cmd_converter lane_departure_checker shift_decider - trajectory_follower - trajectory_follower_nodes + trajectory_follower_node vehicle_cmd_gate ament_lint_auto diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index e698819fc9..40de19bd6c 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -2,63 +2,26 @@ /**: ros__parameters: avoidance: - resample_interval_for_planning: 0.3 - resample_interval_for_output: 4.0 - detection_area_right_expand_dist: 0.0 - detection_area_left_expand_dist: 1.0 - + 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 - - # For target object filtering - threshold_speed_object_is_stopped: 1.0 # [m/s] - threshold_time_object_is_moving: 1.0 # [s] - - object_check_forward_distance: 150.0 # [m] - object_check_backward_distance: 2.0 # [m] - - threshold_distance_object_is_on_center: 1.0 # [m] - - object_check_shiftable_ratio: 0.6 # [-] - object_check_min_road_shoulder_width: 0.5 # [m] - - # For lateral margin - object_envelope_buffer: 0.3 # [m] - lateral_collision_margin: 1.0 # [m] - lateral_collision_safety_buffer: 0.7 # [m] - - prepare_time: 2.0 # [s] - 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] - - road_shoulder_safety_margin: 0.0 # [m] - - max_right_shift_length: 5.0 - max_left_shift_length: 5.0 - - nominal_lateral_jerk: 0.2 # [m/s3] - max_lateral_jerk: 1.0 # [m/s3] - - object_last_seen_threshold: 2.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] - - # bound clipping for objects - enable_bound_clipping: false + enable_update_path_when_object_is_gone: false + enable_safety_check: false + enable_yield_maneuver: false # for debug publish_debug_marker: false print_debug_info: false - # not enabled yet - longitudinal_collision_margin_min_distance: 0.0 # [m] - longitudinal_collision_margin_time: 0.0 - # avoidance is performed for the object type with true target_object: car: true @@ -70,5 +33,80 @@ motorcycle: false pedestrian: false - # ---------- advanced parameters ---------- - avoidance_execution_lateral_threshold: 0.499 + # 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] + # detection range + 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: 50.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.0 # [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/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml index 3423bfa221..e34fa49cba 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml @@ -2,10 +2,8 @@ ros__parameters: backward_path_length: 5.0 forward_path_length: 300.0 - backward_length_buffer_for_end_of_lane: 5.0 backward_length_buffer_for_end_of_pull_over: 5.0 backward_length_buffer_for_end_of_pull_out: 5.0 - minimum_lane_change_length: 12.0 minimum_pull_over_length: 16.0 refine_goal_search_radius_range: 7.5 @@ -23,11 +21,11 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 - expected_front_deceleration: -0.5 + expected_front_deceleration: -1.0 expected_rear_deceleration: -1.0 - expected_front_deceleration_for_abort: -2.0 - expected_rear_deceleration_for_abort: -2.5 + 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: 2.0 diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml index fc8b314b09..891e54a544 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml @@ -2,18 +2,33 @@ ros__parameters: lane_change: lane_change_prepare_duration: 4.0 # [s] - lane_changing_duration: 8.0 # [s] + lane_changing_safety_check_duration: 8.0 # [s] + minimum_lane_change_prepare_distance: 2.0 # [m] + minimum_lane_change_length: 16.5 + backward_length_buffer_for_end_of_lane: 2.0 lane_change_finish_judge_buffer: 3.0 # [m] - minimum_lane_change_velocity: 5.6 # [m/s] + + 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: 10 - abort_lane_change_velocity_thresh: 0.5 - abort_lane_change_angle_thresh: 10.0 # [deg] - abort_lane_change_distance_thresh: 0.3 # [m] - enable_abort_lane_change: true + + # collision check enable_collision_check_at_prepare_phase: true + prepare_phase_ignore_target_speed_thresh: 0.1 # [m/s] use_predicted_path_outside_lanelet: true 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/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml index 75b5facaaa..4c9f4fd41b 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: pull_over: - request_length: 200.0 + 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. @@ -17,7 +17,7 @@ backward_goal_search_length: 20.0 goal_search_interval: 2.0 longitudinal_margin: 3.0 - max_lateral_offset: 1.0 + max_lateral_offset: 0.5 lateral_offset_interval: 0.25 # occupancy grid map use_occupancy_grid: true @@ -34,7 +34,7 @@ maximum_lateral_jerk: 2.0 minimum_lateral_jerk: 0.5 deceleration_interval: 15.0 - after_pull_over_straight_distance: 5.0 + after_pull_over_straight_distance: 1.0 # parallel parking path enable_arc_forward_parking: true enable_arc_backward_parking: true diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml index 31f75d7f7c..5af1c99f8a 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml @@ -6,3 +6,4 @@ backward_length: 15.0 # [m] ignore_width_from_center_line: 0.7 # [m] max_future_movement_time: 10.0 # [second] + threshold_yaw_diff: 0.523 # [rad]