fix(avoidance): use nearest overhang point

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -265,8 +265,8 @@ ObjectData AvoidanceByLaneChange::createObjectData(
                             : 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);
+  object_data.overhang_points =
+    utils::avoidance::calcEnvelopeOverhangDistance(object_data, data.reference_path);
 
   // Check whether the the ego should avoid the object.
   const auto & vehicle_width = planner_data_->parameters.vehicle_width;

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -336,12 +336,8 @@ struct ObjectData  // avoidance target
 {
   ObjectData() = default;
 
-  ObjectData(PredictedObject obj, double lat, double lon, double len, double overhang)
-  : object(std::move(obj)),
-    to_centerline(lat),
-    longitudinal(lon),
-    length(len),
-    overhang_dist(overhang)
+  ObjectData(PredictedObject obj, double lat, double lon, double len)
+  : object(std::move(obj)), to_centerline(lat), longitudinal(lon), length(len)
   {
   }
 
@@ -365,9 +361,6 @@ struct ObjectData  // avoidance target
   // longitudinal length of vehicle, in Frenet coordinate
   double length{0.0};
 
-  // lateral distance to the closest footprint, in Frenet coordinate
-  double overhang_dist{0.0};
-
   // lateral shiftable ratio
   double shiftable_ratio{0.0};
 
@@ -392,9 +385,6 @@ struct ObjectData  // avoidance target
   // the position at the detected moment
   Pose init_pose;
 
-  // the position of the overhang
-  Pose overhang_pose;
-
   // envelope polygon
   Polygon2d envelope_poly{};
 
@@ -425,14 +415,17 @@ struct ObjectData  // avoidance target
   // object direction.
   Direction direction{Direction::NONE};
 
+  // overhang points (sort by distance)
+  std::vector<std::pair<double, Point>> overhang_points{};
+
   // unavoidable reason
   std::string reason{};
 
   // lateral avoid margin
   std::optional<double> avoid_margin{std::nullopt};
 
   // the nearest bound point (use in road shoulder distance calculation)
-  std::optional<Point> nearest_bound_point{std::nullopt};
+  std::optional<std::pair<Point, Point>> narrowest_place{std::nullopt};
 };
 using ObjectDataArray = std::vector<ObjectData>;
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

@@ -176,7 +176,8 @@ class AvoidanceHelper
   {
     using utils::avoidance::calcShiftLength;
 
-    const auto shift_length = calcShiftLength(is_on_right, object.overhang_dist, margin);
+    const auto shift_length =
+      calcShiftLength(is_on_right, object.overhang_points.front().first, margin);
     return is_on_right ? std::min(shift_length, getLeftShiftBound())
                        : std::max(shift_length, getRightShiftBound());
   }

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

@@ -56,8 +56,8 @@ double lerpShiftLengthOnArc(double arc, const AvoidLine & al);
 void fillLongitudinalAndLengthByClosestEnvelopeFootprint(
   const PathWithLaneId & path, const Point & ego_pos, ObjectData & obj);
 
-double calcEnvelopeOverhangDistance(
-  const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose);
+std::vector<std::pair<double, Point>> calcEnvelopeOverhangDistance(
+  const ObjectData & object_data, const PathWithLaneId & path);
 
 void setEndData(
   AvoidLine & al, const double length, const geometry_msgs::msg::Pose & end, const size_t end_idx,

planning/behavior_path_avoidance_module/src/debug.cpp

@@ -113,7 +113,7 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std::
   MarkerArray msg;
 
   for (const auto & object : objects) {
-    if (!object.nearest_bound_point.has_value()) {
+    if (!object.narrowest_place.has_value()) {
       continue;
     }
 
@@ -122,8 +122,8 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std::
         "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP,
         createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.42, 0.999));
 
-      marker.points.push_back(object.overhang_pose.position);
-      marker.points.push_back(object.nearest_bound_point.value());
+      marker.points.push_back(object.narrowest_place.value().first);
+      marker.points.push_back(object.narrowest_place.value().second);
       marker.id = uuidToInt32(object.object.object_id);
       msg.markers.push_back(marker);
     }
@@ -133,7 +133,7 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std::
         "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING,
         createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0));
 
-      marker.pose.position = object.nearest_bound_point.value();
+      marker.pose.position = object.narrowest_place.value().second;
       std::ostringstream string_stream;
       string_stream << object.to_road_shoulder_distance << "[m]";
       marker.text = string_stream.str();

planning/behavior_path_avoidance_module/src/shift_line_generator.cpp

@@ -219,7 +219,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
                                 : -1.0 * feasible_relative_shift_length + current_ego_shift;
 
     const auto infeasible =
-      std::abs(feasible_shift_length - object.overhang_dist) <
+      std::abs(feasible_shift_length - object.overhang_points.front().first) <
       0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral;
     if (infeasible) {
       RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. ");

planning/behavior_path_avoidance_module/src/utils.cpp

@@ -819,8 +819,8 @@ bool isNoNeedAvoidanceBehavior(
     return false;
   }
 
-  const auto shift_length =
-    calcShiftLength(isOnRight(object), object.overhang_dist, object.avoid_margin.value());
+  const auto shift_length = calcShiftLength(
+    isOnRight(object), object.overhang_points.front().first, object.avoid_margin.value());
   if (!isShiftNecessary(isOnRight(object), shift_length)) {
     object.reason = "NotNeedAvoidance";
     return true;
@@ -881,40 +881,41 @@ double getRoadShoulderDistance(
     return 0.0;
   }
 
-  const auto centerline_pose =
-    lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position);
-  const auto & p1_object = object.overhang_pose.position;
-  const auto p_tmp =
-    geometry_msgs::build<Pose>().position(p1_object).orientation(centerline_pose.orientation);
-  const auto p2_object =
-    calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position;
+  std::vector<std::pair<Point, Point>> intersects;
+  for (const auto & p1 : object.overhang_points) {
+    const auto centerline_pose =
+      lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position);
+    const auto p_tmp =
+      geometry_msgs::build<Pose>().position(p1.second).orientation(centerline_pose.orientation);
+    const auto p2 = calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position;
 
-  // TODO(Satoshi OTA): check if the basic point is on right or left of bound.
-  const auto bound = isOnRight(object) ? data.left_bound : data.right_bound;
+    // TODO(Satoshi OTA): check if the basic point is on right or left of bound.
+    const auto bound = isOnRight(object) ? data.left_bound : data.right_bound;
 
-  std::vector<Point> intersects;
-  for (size_t i = 1; i < bound.size(); i++) {
-    const auto opt_intersect =
-      tier4_autoware_utils::intersect(p1_object, p2_object, bound.at(i - 1), bound.at(i));
+    for (size_t i = 1; i < bound.size(); i++) {
+      const auto opt_intersect =
+        tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i));
 
-    if (!opt_intersect) {
-      continue;
-    }
+      if (!opt_intersect) {
+        continue;
+      }
 
-    intersects.push_back(opt_intersect.value());
+      intersects.emplace_back(p1.second, opt_intersect.value());
+    }
   }
 
+  std::sort(intersects.begin(), intersects.end(), [](const auto & a, const auto & b) {
+    return calcDistance2d(a.first, a.second) < calcDistance2d(b.first, b.second);
+  });
+
   if (intersects.empty()) {
     return 0.0;
   }
 
-  std::sort(intersects.begin(), intersects.end(), [&p1_object](const auto & a, const auto & b) {
-    return calcDistance2d(p1_object, a) < calcDistance2d(p1_object, b);
-  });
-
-  object.nearest_bound_point = intersects.front();
+  object.narrowest_place = intersects.front();
 
-  return calcDistance2d(p1_object, object.nearest_bound_point.value());
+  return calcDistance2d(
+    object.narrowest_place.value().first, object.narrowest_place.value().second);
 }
 }  // namespace filtering_utils
 
@@ -1070,34 +1071,22 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint(
   return;
 }
 
-double calcEnvelopeOverhangDistance(
-  const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose)
+std::vector<std::pair<double, Point>> calcEnvelopeOverhangDistance(
+  const ObjectData & object_data, const PathWithLaneId & path)
 {
-  double largest_overhang = isOnRight(object_data) ? -100.0 : 100.0;  // large number
+  std::vector<std::pair<double, Point>> overhang_points{};
 
   for (const auto & p : object_data.envelope_poly.outer()) {
     const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0);
     // TODO(someone): search around first position where the ego should avoid the object.
     const auto idx = findNearestIndex(path.points, point);
     const auto lateral = calcLateralDeviation(getPose(path.points.at(idx)), point);
-
-    const auto & overhang_pose_on_right = [&overhang_pose, &largest_overhang, &point, &lateral]() {
-      if (lateral > largest_overhang) {
-        overhang_pose = point;
-      }
-      return std::max(largest_overhang, lateral);
-    };
-
-    const auto & overhang_pose_on_left = [&overhang_pose, &largest_overhang, &point, &lateral]() {
-      if (lateral < largest_overhang) {
-        overhang_pose = point;
-      }
-      return std::min(largest_overhang, lateral);
-    };
-
-    largest_overhang = isOnRight(object_data) ? overhang_pose_on_right() : overhang_pose_on_left();
+    overhang_points.emplace_back(lateral, point);
   }
-  return largest_overhang;
+  std::sort(overhang_points.begin(), overhang_points.end(), [&](const auto & a, const auto & b) {
+    return isOnRight(object_data) ? b.first < a.first : a.first < b.first;
+  });
+  return overhang_points;
 }
 
 void setEndData(
@@ -1442,10 +1431,10 @@ void fillAvoidanceNecessity(
     0.5 * vehicle_width + object_parameter.safety_buffer_lateral * object_data.distance_factor;
 
   const auto check_necessity = [&](const auto hysteresis_factor) {
-    return (isOnRight(object_data) &&
-            std::abs(object_data.overhang_dist) < safety_margin * hysteresis_factor) ||
+    return (isOnRight(object_data) && std::abs(object_data.overhang_points.front().first) <
+                                        safety_margin * hysteresis_factor) ||
            (!isOnRight(object_data) &&
-            object_data.overhang_dist < safety_margin * hysteresis_factor);
+            object_data.overhang_points.front().first < safety_margin * hysteresis_factor);
   };
 
   const auto id = object_data.object.object_id;
@@ -1665,8 +1654,7 @@ void filterTargetObjects(
     }
 
     // Find the footprint point closest to the path, set to object_data.overhang_distance.
-    o.overhang_dist =
-      calcEnvelopeOverhangDistance(o, data.reference_path, o.overhang_pose.position);
+    o.overhang_points = utils::avoidance::calcEnvelopeOverhangDistance(o, data.reference_path);
     o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data);
     o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters);