Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat: update start goal planner v0.10.2 #905

Merged
merged 14 commits into from
Oct 3, 2023
Merged

feat: update start goal planner v0.10.2 #905

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
14 commits
Select commit Hold shift + click to select a range
b7e5059
refactor(goal_planner): do not finish goal planner (#5037)
kosuke55 Sep 23, 2023
845d213
chore(geometric_parallel_parking): remove unused params and initializ…
kosuke55 Sep 24, 2023
f438521
feat(start_planner): visualize backward driving path (#5088)
kosuke55 Sep 24, 2023
5aeb89e
feat(start_planner): prevent start planner execution in the middle of…
danielsanchezaran Sep 21, 2023
759f648
fix(start/goal_planner): resample path and make params (#5085)
kosuke55 Sep 24, 2023
0bcd1aa
fix(start_planner): fix backward drving path cutting (#5098)
kosuke55 Sep 24, 2023
05d9389
fix(start_planner): remove inverse orientation points (#5100)
kosuke55 Sep 25, 2023
5ab10d5
feat(goal_planner): sort goal candidates by weighted distance (#5110)
kosuke55 Sep 26, 2023
2c857e0
fix(behavior_path_planner): add empty guard to combinePath (#5147)
kosuke55 Sep 27, 2023
e4dd42f
fix(goal_planner): fix goal searcher dying in free space (#5151)
kosuke55 Sep 27, 2023
beff574
perf(goal_planner): speed up goal candidates update (#5114)
kosuke55 Sep 27, 2023
3aa54b6
fix(goal_planner): disable freespace planner when fixed goal (#5152)
kosuke55 Sep 27, 2023
52c42b3
feat(goal_planner): prioritize goals before objects to avoid (#5137)
kosuke55 Sep 28, 2023
d99668a
feat(goal_planner): secure lateral distance for path planning with co…
yuki-takagi-66 Oct 3, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 10 additions & 5 deletions common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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