diff --git a/launch/planning_launch/CMakeLists.txt b/launch/planning_launch/CMakeLists.txt new file mode 100644 index 0000000000000..de7ef164be1b0 --- /dev/null +++ b/launch/planning_launch/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.5) +project(planning_launch) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wno-unused-parameter -Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/launch/planning_launch/README.md b/launch/planning_launch/README.md new file mode 100644 index 0000000000000..84bf70d26b6b9 --- /dev/null +++ b/launch/planning_launch/README.md @@ -0,0 +1,16 @@ +# planning_launch + +## Structure + +![planning_launch](./planning_launch.drawio.svg) + +## Package Dependencies + +Please see `` in `package.xml`. + +## Usage + +```xml + + +``` diff --git a/launch/planning_launch/config/scenario_planning/common/common.param.yaml b/launch/planning_launch/config/scenario_planning/common/common.param.yaml new file mode 100644 index 0000000000000..a23570a5fc791 --- /dev/null +++ b/launch/planning_launch/config/scenario_planning/common/common.param.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + # constraints param for normal driving + normal: + min_acc: -0.5 # min deceleration [m/ss] + max_acc: 1.0 # max acceleration [m/ss] + min_jerk: -0.5 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] + + # constraints to be observed + limit: + min_acc: -2.5 # min deceleration limit [m/ss] + max_acc: 1.0 # max acceleration limit [m/ss] + min_jerk: -1.5 # min jerk limit [m/sss] + max_jerk: 1.5 # max jerk limit [m/sss] diff --git a/launch/planning_launch/config/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml b/launch/planning_launch/config/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml new file mode 100644 index 0000000000000..329714e3d371e --- /dev/null +++ b/launch/planning_launch/config/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml @@ -0,0 +1,25 @@ +/**: + ros__parameters: + resample: + ds_resample: 0.1 + num_resample: 1 + delta_yaw_threshold: 0.785 + + latacc: + enable_constant_velocity_while_turning: false + constant_velocity_dist_threshold: 2.0 + + forward: + max_acc: 1.0 + min_acc: -1.0 + max_jerk: 0.3 + min_jerk: -0.3 + kp: 0.3 + + backward: + start_jerk: -0.1 + min_jerk_mild_stop: -0.3 + min_jerk: -1.5 + min_acc_mild_stop: -1.0 + min_acc: -2.5 + span_jerk: -0.01 diff --git a/launch/planning_launch/config/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml b/launch/planning_launch/config/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml new file mode 100644 index 0000000000000..a6906b7117fd5 --- /dev/null +++ b/launch/planning_launch/config/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + jerk_weight: 0.1 # weight for "smoothness" cost for jerk + over_v_weight: 10000.0 # weight for "over speed limit" cost + over_a_weight: 500.0 # weight for "over accel limit" cost + over_j_weight: 200.0 # weight for "over jerk limit" cost + jerk_filter_ds: 0.1 # resampling ds for jerk filter diff --git a/launch/planning_launch/config/scenario_planning/common/motion_velocity_smoother/L2.param.yaml b/launch/planning_launch/config/scenario_planning/common/motion_velocity_smoother/L2.param.yaml new file mode 100644 index 0000000000000..c993978204f10 --- /dev/null +++ b/launch/planning_launch/config/scenario_planning/common/motion_velocity_smoother/L2.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + pseudo_jerk_weight: 100.0 # weight for "smoothness" cost + over_v_weight: 100000.0 # weight for "over speed limit" cost + over_a_weight: 1000.0 # weight for "over accel limit" cost diff --git a/launch/planning_launch/config/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml b/launch/planning_launch/config/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml new file mode 100644 index 0000000000000..ec38010621cc9 --- /dev/null +++ b/launch/planning_launch/config/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + pseudo_jerk_weight: 200.0 # weight for "smoothness" cost + over_v_weight: 100000.0 # weight for "over speed limit" cost + over_a_weight: 5000.0 # weight for "over accel limit" cost diff --git a/launch/planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml b/launch/planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml new file mode 100644 index 0000000000000..db13e491b1ca5 --- /dev/null +++ b/launch/planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml @@ -0,0 +1,50 @@ +/**: + ros__parameters: + # motion state constraints + max_velocity: 20.0 # max velocity limit [m/s] + + # external velocity limit parameter + margin_to_insert_external_velocity_limit: 0.3 # margin distance to insert external velocity limit [m] + + # curve parameters + max_lateral_accel: 0.8 # max lateral acceleration limit [m/ss] + min_curve_velocity: 2.74 # min velocity at lateral acceleration limit [m/ss] + decel_distance_before_curve: 3.5 # slow speed distance before a curve for lateral acceleration limit + decel_distance_after_curve: 2.0 # slow speed distance after a curve for lateral acceleration limit + + # 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_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] + + # stop velocity + stopping_velocity: 2.778 # change target velocity to this value before v=0 point [m/s] + stopping_distance: 0.0 # distance for the stopping_velocity [m]. 0 means the stopping velocity is not applied. + + # path extraction parameters + extract_ahead_dist: 200.0 # forward trajectory distance used for planning [m] + extract_behind_dist: 5.0 # backward trajectory distance used for planning [m] + delta_yaw_threshold: 1.0472 # Allowed delta yaw between ego pose and trajectory pose [radian] + + # resampling parameters for optimization + max_trajectory_length: 200.0 # max trajectory length for resampling [m] + min_trajectory_length: 150.0 # min trajectory length for resampling [m] + resample_time: 5.0 # resample total time for dense sampling [s] + dense_dt: 0.1 # resample time interval for dense sampling [s] + dense_min_interval_distance: 0.1 # minimum points-interval length for dense sampling [m] + sparse_dt: 0.5 # resample time interval for sparse sampling [s] + sparse_min_interval_distance: 4.0 # minimum points-interval length for sparse sampling [m] + + # resampling parameters for post process + post_max_trajectory_length: 300.0 # max trajectory length for resampling [m] + post_min_trajectory_length: 30.0 # min trajectory length for resampling [m] + post_resample_time: 10.0 # resample total time for dense sampling [s] + post_dense_dt: 0.1 # resample time interval for dense sampling [s] + post_dense_min_interval_distance: 0.1 # minimum points-interval length for dense sampling [m] + post_sparse_dt: 0.1 # resample time interval for sparse sampling [s] + post_sparse_min_interval_distance: 1.0 # minimum points-interval length for sparse sampling [m] + + # system + over_stop_velocity_warn_thr: 1.389 # used to check if the optimization exceeds the input velocity on the stop point diff --git a/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml new file mode 100644 index 0000000000000..bfe310b73725f --- /dev/null +++ b/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -0,0 +1,41 @@ +# see AvoidanceParameters description in avoidance_module_data.hpp for description. +/**: + 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 + + threshold_distance_object_is_on_center: 1.0 # [m] + threshold_speed_object_is_stopped: 1.0 # [m/s] + object_check_forward_distance: 150.0 # [m] + object_check_backward_distance: 2.0 # [m] + lateral_collision_margin: 2.0 # [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] + + 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_hold_max_count: 20 + + # For prevention of large acceleration while avoidance + min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] + max_avoidance_acceleration: 0.5 # [m/ss] + + # 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 diff --git a/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml b/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml new file mode 100644 index 0000000000000..fcdcf5c78cef0 --- /dev/null +++ b/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml @@ -0,0 +1,14 @@ +/**: + 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 + drivable_area_resolution: 0.1 + drivable_area_width: 100.0 + drivable_area_height: 50.0 + refine_goal_search_radius_range: 7.5 + intersection_search_distance: 30.0 diff --git a/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml b/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml new file mode 100644 index 0000000000000..50934a74542ea --- /dev/null +++ b/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + lane_change: + min_stop_distance: 5.0 + stop_time: 2.0 + hysteresis_buffer_distance: 2.0 + lane_change_prepare_duration: 4.0 + lane_changing_duration: 8.0 + lane_change_finish_judge_buffer: 3.0 + minimum_lane_change_velocity: 5.6 + prediction_duration: 8.0 + prediction_time_resolution: 0.5 + static_obstacle_velocity_thresh: 1.5 + maximum_deceleration: 1.0 + enable_abort_lane_change: false + enable_collision_check_at_prepare_phase: false + use_predicted_path_outside_lanelet: false + use_all_predicted_path: false + enable_blocked_by_obstacle: false + lane_change_search_distance: 30.0 diff --git a/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml b/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml new file mode 100644 index 0000000000000..f685f8a6578eb --- /dev/null +++ b/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + lane_following: + expand_drivable_area: false + right_bound_offset: 0.5 + left_bound_offset: 0.5 + lane_change_prepare_duration: 2.0 diff --git a/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml b/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml new file mode 100644 index 0000000000000..434545674d283 --- /dev/null +++ b/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml @@ -0,0 +1,25 @@ +/**: + ros__parameters: + pull_out: + min_stop_distance: 2.0 + stop_time: 0.0 + hysteresis_buffer_distance: 1.0 + pull_out_prepare_duration: 4.0 + pull_out_duration: 2.0 + pull_out_finish_judge_buffer: 1.0 + minimum_pull_out_velocity: 2.0 + prediction_duration: 30.0 + prediction_time_resolution: 0.5 + static_obstacle_velocity_thresh: 1.5 + maximum_deceleration: 1.0 + enable_collision_check_at_prepare_phase: false + use_predicted_path_outside_lanelet: false + use_all_predicted_path: false + use_dynamic_object: true + enable_blocked_by_obstacle: false + pull_out_search_distance: 30.0 + before_pull_out_straight_distance: 5.0 + after_pull_out_straight_distance: 5.0 + maximum_lateral_jerk: 2.0 + minimum_lateral_jerk: 0.5 + deceleration_interval: 15.0 diff --git a/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml new file mode 100644 index 0000000000000..08ab028632a98 --- /dev/null +++ b/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml @@ -0,0 +1,25 @@ +/**: + ros__parameters: + pull_over: + min_stop_distance: 5.0 + stop_time: 2.0 + hysteresis_buffer_distance: 2.0 + pull_over_prepare_duration: 4.0 + pull_over_duration: 2.0 + pull_over_finish_judge_buffer: 3.0 + minimum_pull_over_velocity: 3.0 + prediction_duration: 30.0 + prediction_time_resolution: 0.5 + static_obstacle_velocity_thresh: 1.5 + maximum_deceleration: 1.0 + enable_collision_check_at_prepare_phase: false + use_predicted_path_outside_lanelet: false + use_all_predicted_path: false + enable_blocked_by_obstacle: false + pull_over_search_distance: 30.0 + after_pull_over_straight_distance: 5.0 + before_pull_over_straight_distance: 5.0 + margin_from_boundary: 0.5 + maximum_lateral_jerk: 2.0 + minimum_lateral_jerk: 0.5 + deceleration_interval: 15.0 diff --git a/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml b/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml new file mode 100644 index 0000000000000..79044041b4889 --- /dev/null +++ b/launch/planning_launch/config/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/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml b/launch/planning_launch/config/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml new file mode 100644 index 0000000000000..0bf42387a6621 --- /dev/null +++ b/launch/planning_launch/config/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/launch/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml b/launch/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml new file mode 100644 index 0000000000000..8150af7436212 --- /dev/null +++ b/launch/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml @@ -0,0 +1,90 @@ +/**: + ros__parameters: + # trajectory total/fixing length + trajectory_length: 300 # total trajectory length[m] + forward_fixing_distance: 5.0 # forward fixing length from base_link[m] + backward_fixing_distance: 3.0 # backward fixing length from base_link[m] + + # clearance(distance) when generating trajectory + clearance_from_road: 0.2 # clearance from road boundary[m] + clearance_from_object: 1.0 # clearance from object[m] + min_object_clearance_for_joint: 3.2 # minimum clearance from object[m] when judging is_including_only_smooth_range + extra_desired_clearance_from_road: 0.0 # extra desired clearance from road + + # clearance for unique points + clearance_for_straight_line: 0.05 # minimum optimizing range around straight points + clearance_for_joint: 0.1 # minimum optimizing range around joint points + clearance_for_only_smoothing: 0.1 # minimum optimizing range when applying only smoothing + range_for_extend_joint: 0.1 # minimum optimizing range around joint points for extending + clearance_from_object_for_straight: 10.0 # minimum clearance from object when judging straight line + + # avoiding param + max_avoiding_objects_velocity_ms: 0.5 # maximum velocity for avoiding objects[m/s] + max_avoiding_ego_velocity_ms: 6.0 # maximum ego velocity when avoiding objects[m/s] + center_line_width: 1.7 # replaced with /vehicle_info/vehicle_width when is_using_vehicle_config is true, center line width around path points used for judging that object is required to avoid or not + acceleration_for_non_deceleration_range: 1.0 # assumed acceleration for calculating non deceleration range + + # solving quadratic programming + qp_max_iteration: 10000 # max iteration when solving QP + qp_eps_abs: 1.0e-8 # eps abs when solving OSQP + qp_eps_rel: 1.0e-11 # eps rel when solving OSQP + qp_eps_abs_for_extending: 1.0e-8 # eps abs when solving OSQP for extending + qp_eps_rel_for_extending: 1.0e-9 # eps rel when solving OSQP for extending + qp_eps_abs_for_visualizing: 1.0e-6 # eps abs when solving OSQP for visualizing + qp_eps_rel_for_visualizing: 1.0e-8 # eps rel when solving OSQP for visualizing + + # constrain space + is_getting_constraints_close2path_points: true # generate trajectory closer to path points + max_x_constrain_search_range: 0.0 # maximum x search range in local coordinate + coef_x_constrain_search_resolution: 1.0 # coef for fine sampling when exploring x direction + coef_y_constrain_search_resolution: 0.5 # coef for fine sampling when exploring y direction + keep_space_shape_x: 0.2 # keep space for x direction from base_link[m] + keep_space_shape_y: 0.1 # replaced with /vehicle_info/vehicle_width when is_using_vehicle_config is true, keep space for y direction from base_link[m] + max_lon_space_for_driveable_constraint: 0.5 # maximum lon(x) space when optimizing point[m] + + is_publishing_clearance_map: true # publish clearance map as nav_msgs::OccupancyGrid + is_publishing_area_with_objects: true # publish occupancy map as nav_msgs::OccupancyGrid + enable_avoidance: true # enable avoidance function + is_using_vehicle_config: true # use vehicle config + num_sampling_points: 100 # number of optimizing points + num_joint_buffer_points: 3 # number of joint buffer points + num_joint_buffer_points_for_extending: 6 # number of joint buffer points for extending + num_offset_for_begin_idx: 2 # number of shifting points for beginning non-fix idx + num_fix_points_for_extending: 50 # number of fixing points when extending + delta_arc_length_for_optimization: 1.0 # delta arc length when optimizing[m] + delta_arc_length_for_trajectory: 0.1 # delta arc length for trajectory[m] + delta_dist_threshold_for_closest_point: 3.0 # delta dist thres for closest point[m] + delta_yaw_threshold_for_closest_point: 1.046 #M_PI/3.0, delta yaw thres for closest point + delta_yaw_threshold_for_straight: 0.02 # delta dist thres for straight point + + # mpt param + # vehicle param for mpt + max_steer_deg: 40.0 # max steering angle [deg] + steer_tau: 0.1 # steering dynamics time constant (1d approximation) [s] + + # trajectory param for mpt + num_curvature_sampling_points: 5 # number of sampling points when calculating curvature + delta_arc_length_for_mpt_points: 0.5 # delta arc length when generating mpt[m] + forward_fixing_mpt_distance: 10 # number of fixing points around ego vehicle + + # optimization param for mpt + is_hard_fixing_terminal_point: false # hard fixing terminal point + base_point_weight: 2000.0 # slack weight for lateral error around base_link + top_point_weight: 1000.0 # slack weight for lateral error around vehicle-top point + mid_point_weight: 1000.0 # slack weight for lateral error around the middle point + lat_error_weight: 10.0 # weight for lateral error + yaw_error_weight: 0.0 # weight for yaw error + steer_input_weight: 0.01 # weight for steering input + steer_rate_weight: 1.0 # weight for steering rate + steer_acc_weight: 0.000001 # weight for steering acceleration + terminal_lat_error_weight: 0.0 # weight for lateral error at terminal point + terminal_yaw_error_weight: 100.0 # weight for yaw error at terminal point + terminal_path_lat_error_weight: 1000.0 # weight for lateral error at path end point + terminal_path_yaw_error_weight: 1000.0 # weight for yaw error at path end point + zero_ff_steer_angle: 0.5 # threshold that feed-forward angle becomes zero + + # replanning & trimming trajectory param outside algorithm + min_delta_dist_for_replan: 10.0 # minimum delta dist thres for replan[m] + min_delta_time_sec_for_replan: 1.0 # minimum delta time for replan[second] + max_dist_for_extending_end_point: 5.0 # minimum delta dist thres for extending last point[m] + distance_for_path_shape_change_detection: 2.0 # minimum delta dist thres for detecting path shape change diff --git a/launch/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml b/launch/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml new file mode 100644 index 0000000000000..dd76f2c4e540d --- /dev/null +++ b/launch/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml @@ -0,0 +1,38 @@ +/**: + ros__parameters: + adaptive_cruise_control: + # Adaptive Cruise Control (ACC) config + use_object_to_estimate_vel: true # use tracking objects for estimating object velocity or not + use_pcl_to_estimate_vel: true # use pcl for estimating object velocity or not + consider_obj_velocity: true # consider forward vehicle velocity to ACC or not + + # general parameter for ACC + obstacle_velocity_thresh_to_start_acc: 1.5 # start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s] + obstacle_velocity_thresh_to_stop_acc: 1.0 # stop adaptive cruise control when the velocity of the forward obstacle falls below this value [m/s] + emergency_stop_acceleration: -5.0 # supposed minimum acceleration (deceleration) in emergency stop [m/ss] + emergency_stop_idling_time: 0.5 # supposed idling time to start emergency stop [s] + min_dist_stop: 4.0 # minimum distance of emergency stop [m] + max_standard_acceleration: 0.5 # supposed maximum acceleration in active cruise control [m/ss] + min_standard_acceleration: -1.0 # supposed minimum acceleration (deceleration) in active cruise control + standard_idling_time: 0.5 # supposed idling time to react object in active cruise control [s] + min_dist_standard: 4.0 # minimum distance in active cruise control [m] + obstacle_min_standard_acceleration: -1.5 # supposed minimum acceleration of forward obstacle [m/ss] + margin_rate_to_change_vel: 0.3 # margin to insert upper velocity [-] + + # pid parameter for ACC + p_coefficient_positive: 0.1 # coefficient P in PID control (used when target dist -current_dist >=0) [-] + p_coefficient_negative: 0.3 # coefficient P in PID control (used when target dist -current_dist <0) [-] + d_coefficient_positive: 0.0 # coefficient D in PID control (used when delta_dist >=0) [-] + d_coefficient_negative: 0.2 # coefficient D in PID control (used when delta_dist <0) [-] + + # parameter for object velocity estimation + object_polygon_length_margin: 2.0 # The distance to extend the polygon length the object in pointcloud-object matching [m] + object_polygon_width_margin: 0.5 # The distance to extend the polygon width the object in pointcloud-object matching [m] + valid_estimated_vel_diff_time: 1.0 # Maximum time difference treated as continuous points in speed estimation using a point cloud [s] + valid_vel_que_time: 0.5 # Time width of information used for speed estimation in speed estimation using a point cloud [s] + valid_estimated_vel_max: 20.0 # Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s] + valid_estimated_vel_min: -20.0 # Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s] + thresh_vel_to_stop: 1.5 # Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s] + lowpass_gain_of_upper_velocity: 0.75 # Lowpass-gain of upper velocity + use_rough_velocity_estimation: false # Use rough estimated velocity if the velocity estimation is failed (#### If this parameter is true, the vehicle may collide with the front car. Be careful. ####) + rough_velocity_rate: 0.9 # In the rough velocity estimation, the velocity of front car is estimated as self current velocity * this value diff --git a/launch/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml b/launch/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml new file mode 100644 index 0000000000000..82a77203343f7 --- /dev/null +++ b/launch/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml @@ -0,0 +1,25 @@ +/**: + ros__parameters: + stop_planner: + stop_margin: 5.0 # stop margin distance from obstacle on the path [m] + min_behavior_stop_margin: 5.0 # stop margin distance when any other stop point is inserted in stop margin [m] + step_length: 1.0 # step length for pointcloud search range [m] + extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance + expand_stop_range: 0.0 # margin of vehicle footprint [m] + + slow_down_planner: + # slow down planner parameters + forward_margin: 5.0 # margin distance from slow down point to vehicle front [m] + backward_margin: 0.0 # margin distance from slow down point to vehicle rear [m] + expand_slow_down_range: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m] + max_slow_down_vel: 1.38 # max slow down velocity [m/s] + min_slow_down_vel: 0.28 # min slow down velocity [m/s] + + # slow down constraint parameters + consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel + forward_margin_min: 1.0 # min margin for relaxing slow down margin [m/s] + forward_margin_span: -0.1 # fineness param for relaxing slow down margin [m/s] + jerk_min_slow_down: -0.6 # min slow down jerk constraint [m/sss] + jerk_span: -0.01 # fineness param for planning deceleration jerk [m/sss] + jerk_start: -0.1 # init jerk used for deceleration planning [m/sss] + slow_down_vel: 1.38 # target slow down velocity [m/s] diff --git a/launch/planning_launch/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml b/launch/planning_launch/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml new file mode 100644 index 0000000000000..8354ea60beca8 --- /dev/null +++ b/launch/planning_launch/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + + # obstacle check + use_pointcloud: true # use pointcloud as obstacle check + use_dynamic_object: true # use dynamic object as obstacle check + surround_check_distance: 0.5 # if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m] + surround_check_recover_distance: 0.8 # if no object exists in this distance, transit to "non-surrounding-obstacle" status [m] + state_clear_time: 2.0 + + # ego stop state + stop_state_ego_speed: 0.1 #[m/s] diff --git a/launch/planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml b/launch/planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml new file mode 100644 index 0000000000000..72648a9dfdd5c --- /dev/null +++ b/launch/planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml @@ -0,0 +1,38 @@ +/**: + ros__parameters: + # -- Node Configurations -- + planning_algorithm: "astar" + waypoints_velocity: 5.0 + update_rate: 10.0 + th_arrived_distance_m: 1.0 + th_stopped_time_sec: 1.0 + th_stopped_velocity_mps: 0.01 + th_course_out_distance_m: 1.0 + replan_when_obstacle_found: true + replan_when_course_out: true + + # -- Configurations common to the all planners -- + # base configs + time_limit: 30000.0 + # robot configs # TODO replace by vehicle_info + robot_length: 4.5 + robot_width: 1.75 + robot_base2back: 1.0 + minimum_turning_radius: 9.0 + maximum_turning_radius: 9.0 + turning_radius_size: 1 + # search configs + theta_size: 144 + angle_goal_range: 6.0 + curve_weight: 1.2 + reverse_weight: 2.0 + lateral_goal_range: 0.5 + longitudinal_goal_range: 2.0 + # costmap configs + obstacle_threshold: 100 + + # -- A* search Configurations -- + astar: + only_behind_solutions: false + use_back: true + distance_heuristic_weight: 1.0 diff --git a/launch/planning_launch/launch/mission_planning/mission_planning.launch.py b/launch/planning_launch/launch/mission_planning/mission_planning.launch.py new file mode 100644 index 0000000000000..cc3952f93be96 --- /dev/null +++ b/launch/planning_launch/launch/mission_planning/mission_planning.launch.py @@ -0,0 +1,73 @@ +# Copyright 2021 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + container = ComposableNodeContainer( + name="mission_planning_container", + namespace="", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + ComposableNode( + package="mission_planner", + plugin="mission_planner::MissionPlannerLanelet2", + name="mission_planner", + remappings=[ + ("input/vector_map", "/map/vector_map"), + ("input/goal_pose", "/planning/mission_planning/goal"), + ("input/checkpoint", "/planning/mission_planning/checkpoint"), + ("output/route", "/planning/mission_planning/route"), + ("debug/route_marker", "/planning/mission_planning/route_marker"), + ], + parameters=[ + { + "map_frame": "map", + "base_link_frame": "base_link", + } + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ), + ComposableNode( + package="mission_planner", + plugin="mission_planner::GoalPoseVisualizer", + name="goal_pose_visualizer", + remappings=[ + ("input/route", "/planning/mission_planning/route"), + ("output/goal_pose", "/planning/mission_planning/echo_back_goal_pose"), + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ), + ], + ) + return launch.LaunchDescription( + [ + DeclareLaunchArgument( + "use_intra_process", + default_value="false", + description="use ROS2 component container communication", + ), + container, + ] + ) diff --git a/launch/planning_launch/launch/mission_planning/mission_planning.launch.xml b/launch/planning_launch/launch/mission_planning/mission_planning.launch.xml new file mode 100644 index 0000000000000..712840021ba96 --- /dev/null +++ b/launch/planning_launch/launch/mission_planning/mission_planning.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/launch/planning_launch/launch/planning.launch.xml b/launch/planning_launch/launch/planning.launch.xml new file mode 100644 index 0000000000000..665116d32cbeb --- /dev/null +++ b/launch/planning_launch/launch/planning.launch.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/planning_launch/launch/planning_diagnostics/planning_error_monitor.launch.xml b/launch/planning_launch/launch/planning_diagnostics/planning_error_monitor.launch.xml new file mode 100644 index 0000000000000..0555cbf965a2c --- /dev/null +++ b/launch/planning_launch/launch/planning_diagnostics/planning_error_monitor.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/launch/planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/planning_launch/launch/scenario_planning/lane_driving.launch.xml new file mode 100644 index 0000000000000..e074edf502f9b --- /dev/null +++ b/launch/planning_launch/launch/scenario_planning/lane_driving.launch.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py new file mode 100644 index 0000000000000..6a80e7fe05c39 --- /dev/null +++ b/launch/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -0,0 +1,406 @@ +# Copyright 2021 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import ExecuteProcess +from launch.actions import GroupAction +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import yaml + + +def launch_setup(context, *args, **kwargs): + + # vehicle information parameter + vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) + with open(vehicle_info_param_path, "r") as f: + vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + # behavior path planner + side_shift_param_path = os.path.join( + get_package_share_directory("planning_launch"), + "config", + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_path_planner", + "side_shift", + "side_shift.param.yaml", + ) + with open(side_shift_param_path, "r") as f: + side_shift_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + avoidance_param_path = os.path.join( + get_package_share_directory("planning_launch"), + "config", + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_path_planner", + "avoidance", + "avoidance.param.yaml", + ) + with open(avoidance_param_path, "r") as f: + avoidance_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + lane_change_param_path = os.path.join( + get_package_share_directory("planning_launch"), + "config", + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_path_planner", + "lane_change", + "lane_change.param.yaml", + ) + with open(lane_change_param_path, "r") as f: + lane_change_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + lane_following_param_path = os.path.join( + get_package_share_directory("planning_launch"), + "config", + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_path_planner", + "lane_following", + "lane_following.param.yaml", + ) + with open(lane_following_param_path, "r") as f: + lane_following_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + pull_over_param_path = os.path.join( + get_package_share_directory("planning_launch"), + "config", + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_path_planner", + "pull_over", + "pull_over.param.yaml", + ) + with open(pull_over_param_path, "r") as f: + pull_over_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + pull_out_param_path = os.path.join( + get_package_share_directory("planning_launch"), + "config", + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_path_planner", + "pull_out", + "pull_out.param.yaml", + ) + with open(pull_out_param_path, "r") as f: + pull_out_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + behavior_path_planner_param_path = os.path.join( + get_package_share_directory("planning_launch"), + "config", + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_path_planner", + "behavior_path_planner.param.yaml", + ) + with open(behavior_path_planner_param_path, "r") as f: + behavior_path_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + behavior_path_planner_component = ComposableNode( + package="behavior_path_planner", + plugin="behavior_path_planner::BehaviorPathPlannerNode", + name="behavior_path_planner", + namespace="", + remappings=[ + ("~/input/route", LaunchConfiguration("input_route_topic_name")), + ("~/input/vector_map", LaunchConfiguration("map_topic_name")), + ("~/input/perception", "/perception/object_recognition/objects"), + ("~/input/odometry", "/localization/kinematic_state"), + ( + "~/input/external_approval", + "/planning/scenario_planning/lane_driving/behavior_planning/" + "behavior_path_planner/path_change_approval", + ), + ( + "~/input/force_approval", + "/planning/scenario_planning/lane_driving/behavior_planning/" + "behavior_path_planner/path_change_force", + ), + ("~/output/path", "path_with_lane_id"), + ( + "~/output/ready", + "/planning/scenario_planning/lane_driving/behavior_planning/" + "behavior_path_planner/ready_module", + ), + ( + "~/output/running", + "/planning/scenario_planning/lane_driving/behavior_planning/" + "behavior_path_planner/running_modules", + ), + ( + "~/output/force_available", + "/planning/scenario_planning/lane_driving/behavior_planning/" + "behavior_path_planner/force_available", + ), + ("~/output/turn_indicators_cmd", "/planning/turn_indicators_cmd"), + ("~/output/hazard_lights_cmd", "/planning/hazard_lights_cmd"), + ], + parameters=[ + side_shift_param, + avoidance_param, + lane_change_param, + lane_following_param, + pull_over_param, + pull_out_param, + behavior_path_planner_param, + vehicle_info_param, + { + "bt_tree_config_path": [ + FindPackageShare("behavior_path_planner"), + "/config/behavior_path_planner_tree.xml", + ], + "planning_hz": 10.0, + }, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # behavior velocity planner + blind_spot_param_path = os.path.join( + get_package_share_directory("behavior_velocity_planner"), + "config", + "blind_spot.param.yaml", + ) + with open(blind_spot_param_path, "r") as f: + blind_spot_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + crosswalk_param_path = os.path.join( + get_package_share_directory("behavior_velocity_planner"), + "config", + "crosswalk.param.yaml", + ) + with open(crosswalk_param_path, "r") as f: + crosswalk_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + detection_area_param_path = os.path.join( + get_package_share_directory("behavior_velocity_planner"), + "config", + "detection_area.param.yaml", + ) + with open(detection_area_param_path, "r") as f: + detection_area_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + intersection_param_path = os.path.join( + get_package_share_directory("behavior_velocity_planner"), + "config", + "intersection.param.yaml", + ) + with open(intersection_param_path, "r") as f: + intersection_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + stop_line_param_path = os.path.join( + get_package_share_directory("behavior_velocity_planner"), + "config", + "stop_line.param.yaml", + ) + with open(stop_line_param_path, "r") as f: + stop_line_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + traffic_light_param_path = os.path.join( + get_package_share_directory("behavior_velocity_planner"), + "config", + "traffic_light.param.yaml", + ) + with open(traffic_light_param_path, "r") as f: + traffic_light_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + virtual_traffic_light_param_path = os.path.join( + get_package_share_directory("behavior_velocity_planner"), + "config", + "virtual_traffic_light.param.yaml", + ) + with open(virtual_traffic_light_param_path, "r") as f: + virtual_traffic_light_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + occlusion_spot_param_path = os.path.join( + get_package_share_directory("behavior_velocity_planner"), + "config", + "occlusion_spot.param.yaml", + ) + with open(occlusion_spot_param_path, "r") as f: + occlusion_spot_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + no_stopping_area_param_path = os.path.join( + get_package_share_directory("behavior_velocity_planner"), + "config", + "no_stopping_area.param.yaml", + ) + with open(no_stopping_area_param_path, "r") as f: + no_stopping_area_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + behavior_velocity_planner_component = ComposableNode( + package="behavior_velocity_planner", + plugin="behavior_velocity_planner::BehaviorVelocityPlannerNode", + name="behavior_velocity_planner", + namespace="", + remappings=[ + ("~/input/path_with_lane_id", "path_with_lane_id"), + ("~/input/vector_map", "/map/vector_map"), + ("~/input/vehicle_odometry", "/localization/kinematic_state"), + ("~/input/dynamic_objects", "/perception/object_recognition/objects"), + ( + "~/input/no_ground_pointcloud", + "/perception/obstacle_segmentation/pointcloud", + ), + ( + "~/input/traffic_signals", + "/perception/traffic_light_recognition/traffic_signals", + ), + ( + "~/input/external_traffic_signals", + "/external/traffic_light_recognition/traffic_signals", + ), + ("~/input/virtual_traffic_light_states", "/awapi/tmp/virtual_traffic_light_states"), + ( + "~/input/occupancy_grid", + "/perception/occupancy_grid_map/map", + ), + ("~/output/path", "path"), + ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"), + ( + "~/output/infrastructure_commands", + "/planning/scenario_planning/status/infrastructure_commands", + ), + ("~/output/traffic_signal", "debug/traffic_signal"), + ], + parameters=[ + { + "launch_stop_line": True, + "launch_crosswalk": True, + "launch_traffic_light": True, + "launch_intersection": True, + "launch_blind_spot": True, + "launch_detection_area": True, + "launch_virtual_traffic_light": True, + "launch_occlusion_spot": True, + "launch_no_stopping_area": True, + "forward_path_length": 1000.0, + "backward_path_length": 5.0, + "max_accel": -2.8, + "delay_response_time": 1.3, + }, + blind_spot_param, + crosswalk_param, + detection_area_param, + intersection_param, + stop_line_param, + traffic_light_param, + virtual_traffic_light_param, + occlusion_spot_param, + no_stopping_area_param, + vehicle_info_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + container = ComposableNodeContainer( + name="behavior_planning_container", + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[ + behavior_path_planner_component, + behavior_velocity_planner_component, + ], + output="screen", + ) + + group = GroupAction( + [ + container, + ExecuteProcess( + cmd=[ + "ros2", + "topic", + "pub", + "/planning/scenario_planning/lane_driving/behavior_planning/" + "behavior_path_planner/path_change_approval", + "tier4_planning_msgs/msg/Approval", + "{approval: true}", + "-r", + "10", + ] + ), + ] + ) + + return [group] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, description=description) + ) + + add_launch_arg( + "vehicle_info_param_file", + [ + FindPackageShare("vehicle_info_util"), + "/config/vehicle_info.param.yaml", + ], + "path to the parameter file of vehicle information", + ) + + add_launch_arg( + "input_route_topic_name", "/planning/mission_planning/route", "input topic of route" + ) + add_launch_arg("map_topic_name", "/map/vector_map", "input topic of map") + + # component + add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") + add_launch_arg("use_multithread", "false", "use multithread") + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + launch_arguments + + [ + set_container_executable, + set_container_mt_executable, + ] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/launch/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml new file mode 100644 index 0000000000000..234da6fad28c4 --- /dev/null +++ b/launch/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py new file mode 100644 index 0000000000000..43e1237dc0a96 --- /dev/null +++ b/launch/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py @@ -0,0 +1,265 @@ +# Copyright 2021 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import yaml + + +def launch_setup(context, *args, **kwargs): + # vehicle information param path + vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) + with open(vehicle_info_param_path, "r") as f: + vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + # planning common param path + common_param_path = os.path.join( + get_package_share_directory("planning_launch"), + "config", + "scenario_planning", + "common", + "common.param.yaml", + ) + with open(common_param_path, "r") as f: + common_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + # obstacle avoidance planner + obstacle_avoidance_planner_param_path = os.path.join( + get_package_share_directory("planning_launch"), + "config", + "scenario_planning", + "lane_driving", + "motion_planning", + "obstacle_avoidance_planner", + "obstacle_avoidance_planner.param.yaml", + ) + with open(obstacle_avoidance_planner_param_path, "r") as f: + obstacle_avoidance_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] + obstacle_avoidance_planner_component = ComposableNode( + package="obstacle_avoidance_planner", + plugin="ObstacleAvoidancePlanner", + name="obstacle_avoidance_planner", + namespace="", + remappings=[ + ("~/input/objects", "/perception/object_recognition/objects"), + ("~/input/path", LaunchConfiguration("input_path_topic")), + ("~/output/path", "obstacle_avoidance_planner/trajectory"), + ], + parameters=[ + obstacle_avoidance_planner_param, + vehicle_info_param, + {"is_showing_debug_info": False}, + {"is_stopping_if_outside_drivable_area": True}, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # surround obstacle checker + surround_obstacle_checker_param_path = os.path.join( + get_package_share_directory("planning_launch"), + "config", + "scenario_planning", + "lane_driving", + "motion_planning", + "surround_obstacle_checker", + "surround_obstacle_checker.param.yaml", + ) + with open(surround_obstacle_checker_param_path, "r") as f: + surround_obstacle_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] + surround_obstacle_checker_component = ComposableNode( + package="surround_obstacle_checker", + plugin="SurroundObstacleCheckerNode", + name="surround_obstacle_checker", + namespace="", + remappings=[ + ("~/output/no_start_reason", "/planning/scenario_planning/status/no_start_reason"), + ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"), + ("~/output/trajectory", "surround_obstacle_checker/trajectory"), + ( + "~/input/pointcloud", + "/perception/obstacle_segmentation/pointcloud", + ), + ("~/input/objects", "/perception/object_recognition/objects"), + ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"), + ], + parameters=[ + surround_obstacle_checker_param, + vehicle_info_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # relay + relay_component = ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="skip_surround_obstacle_check_relay", + namespace="", + parameters=[ + { + "input_topic": "obstacle_avoidance_planner/trajectory", + "output_topic": "surround_obstacle_checker/trajectory", + "type": "autoware_planning_msgs/msg/Trajectory", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # obstacle stop planner + obstacle_stop_planner_param_path = os.path.join( + get_package_share_directory("planning_launch"), + "config", + "scenario_planning", + "lane_driving", + "motion_planning", + "obstacle_stop_planner", + "obstacle_stop_planner.param.yaml", + ) + obstacle_stop_planner_acc_param_path = os.path.join( + get_package_share_directory("planning_launch"), + "config", + "scenario_planning", + "lane_driving", + "motion_planning", + "obstacle_stop_planner", + "adaptive_cruise_control.param.yaml", + ) + with open(obstacle_stop_planner_param_path, "r") as f: + obstacle_stop_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(obstacle_stop_planner_acc_param_path, "r") as f: + obstacle_stop_planner_acc_param = yaml.safe_load(f)["/**"]["ros__parameters"] + obstacle_stop_planner_component = ComposableNode( + package="obstacle_stop_planner", + plugin="motion_planning::ObstacleStopPlannerNode", + name="obstacle_stop_planner", + namespace="", + remappings=[ + ("~/output/stop_reason", "/planning/scenario_planning/status/stop_reason"), + ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"), + ("~/output/max_velocity", "/planning/scenario_planning/max_velocity_candidates"), + ( + "~/output/velocity_limit_clear_command", + "/planning/scenario_planning/clear_velocity_limit", + ), + ("~/output/trajectory", "/planning/scenario_planning/lane_driving/trajectory"), + ( + "~/input/pointcloud", + "/perception/obstacle_segmentation/pointcloud", + ), + ("~/input/objects", "/perception/object_recognition/objects"), + ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/trajectory", "surround_obstacle_checker/trajectory"), + ], + parameters=[ + common_param, + obstacle_stop_planner_param, + obstacle_stop_planner_acc_param, + vehicle_info_param, + {"enable_slow_down": False}, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + container = ComposableNodeContainer( + name="motion_planning_container", + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[ + obstacle_avoidance_planner_component, + obstacle_stop_planner_component, + ], + ) + + surround_obstacle_checker_loader = LoadComposableNodes( + composable_node_descriptions=[surround_obstacle_checker_component], + target_container=container, + condition=IfCondition(LaunchConfiguration("use_surround_obstacle_check")), + ) + + relay_loader = LoadComposableNodes( + composable_node_descriptions=[relay_component], + target_container=container, + condition=UnlessCondition(LaunchConfiguration("use_surround_obstacle_check")), + ) + + group = GroupAction([container, surround_obstacle_checker_loader, relay_loader]) + + return [group] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, description=description) + ) + + # vehicle information parameter file + add_launch_arg( + "vehicle_info_param_file", + [ + FindPackageShare("vehicle_info_util"), + "/config/vehicle_info.param.yaml", + ], + "path to the parameter file of vehicle information", + ) + + # obstacle_avoidance_planner + add_launch_arg( + "input_path_topic", + "/planning/scenario_planning/lane_driving/behavior_planning/path", + "input path topic of obstacle_avoidance_planner", + ) + + # surround obstacle checker + add_launch_arg("use_surround_obstacle_check", "true", "launch surround_obstacle_checker or not") + + add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") + add_launch_arg("use_multithread", "false", "use multithread") + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + launch_arguments + + [ + set_container_executable, + set_container_mt_executable, + ] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/launch/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml new file mode 100644 index 0000000000000..6a591ae51f859 --- /dev/null +++ b/launch/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/planning_launch/launch/scenario_planning/parking.launch.py b/launch/planning_launch/launch/scenario_planning/parking.launch.py new file mode 100644 index 0000000000000..f61ea64bcf645 --- /dev/null +++ b/launch/planning_launch/launch/scenario_planning/parking.launch.py @@ -0,0 +1,170 @@ +# Copyright 2021 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import PushRosNamespace +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import yaml + + +def launch_setup(context, *args, **kwargs): + + vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) + with open(vehicle_info_param_path, "r") as f: + vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + freespace_planner_param_path = os.path.join( + get_package_share_directory("planning_launch"), + "config", + "scenario_planning", + "parking", + "freespace_planner", + "freespace_planner.param.yaml", + ) + with open(freespace_planner_param_path, "r") as f: + freespace_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + container = ComposableNodeContainer( + name="parking_container", + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[ + ComposableNode( + package="costmap_generator", + plugin="CostmapGenerator", + name="costmap_generator", + remappings=[ + ("~/input/objects", "/perception/object_recognition/objects"), + ( + "~/input/points_no_ground", + "/perception/obstacle_segmentation/pointcloud", + ), + ("~/input/vector_map", "/map/vector_map"), + ("~/input/scenario", "/planning/scenario_planning/scenario"), + ("~/output/grid_map", "costmap_generator/grid_map"), + ("~/output/occupancy_grid", "costmap_generator/occupancy_grid"), + ], + parameters=[ + { + "costmap_frame": "map", + "vehicle_frame": "base_link", + "map_frame": "map", + "update_rate": 10.0, + "use_wayarea": True, + "use_objects": True, + "use_points": True, + "grid_min_value": 0.0, + "grid_max_value": 1.0, + "grid_resolution": 0.2, + "grid_length_x": 70.0, + "grid_length_y": 70.0, + "grid_position_x": 0.0, + "grid_position_y": 0.0, + "maximum_lidar_height_thres": 0.3, + "minimum_lidar_height_thres": -2.2, + "expand_polygon_size": 1.0, + "size_of_expansion_kernel": 9, + }, + vehicle_info_param, + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ), + ComposableNode( + package="freespace_planner", + plugin="freespace_planner::FreespacePlannerNode", + name="freespace_planner", + remappings=[ + ("~/input/route", "/planning/mission_planning/route"), + ("~/input/occupancy_grid", "costmap_generator/occupancy_grid"), + ("~/input/scenario", "/planning/scenario_planning/scenario"), + ("~/input/odometry", "/localization/kinematic_state"), + ("~/output/trajectory", "/planning/scenario_planning/parking/trajectory"), + ("is_completed", "/planning/scenario_planning/parking/is_completed"), + ], + parameters=[ + freespace_planner_param, + vehicle_info_param, + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ), + ], + ) + + group = GroupAction( + [ + PushRosNamespace("parking"), + container, + ] + ) + + return [group] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, description=description) + ) + + add_launch_arg( + "vehicle_info_param_file", + [ + FindPackageShare("vehicle_info_util"), + "/config/vehicle_info.param.yaml", + ], + "path to the parameter file of vehicle information", + ) + + # component + add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") + add_launch_arg("use_multithread", "false", "use multithread") + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + launch_arguments + + [ + set_container_executable, + set_container_mt_executable, + ] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/launch/planning_launch/launch/scenario_planning/parking.launch.xml b/launch/planning_launch/launch/scenario_planning/parking.launch.xml new file mode 100644 index 0000000000000..b898d29c1500b --- /dev/null +++ b/launch/planning_launch/launch/scenario_planning/parking.launch.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/planning_launch/launch/scenario_planning/scenario_planning.launch.xml new file mode 100644 index 0000000000000..bc586a1908ad7 --- /dev/null +++ b/launch/planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/planning_launch/package.xml b/launch/planning_launch/package.xml new file mode 100644 index 0000000000000..ccb151e6cb70d --- /dev/null +++ b/launch/planning_launch/package.xml @@ -0,0 +1,30 @@ + + + planning_launch + 0.1.0 + The planning_launch package + + Takamasa Horibe + + Apache License 2.0 + + ament_cmake_auto + + behavior_velocity_planner + costmap_generator + external_velocity_limit_selector + freespace_planner + mission_planner + motion_velocity_smoother + obstacle_avoidance_planner + obstacle_stop_planner + scenario_selector + surround_obstacle_checker + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/launch/planning_launch/planning_launch.drawio.svg b/launch/planning_launch/planning_launch.drawio.svg new file mode 100644 index 0000000000000..fba2aefed5e1b --- /dev/null +++ b/launch/planning_launch/planning_launch.drawio.svg @@ -0,0 +1,318 @@ + + + + + + + +
+
+
+ planning.launch.xml +
+
+
+ + package: planning_launch + +
+
+
+
+ + + planning.launch.xml... + + + + + + + +
+
+
+ mission_planning.launch.py +
+
+
+ package: planning_launch +
+
+
+
+ + + mission_planning.launch.py... + + + + + + + + + + + +
+
+
+ launch name +
+
+
+ + package: package name + +
+
+
+
+ + + launch name... + + + + + + + +
+
+
+ ex: +
+
+
+ + + ex: + + + + + + + +
+
+
+ node name +
+
+
+ + package: package name + +
+
+
+
+ + + node name... + + + + + + + +
+
+
+ other name +
+
+
+ + package: package name + +
+
+
+
+ + + other name... + + + + + + + +
+
+
+ scenario_planning.launch.xml +
+
+
+ package: planning_launch +
+
+
+
+ + + scenario_planning.launch.xml... + + + + + + + +
+
+
+ mission_planner.launch.xml +
+
+
+ package: mission_planner +
+
+
+
+ + + mission_planner.launch.xml... + + + + + + + +
+
+
+ goal_pose_visualizer.launch.xml +
+
+
+ package: mission_planner +
+
+
+
+ + + goal_pose_visualizer.launch.xml... + + + + + + + + + + + +
+
+
+ scenario_selector.launch.xml +
+
+
+ package: scenario_selector +
+
+
+
+ + + scenario_selector.launch.xml... + + + + + + + + + +
+
+
+ motion_velocity_optimizer.launch.xml +
+
+
+ package: motion_velocity_optimizer +
+
+
+
+ + + motion_velocity_optimizer.launch.xml... + + + + + + + + + +
+
+
+ lane_driving.launch.xml +
+
+
+ package: planning_launch +
+
+
+
+ + + lane_driving.launch.xml... + + + + + + + +
+
+
+ parking.launch.py +
+
+
+ package: planning_launch +
+
+
+
+ + + parking.launch.py... + + + + + + + + + + + + + Viewer does not support full SVG 1.1 + + + + \ No newline at end of file