fixup! feat(avoidance): suppress unnecessary avoidance path

autowarefoundation · Nov 21, 2023 · 354aa32 · 354aa32
1 parent 1feab0f
commit 354aa32
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 30 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
@@ -131,6 +131,11 @@ void filterTargetObjects(
   const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters);
 
+double getRoadShoulderDistance(
+  ObjectData & object, const AvoidancePlanningData & data,
+  const std::shared_ptr<const PlannerData> & planner_data,
+  const std::shared_ptr<AvoidanceParameters> & parameters);
+
 double extendToRoadShoulderDistanceWithPolygon(
   const std::shared_ptr<route_handler::RouteHandler> & rh,
   const lanelet::ConstLineString3d & target_line, const double to_road_shoulder_distance,

diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp
@@ -629,6 +629,36 @@ bool isNoNeedAvoidanceBehavior(
 
   return false;
 }
+
+std::optional<double> getAvoidMargin(
+  const ObjectData & object, const std::shared_ptr<const PlannerData> & planner_data,
+  const std::shared_ptr<AvoidanceParameters> & parameters)
+{
+  const auto & vehicle_width = planner_data->parameters.vehicle_width;
+  const auto object_type = utils::getHighestProbLabel(object.object.classification);
+  const auto object_parameter = parameters->object_parameters.at(object_type);
+
+  const auto max_avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor +
+                                object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
+  const auto min_avoid_margin = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width;
+  const auto soft_lateral_distance_limit =
+    object.to_road_shoulder_distance - parameters->soft_road_shoulder_margin - 0.5 * vehicle_width;
+  const auto hard_lateral_distance_limit =
+    object.to_road_shoulder_distance - parameters->hard_road_shoulder_margin - 0.5 * vehicle_width;
+
+  // Step1. check avoidable or not.
+  if (hard_lateral_distance_limit < min_avoid_margin) {
+    return std::nullopt;
+  }
+
+  // Step2. check if it should expand road shoulder margin.
+  if (soft_lateral_distance_limit < min_avoid_margin) {
+    return min_avoid_margin;
+  }
+
+  // Step3. nominal case. avoid margin is limited by soft constraint.
+  return std::min(soft_lateral_distance_limit, max_avoid_margin);
+}
 }  // namespace filtering_utils
 
 bool isOnRight(const ObjectData & obj)
@@ -1373,36 +1403,6 @@ double getRoadShoulderDistance(
   return road_shoulder_distance;
 }
 
-std::optional<double> getAvoidMargin(
-  const ObjectData & object, const std::shared_ptr<const PlannerData> & planner_data,
-  const std::shared_ptr<AvoidanceParameters> & parameters)
-{
-  const auto & vehicle_width = planner_data->parameters.vehicle_width;
-  const auto object_type = utils::getHighestProbLabel(object.object.classification);
-  const auto object_parameter = parameters->object_parameters.at(object_type);
-
-  const auto max_avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor +
-                                object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
-  const auto min_avoid_margin = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width;
-  const auto soft_lateral_distance_limit =
-    object.to_road_shoulder_distance - parameters->soft_road_shoulder_margin - 0.5 * vehicle_width;
-  const auto hard_lateral_distance_limit =
-    object.to_road_shoulder_distance - parameters->hard_road_shoulder_margin - 0.5 * vehicle_width;
-
-  // Step1. check avoidable or not.
-  if (hard_lateral_distance_limit < min_avoid_margin) {
-    return std::nullopt;
-  }
-
-  // Step2. check if it should expand road shoulder margin.
-  if (soft_lateral_distance_limit < min_avoid_margin) {
-    return min_avoid_margin;
-  }
-
-  // Step3. nominal case. avoid margin is limited by soft constraint.
-  return std::min(soft_lateral_distance_limit, max_avoid_margin);
-}
-
 void filterTargetObjects(
   ObjectDataArray & objects, AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug,
   const std::shared_ptr<const PlannerData> & planner_data,