diff --git a/planning_launch/config/scenario_planning/common/common.param.yaml b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml similarity index 100% rename from planning_launch/config/scenario_planning/common/common.param.yaml rename to autoware_launch/config/planning/scenario_planning/common/common.param.yaml diff --git a/planning_launch/config/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml b/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml similarity index 100% rename from planning_launch/config/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml rename to autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml diff --git a/planning_launch/config/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml b/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml similarity index 100% rename from planning_launch/config/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml rename to autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml diff --git a/planning_launch/config/scenario_planning/common/motion_velocity_smoother/L2.param.yaml b/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/L2.param.yaml similarity index 100% rename from planning_launch/config/scenario_planning/common/motion_velocity_smoother/L2.param.yaml rename to autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/L2.param.yaml diff --git a/planning_launch/config/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml b/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml similarity index 100% rename from planning_launch/config/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml rename to autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml diff --git a/planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml b/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml similarity index 97% rename from planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml rename to autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml index 7f90e07b40..64b2d95c6f 100644 --- a/planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml @@ -9,7 +9,7 @@ # curve parameters max_lateral_accel: 1.0 # max lateral acceleration limit [m/ss] - min_curve_velocity: 2.74 # min velocity at lateral acceleration limit [m/ss] + min_curve_velocity: 0.5 # min velocity at lateral acceleration limit and steering angle rate limit [m/s] 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 min_decel_for_lateral_acc_lim_filter: -2.5 # deceleration limit applied in the lateral acceleration filter to avoid sudden braking [m/ss] diff --git a/planning_launch/config/scenario_planning/common/nearest_search.param.yaml b/autoware_launch/config/planning/scenario_planning/common/nearest_search.param.yaml similarity index 100% rename from planning_launch/config/scenario_planning/common/nearest_search.param.yaml rename to autoware_launch/config/planning/scenario_planning/common/nearest_search.param.yaml diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml similarity index 100% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml similarity index 80% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml index e34fa49cba..055cec3acb 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + planning_hz: 10.0 backward_path_length: 5.0 forward_path_length: 300.0 backward_length_buffer_for_end_of_pull_over: 5.0 @@ -14,6 +15,7 @@ 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 @@ -21,11 +23,12 @@ 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 + expected_front_deceleration_for_abort: -2.0 + expected_rear_deceleration_for_abort: -2.5 rear_vehicle_reaction_time: 2.0 - rear_vehicle_safety_time_margin: 2.0 + rear_vehicle_safety_time_margin: 1.0 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree.xml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree.xml new file mode 100644 index 0000000000..748b70b33f --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree.xml @@ -0,0 +1,94 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + desc + + + desc + + + + + + desc + + + desc + + + + desc + + + desc + + + + + + + + desc + + + + + desc + + + desc + + + + + + + + diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree_lane_change_only.xml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree_lane_change_only.xml new file mode 100644 index 0000000000..5c4d2b589d --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree_lane_change_only.xml @@ -0,0 +1,86 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + desc + + + desc + + + + + + desc + + + desc + + + + desc + + + desc + + + + + + + + desc + + + + + desc + + + desc + + + + + + + + diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree_pull_out.xml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree_pull_out.xml new file mode 100644 index 0000000000..e5876e9287 --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree_pull_out.xml @@ -0,0 +1,100 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + desc + + + desc + + + + + + + + + desc + + + desc + + + + + desc + + + desc + + + + + + + + + + desc + + + + + + desc + + + desc + + + + + + + + + diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml similarity index 100% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml similarity index 85% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml index 891e54a544..41de868418 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml @@ -5,9 +5,9 @@ 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_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] diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml similarity index 100% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml similarity index 91% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml index 68000747b7..9fcbc2a173 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml @@ -5,18 +5,18 @@ th_stopped_velocity: 0.01 th_stopped_time: 1.0 collision_check_margin: 1.0 - collision_check_distance_from_end: 1.0 + pull_out_finish_judge_buffer: 1.0 # shift pull out enable_shift_pull_out: true shift_pull_out_velocity: 2.0 pull_out_sampling_num: 4 + before_pull_out_straight_distance: 0.0 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 diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml similarity index 91% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml index f09439edf7..44d859a477 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml +++ b/autoware_launch/config/planning/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: 100.0 + request_length: 200.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,9 +17,6 @@ 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 @@ -35,7 +32,8 @@ maximum_lateral_jerk: 2.0 minimum_lateral_jerk: 0.5 deceleration_interval: 15.0 - after_pull_over_straight_distance: 1.0 + after_pull_over_straight_distance: 5.0 + before_pull_over_straight_distance: 5.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_path_planner/side_shift/side_shift.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml similarity index 85% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml index 815301d55a..79044041b4 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml @@ -6,4 +6,3 @@ shifting_lateral_jerk: 0.2 min_shifting_distance: 5.0 min_shifting_speed: 5.56 - shift_request_time_limit: 1.0 diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml similarity index 80% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml index 5ac74c2015..7aab0af21f 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml @@ -10,9 +10,11 @@ launch_occlusion_spot: true launch_no_stopping_area: true launch_run_out: false + launch_speed_bump: false forward_path_length: 1000.0 backward_path_length: 5.0 max_accel: -2.8 max_jerk: -5.0 system_delay: 0.5 delay_response_time: 0.5 + is_publish_debug_path: false # publish all debug path with lane id in each module diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml similarity index 100% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml similarity index 96% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml index 2cc5260e4e..2bbc5d31fc 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml @@ -26,6 +26,7 @@ stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) max_yield_timeout: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. + ego_yield_query_stop_duration: 0.1 # [s] the amount of time which ego should be stopping to query whether it yields or not # param for input data tl_state_timeout: 3.0 # [s] timeout threshold for traffic light signal diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml similarity index 83% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml index 63ca5fc7d9..9174045bf0 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml @@ -6,4 +6,3 @@ dead_line_margin: 5.0 use_pass_judge_line: false state_clear_time: 2.0 - hold_stop_margin_distance: 0.0 diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml similarity index 95% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index f78e8b16aa..220c087d18 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -19,7 +19,7 @@ collision_start_margin_time: 5.0 # [s] collision_end_margin_time: 2.0 # [s] use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck - assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of frontcar when frontcar as well as ego are turning + assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning enable_front_car_decel_prediction: false # By default this feature is disabled merge_from_private_road: diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml similarity index 100% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml similarity index 88% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml index 0b93ea5308..957f7988a6 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml @@ -4,7 +4,7 @@ detection_method: "occupancy_grid" # [-] candidate is "occupancy_grid" or "predicted_object" pass_judge: "smooth_velocity" # [-] candidate is "smooth_velocity" or "current_velocity" use_object_info: true # [-] whether to reflect object info to occupancy grid map or not - use_moving_object_ray_cast: true # [-] whether to reflect moving object ray shadow grid map + use_moving_object_ray_cast: true # [-] whether to reflect moving object ray_cast to occupancy grid map or not use_partition_lanelet: true # [-] whether to use partition lanelet map data pedestrian_vel: 1.5 # [m/s] assume pedestrian is dashing from occlusion at this velocity pedestrian_radius: 0.3 # [m] assume pedestrian width(0.2m) + margin(0.1m) @@ -18,8 +18,8 @@ lateral_distance: 1.5 # [m] maximum lateral distance to consider hidden collision motion: safety_ratio: 0.8 # [-] jerk/acceleration ratio for safety - max_slow_down_jerk: -0.3 # [m/s^3] minimum jerk deceleration for safe brake. - max_slow_down_accel: -1.5 # [m/s^2] minimum accel deceleration for safe brake. + max_slow_down_jerk: -0.5 # [m/s^3] minimum jerk deceleration for safe brake. + max_slow_down_accel: -1.8 # [m/s^2] minimum accel deceleration for safe brake. non_effective_jerk: -0.3 # [m/s^3] weak jerk for velocity planning. non_effective_acceleration: -1.0 # [m/s^2] weak deceleration for velocity planning. min_allowed_velocity: 1.0 # [m/s] minimum velocity allowed @@ -28,7 +28,7 @@ min_occlusion_spot_size: 1.0 # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle. slice_length: 10.0 # [m] size of slices in both length and distance relative to the ego path. min_longitudinal_offset: 1.0 # [m] detection area safety buffer from front bumper. - max_lateral_distance: 5.0 # [m] buffer around the ego path used to build the detection area. + max_lateral_distance: 6.0 # [m] buffer around the ego path used to build the detection area. grid: free_space_max: 43 # [-] maximum value of a free space cell in the occupancy grid - occupied_min: 57 # [-] minimum value of an occupied cell in the occupancy grid + occupied_min: 58 # [-] minimum value of an occupied cell in the occupancy grid diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml similarity index 100% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/speed_bump.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/speed_bump.param.yaml new file mode 100644 index 0000000000..ef61bdb743 --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/speed_bump.param.yaml @@ -0,0 +1,13 @@ +/**: + ros__parameters: + speed_bump: + slow_start_margin: 1.0 + slow_end_margin: 1.0 + print_debug_info: false + + # limits for speed bump height and slow down speed to create a linear equation + speed_calculation: + min_height: 0.05 # [m] + max_height: 0.30 # [m] + min_speed: 1.39 # [m/s] = [5 kph] + max_speed: 2.78 # [m/s] = [10 kph] diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml similarity index 52% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml index 0118452b72..a2b5ac5d5f 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml @@ -2,6 +2,5 @@ ros__parameters: stop_line: stop_margin: 0.0 + stop_check_dist: 2.0 stop_duration_sec: 1.0 - hold_stop_margin_distance: 2.0 - use_initialization_stop_line_state: true diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml similarity index 100% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_traffic_light.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_traffic_light.param.yaml similarity index 85% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_traffic_light.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_traffic_light.param.yaml index 8879408d40..267dde50c7 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_traffic_light.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_traffic_light.param.yaml @@ -4,6 +4,5 @@ max_delay_sec: 3.0 near_line_distance: 1.0 dead_line_margin: 1.0 - hold_stop_margin_distance: 0.0 max_yaw_deviation_deg: 90.0 check_timeout_after_stop_line: true diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml similarity index 100% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml similarity index 100% rename from planning_launch/config/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml diff --git a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml similarity index 100% rename from planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml diff --git a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml similarity index 100% rename from planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml diff --git a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml similarity index 100% rename from planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml diff --git a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml similarity index 91% rename from planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml index 55a21d6cd7..8954a46ab1 100644 --- a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml @@ -4,6 +4,9 @@ lowpass_gain: 0.9 # gain parameter for low pass filter [-] max_velocity: 20.0 # max velocity [m/s] enable_slow_down: False # whether to use slow down planner [-] + voxel_grid_x: 0.05 # voxel grid x parameter for filtering pointcloud [m] + voxel_grid_y: 0.05 # voxel grid y parameter for filtering pointcloud [m] + voxel_grid_z: 100000.0 # voxel grid z parameter for filtering pointcloud [m] stop_planner: # params for stop position diff --git a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml similarity index 100% rename from planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml diff --git a/planning_launch/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml similarity index 77% rename from planning_launch/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml index 982fb7e1d7..6aa4e71774 100644 --- a/planning_launch/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml @@ -12,4 +12,4 @@ stop_state_ego_speed: 0.1 #[m/s] # debug - publish_debug_footprints: false # publish vehicle footprint & footprints with surround_check_distance and surround_check_recover_distance offsets + publish_debug_footprints: true # publish vehicle footprint & footprints with surround_check_distance and surround_check_recover_distance offsets diff --git a/planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml similarity index 85% rename from planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml rename to autoware_launch/config/planning/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml index d2226091b5..d6152c4f56 100644 --- a/planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: # -- Node Configurations -- - planning_algorithm: "astar" # options: astar, rrtstar + planning_algorithm: "astar" waypoints_velocity: 5.0 update_rate: 10.0 th_arrived_distance_m: 1.0 @@ -15,6 +15,10 @@ # -- 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 diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index f964f2da2b..c848c487a6 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -109,10 +109,18 @@ - + + - + + + + + diff --git a/autoware_launch/launch/components/tier4_planning_component.launch.xml b/autoware_launch/launch/components/tier4_planning_component.launch.xml new file mode 100644 index 0000000000..b0ee0489ba --- /dev/null +++ b/autoware_launch/launch/components/tier4_planning_component.launch.xml @@ -0,0 +1,123 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware_launch/package.xml b/autoware_launch/package.xml index eaa526cead..4cc04cc4f4 100644 --- a/autoware_launch/package.xml +++ b/autoware_launch/package.xml @@ -12,7 +12,6 @@ ad_api_adaptors global_parameter_loader - planning_launch python3-bson python3-tornado rviz2 @@ -22,6 +21,7 @@ tier4_localization_launch tier4_map_launch tier4_perception_launch + tier4_planning_launch tier4_sensing_launch tier4_simulator_launch tier4_vehicle_launch diff --git a/planning_launch/CMakeLists.txt b/planning_launch/CMakeLists.txt deleted file mode 100644 index de7ef164be..0000000000 --- a/planning_launch/CMakeLists.txt +++ /dev/null @@ -1,23 +0,0 @@ -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/planning_launch/README.md b/planning_launch/README.md deleted file mode 100644 index 84bf70d26b..0000000000 --- a/planning_launch/README.md +++ /dev/null @@ -1,16 +0,0 @@ -# planning_launch - -## Structure - -![planning_launch](./planning_launch.drawio.svg) - -## Package Dependencies - -Please see `` in `package.xml`. - -## Usage - -```xml - - -``` diff --git a/planning_launch/config/planning_diagnostics/planning_error_monitor.param.yaml b/planning_launch/config/planning_diagnostics/planning_error_monitor.param.yaml deleted file mode 100644 index d52f24553b..0000000000 --- a/planning_launch/config/planning_diagnostics/planning_error_monitor.param.yaml +++ /dev/null @@ -1,7 +0,0 @@ -/**: - ros__parameters: - # trajectory check - error_interval: 100.0 # error interval distance threshold [m] - error_curvature: 2.0 # error curvature threshold [rad/m] - error_sharp_angle: 0.785398 # error sharp angle threshold [rad] - ignore_too_close_points: 0.01 # ignore too close distance threshold [m] diff --git a/planning_launch/launch/mission_planning/mission_planning.launch.xml b/planning_launch/launch/mission_planning/mission_planning.launch.xml deleted file mode 100644 index c1ace97b97..0000000000 --- a/planning_launch/launch/mission_planning/mission_planning.launch.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/planning_launch/launch/planning.launch.xml b/planning_launch/launch/planning.launch.xml deleted file mode 100644 index 8116564f87..0000000000 --- a/planning_launch/launch/planning.launch.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/planning_launch/launch/planning_diagnostics/planning_error_monitor.launch.xml b/planning_launch/launch/planning_diagnostics/planning_error_monitor.launch.xml deleted file mode 100644 index e3960f8ec0..0000000000 --- a/planning_launch/launch/planning_diagnostics/planning_error_monitor.launch.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/planning_launch/launch/scenario_planning/lane_driving.launch.xml b/planning_launch/launch/scenario_planning/lane_driving.launch.xml deleted file mode 100644 index a762fefa93..0000000000 --- a/planning_launch/launch/scenario_planning/lane_driving.launch.xml +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py deleted file mode 100644 index 0f00aeb60f..0000000000 --- a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ /dev/null @@ -1,498 +0,0 @@ -# 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 IncludeLaunchDescription -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -from launch.conditions import UnlessCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from launch.substitutions import PythonExpression -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode -from launch_ros.substitutions import FindPackageShare -import yaml - - -def generate_launch_description(): - nearest_search_param_path = os.path.join( - get_package_share_directory("planning_launch"), - "config", - "scenario_planning", - "common", - "nearest_search.param.yaml", - ) - with open(nearest_search_param_path, "r") as f: - nearest_search_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"] - - drivable_area_expansion_param_path = os.path.join( - get_package_share_directory("planning_launch"), - "config", - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_path_planner", - "drivable_area_expansion.param.yaml", - ) - with open(drivable_area_expansion_param_path, "r") as f: - drivable_area_expansion_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/accel", "/localization/acceleration"), - ("~/input/scenario", "/planning/scenario_planning/scenario"), - ("~/output/path", "path_with_lane_id"), - ("~/output/turn_indicators_cmd", "/planning/turn_indicators_cmd"), - ("~/output/hazard_lights_cmd", "/planning/hazard_lights_cmd"), - ], - parameters=[ - nearest_search_param, - side_shift_param, - avoidance_param, - lane_change_param, - lane_following_param, - pull_over_param, - pull_out_param, - drivable_area_expansion_param, - behavior_path_planner_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")}], - ) - - # smoother param - 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"] - - motion_velocity_smoother_param_path = os.path.join( - get_package_share_directory("planning_launch"), - "config", - "scenario_planning", - "common", - "motion_velocity_smoother", - "motion_velocity_smoother.param.yaml", - ) - with open(motion_velocity_smoother_param_path, "r") as f: - motion_velocity_smoother_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - smoother_type_param_path = os.path.join( - get_package_share_directory("planning_launch"), - "config", - "scenario_planning", - "common", - "motion_velocity_smoother", - "Analytical.param.yaml", - ) - with open(smoother_type_param_path, "r") as f: - smoother_type_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - # behavior velocity planner - blind_spot_param_path = os.path.join( - get_package_share_directory("planning_launch"), - "config", - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "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("planning_launch"), - "config", - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "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("planning_launch"), - "config", - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "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("planning_launch"), - "config", - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "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("planning_launch"), - "config", - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "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("planning_launch"), - "config", - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "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("planning_launch"), - "config", - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "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("planning_launch"), - "config", - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "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("planning_launch"), - "config", - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "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"] - - run_out_param_path = os.path.join( - get_package_share_directory("planning_launch"), - "config", - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "run_out.param.yaml", - ) - with open(run_out_param_path, "r") as f: - run_out_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - behavior_velocity_planner_param_path = os.path.join( - get_package_share_directory("planning_launch"), - "config", - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "behavior_velocity_planner.param.yaml", - ) - with open(behavior_velocity_planner_param_path, "r") as f: - behavior_velocity_planner_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/accel", "/localization/acceleration"), - ("~/input/dynamic_objects", "/perception/object_recognition/objects"), - ( - "~/input/no_ground_pointcloud", - "/perception/obstacle_segmentation/pointcloud", - ), - ( - "~/input/compare_map_filtered_pointcloud", - "compare_map_filtered/pointcloud", - ), - ( - "~/input/traffic_signals", - "/perception/traffic_light_recognition/traffic_signals", - ), - ( - "~/input/external_traffic_signals", - "/external/traffic_light_recognition/traffic_signals", - ), - ( - "~/input/external_velocity_limit_mps", - "/planning/scenario_planning/max_velocity_default", - ), - ("~/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=[ - behavior_velocity_planner_param, - common_param, - nearest_search_param, - motion_velocity_smoother_param, - smoother_type_param, - 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, - run_out_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", - ) - - 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")), - ) - - # This condition is true if run_out module is enabled and its detection method is Points - launch_run_out_with_points_method = PythonExpression( - [ - LaunchConfiguration( - "launch_run_out", default=behavior_velocity_planner_param["launch_run_out"] - ), - " and ", - "'", - run_out_param["run_out"]["detection_method"], - "' == 'Points'", - ] - ) - - # load compare map for run out module - load_compare_map = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [ - FindPackageShare("planning_launch"), - "/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py", - ] - ), - launch_arguments={ - "use_pointcloud_container": LaunchConfiguration("use_pointcloud_container"), - "container_name": LaunchConfiguration("container_name"), - "use_multithread": "true", - }.items(), - # launch compare map only when run_out module is enabled and detection method is Points - condition=IfCondition(launch_run_out_with_points_method), - ) - - load_vector_map_inside_area_filter = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [ - FindPackageShare("planning_launch"), - "/launch/scenario_planning/lane_driving/behavior_planning/vector_map_inside_area_filter.launch.py", - ] - ), - launch_arguments={ - "use_pointcloud_container": LaunchConfiguration("use_pointcloud_container"), - "container_name": LaunchConfiguration("container_name"), - "use_multithread": "true", - "polygon_type": "no_obstacle_segmentation_area_for_run_out", - }.items(), - # launch vector map filter only when run_out module is enabled and detection method is Points - condition=IfCondition(launch_run_out_with_points_method), - ) - - return launch.LaunchDescription( - [ - DeclareLaunchArgument( - "input_route_topic_name", default_value="/planning/mission_planning/route" - ), - DeclareLaunchArgument("map_topic_name", default_value="/map/vector_map"), - DeclareLaunchArgument("use_intra_process", default_value="false"), - DeclareLaunchArgument("use_multithread", default_value="false"), - set_container_executable, - set_container_mt_executable, - container, - load_compare_map, - load_vector_map_inside_area_filter, - ] - ) diff --git a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml deleted file mode 100644 index ddd7e8b894..0000000000 --- a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py b/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py deleted file mode 100644 index 60b31f475e..0000000000 --- a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py +++ /dev/null @@ -1,89 +0,0 @@ -# Copyright 2022 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. - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -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 - - -def generate_launch_description(): - def add_launch_arg(name: str, default_value=None): - return DeclareLaunchArgument(name, default_value=default_value) - - 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")), - ) - - composable_nodes = [ - ComposableNode( - package="compare_map_segmentation", - plugin="compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent", - name="voxel_distance_based_compare_map_filter_node", - remappings=[ - ("input", "vector_map_inside_area_filtered/pointcloud"), - ("map", "/map/pointcloud_map"), - ("output", "compare_map_filtered/pointcloud"), - ], - parameters=[ - { - "distance_threshold": 0.7, - } - ], - extra_arguments=[ - {"use_intra_process_comms": False} # this node has QoS of transient local - ], - ), - ] - - compare_map_container = ComposableNodeContainer( - name="compare_map_container", - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=composable_nodes, - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), - output="screen", - ) - - load_composable_nodes = LoadComposableNodes( - composable_node_descriptions=composable_nodes, - target_container=LaunchConfiguration("container_name"), - condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), - ) - - return LaunchDescription( - [ - add_launch_arg("use_multithread", "true"), - add_launch_arg("use_pointcloud_container", "true"), - add_launch_arg("container_name", "compare_map_container"), - set_container_executable, - set_container_mt_executable, - compare_map_container, - load_composable_nodes, - ] - ) diff --git a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/vector_map_inside_area_filter.launch.py b/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/vector_map_inside_area_filter.launch.py deleted file mode 100644 index 3921dcfe7d..0000000000 --- a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/vector_map_inside_area_filter.launch.py +++ /dev/null @@ -1,88 +0,0 @@ -# Copyright 2022 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. - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -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 - - -def generate_launch_description(): - def add_launch_arg(name: str, default_value=None): - return DeclareLaunchArgument(name, default_value=default_value) - - 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")), - ) - - composable_nodes = [ - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VectorMapInsideAreaFilterComponent", - name="vector_map_inside_area_filter_node", - remappings=[ - ("input", "/perception/obstacle_segmentation/pointcloud"), - ("input/vector_map", "/map/vector_map"), - ("output", "vector_map_inside_area_filtered/pointcloud"), - ], - parameters=[ - { - "polygon_type": LaunchConfiguration("polygon_type"), - } - ], - # this node has QoS of transient local - extra_arguments=[{"use_intra_process_comms": False}], - ), - ] - - vector_map_area_filter_container = ComposableNodeContainer( - name="vector_map_area_filter_container", - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=composable_nodes, - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), - output="screen", - ) - - load_composable_nodes = LoadComposableNodes( - composable_node_descriptions=composable_nodes, - target_container=LaunchConfiguration("container_name"), - condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), - ) - - return LaunchDescription( - [ - add_launch_arg("use_multithread", "true"), - add_launch_arg("use_pointcloud_container", "true"), - add_launch_arg("container_name", "vector_map_area_filter_container"), - set_container_executable, - set_container_mt_executable, - vector_map_area_filter_container, - load_composable_nodes, - ] - ) diff --git a/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py deleted file mode 100644 index 56a2468124..0000000000 --- a/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ /dev/null @@ -1,338 +0,0 @@ -# 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 SetLaunchConfiguration -from launch.conditions import IfCondition -from launch.conditions import LaunchConfigurationEquals -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 -import yaml - - -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( - "cruise_planner_type", - "obstacle_stop_planner", - "cruise_planner: obstacle_stop_planner, obstacle_cruise_planner, none`", - ) - - # 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"] - - nearest_search_param_path = os.path.join( - get_package_share_directory("planning_launch"), - "config", - "scenario_planning", - "common", - "nearest_search.param.yaml", - ) - with open(nearest_search_param_path, "r") as f: - nearest_search_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=[ - nearest_search_param, - obstacle_avoidance_planner_param, - {"is_showing_debug_info": False}, - {"is_stopping_if_outside_drivable_area": True}, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - # obstacle velocity limiter - obstacle_velocity_limiter_param_path = os.path.join( - get_package_share_directory("planning_launch"), - "config", - "scenario_planning", - "lane_driving", - "motion_planning", - "obstacle_velocity_limiter", - "obstacle_velocity_limiter.param.yaml", - ) - with open(obstacle_velocity_limiter_param_path, "r") as f: - obstacle_velocity_limiter_param = yaml.safe_load(f)["/**"]["ros__parameters"] - obstacle_velocity_limiter_component = ComposableNode( - package="obstacle_velocity_limiter", - plugin="obstacle_velocity_limiter::ObstacleVelocityLimiterNode", - name="obstacle_velocity_limiter", - namespace="", - remappings=[ - ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"), - ("~/input/odometry", "/localization/kinematic_state"), - ("~/input/dynamic_obstacles", "/perception/object_recognition/objects"), - ("~/input/occupancy_grid", "/perception/occupancy_grid_map/map"), - ("~/input/obstacle_pointcloud", "/perception/obstacle_segmentation/pointcloud"), - ("~/input/map", "/map/vector_map"), - ("~/output/debug_markers", "debug_markers"), - ("~/output/trajectory", "obstacle_velocity_limiter/trajectory"), - ], - parameters=[ - obstacle_velocity_limiter_param, - {"obstacles.dynamic_source": "static_only"}, - ], - 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="surround_obstacle_checker::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/max_velocity", "/planning/scenario_planning/max_velocity_candidates"), - ( - "~/output/velocity_limit_clear_command", - "/planning/scenario_planning/clear_velocity_limit", - ), - ( - "~/input/pointcloud", - "/perception/obstacle_segmentation/pointcloud", - ), - ("~/input/objects", "/perception/object_recognition/objects"), - ("~/input/odometry", "/localization/kinematic_state"), - ], - parameters=[ - nearest_search_param, - surround_obstacle_checker_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - # obstacle cruise planner - obstacle_cruise_planner_param_path = os.path.join( - get_package_share_directory("planning_launch"), - "config", - "scenario_planning", - "lane_driving", - "motion_planning", - "obstacle_cruise_planner", - "obstacle_cruise_planner.param.yaml", - ) - with open(obstacle_cruise_planner_param_path, "r") as f: - obstacle_cruise_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] - obstacle_cruise_planner_component = ComposableNode( - package="obstacle_cruise_planner", - plugin="motion_planning::ObstacleCruisePlannerNode", - name="obstacle_cruise_planner", - namespace="", - remappings=[ - ("~/input/trajectory", "obstacle_velocity_limiter/trajectory"), - ("~/input/odometry", "/localization/kinematic_state"), - ("~/input/acceleration", "/localization/acceleration"), - ("~/input/objects", "/perception/object_recognition/objects"), - ("~/output/trajectory", "/planning/scenario_planning/lane_driving/trajectory"), - ("~/output/velocity_limit", "/planning/scenario_planning/max_velocity_candidates"), - ("~/output/clear_velocity_limit", "/planning/scenario_planning/clear_velocity_limit"), - ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"), - ], - parameters=[ - common_param, - nearest_search_param, - obstacle_cruise_planner_param, - ], - 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/acceleration", "/localization/acceleration"), - ( - "~/input/pointcloud", - "/perception/obstacle_segmentation/pointcloud", - ), - ("~/input/objects", "/perception/object_recognition/objects"), - ("~/input/odometry", "/localization/kinematic_state"), - ("~/input/trajectory", "obstacle_velocity_limiter/trajectory"), - ], - parameters=[ - common_param, - nearest_search_param, - obstacle_stop_planner_param, - obstacle_stop_planner_acc_param, - {"enable_slow_down": False}, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - obstacle_cruise_planner_relay_component = ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="obstacle_cruise_planner_relay", - namespace="", - parameters=[ - {"input_topic": "obstacle_velocity_limiter/trajectory"}, - {"output_topic": "/planning/scenario_planning/lane_driving/trajectory"}, - {"type": "autoware_auto_planning_msgs/msg/Trajectory"}, - ], - 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_velocity_limiter_component, - ], - ) - - obstacle_stop_planner_loader = LoadComposableNodes( - composable_node_descriptions=[obstacle_stop_planner_component], - target_container=container, - condition=LaunchConfigurationEquals("cruise_planner_type", "obstacle_stop_planner"), - ) - - obstacle_cruise_planner_loader = LoadComposableNodes( - composable_node_descriptions=[obstacle_cruise_planner_component], - target_container=container, - condition=LaunchConfigurationEquals("cruise_planner_type", "obstacle_cruise_planner"), - ) - - obstacle_cruise_planner_relay_loader = LoadComposableNodes( - composable_node_descriptions=[obstacle_cruise_planner_relay_component], - target_container=container, - condition=LaunchConfigurationEquals("cruise_planner_type", "none"), - ) - - surround_obstacle_checker_loader = LoadComposableNodes( - composable_node_descriptions=[surround_obstacle_checker_component], - target_container=container, - condition=IfCondition(LaunchConfiguration("use_surround_obstacle_check")), - ) - - 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 - + [ - DeclareLaunchArgument( - "input_path_topic", - default_value="/planning/scenario_planning/lane_driving/behavior_planning/path", - ), - DeclareLaunchArgument("use_surround_obstacle_check", default_value="true"), - DeclareLaunchArgument("use_intra_process", default_value="false"), - DeclareLaunchArgument("use_multithread", default_value="false"), - set_container_executable, - set_container_mt_executable, - container, - surround_obstacle_checker_loader, - obstacle_stop_planner_loader, - obstacle_cruise_planner_loader, - obstacle_cruise_planner_relay_loader, - ] - ) diff --git a/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml deleted file mode 100644 index de101a8040..0000000000 --- a/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/planning_launch/launch/scenario_planning/parking.launch.py b/planning_launch/launch/scenario_planning/parking.launch.py deleted file mode 100644 index e3d9e750a0..0000000000 --- a/planning_launch/launch/scenario_planning/parking.launch.py +++ /dev/null @@ -1,137 +0,0 @@ -# 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 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 -import yaml - - -def generate_launch_description(): - 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, - }, - ], - 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, - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ), - ], - ) - - 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( - [ - DeclareLaunchArgument( - "use_intra_process", - default_value="false", - description="use ROS2 component container communication", - ), - DeclareLaunchArgument( - "use_multithread", default_value="false", description="use multithread" - ), - set_container_executable, - set_container_mt_executable, - GroupAction([PushRosNamespace("parking"), container]), - ] - ) diff --git a/planning_launch/launch/scenario_planning/parking.launch.xml b/planning_launch/launch/scenario_planning/parking.launch.xml deleted file mode 100644 index 9fbf153549..0000000000 --- a/planning_launch/launch/scenario_planning/parking.launch.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/planning_launch/launch/scenario_planning/scenario_planning.launch.xml deleted file mode 100644 index fc06076d5a..0000000000 --- a/planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ /dev/null @@ -1,57 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/planning_launch/package.xml b/planning_launch/package.xml deleted file mode 100644 index e8e473ca5d..0000000000 --- a/planning_launch/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - 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 - obstacle_velocity_limiter - scenario_selector - surround_obstacle_checker - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/planning_launch/planning_launch.drawio.svg b/planning_launch/planning_launch.drawio.svg deleted file mode 100644 index fba2aefed5..0000000000 --- a/planning_launch/planning_launch.drawio.svg +++ /dev/null @@ -1,318 +0,0 @@ - - - - - - - -
-
-
- 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