Skip to content

Commit 07a84d4

Browse files
danielsanchezarandanielsanchezaran
authored
feat(start_planner): prevent hindering rear vehicles (#6545)
* wip add function to check if other vehicles can pass Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * further refactoring Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * change breaks for return Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * added way to check closest overhang point to target centerline Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * wip get distance to left and right bounds Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add check for param dereferencing Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * check gap and rear vehicle width Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * bugfix boolean condition was inverted Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * refactor Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * rename param Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * remove prints Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * use a better function to get the previous lanelets Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * update docs Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * update description Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * typo Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * update readme Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * Use the merging side to choose what lane bound to use Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * delete unused function Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add debug message Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> --------- Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
1 parent 7081a61 commit 07a84d4

File tree

12 files changed

+220
-104
lines changed

12 files changed

+220
-104
lines changed

‎planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp

-4
Original file line numberDiff line numberDiff line change
@@ -141,10 +141,6 @@ bool hasEnoughLengthToLaneChangeAfterAbort(
141141
const Pose & curent_pose, const double abort_return_dist,
142142
const LaneChangeParameters & lane_change_parameters, const Direction direction);
143143

144-
lanelet::ConstLanelets getBackwardLanelets(
145-
const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
146-
const Pose & current_pose, const double backward_length);
147-
148144
double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0);
149145

150146
double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer);

‎planning/behavior_path_lane_change_module/src/scene.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -843,7 +843,7 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects(
843843

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

849849
// filter objects to get target object index

‎planning/behavior_path_lane_change_module/src/utils/utils.cpp

-37
Original file line numberDiff line numberDiff line change
@@ -674,43 +674,6 @@ bool hasEnoughLengthToLaneChangeAfterAbort(
674674
return true;
675675
}
676676

677-
// TODO(Azu): In the future, get back lanelet within `to_back_dist` [m] from queried lane
678-
lanelet::ConstLanelets getBackwardLanelets(
679-
const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
680-
const Pose & current_pose, const double backward_length)
681-
{
682-
if (target_lanes.empty()) {
683-
return {};
684-
}
685-
686-
const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose);
687-
688-
if (arc_length.length >= backward_length) {
689-
return {};
690-
}
691-
692-
const auto & front_lane = target_lanes.front();
693-
const auto preceding_lanes = route_handler.getPrecedingLaneletSequence(
694-
front_lane, std::abs(backward_length - arc_length.length), {front_lane});
695-
696-
lanelet::ConstLanelets backward_lanes{};
697-
const auto num_of_lanes = std::invoke([&preceding_lanes]() {
698-
size_t sum{0};
699-
for (const auto & lanes : preceding_lanes) {
700-
sum += lanes.size();
701-
}
702-
return sum;
703-
});
704-
705-
backward_lanes.reserve(num_of_lanes);
706-
707-
for (const auto & lanes : preceding_lanes) {
708-
backward_lanes.insert(backward_lanes.end(), lanes.begin(), lanes.end());
709-
}
710-
711-
return backward_lanes;
712-
}
713-
714677
double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer)
715678
{
716679
return lateral_buffer + 0.5 * vehicle_width;

‎planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -299,6 +299,10 @@ lanelet::ConstLanelets extendPrevLane(
299299
lanelet::ConstLanelets extendLanes(
300300
const std::shared_ptr<RouteHandler> route_handler, const lanelet::ConstLanelets & lanes);
301301

302+
lanelet::ConstLanelets getBackwardLanelets(
303+
const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
304+
const Pose & current_pose, const double backward_length);
305+
302306
lanelet::ConstLanelets getExtendedCurrentLanes(
303307
const std::shared_ptr<const PlannerData> & planner_data);
304308

‎planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -78,7 +78,7 @@ PredictedObjects filterObjects(
7878
const std::shared_ptr<ObjectsFilteringParams> & params)
7979
{
8080
// Guard
81-
if (objects->objects.empty()) {
81+
if (objects->objects.empty() || !params) {
8282
return PredictedObjects();
8383
}
8484

‎planning/behavior_path_planner_common/src/utils/utils.cpp

+37
Original file line numberDiff line numberDiff line change
@@ -1514,6 +1514,43 @@ lanelet::ConstLanelets getExtendedCurrentLanesFromPath(
15141514
return lanes;
15151515
}
15161516

1517+
// TODO(Azu): In the future, get back lanelet within `to_back_dist` [m] from queried lane
1518+
lanelet::ConstLanelets getBackwardLanelets(
1519+
const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
1520+
const Pose & current_pose, const double backward_length)
1521+
{
1522+
if (target_lanes.empty()) {
1523+
return {};
1524+
}
1525+
1526+
const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose);
1527+
1528+
if (arc_length.length >= backward_length) {
1529+
return {};
1530+
}
1531+
1532+
const auto & front_lane = target_lanes.front();
1533+
const auto preceding_lanes = route_handler.getPrecedingLaneletSequence(
1534+
front_lane, std::abs(backward_length - arc_length.length), {front_lane});
1535+
1536+
lanelet::ConstLanelets backward_lanes{};
1537+
const auto num_of_lanes = std::invoke([&preceding_lanes]() {
1538+
size_t sum{0};
1539+
for (const auto & lanes : preceding_lanes) {
1540+
sum += lanes.size();
1541+
}
1542+
return sum;
1543+
});
1544+
1545+
backward_lanes.reserve(num_of_lanes);
1546+
1547+
for (const auto & lanes : preceding_lanes) {
1548+
backward_lanes.insert(backward_lanes.end(), lanes.begin(), lanes.end());
1549+
}
1550+
1551+
return backward_lanes;
1552+
}
1553+
15171554
lanelet::ConstLanelets calcLaneAroundPose(
15181555
const std::shared_ptr<RouteHandler> route_handler, const Pose & pose, const double forward_length,
15191556
const double backward_length, const double dist_threshold, const double yaw_threshold)

‎planning/behavior_path_start_planner_module/README.md

+26-25
Original file line numberDiff line numberDiff line change
@@ -222,9 +222,9 @@ This ordering is beneficial when the priority is to minimize the backward distan
222222

223223
- **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)
224224

225-
- **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.
225+
- **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.
226226

227-
- **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.
227+
- **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.
228228

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

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

253-
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.
253+
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.
254254

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

320320
## General parameters for start_planner
321321

322-
| Name | Unit | Type | Description | Default value |
323-
| :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------------- |
324-
| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 |
325-
| 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 |
326-
| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 |
327-
| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 |
328-
| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 |
329-
| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 |
330-
| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 |
331-
| collision_check_margins | [m] | double | Obstacle collision check margins list | [2.0, 1.0, 0.5, 0.1] |
332-
| shift_collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | -10.0 |
333-
| geometric_collision_check_distance_from_end | [m] | double | collision check distance from end geometric end pose | 0.0 |
334-
| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 |
335-
| object_types_to_check_for_path_generation.check_car | - | bool | flag to check car for path generation | true |
336-
| object_types_to_check_for_path_generation.check_truck | - | bool | flag to check truck for path generation | true |
337-
| object_types_to_check_for_path_generation.check_bus | - | bool | flag to check bus for path generation | true |
338-
| object_types_to_check_for_path_generation.check_bicycle | - | bool | flag to check bicycle for path generation | true |
339-
| object_types_to_check_for_path_generation.check_motorcycle | - | bool | flag to check motorcycle for path generation | true |
340-
| object_types_to_check_for_path_generation.check_pedestrian | - | bool | flag to check pedestrian for path generation | true |
341-
| object_types_to_check_for_path_generation.check_unknown | - | bool | flag to check unknown for path generation | true |
342-
| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 |
322+
| Name | Unit | Type | Description | Default value |
323+
| :---------------------------------------------------------- | :---- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------------- |
324+
| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 |
325+
| 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 |
326+
| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 |
327+
| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 |
328+
| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 |
329+
| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 |
330+
| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 |
331+
| collision_check_margins | [m] | double | Obstacle collision check margins list | [2.0, 1.0, 0.5, 0.1] |
332+
| shift_collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | -10.0 |
333+
| geometric_collision_check_distance_from_end | [m] | double | collision check distance from end geometric end pose | 0.0 |
334+
| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 |
335+
| 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 |
336+
| object_types_to_check_for_path_generation.check_car | - | bool | flag to check car for path generation | true |
337+
| object_types_to_check_for_path_generation.check_truck | - | bool | flag to check truck for path generation | true |
338+
| object_types_to_check_for_path_generation.check_bus | - | bool | flag to check bus for path generation | true |
339+
| object_types_to_check_for_path_generation.check_bicycle | - | bool | flag to check bicycle for path generation | true |
340+
| object_types_to_check_for_path_generation.check_motorcycle | - | bool | flag to check motorcycle for path generation | true |
341+
| object_types_to_check_for_path_generation.check_pedestrian | - | bool | flag to check pedestrian for path generation | true |
342+
| object_types_to_check_for_path_generation.check_unknown | - | bool | flag to check unknown for path generation | true |
343+
| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 |
343344

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

‎planning/behavior_path_start_planner_module/config/start_planner.param.yaml

+1
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77
th_stopped_time: 1.0
88
collision_check_margins: [2.0, 1.0, 0.5, 0.1]
99
collision_check_margin_from_front_object: 5.0
10+
extra_width_margin_for_rear_obstacle: 0.5
1011
th_moving_object_velocity: 1.0
1112
object_types_to_check_for_path_generation:
1213
check_car: true

‎planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -64,6 +64,7 @@ struct StartPlannerParameters
6464
double th_distance_to_middle_of_the_road{0.0};
6565
double intersection_search_length{0.0};
6666
double length_ratio_for_turn_signal_deactivation_near_intersection{0.0};
67+
double extra_width_margin_for_rear_obstacle{0.0};
6768
std::vector<double> collision_check_margins{};
6869
double collision_check_margin_from_front_object{0.0};
6970
double th_moving_object_velocity{0.0};

‎planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -207,7 +207,8 @@ class StartPlannerModule : public SceneModuleInterface
207207

208208
bool isModuleRunning() const;
209209
bool isCurrentPoseOnMiddleOfTheRoad() const;
210-
bool isOverlapWithCenterLane() const;
210+
bool isPreventingRearVehicleFromPassingThrough() const;
211+
211212
bool isCloseToOriginalStartPose() const;
212213
bool hasArrivedAtGoal() const;
213214
bool isMoving() const;

‎planning/behavior_path_start_planner_module/src/manager.cpp

+6
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
4444
p.intersection_search_length = node->declare_parameter<double>(ns + "intersection_search_length");
4545
p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter<double>(
4646
ns + "length_ratio_for_turn_signal_deactivation_near_intersection");
47+
p.extra_width_margin_for_rear_obstacle =
48+
node->declare_parameter<double>(ns + "extra_width_margin_for_rear_obstacle");
4749
p.collision_check_margins =
4850
node->declare_parameter<std::vector<double>>(ns + "collision_check_margins");
4951
p.collision_check_margin_from_front_object =
@@ -371,6 +373,10 @@ void StartPlannerModuleManager::updateModuleParams(
371373
updateParam<double>(
372374
parameters, ns + "length_ratio_for_turn_signal_deactivation_near_intersection",
373375
p->length_ratio_for_turn_signal_deactivation_near_intersection);
376+
updateParam<double>(
377+
parameters, ns + "extra_width_margin_for_rear_obstacle",
378+
p->extra_width_margin_for_rear_obstacle);
379+
374380
updateParam<std::vector<double>>(
375381
parameters, ns + "collision_check_margins", p->collision_check_margins);
376382
updateParam<double>(

0 commit comments

Comments
 (0)