feat(avoidance): improve object detection area in order not to preven…

…t endless loop (autowarefoundation#6084) * perf(avoidance): reduce heavy process Signed-off-by: satoshi-ota <[email protected]> * fix(avoidance): filter objects by precise lon distance Signed-off-by: satoshi-ota <[email protected]> * refactor(avoidance): remove unused function Signed-off-by: satoshi-ota <[email protected]> * feat(avoidance): improve detection area Signed-off-by: satoshi-ota <[email protected]> * fix(avoidance): return shift line Signed-off-by: satoshi-ota <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]>
TaikiYamada4 · Jan 16, 2024 · 3928295 · 3928295
1 parent 3acbdd7
commit 3928295
Show file tree

Hide file tree

Showing 5 changed files with 143 additions and 125 deletions.
diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp
@@ -187,7 +187,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
 }
 
 void AvoidanceByLaneChange::fillAvoidanceTargetObjects(
-  AvoidancePlanningData & data, DebugData & debug) const
+  AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const
 {
   const auto p = std::dynamic_pointer_cast<AvoidanceParameters>(avoidance_parameters_);
 
@@ -227,7 +227,9 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects(
       [&](const auto & object) { return createObjectData(data, object); });
   }
 
-  utils::avoidance::filterTargetObjects(target_lane_objects, data, debug, planner_data_, p);
+  utils::avoidance::filterTargetObjects(
+    target_lane_objects, data, avoidance_parameters_->object_check_max_forward_distance,
+    planner_data_, p);
 }
 
 ObjectData AvoidanceByLaneChange::createObjectData(

diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp
@@ -77,11 +77,6 @@ std::vector<DrivableAreaInfo::Obstacle> generateObstaclePolygonsForDrivableArea(
   const ObjectDataArray & objects, const std::shared_ptr<AvoidanceParameters> & parameters,
   const double vehicle_width);
 
-double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v);
-
-bool isCentroidWithinLanelets(
-  const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets);
-
 lanelet::ConstLanelets getAdjacentLane(
   const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift);
@@ -128,12 +123,7 @@ void compensateDetectionLost(
   ObjectDataArray & other_objects);
 
 void filterTargetObjects(
-  ObjectDataArray & objects, AvoidancePlanningData & data, DebugData & debug,
-  const std::shared_ptr<const PlannerData> & planner_data,
-  const std::shared_ptr<AvoidanceParameters> & parameters);
-
-double getRoadShoulderDistance(
-  ObjectData & object, const AvoidancePlanningData & data,
+  ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range,
   const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters);
 
@@ -157,9 +147,10 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
   DebugData & debug);
 
 std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
-  const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
-  const AvoidancePlanningData & data, const std::shared_ptr<AvoidanceParameters> & parameters,
-  const double object_check_forward_distance, const bool is_running, DebugData & debug);
+  const PathWithLaneId & reference_path, const PathWithLaneId & spline_path,
+  const std::shared_ptr<const PlannerData> & planner_data, const AvoidancePlanningData & data,
+  const std::shared_ptr<AvoidanceParameters> & parameters,
+  const double object_check_forward_distance, DebugData & debug);
 
 DrivableLanes generateExpandDrivableLanes(
   const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,

diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp
@@ -287,21 +287,18 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
 void AvoidanceModule::fillAvoidanceTargetObjects(
   AvoidancePlanningData & data, DebugData & debug) const
 {
+  using utils::avoidance::fillAvoidanceNecessity;
   using utils::avoidance::fillObjectStoppableJudge;
   using utils::avoidance::filterTargetObjects;
   using utils::avoidance::getTargetLanelets;
-
-  // Add margin in order to prevent avoidance request chattering only when the module is running.
-  const auto is_running = getCurrentStatus() == ModuleStatus::RUNNING ||
-                          getCurrentStatus() == ModuleStatus::WAITING_APPROVAL;
+  using utils::avoidance::separateObjectsByPath;
 
   // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets.
-  const auto sparse_resample_path = utils::resamplePathWithSpline(
-    helper_->getPreviousSplineShiftPath().path, parameters_->resample_interval_for_output);
-  const auto [object_within_target_lane, object_outside_target_lane] =
-    utils::avoidance::separateObjectsByPath(
-      sparse_resample_path, planner_data_, data, parameters_, helper_->getForwardDetectionRange(),
-      is_running, debug);
+  constexpr double MARGIN = 10.0;
+  const auto forward_detection_range = helper_->getForwardDetectionRange();
+  const auto [object_within_target_lane, object_outside_target_lane] = separateObjectsByPath(
+    helper_->getPreviousReferencePath(), helper_->getPreviousSplineShiftPath().path, planner_data_,
+    data, parameters_, forward_detection_range + MARGIN, debug);
 
   for (const auto & object : object_outside_target_lane.objects) {
     ObjectData other_object;
@@ -316,11 +313,13 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
   }
 
   // Filter out the objects to determine the ones to be avoided.
-  filterTargetObjects(objects, data, debug, planner_data_, parameters_);
+  filterTargetObjects(objects, data, forward_detection_range, planner_data_, parameters_);
 
   // Calculate the distance needed to safely decelerate the ego vehicle to a stop line.
+  const auto & vehicle_width = planner_data_->parameters.vehicle_width;
   const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false);
   std::for_each(data.target_objects.begin(), data.target_objects.end(), [&, this](auto & o) {
+    fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_);
     o.to_stop_line = calcDistanceToStopLine(o);
     fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_);
   });
@@ -380,21 +379,10 @@ ObjectData AvoidanceModule::createObjectData(
   utils::avoidance::fillInitialPose(object_data, detected_objects_);
 
   // Calc lateral deviation from path to target object.
-  object_data.to_centerline =
-    lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance;
   object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0
                             ? Direction::LEFT
                             : Direction::RIGHT;
 
-  // Find the footprint point closest to the path, set to object_data.overhang_distance.
-  object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance(
-    object_data, data.reference_path, object_data.overhang_pose.position);
-
-  // Check whether the the ego should avoid the object.
-  const auto & vehicle_width = planner_data_->parameters.vehicle_width;
-  utils::avoidance::fillAvoidanceNecessity(
-    object_data, registered_objects_, vehicle_width, parameters_);
-
   return object_data;
 }
 

diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp
@@ -1101,8 +1101,8 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine(
   const double variable_prepare_distance =
     std::max(nominal_prepare_distance - last_sl_distance, 0.0);
 
-  double prepare_distance_scaled =
-    std::clamp(nominal_prepare_distance, helper_->getMinimumPrepareDistance(), last_sl_distance);
+  double prepare_distance_scaled = std::max(
+    helper_->getMinimumPrepareDistance(), std::max(nominal_prepare_distance, last_sl_distance));
   double avoid_distance_scaled = nominal_avoid_distance;
   if (remaining_distance < prepare_distance_scaled + avoid_distance_scaled) {
     const auto scale = (remaining_distance - last_sl_distance) /
@@ -1122,7 +1122,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine(
     al.end_idx =
       utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled);
     al.end = data.reference_path.points.at(al.end_idx).point.pose;
-    al.end_longitudinal = arclength_from_ego.at(al.end_idx);
+    al.end_longitudinal = prepare_distance_scaled;
     al.end_shift_length = last_sl.end_shift_length;
     al.start_shift_length = last_sl.end_shift_length;
     ret.push_back(al);
@@ -1136,7 +1136,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine(
     al.start_idx =
       utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled);
     al.start = data.reference_path.points.at(al.start_idx).point.pose;
-    al.start_longitudinal = arclength_from_ego.at(al.start_idx);
+    al.start_longitudinal = prepare_distance_scaled;
     al.end_idx = utils::avoidance::findPathIndexFromArclength(
       arclength_from_ego, prepare_distance_scaled + avoid_distance_scaled);
     al.end = data.reference_path.points.at(al.end_idx).point.pose;