fix(utils): fix unstable drivable area generation (#4293)

* fix(utils): generate proper reference path even when ego is shifted Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(utils): add function to get reference lanelets for avoidance module Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(avoidance): use unique utils function Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(utils): fix endless loop Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(utils): guard empty path Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(utils): use constraints at nearest index search Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
autowarefoundation · Jul 21, 2023 · d70c718 · d70c718
1 parent fc5ec80
commit d70c718
Show file tree

Hide file tree

Showing 5 changed files with 309 additions and 81 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp
@@ -88,6 +88,9 @@ lanelet::ConstLanelets getTargetLanelets(
   const std::shared_ptr<const PlannerData> & planner_data, lanelet::ConstLanelets & route_lanelets,
   const double left_offset, const double right_offset);
 
+lanelet::ConstLanelets getCurrentLanesFromPath(
+  const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data);
+
 void insertDecelPoint(
   const Point & p_src, const double offset, const double velocity, PathWithLaneId & path,
   boost::optional<Pose> & p_out);

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp
@@ -406,7 +406,9 @@ DrivableAreaInfo combineDrivableAreaInfo(
 void extractObstaclesFromDrivableArea(
   PathWithLaneId & path, const std::vector<DrivableAreaInfo::Obstacle> & obstacles);
 
-void makeBoundLongitudinallyMonotonic(PathWithLaneId & path, const bool is_bound_left);
+void makeBoundLongitudinallyMonotonic(
+  PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
+  const bool is_bound_left);
 
 std::optional<lanelet::Polygon3d> getPolygonByPoint(
   const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstPoint3d & point,

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -256,8 +256,8 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb
     calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0));
 
   // lanelet info
-  data.current_lanelets =
-    utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_);
+  data.current_lanelets = utils::avoidance::getCurrentLanesFromPath(
+    *getPreviousModuleOutput().reference_path, planner_data_);
 
   // keep avoidance state
   data.state = avoidance_data_.state;
@@ -2869,8 +2869,8 @@ void AvoidanceModule::updateData()
     helper_.setPreviousSplineShiftPath(toShiftedPath(*getPreviousModuleOutput().path));
     helper_.setPreviousLinearShiftPath(toShiftedPath(*getPreviousModuleOutput().path));
     helper_.setPreviousReferencePath(*getPreviousModuleOutput().path);
-    helper_.setPreviousDrivingLanes(
-      utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_));
+    helper_.setPreviousDrivingLanes(utils::avoidance::getCurrentLanesFromPath(
+      *getPreviousModuleOutput().reference_path, planner_data_));
   }
 
   debug_data_ = DebugData();

diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp
@@ -498,6 +498,25 @@ lanelet::ConstLanelets getTargetLanelets(
   return target_lanelets;
 }
 
+lanelet::ConstLanelets getCurrentLanesFromPath(
+  const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data)
+{
+  if (path.points.empty()) {
+    throw std::logic_error("empty path.");
+  }
+
+  if (path.points.front().lane_ids.empty()) {
+    throw std::logic_error("empty lane ids.");
+  }
+
+  const auto start_id = path.points.front().lane_ids.front();
+  const auto start_lane = planner_data->route_handler->getLaneletsFromId(start_id);
+  const auto & p = planner_data->parameters;
+
+  return planner_data->route_handler->getLaneletSequence(
+    start_lane, p.backward_path_length, p.forward_path_length);
+}
+
 void insertDecelPoint(
   const Point & p_src, const double offset, const double velocity, PathWithLaneId & path,
   boost::optional<Pose> & p_out)