fixup! feat(avoidance): suppress unnecessary avoidance path

autowarefoundation · Nov 19, 2023 · 2525ffb · 2525ffb
1 parent 94b6aff
commit 2525ffb
Show file tree

Hide file tree

Showing 3 changed files with 33 additions and 18 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
@@ -94,6 +94,10 @@ lanelet::ConstLanelets getTargetLanelets(
 lanelet::ConstLanelets getCurrentLanesFromPath(
   const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data);
 
+lanelet::ConstLanelets getExtendLanes(
+  const lanelet::ConstLanelets & lanelets, const Pose & ego_pose,
+  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/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -264,24 +264,8 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
   data.current_lanelets = utils::avoidance::getCurrentLanesFromPath(
     *getPreviousModuleOutput().reference_path, planner_data_);
 
-  data.extend_lanelets = data.current_lanelets;
-  while (rclcpp::ok()) {
-    const auto distance = utils::getArcLengthToTargetLanelet(
-      data.extend_lanelets, data.extend_lanelets.back(), getEgoPose());
-
-    if (distance > planner_data_->parameters.forward_path_length) {
-      break;
-    }
-
-    const auto next_lanelets =
-      planner_data_->route_handler->getNextLanelets(data.extend_lanelets.back());
-
-    if (next_lanelets.empty()) {
-      break;
-    }
-
-    data.extend_lanelets.push_back(next_lanelets.front());
-  }
+  data.extend_lanelets =
+    utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_);
 
   // expand drivable lanes
   std::for_each(

diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp
@@ -971,6 +971,33 @@ lanelet::ConstLanelets getCurrentLanesFromPath(
     start_lane, p.backward_path_length, p.forward_path_length);
 }
 
+lanelet::ConstLanelets getExtendLanes(
+  const lanelet::ConstLanelets & lanelets, const Pose & ego_pose,
+  const std::shared_ptr<const PlannerData> & planner_data)
+{
+  lanelet::ConstLanelets extend_lanelets = lanelets;
+
+  while (rclcpp::ok()) {
+    const double lane_length = lanelet::utils::getLaneletLength2d(extend_lanelets);
+    const auto arc_coodinates = lanelet::utils::getArcCoordinates(extend_lanelets, ego_pose);
+    const auto forward_length = lane_length - arc_coodinates.length;
+
+    if (forward_length > planner_data->parameters.forward_path_length) {
+      break;
+    }
+
+    const auto next_lanelets = planner_data->route_handler->getNextLanelets(extend_lanelets.back());
+
+    if (next_lanelets.empty()) {
+      break;
+    }
+
+    extend_lanelets.push_back(next_lanelets.front());
+  }
+
+  return extend_lanelets;
+}
+
 void insertDecelPoint(
   const Point & p_src, const double offset, const double velocity, PathWithLaneId & path,
   boost::optional<Pose> & p_out)