feat(start_planner): prevent hindering rear vehicles (autowarefoundat…

…ion#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>
Interplai · Jun 3, 2024 · ef61653 · ef61653
1 parent 3aa14ba
commit ef61653
Show file tree

Hide file tree

Showing 12 changed files with 220 additions and 104 deletions.
diff --git a/...behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/...behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp
@@ -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);

diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp
@@ -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

diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp
@@ -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;

diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp
@@ -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);
 

diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp
@@ -78,7 +78,7 @@ PredictedObjects filterObjects(
   const std::shared_ptr<ObjectsFilteringParams> & params)
 {
   // Guard
-  if (objects->objects.empty()) {
+  if (objects->objects.empty() || !params) {
     return PredictedObjects();
   }
 

diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp
@@ -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)

diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md
@@ -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
@@ -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;
@@ -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}
@@ -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**
 

diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml
@@ -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

diff --git a/...ior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/...ior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp
@@ -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};

diff --git a/..._start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/..._start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp
@@ -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;

diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp
@@ -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 =
@@ -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>(