fix(avoidance): fix lateral distance calculation (autowarefoundation#…

…4820) * fix(avoidance): fix calculation logic for road shoulder distance Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(avoidance): fix overhang distance calculation Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(avoidance): don't set avoidable for invalid shift Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
kminoda · Sep 7, 2023 · 6c52d51 · 6c52d51
1 parent f537e9c
commit 6c52d51
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 6 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
@@ -70,7 +70,7 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint(
   const PathWithLaneId & path, const Point & ego_pos, ObjectData & obj);
 
 double calcEnvelopeOverhangDistance(
-  const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose);
+  const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose);
 
 void setEndData(
   AvoidLine & al, const double length, const geometry_msgs::msg::Pose & end, const size_t end_idx,

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
@@ -331,7 +331,7 @@ ObjectData AvoidanceModule::createObjectData(
 
   // Find the footprint point closest to the path, set to object_data.overhang_distance.
   object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance(
-    object_data, object_closest_pose, object_data.overhang_pose.position);
+    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;
@@ -982,6 +982,9 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
     if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) {
       avoid_lines.push_back(al_avoid);
       avoid_lines.push_back(al_return);
+    } else {
+      o.reason = "InvalidShiftLine";
+      continue;
     }
 
     o.is_avoidable = true;

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp
@@ -216,7 +216,7 @@ ObjectData AvoidanceByLaneChange::createObjectData(
 
   // Find the footprint point closest to the path, set to object_data.overhang_distance.
   object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance(
-    object_data, object_closest_pose, object_data.overhang_pose.position);
+    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;

diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp
@@ -406,13 +406,14 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint(
 }
 
 double calcEnvelopeOverhangDistance(
-  const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose)
+  const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose)
 {
   double largest_overhang = isOnRight(object_data) ? -100.0 : 100.0;  // large number
 
   for (const auto & p : object_data.envelope_poly.outer()) {
     const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0);
-    const auto lateral = tier4_autoware_utils::calcLateralDeviation(base_pose, point);
+    const auto idx = findFirstNearestIndex(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) {
@@ -1014,7 +1015,7 @@ void filterTargetObjects(
         const auto lines =
           rh->getFurthestLinestring(target_lanelet, get_right, get_left, get_opposite, true);
         const auto & line = isOnRight(o) ? lines.back() : lines.front();
-        const auto d = distance2d(to2D(overhang_basic_pose), to2D(line.basicLineString()));
+        const auto d = boost::geometry::distance(o.envelope_poly, to2D(line.basicLineString()));
         if (d < o.to_road_shoulder_distance) {
           o.to_road_shoulder_distance = d;
           target_line = line;