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

Author: danielsanchezaran

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);
 

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();
   }
 

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)