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(start_planner): prevent hindering rear vehicles #6545

Merged
merged 19 commits into from
Mar 7, 2024
Merged

feat(start_planner): prevent hindering rear vehicles #6545

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
19 commits
Select commit Hold shift + click to select a range
7808017
wip add function to check if other vehicles can pass
danielsanchezaran Mar 1, 2024
8681a9f
further refactoring
danielsanchezaran Feb 28, 2024
bafcae3
change breaks for return
danielsanchezaran Mar 1, 2024
8bb563e
added way to check closest overhang point to target centerline
danielsanchezaran Mar 1, 2024
9b689a6
wip get distance to left and right bounds
danielsanchezaran Mar 4, 2024
73925ac
add check for param dereferencing
danielsanchezaran Mar 4, 2024
971e9b5
check gap and rear vehicle width
danielsanchezaran Mar 4, 2024
8406b2e
bugfix boolean condition was inverted
danielsanchezaran Mar 4, 2024
51467c7
refactor
danielsanchezaran Mar 5, 2024
d827c77
rename param
danielsanchezaran Mar 5, 2024
5e6f672
remove prints
danielsanchezaran Mar 5, 2024
b6cd6ed
use a better function to get the previous lanelets
danielsanchezaran Mar 5, 2024
6d791b6
update docs
danielsanchezaran Mar 5, 2024
56239b0
update description
danielsanchezaran Mar 6, 2024
7e082b6
typo
danielsanchezaran Mar 6, 2024
2bfd71e
update readme
danielsanchezaran Mar 6, 2024
766abe1
Use the merging side to choose what lane bound to use
danielsanchezaran Mar 7, 2024
887df43
delete unused function
danielsanchezaran Mar 7, 2024
ec5179b
add debug message
danielsanchezaran Mar 7, 2024
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
4 changes: 0 additions & 4 deletions ...behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -141,10 +141,6 @@ bool hasEnoughLengthToLaneChangeAfterAbort(
const Pose & curent_pose, const double abort_return_dist,
const LaneChangeParameters & lane_change_parameters, const Direction direction);

lanelet::ConstLanelets getBackwardLanelets(
const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
const Pose & current_pose, const double backward_length);

double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0);

double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer);
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_path_lane_change_module/src/scene.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -843,7 +843,7 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects(

// get backward lanes
const auto backward_length = lane_change_parameters_->backward_lane_length;
const auto target_backward_lanes = utils::lane_change::getBackwardLanelets(
const auto target_backward_lanes = behavior_path_planner::utils::getBackwardLanelets(
route_handler, target_lanes, current_pose, backward_length);

// filter objects to get target object index
Expand Down
37 changes: 0 additions & 37 deletions planning/behavior_path_lane_change_module/src/utils/utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021 Tier IV, Inc.

Check notice on line 1 in planning/behavior_path_lane_change_module/src/utils/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Overall Code Complexity 

The mean cyclomatic complexity decreases from 4.67 to 4.66, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.

Check notice on line 1 in planning/behavior_path_lane_change_module/src/utils/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Primitive Obsession 

The ratio of primitive types in function arguments increases from 34.29% to 34.56%, threshold = 30.0%. The functions in this file have too many primitive types (e.g. int, double, float) in their function argument lists. Using many primitive types lead to the code smell Primitive Obsession. Avoid adding more primitive arguments.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -674,43 +674,6 @@
return true;
}

// TODO(Azu): In the future, get back lanelet within `to_back_dist` [m] from queried lane
lanelet::ConstLanelets getBackwardLanelets(
const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
const Pose & current_pose, const double backward_length)
{
if (target_lanes.empty()) {
return {};
}

const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose);

if (arc_length.length >= backward_length) {
return {};
}

const auto & front_lane = target_lanes.front();
const auto preceding_lanes = route_handler.getPrecedingLaneletSequence(
front_lane, std::abs(backward_length - arc_length.length), {front_lane});

lanelet::ConstLanelets backward_lanes{};
const auto num_of_lanes = std::invoke([&preceding_lanes]() {
size_t sum{0};
for (const auto & lanes : preceding_lanes) {
sum += lanes.size();
}
return sum;
});

backward_lanes.reserve(num_of_lanes);

for (const auto & lanes : preceding_lanes) {
backward_lanes.insert(backward_lanes.end(), lanes.begin(), lanes.end());
}

return backward_lanes;
}

double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer)
{
return lateral_buffer + 0.5 * vehicle_width;
Expand Down
4 changes: 4 additions & 0 deletions planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -299,6 +299,10 @@ lanelet::ConstLanelets extendPrevLane(
lanelet::ConstLanelets extendLanes(
const std::shared_ptr<RouteHandler> route_handler, const lanelet::ConstLanelets & lanes);

lanelet::ConstLanelets getBackwardLanelets(
const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
const Pose & current_pose, const double backward_length);

lanelet::ConstLanelets getExtendedCurrentLanes(
const std::shared_ptr<const PlannerData> & planner_data);

Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,7 +78,7 @@ PredictedObjects filterObjects(
const std::shared_ptr<ObjectsFilteringParams> & params)
{
// Guard
if (objects->objects.empty()) {
if (objects->objects.empty() || !params) {
return PredictedObjects();
}

Expand Down
37 changes: 37 additions & 0 deletions planning/behavior_path_planner_common/src/utils/utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2023 TIER IV, Inc.

Check notice on line 1 in planning/behavior_path_planner_common/src/utils/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Lines of Code in a Single File 

The lines of code increases from 1275 to 1303, improve code health by reducing it to 1000. The number of Lines of Code in a single file. More Lines of Code lowers the code health.

Check notice on line 1 in planning/behavior_path_planner_common/src/utils/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Overall Code Complexity 

The mean cyclomatic complexity increases from 4.47 to 4.48, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.

Check notice on line 1 in planning/behavior_path_planner_common/src/utils/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Primitive Obsession 

The ratio of primitive types in function arguments decreases from 31.29% to 31.14%, threshold = 30.0%. The functions in this file have too many primitive types (e.g. int, double, float) in their function argument lists. Using many primitive types lead to the code smell Primitive Obsession. Avoid adding more primitive arguments.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -1514,6 +1514,43 @@
return lanes;
}

// TODO(Azu): In the future, get back lanelet within `to_back_dist` [m] from queried lane
lanelet::ConstLanelets getBackwardLanelets(

Check warning on line 1518 in planning/behavior_path_planner_common/src/utils/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner_common/src/utils/utils.cpp#L1518 

Added line #L1518 was not covered by tests
const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
const Pose & current_pose, const double backward_length)
{
if (target_lanes.empty()) {
return {};

Check warning on line 1523 in planning/behavior_path_planner_common/src/utils/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner_common/src/utils/utils.cpp#L1523 

Added line #L1523 was not covered by tests
}

const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose);

Check warning on line 1526 in planning/behavior_path_planner_common/src/utils/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner_common/src/utils/utils.cpp#L1526 

Added line #L1526 was not covered by tests

if (arc_length.length >= backward_length) {
return {};

Check warning on line 1529 in planning/behavior_path_planner_common/src/utils/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner_common/src/utils/utils.cpp#L1529 

Added line #L1529 was not covered by tests
}

const auto & front_lane = target_lanes.front();
const auto preceding_lanes = route_handler.getPrecedingLaneletSequence(
front_lane, std::abs(backward_length - arc_length.length), {front_lane});

lanelet::ConstLanelets backward_lanes{};
const auto num_of_lanes = std::invoke([&preceding_lanes]() {
size_t sum{0};
for (const auto & lanes : preceding_lanes) {
sum += lanes.size();

Check warning on line 1540 in planning/behavior_path_planner_common/src/utils/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner_common/src/utils/utils.cpp#L1540 

Added line #L1540 was not covered by tests
}
return sum;
});

backward_lanes.reserve(num_of_lanes);

for (const auto & lanes : preceding_lanes) {
backward_lanes.insert(backward_lanes.end(), lanes.begin(), lanes.end());
}

return backward_lanes;
}

Check warning on line 1552 in planning/behavior_path_planner_common/src/utils/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner_common/src/utils/utils.cpp#L1552 

Added line #L1552 was not covered by tests

lanelet::ConstLanelets calcLaneAroundPose(
const std::shared_ptr<RouteHandler> route_handler, const Pose & pose, const double forward_length,
const double backward_length, const double dist_threshold, const double yaw_threshold)
Expand Down
51 changes: 26 additions & 25 deletions planning/behavior_path_start_planner_module/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -222,9 +222,9 @@ This ordering is beneficial when the priority is to minimize the backward distan

- **Applying RSS in Dynamic Collision Detection**: Collision detection is based on the RSS (Responsibility-Sensitive Safety) model to evaluate if a safe distance is maintained. See [safety check feature explanation](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md)

- **Collision check performed range**: Safety checks for collisions with dynamic objects are conducted within the defined boundaries between the start and end points of each maneuver, ensuring there is no overlap with the road lane's centerline. This is to avoid hindering the progress of following vehicles.
- **Collision check performed range**: Safety checks for collisions with dynamic objects are conducted within the defined boundaries between the start and end points of each maneuver, ensuring the ego vehicle does not impede or hinder the progress of dynamic objects that come from behind it.

- **Collision response policy**: Should a collision with dynamic objects be detected along the generated path, deactivate module decision is registered if collision detection occurs before departure. If the vehicle has already commenced movement, an attempt to stop will be made, provided it's feasible within the braking constraints and without crossing the travel lane's centerline.
- **Collision response policy**: Should a collision with dynamic objects be detected along the generated path, deactivate module decision is registered if collision detection occurs before departure. If the vehicle has already commenced movement, an attempt to stop will be made, provided it's feasible within the braking constraints and that the rear vehicle can pass through the gap between the ego vehicle and the lane border.

```plantuml
@startuml
Expand All @@ -235,7 +235,7 @@ if (Collision with dynamic objects detected?) then (yes)
if (Before departure?) then (yes)
:Deactivate module decision is registered;
else (no)
if (Can stop within constraints \n && \n no crossing centerline?) then (yes)
if (Can stop within constraints \n && \n Has sufficient space for rear vehicle to drive?) then (yes)
:Stop;
else (no)
:Continue with caution;
Expand All @@ -250,7 +250,7 @@ stop

#### **example of safety check performed range for shift pull out**

Give an example of safety verification range for shift pull out. The safety check is performed from the start of the shift to the end of the shift. And if the vehicle footprint overlaps with the center line of the road lane, the safety check against dynamic objects is disabled.
Give an example of safety verification range for shift pull out. The safety check is performed from the start of the shift to the end of the shift. And if the vehicle footprint does not leave enough space for a rear vehicle to drive through, the safety check against dynamic objects is disabled.

<figure markdown>
![safety check performed range for shift pull out](images/collision_check_range_shift_pull_out.drawio.svg){width=1100}
Expand Down Expand Up @@ -319,27 +319,28 @@ PullOutPath --o PullOutPlannerBase

## General parameters for start_planner

| Name | Unit | Type | Description | Default value |
| :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------------- |
| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 |
| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 |
| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 |
| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 |
| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 |
| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 |
| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 |
| collision_check_margins | [m] | double | Obstacle collision check margins list | [2.0, 1.0, 0.5, 0.1] |
| shift_collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | -10.0 |
| geometric_collision_check_distance_from_end | [m] | double | collision check distance from end geometric end pose | 0.0 |
| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 |
| object_types_to_check_for_path_generation.check_car | - | bool | flag to check car for path generation | true |
| object_types_to_check_for_path_generation.check_truck | - | bool | flag to check truck for path generation | true |
| object_types_to_check_for_path_generation.check_bus | - | bool | flag to check bus for path generation | true |
| object_types_to_check_for_path_generation.check_bicycle | - | bool | flag to check bicycle for path generation | true |
| object_types_to_check_for_path_generation.check_motorcycle | - | bool | flag to check motorcycle for path generation | true |
| object_types_to_check_for_path_generation.check_pedestrian | - | bool | flag to check pedestrian for path generation | true |
| object_types_to_check_for_path_generation.check_unknown | - | bool | flag to check unknown for path generation | true |
| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 |
| Name | Unit | Type | Description | Default value |
| :---------------------------------------------------------- | :---- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------------- |
| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 |
| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 |
| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 |
| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 |
| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 |
| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 |
| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 |
| collision_check_margins | [m] | double | Obstacle collision check margins list | [2.0, 1.0, 0.5, 0.1] |
| shift_collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | -10.0 |
| geometric_collision_check_distance_from_end | [m] | double | collision check distance from end geometric end pose | 0.0 |
| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 |
| extra_width_margin_for_rear_obstacle | [m] | double | extra width that is added to the perceived rear obstacle's width when deciding if the rear obstacle can overtake the ego vehicle while it is merging to a lane from the shoulder lane. | 0.5 |
| object_types_to_check_for_path_generation.check_car | - | bool | flag to check car for path generation | true |
| object_types_to_check_for_path_generation.check_truck | - | bool | flag to check truck for path generation | true |
| object_types_to_check_for_path_generation.check_bus | - | bool | flag to check bus for path generation | true |
| object_types_to_check_for_path_generation.check_bicycle | - | bool | flag to check bicycle for path generation | true |
| object_types_to_check_for_path_generation.check_motorcycle | - | bool | flag to check motorcycle for path generation | true |
| object_types_to_check_for_path_generation.check_pedestrian | - | bool | flag to check pedestrian for path generation | true |
| object_types_to_check_for_path_generation.check_unknown | - | bool | flag to check unknown for path generation | true |
| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 |

### **Ego vehicle's velocity planning**

Expand Down
1 change: 1 addition & 0 deletions planning/behavior_path_start_planner_module/config/start_planner.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
th_stopped_time: 1.0
collision_check_margins: [2.0, 1.0, 0.5, 0.1]
collision_check_margin_from_front_object: 5.0
extra_width_margin_for_rear_obstacle: 0.5
th_moving_object_velocity: 1.0
object_types_to_check_for_path_generation:
check_car: true
Expand Down
1 change: 1 addition & 0 deletions ...ior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,7 @@ struct StartPlannerParameters
double th_distance_to_middle_of_the_road{0.0};
double intersection_search_length{0.0};
double length_ratio_for_turn_signal_deactivation_near_intersection{0.0};
double extra_width_margin_for_rear_obstacle{0.0};
std::vector<double> collision_check_margins{};
double collision_check_margin_from_front_object{0.0};
double th_moving_object_velocity{0.0};
Expand Down
3 changes: 2 additions & 1 deletion ..._start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -207,7 +207,8 @@ class StartPlannerModule : public SceneModuleInterface

bool isModuleRunning() const;
bool isCurrentPoseOnMiddleOfTheRoad() const;
bool isOverlapWithCenterLane() const;
bool isPreventingRearVehicleFromPassingThrough() const;

bool isCloseToOriginalStartPose() const;
bool hasArrivedAtGoal() const;
bool isMoving() const;
Expand Down
6 changes: 6 additions & 0 deletions planning/behavior_path_start_planner_module/src/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,8 @@
p.intersection_search_length = node->declare_parameter<double>(ns + "intersection_search_length");
p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter<double>(
ns + "length_ratio_for_turn_signal_deactivation_near_intersection");
p.extra_width_margin_for_rear_obstacle =
node->declare_parameter<double>(ns + "extra_width_margin_for_rear_obstacle");

Check warning on line 48 in planning/behavior_path_start_planner_module/src/manager.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method 

StartPlannerModuleManager::init increases from 288 to 290 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.

Check warning on line 48 in planning/behavior_path_start_planner_module/src/manager.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/manager.cpp#L47-L48 

Added lines #L47 - L48 were not covered by tests
p.collision_check_margins =
node->declare_parameter<std::vector<double>>(ns + "collision_check_margins");
p.collision_check_margin_from_front_object =
Expand Down Expand Up @@ -371,6 +373,10 @@
updateParam<double>(
parameters, ns + "length_ratio_for_turn_signal_deactivation_near_intersection",
p->length_ratio_for_turn_signal_deactivation_near_intersection);
updateParam<double>(
parameters, ns + "extra_width_margin_for_rear_obstacle",
p->extra_width_margin_for_rear_obstacle);

Check warning on line 378 in planning/behavior_path_start_planner_module/src/manager.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/manager.cpp#L376-L378 

Added lines #L376 - L378 were not covered by tests

Check warning on line 379 in planning/behavior_path_start_planner_module/src/manager.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method 

StartPlannerModuleManager::updateModuleParams increases from 352 to 355 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
updateParam<std::vector<double>>(
parameters, ns + "collision_check_margins", p->collision_check_margins);
updateParam<double>(
Expand Down
Loading
Loading