Skip to content

Commit

Permalink
Merge pull request #905 from tier4/beta/v0.10.2_start_goal
Browse files Browse the repository at this point in the history 
feat: update start goal planner v0.10.2
  • Loading branch information
@tkimura4
tkimura4 authored Oct 3, 2023
2 parents f73de99 + d99668a commit 0c0a75f
Show file tree
Hide file tree
Showing 36 changed files with 571 additions and 285 deletions.
15 changes: 10 additions & 5 deletions common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1392,7 +1392,8 @@ void insertOrientation(T & points, const bool is_driving_forward)

/**
* @brief Remove points with invalid orientation differences from a given points container
* (trajectory, path, ...)
* (trajectory, path, ...). Check the difference between the angles of two points and the difference
* between the azimuth angle between the two points and the angle of the next point.
* @param points Points of trajectory, path, or other point container (input / output)
* @param max_yaw_diff Maximum acceptable yaw angle difference between two consecutive points in
* radians (default: M_PI_2)
Expand All @@ -1401,10 +1402,14 @@ template <class T>
void removeInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2)
{
for (size_t i = 1; i < points.size();) {
const double yaw1 = tf2::getYaw(tier4_autoware_utils::getPose(points.at(i - 1)).orientation);
const double yaw2 = tf2::getYaw(tier4_autoware_utils::getPose(points.at(i)).orientation);

if (max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2))) {
const auto p1 = tier4_autoware_utils::getPose(points.at(i - 1));
const auto p2 = tier4_autoware_utils::getPose(points.at(i));
const double yaw1 = tf2::getYaw(p1.orientation);
const double yaw2 = tf2::getYaw(p2.orientation);

if (
max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2)) ||
!tier4_autoware_utils::isDrivingForward(p1, p2)) {
points.erase(points.begin() + i);
} else {
++i;
Expand Down
8 changes: 7 additions & 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 @@ -5,10 +5,14 @@
th_arrived_distance: 1.0
th_stopped_velocity: 0.01
th_stopped_time: 2.0 # It must be greater than the state_machine's.
center_line_path_interval: 1.0

# goal search
goal_search:
search_priority: "efficient_path" # "efficient_path" or "close_goal"
goal_priority: "minimum_weighted_distance" # "minimum_weighted_distance" or "minimum_longitudinal_distance"
minimum_weighted_distance:
lateral_weight: 40.0
prioritize_goals_before_objects: true
parking_policy: "left_side" # "left_side" or "right_side"
forward_goal_search_length: 20.0
backward_goal_search_length: 20.0
Expand Down Expand Up @@ -43,6 +47,8 @@
decide_path_distance: 10.0
maximum_deceleration: 1.0
maximum_jerk: 1.0
path_priority: "efficient_path" # "efficient_path" or "close_goal"
efficient_path_order: ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] # only lane based pull over(exclude freespace parking)

# shift parking
shift_parking:
Expand Down
1 change: 1 addition & 0 deletions planning/behavior_path_planner/config/start_planner/start_planner.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
collision_check_distance_from_end: 1.0
th_moving_object_velocity: 1.0
th_distance_to_middle_of_the_road: 0.1
center_line_path_interval: 1.0
# shift pull out
enable_shift_pull_out: true
check_shift_path_lane_departure: false
Expand Down
52 changes: 28 additions & 24 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 @@ -118,11 +118,12 @@ Either one is activated when all conditions are met.

## General parameters for goal_planner

| 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 |
| 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 |
| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 |

## **collision check**

Expand Down Expand Up @@ -160,31 +161,34 @@ searched for in certain range of the shoulder lane.

### Parameters for goal search

| Name | Unit | Type | Description | Default value |
| :------------------------------ | :--- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- |
| search_priority | [-] | string | In case `efficient_path` use a goal that can generate an efficient path( priority is `shift_parking` -> `arc_forward_parking` -> `arc_backward_parking`). In case `close_goal` use the closest goal to the original one. | efficient_path |
| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 |
| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 |
| goal_search_interval | [m] | double | distance interval for goal search | 2.0 |
| 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 | 0.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 |
| Name | Unit | Type | Description | Default value |
| :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | :-------------------------- |
| goal_priority | [-] | string | In case `minimum_weighted_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_longitudinal_distance`, sort with weighted lateral distance against longitudinal distance. | `minimum_weighted_distance` |
| prioritize_goals_before_objects | [-] | bool | If there are objects that may need to be avoided, prioritize the goal in front of them | true |
| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 |
| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 |
| goal_search_interval | [m] | double | distance interval for goal search | 2.0 |
| 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 | 0.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 |

## **Pull Over**

There are three path generation methods.
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 |
| 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 |
| path_priority | [-] | string | In case `efficient_path` use a goal that can generate an efficient path which is set in `efficient_path_order`. In case `close_goal` use the closest goal to the original one. | efficient_path |
| efficient_path_order | [-] | string | efficient order of pull over planner along lanes　excluding freespace pull over | ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] |

### **shift parking**

Expand Down
1 change: 1 addition & 0 deletions planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,7 @@ PullOutPath --o PullOutPlannerBase
| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 |
| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 |
| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 |
| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 |

## Safety check with static obstacles

Expand Down
5 changes: 4 additions & 1 deletion ...h_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,6 +103,7 @@ struct FreespacePlannerDebugData
struct GoalPlannerDebugData
{
FreespacePlannerDebugData freespace_planner{};
std::vector<Polygon2d> ego_polygons_expanded{};
};

class GoalPlannerModule : public SceneModuleInterface
Expand Down Expand Up @@ -224,7 +225,6 @@ class GoalPlannerModule : public SceneModuleInterface
bool isStopped(
std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> & odometry_buffer, const double time);
bool hasFinishedCurrentPath();
bool hasFinishedGoalPlanner();
bool isOnModifiedGoal() const;
double calcModuleRequestLength() const;
void resetStatus();
Expand All @@ -250,6 +250,9 @@ class GoalPlannerModule : public SceneModuleInterface
BehaviorModuleOutput planWithGoalModification();
BehaviorModuleOutput planWaitingApprovalWithGoalModification();
void selectSafePullOverPath();
void sortPullOverPathCandidatesByGoalPriority(
std::vector<PullOverPath> & pull_over_path_candidates,
const GoalCandidates & goal_candidates) const;

// deal with pull over partial paths
PathWithLaneId getCurrentPath() const;
Expand Down
Loading

0 comments on commit 0c0a75f

Please sign in to comment.