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 <[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]>
tier4 · Mar 7, 2024 · f2e309d · f2e309d
1 parent 5a9e616
commit f2e309d
Show file tree

Hide file tree

Showing 3 changed files with 42 additions and 1 deletion.
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)