Skip to content

Commit

Permalink
feat(start_planner): prevent hindering rear vehicles (#6545)
Browse files Browse the repository at this point in the history 
* wip add function to check if other vehicles can pass

Signed-off-by: Daniel Sanchez <[email protected]>

* further refactoring

Signed-off-by: Daniel Sanchez <[email protected]>

* change breaks for return

Signed-off-by: Daniel Sanchez <[email protected]>

* added way to check closest overhang point to target centerline

Signed-off-by: Daniel Sanchez <[email protected]>

* wip get distance to left and right bounds

Signed-off-by: Daniel Sanchez <[email protected]>

* add check for param dereferencing

Signed-off-by: Daniel Sanchez <[email protected]>

* check gap and rear vehicle width

Signed-off-by: Daniel Sanchez <[email protected]>

* bugfix boolean condition was inverted

Signed-off-by: Daniel Sanchez <[email protected]>

* refactor

Signed-off-by: Daniel Sanchez <[email protected]>

* rename param

Signed-off-by: Daniel Sanchez <[email protected]>

* remove prints

Signed-off-by: Daniel Sanchez <[email protected]>

* use a better function to get the previous lanelets

Signed-off-by: Daniel Sanchez <[email protected]>

* update docs

Signed-off-by: Daniel Sanchez <[email protected]>

* update description

Signed-off-by: Daniel Sanchez <[email protected]>

* typo

Signed-off-by: Daniel Sanchez <[email protected]>

* update readme

Signed-off-by: Daniel Sanchez <[email protected]>

* Use the merging side to choose what lane bound to use

Signed-off-by: Daniel Sanchez <[email protected]>

* delete unused function

Signed-off-by: Daniel Sanchez <[email protected]>

* add debug message

Signed-off-by: Daniel Sanchez <[email protected]>

---------

Signed-off-by: Daniel Sanchez <[email protected]>
Signed-off-by: Kotaro Yoshimoto <[email protected]>
  • Loading branch information
@danielsanchezaran @HansRobo
danielsanchezaran authored and HansRobo committed Mar 12, 2024
1 parent 0bfa5e1 commit 98c5081
Show file tree
Hide file tree
Showing 12 changed files with 220 additions and 104 deletions.
4 changes: 0 additions & 4 deletions ...behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp
View file
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
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
Original file line number Diff line number Diff line change
Expand Up @@ -674,43 +674,6 @@ bool hasEnoughLengthToLaneChangeAfterAbort(
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
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
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
Original file line number Diff line number Diff line change
Expand Up @@ -1514,6 +1514,43 @@ lanelet::ConstLanelets getExtendedCurrentLanesFromPath(
return lanes;
}

// 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;
}

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
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
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
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
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
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
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");
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 @@ void StartPlannerModuleManager::updateModuleParams(
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);

updateParam<std::vector<double>>(
parameters, ns + "collision_check_margins", p->collision_check_margins);
updateParam<double>(
Expand Down
Loading

0 comments on commit 98c5081

Please sign in to comment.