Skip to content

Commit

Permalink
feat(goal_planner): do not use minimum_request_length for fixed goal …
Browse files Browse the repository at this point in the history 
…planner (autowarefoundation#4831)

Signed-off-by: kosuke55 <[email protected]>
  • Loading branch information
@kosuke55
kosuke55 committed Sep 6, 2023
1 parent d309abd commit f9f2c1c
Show file tree
Hide file tree
Showing 5 changed files with 33 additions and 30 deletions.
2 changes: 1 addition & 1 deletion planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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
Expand Down
37 changes: 20 additions & 17 deletions planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,16 +86,16 @@ 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.

<img src="https://user-images.githubusercontent.com/39142679/237929955-c0adf01b-9e3c-45e3-848d-98cf11e52b65.png" width="600">

### rough_goal_planner

#### 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.
Expand All @@ -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`.

<img src="https://user-images.githubusercontent.com/39142679/237929941-2ce26ea5-c84d-4d17-8cdc-103f5246db90.png" width="600">
Expand All @@ -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**

Expand Down Expand Up @@ -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**

Expand Down
2 changes: 1 addition & 1 deletion ...path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down
20 changes: 10 additions & 10 deletions planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,6 @@ GoalPlannerModuleManager::GoalPlannerModuleManager(
// general params
{
std::string ns = "goal_planner.";
p.minimum_request_length = node->declare_parameter<double>(ns + "minimum_request_length");
p.th_stopped_velocity = node->declare_parameter<double>(ns + "th_stopped_velocity");
p.th_arrived_distance = node->declare_parameter<double>(ns + "th_arrived_distance");
p.th_stopped_time = node->declare_parameter<double>(ns + "th_stopped_time");
Expand Down Expand Up @@ -96,6 +95,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager(
// pull over general params
{
std::string ns = "goal_planner.pull_over.";
node->declare_parameter<double>(ns + "minimum_request_length");
p.pull_over_velocity = node->declare_parameter<double>(ns + "pull_over_velocity");
p.pull_over_minimum_velocity =
node->declare_parameter<double>(ns + "pull_over_minimum_velocity");
Expand Down

0 comments on commit f9f2c1c

Please sign in to comment.