perf(static_obstacle_avoidance): remove redundant calculation related…

… to lanelet functions (#9355)

* add traffic light distance and modified goal allowance to avoidance data

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

* add closest lanelet related variable to avoidanceData structure

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

* use route handler for checking closest lanelet

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

* use std::optional

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

---------

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp

@@ -27,6 +27,7 @@
 
 #include <limits>
 #include <memory>
+#include <optional>
 #include <string>
 #include <unordered_map>
 #include <utility>
@@ -594,6 +595,12 @@ struct AvoidancePlanningData
 
   double to_return_point{std::numeric_limits<double>::max()};
 
+  std::optional<double> distance_to_red_traffic_light{std::nullopt};
+
+  std::optional<lanelet::ConstLanelet> closest_lanelet{std::nullopt};
+
+  bool is_allowed_goal_modification{false};
+
   bool request_operator{false};
 };
 

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp

@@ -212,20 +212,20 @@ class AvoidanceHelper
                        : std::max(shift_length, getRightShiftBound());
   }
 
-  double getForwardDetectionRange() const
+  double getForwardDetectionRange(
+    const std::optional<lanelet::ConstLanelet> & closest_lanelet) const
   {
     if (parameters_->use_static_detection_area) {
       return parameters_->object_check_max_forward_distance;
     }
 
     const auto & route_handler = data_->route_handler;
 
-    lanelet::ConstLanelet closest_lane;
-    if (!route_handler->getClosestLaneletWithinRoute(getEgoPose(), &closest_lane)) {
+    if (!closest_lanelet.has_value()) {
       return parameters_->object_check_max_forward_distance;
     }
 
-    const auto limit = route_handler->getTrafficRulesPtr()->speedLimit(closest_lane);
+    const auto limit = route_handler->getTrafficRulesPtr()->speedLimit(closest_lanelet.value());
     const auto speed = isShifted() ? limit.speedLimit.value() : getEgoSpeed();
 
     const auto max_shift_length = std::max(

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp

@@ -54,7 +54,8 @@ double calcShiftLength(
   const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin);
 
 bool isWithinLanes(
-  const lanelet::ConstLanelets & lanelets, const std::shared_ptr<const PlannerData> & planner_data);
+  const std::optional<lanelet::ConstLanelet> & closest_lanelet,
+  const std::shared_ptr<const PlannerData> & planner_data);
 
 /**
  * @brief check if the ego has to shift driving position.
@@ -261,12 +262,12 @@ DrivableLanes generateExpandedDrivableLanes(
 double calcDistanceToReturnDeadLine(
   const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,
   const std::shared_ptr<const PlannerData> & planner_data,
-  const std::shared_ptr<AvoidanceParameters> & parameters);
+  const std::shared_ptr<AvoidanceParameters> & parameters,
+  const std::optional<double> distance_to_red_traffic, const bool is_allowed_goal_modification);
 
 double calcDistanceToAvoidStartLine(
-  const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,
-  const std::shared_ptr<const PlannerData> & planner_data,
-  const std::shared_ptr<AvoidanceParameters> & parameters);
+  const lanelet::ConstLanelets & lanelets, const std::shared_ptr<AvoidanceParameters> & parameters,
+  const std::optional<double> distance_to_red_traffic);
 
 /**
  * @brief calculate error eclipse radius based on object pose covariance.

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp

@@ -32,6 +32,7 @@
 #include <algorithm>
 #include <limits>
 #include <memory>
+#include <optional>
 #include <set>
 #include <string>
 #include <vector>
@@ -210,9 +211,13 @@ void StaticObstacleAvoidanceModule::fillFundamentalData(
   data.extend_lanelets = utils::static_obstacle_avoidance::getExtendLanes(
     data.current_lanelets, getEgoPose(), planner_data_);
 
+  lanelet::ConstLanelet closest_lanelet{};
+  if (planner_data_->route_handler->getClosestLaneletWithinRoute(getEgoPose(), &closest_lanelet))
+    data.closest_lanelet = closest_lanelet;
+
   // expand drivable lanes
   const auto is_within_current_lane =
-    utils::static_obstacle_avoidance::isWithinLanes(data.current_lanelets, planner_data_);
+    utils::static_obstacle_avoidance::isWithinLanes(data.closest_lanelet, planner_data_);
   const auto red_signal_lane_itr = std::find_if(
     data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
       if (utils::traffic_light::isTrafficSignalStop({lanelet}, planner_data_)) {
@@ -285,11 +290,17 @@ void StaticObstacleAvoidanceModule::fillFundamentalData(
     data.reference_path, 0, data.reference_path.points.size(),
     autoware::motion_utils::calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0));
 
+  data.is_allowed_goal_modification =
+    utils::isAllowedGoalModification(planner_data_->route_handler);
+  data.distance_to_red_traffic_light = utils::traffic_light::calcDistanceToRedTrafficLight(
+    data.current_lanelets, data.reference_path_rough, planner_data_);
+
   data.to_return_point = utils::static_obstacle_avoidance::calcDistanceToReturnDeadLine(
-    data.current_lanelets, data.reference_path_rough, planner_data_, parameters_);
+    data.current_lanelets, data.reference_path_rough, planner_data_, parameters_,
+    data.distance_to_red_traffic_light, data.is_allowed_goal_modification);
 
   data.to_start_point = utils::static_obstacle_avoidance::calcDistanceToAvoidStartLine(
-    data.current_lanelets, data.reference_path_rough, planner_data_, parameters_);
+    data.current_lanelets, parameters_, data.distance_to_red_traffic_light);
 
   // filter only for the latest detected objects.
   fillAvoidanceTargetObjects(data, debug);
@@ -323,17 +334,16 @@ void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects(
   using utils::static_obstacle_avoidance::filterTargetObjects;
   using utils::static_obstacle_avoidance::separateObjectsByPath;
   using utils::static_obstacle_avoidance::updateRoadShoulderDistance;
-  using utils::traffic_light::calcDistanceToRedTrafficLight;
 
   // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets.
   constexpr double MARGIN = 10.0;
   const auto forward_detection_range = [&]() {
-    const auto to_traffic_light = calcDistanceToRedTrafficLight(
-      data.current_lanelets, helper_->getPreviousReferencePath(), planner_data_);
-    if (!to_traffic_light.has_value()) {
-      return helper_->getForwardDetectionRange();
+    if (!data.distance_to_red_traffic_light.has_value()) {
+      return helper_->getForwardDetectionRange(data.closest_lanelet);
     }
-    return std::min(helper_->getForwardDetectionRange(), to_traffic_light.value());
+    return std::min(
+      helper_->getForwardDetectionRange(data.closest_lanelet),
+      data.distance_to_red_traffic_light.value());
   }();
 
   const auto [object_within_target_lane, object_outside_target_lane] = separateObjectsByPath(

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

@@ -35,6 +35,7 @@
 #include <algorithm>
 #include <limits>
 #include <memory>
+#include <optional>
 #include <string>
 #include <vector>
 
@@ -1161,35 +1162,35 @@ double calcShiftLength(
 }
 
 bool isWithinLanes(
-  const lanelet::ConstLanelets & lanelets, const std::shared_ptr<const PlannerData> & planner_data)
+  const std::optional<lanelet::ConstLanelet> & closest_lanelet,
+  const std::shared_ptr<const PlannerData> & planner_data)
 {
   const auto & rh = planner_data->route_handler;
   const auto & ego_pose = planner_data->self_odometry->pose.pose;
   const auto transform = autoware::universe_utils::pose2transform(ego_pose);
   const auto footprint = autoware::universe_utils::transformVector(
     planner_data->parameters.vehicle_info.createFootprint(), transform);
 
-  lanelet::ConstLanelet closest_lanelet{};
-  if (!lanelet::utils::query::getClosestLanelet(lanelets, ego_pose, &closest_lanelet)) {
+  if (!closest_lanelet.has_value()) {
     return true;
   }
 
   lanelet::ConstLanelets concat_lanelets{};
 
   // push previous lanelet
   lanelet::ConstLanelets prev_lanelet;
-  if (rh->getPreviousLaneletsWithinRoute(closest_lanelet, &prev_lanelet)) {
+  if (rh->getPreviousLaneletsWithinRoute(closest_lanelet.value(), &prev_lanelet)) {
     concat_lanelets.push_back(prev_lanelet.front());
   }
 
   // push nearest lanelet
   {
-    concat_lanelets.push_back(closest_lanelet);
+    concat_lanelets.push_back(closest_lanelet.value());
   }
 
   // push next lanelet
   lanelet::ConstLanelet next_lanelet;
-  if (rh->getNextLaneletWithinRoute(closest_lanelet, &next_lanelet)) {
+  if (rh->getNextLaneletWithinRoute(closest_lanelet.value(), &next_lanelet)) {
     concat_lanelets.push_back(next_lanelet);
   }
 
@@ -1950,13 +1951,11 @@ void filterTargetObjects(
       ? autoware::motion_utils::calcSignedArcLength(
           data.reference_path_rough.points, ego_idx, data.reference_path_rough.points.size() - 1)
       : std::numeric_limits<double>::max();
-  const auto & is_allowed_goal_modification =
-    utils::isAllowedGoalModification(planner_data->route_handler);
 
   for (auto & o : objects) {
     if (!filtering_utils::isSatisfiedWithCommonCondition(
           o, data.reference_path_rough, forward_detection_range, to_goal_distance,
-          planner_data->self_odometry->pose.pose.position, is_allowed_goal_modification,
+          planner_data->self_odometry->pose.pose.position, data.is_allowed_goal_modification,
           parameters)) {
       data.other_objects.push_back(o);
       continue;
@@ -2556,9 +2555,8 @@ DrivableLanes generateExpandedDrivableLanes(
 }
 
 double calcDistanceToAvoidStartLine(
-  const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,
-  const std::shared_ptr<const PlannerData> & planner_data,
-  const std::shared_ptr<AvoidanceParameters> & parameters)
+  const lanelet::ConstLanelets & lanelets, const std::shared_ptr<AvoidanceParameters> & parameters,
+  const std::optional<double> distance_to_red_traffic)
 {
   if (lanelets.empty()) {
     return std::numeric_limits<double>::lowest();
@@ -2568,11 +2566,10 @@ double calcDistanceToAvoidStartLine(
 
   // dead line stop factor(traffic light)
   if (parameters->enable_dead_line_for_traffic_light) {
-    const auto to_traffic_light = calcDistanceToRedTrafficLight(lanelets, path, planner_data);
-    if (to_traffic_light.has_value()) {
+    if (distance_to_red_traffic.has_value()) {
       distance_to_return_dead_line = std::max(
         distance_to_return_dead_line,
-        to_traffic_light.value() + parameters->dead_line_buffer_for_traffic_light);
+        distance_to_red_traffic.value() + parameters->dead_line_buffer_for_traffic_light);
     }
   }
 
@@ -2582,7 +2579,8 @@ double calcDistanceToAvoidStartLine(
 double calcDistanceToReturnDeadLine(
   const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,
   const std::shared_ptr<const PlannerData> & planner_data,
-  const std::shared_ptr<AvoidanceParameters> & parameters)
+  const std::shared_ptr<AvoidanceParameters> & parameters,
+  const std::optional<double> distance_to_red_traffic, const bool is_allowed_goal_modification)
 {
   if (lanelets.empty()) {
     return std::numeric_limits<double>::max();
@@ -2592,18 +2590,15 @@ double calcDistanceToReturnDeadLine(
 
   // dead line stop factor(traffic light)
   if (parameters->enable_dead_line_for_traffic_light) {
-    const auto to_traffic_light = calcDistanceToRedTrafficLight(lanelets, path, planner_data);
-    if (to_traffic_light.has_value()) {
+    if (distance_to_red_traffic.has_value()) {
       distance_to_return_dead_line = std::min(
         distance_to_return_dead_line,
-        to_traffic_light.value() - parameters->dead_line_buffer_for_traffic_light);
+        distance_to_red_traffic.value() - parameters->dead_line_buffer_for_traffic_light);
     }
   }
 
   // dead line for goal
-  if (
-    !utils::isAllowedGoalModification(planner_data->route_handler) &&
-    parameters->enable_dead_line_for_goal) {
+  if (!is_allowed_goal_modification && parameters->enable_dead_line_for_goal) {
     if (planner_data->route_handler->isInGoalRouteSection(lanelets.back())) {
       const auto & ego_pos = planner_data->self_odometry->pose.pose.position;
       const auto to_goal_distance =