diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index d174b54b9ccbe..9e5a05a2d60cd 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -5,15 +5,14 @@ th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 1.0 - collision_check_margins: [2.0, 1.5, 1.0] - collision_check_distance_from_end: 1.0 + collision_check_margins: [2.0, 1.0, 0.5, 0.1] collision_check_margin_from_front_object: 5.0 th_moving_object_velocity: 1.0 th_distance_to_middle_of_the_road: 0.5 center_line_path_interval: 1.0 # shift pull out enable_shift_pull_out: true - check_shift_path_lane_departure: false + shift_collision_check_distance_from_end: -10.0 minimum_shift_pull_out_distance: 0.0 deceleration_interval: 15.0 lateral_jerk: 0.5 @@ -23,6 +22,7 @@ maximum_curvature: 0.07 # geometric pull out enable_geometric_pull_out: true + geometric_collision_check_distance_from_end: 0.0 divide_pull_out_path: true geometric_pull_out_velocity: 1.0 arc_path_interval: 1.0 @@ -32,7 +32,7 @@ # search start pose backward enable_back: true search_priority: "efficient_path" # "efficient_path" or "short_back_distance" - max_back_distance: 30.0 + max_back_distance: 20.0 backward_search_resolution: 2.0 backward_path_update_duration: 3.0 ignore_distance_from_lane_end: 15.0 diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 8f8d2baf9c85e..f42aef4075777 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -63,7 +63,6 @@ struct StartPlannerParameters double intersection_search_length{0.0}; double length_ratio_for_turn_signal_deactivation_near_intersection{0.0}; std::vector collision_check_margins{}; - double collision_check_distance_from_end{0.0}; double collision_check_margin_from_front_object{0.0}; double th_moving_object_velocity{0.0}; double center_line_path_interval{0.0}; @@ -71,6 +70,7 @@ struct StartPlannerParameters // shift pull out bool enable_shift_pull_out{false}; bool check_shift_path_lane_departure{false}; + double shift_collision_check_distance_from_end{0.0}; double minimum_shift_pull_out_distance{0.0}; int lateral_acceleration_sampling_num{0}; double lateral_jerk{0.0}; @@ -80,6 +80,7 @@ struct StartPlannerParameters double deceleration_interval{0.0}; // geometric pull out bool enable_geometric_pull_out{false}; + double geometric_collision_check_distance_from_end; bool divide_pull_out_path{false}; ParallelParkingParameters parallel_parking_parameters{}; // search start pose backward diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index cffce8218c554..0dccceb1e3919 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -174,7 +174,8 @@ class StartPlannerModule : public SceneModuleInterface const Pose & start_pose_candidate, const std::shared_ptr & planner, const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin); - PathWithLaneId extractCollisionCheckSection(const PullOutPath & path); + PathWithLaneId extractCollisionCheckSection( + const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type); void updateStatusWithCurrentPath( const behavior_path_planner::PullOutPath & path, const Pose & start_pose, const behavior_path_planner::PlannerType & planner_type); diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 8cc0b34082e44..7fb52d59c418b 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -45,8 +45,6 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); p.collision_check_margins = node->declare_parameter>(ns + "collision_check_margins"); - p.collision_check_distance_from_end = - node->declare_parameter(ns + "collision_check_distance_from_end"); p.collision_check_margin_from_front_object = node->declare_parameter(ns + "collision_check_margin_from_front_object"); p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); @@ -55,6 +53,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); p.check_shift_path_lane_departure = node->declare_parameter(ns + "check_shift_path_lane_departure"); + p.shift_collision_check_distance_from_end = + node->declare_parameter(ns + "shift_collision_check_distance_from_end"); p.minimum_shift_pull_out_distance = node->declare_parameter(ns + "minimum_shift_pull_out_distance"); p.lateral_acceleration_sampling_num = @@ -66,6 +66,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.deceleration_interval = node->declare_parameter(ns + "deceleration_interval"); // geometric pull out p.enable_geometric_pull_out = node->declare_parameter(ns + "enable_geometric_pull_out"); + p.geometric_collision_check_distance_from_end = + node->declare_parameter(ns + "geometric_collision_check_distance_from_end"); p.divide_pull_out_path = node->declare_parameter(ns + "divide_pull_out_path"); p.parallel_parking_parameters.pull_out_velocity = node->declare_parameter(ns + "geometric_pull_out_velocity"); diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 8937c8a837694..135a0b7fbf86c 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -657,8 +657,8 @@ bool StartPlannerModule::findPullOutPath( // check collision if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint, extractCollisionCheckSection(*pull_out_path), pull_out_lane_stop_objects, - collision_check_margin)) { + vehicle_footprint, extractCollisionCheckSection(*pull_out_path, planner->getPlannerType()), + pull_out_lane_stop_objects, collision_check_margin)) { return false; } @@ -672,8 +672,17 @@ bool StartPlannerModule::findPullOutPath( return true; } -PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPath & path) +PathWithLaneId StartPlannerModule::extractCollisionCheckSection( + const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type) { + const std::map collision_check_distances = { + {behavior_path_planner::PlannerType::SHIFT, + parameters_->shift_collision_check_distance_from_end}, + {behavior_path_planner::PlannerType::GEOMETRIC, + parameters_->geometric_collision_check_distance_from_end}}; + + const double collision_check_distance_from_end = collision_check_distances.at(planner_type); + PathWithLaneId combined_path; for (const auto & partial_path : path.partial_paths) { combined_path.points.insert( @@ -682,7 +691,7 @@ PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPat // calculate collision check end idx const size_t collision_check_end_idx = std::invoke([&]() { const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( - combined_path.points, path.end_pose.position, parameters_->collision_check_distance_from_end); + combined_path.points, path.end_pose.position, collision_check_distance_from_end); if (collision_check_end_pose) { return motion_utils::findNearestIndex( @@ -1403,9 +1412,14 @@ void StartPlannerModule::setDebugData() // visualize collision_check_end_pose and footprint { const auto local_footprint = vehicle_info_.createFootprint(); + std::map collision_check_distances = { + {PlannerType::SHIFT, parameters_->shift_collision_check_distance_from_end}, + {PlannerType::GEOMETRIC, parameters_->geometric_collision_check_distance_from_end}}; + + double collision_check_distance_from_end = collision_check_distances[status_.planner_type]; const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( getFullPath().points, status_.pull_out_path.end_pose.position, - parameters_->collision_check_distance_from_end); + collision_check_distance_from_end); if (collision_check_end_pose) { add(createPoseMarkerArray( *collision_check_end_pose, "static_collision_check_end_pose", 0, 1.0, 0.0, 0.0));