diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 39d05a7fb761b..2318e81783479 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -2,7 +2,6 @@ ros__parameters: goal_planner: # general params - minimum_request_length: 100.0 th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 2.0 # It must be greater than the state_machine's. @@ -36,6 +35,7 @@ # pull over pull_over: + minimum_request_length: 100.0 pull_over_velocity: 3.0 pull_over_minimum_velocity: 1.38 decide_path_distance: 10.0 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index 99dadc959f5a5..e026a083eb277 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -86,8 +86,8 @@ Either one is activated when all conditions are met. ### fixed_goal_planner -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. - Route is set with `allow_goal_modification=false` by default. +- ego-vehicle is in the same lane as the goal. @@ -95,7 +95,7 @@ Either one is activated when all conditions are met. #### pull over on road lane -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`. - Route is set with `allow_goal_modification=true` . - We can set this option with [SetRoute](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/routing/srv/SetRoute.srv#L2) api service. - We support `2D Rough Goal Pose` with the key bind `r` in RViz, but in the future there will be a panel of tools to manipulate various Route API from RViz. @@ -105,7 +105,7 @@ Either one is activated when all conditions are met. #### pull over on shoulder lane -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`. - Goal is set in the `road_shoulder`. @@ -118,17 +118,11 @@ Either one is activated when all conditions are met. ## General parameters for goal_planner -| Name | Unit | Type | Description | Default value | -| :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, the module is activated. | 100.0 | -| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | -| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | -| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | -| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | -| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | -| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | +| Name | Unit | Type | Description | Default value | +| :------------------ | :---- | :----- | :------------------------------------------------- | :------------ | +| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | ## **collision check** @@ -174,12 +168,21 @@ searched for in certain range of the shoulder lane. | longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | | max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | | lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 15.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | -## **Path Generation** +## **Pull Over** There are three path generation methods. -The path is generated with a certain margin (default: `0.5 m`) from left boundary of shoulder lane. +The path is generated with a certain margin (default: `0.5 m`) from the boundary of shoulder lane. + +| Name | Unit | Type | Description | Default value | +| :------------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| pull_over_minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, pull over is activated. | 100.0 | +| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | +| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | +| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | +| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | ### **shift parking** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index f87944641f59b..79d3a0c6e9401 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -40,7 +40,6 @@ enum class ParkingPolicy { struct GoalPlannerParameters { // general params - double minimum_request_length; double th_arrived_distance; double th_stopped_velocity; double th_stopped_time; @@ -71,6 +70,7 @@ struct GoalPlannerParameters double object_recognition_collision_check_max_extra_stopping_margin; // pull over general params + double pull_over_minimum_request_length; double pull_over_velocity; double pull_over_minimum_velocity; double decide_path_distance; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index f37149f706436..d30e63bc6357e 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -306,25 +306,25 @@ bool GoalPlannerModule::isExecutionRequested() const return false; } + // if goal modification is not allowed + // 1) goal_pose is in current_lanes, plan path to the original fixed goal + // 2) goal_pose is NOT in current_lanes, do not execute goal_planner + if (!goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_)) { + return goal_is_in_current_lanes; + } + // if goal arc coordinates can be calculated, check if goal is in request_length const double self_to_goal_arc_length = utils::getSignedDistance(current_pose, goal_pose, current_lanes); const double request_length = goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_) ? calcModuleRequestLength() - : parameters_->minimum_request_length; + : parameters_->pull_over_minimum_request_length; if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) { // if current position is far from goal or behind goal, do not execute goal_planner return false; } - // if goal modification is not allowed - // 1) goal_pose is in current_lanes, plan path to the original fixed goal - // 2) goal_pose is NOT in current_lanes, do not execute goal_planner - if (!goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_)) { - return goal_is_in_current_lanes; - } - // if (A) or (B) is met execute pull over // (A) target lane is `road` and same to the current lanes // (B) target lane is `road_shoulder` and neighboring to the current lanes @@ -348,13 +348,13 @@ double GoalPlannerModule::calcModuleRequestLength() const { const auto min_stop_distance = calcFeasibleDecelDistance(0.0); if (!min_stop_distance) { - return parameters_->minimum_request_length; + return parameters_->pull_over_minimum_request_length; } const double minimum_request_length = *min_stop_distance + parameters_->backward_goal_search_length + approximate_pull_over_distance_; - return std::max(minimum_request_length, parameters_->minimum_request_length); + return std::max(minimum_request_length, parameters_->pull_over_minimum_request_length); } Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index bcee6aeaf0c36..eaac7a4912aff 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -35,7 +35,6 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // general params { std::string ns = "goal_planner."; - p.minimum_request_length = node->declare_parameter(ns + "minimum_request_length"); p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); @@ -96,6 +95,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // pull over general params { std::string ns = "goal_planner.pull_over."; + node->declare_parameter(ns + "minimum_request_length"); p.pull_over_velocity = node->declare_parameter(ns + "pull_over_velocity"); p.pull_over_minimum_velocity = node->declare_parameter(ns + "pull_over_minimum_velocity");