From 24bc357bfbc8b895a0e29581e73cb0838679000e Mon Sep 17 00:00:00 2001
From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Date: Mon, 5 Feb 2024 17:19:09 +0900
Subject: [PATCH] fix(avoidance): don't use polygon centroid in shiftable ratio
 calculation (#6285)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
Co-authored-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
---
 .../src/utils.cpp                             | 36 ++++++++++---------
 1 file changed, 20 insertions(+), 16 deletions(-)

diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp
index b5d351cbc48a8..edc76f09f1763 100644
--- a/planning/behavior_path_avoidance_module/src/utils.cpp
+++ b/planning/behavior_path_avoidance_module/src/utils.cpp
@@ -21,6 +21,7 @@
 #include "behavior_path_planner_common/utils/traffic_light_utils.hpp"
 #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
 
+#include <lanelet2_extension/utility/message_conversion.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/ros/uuid_helper.hpp>
 
@@ -476,11 +477,11 @@ bool isObjectOnRoadShoulder(
   ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler,
   const std::shared_ptr<AvoidanceParameters> & parameters)
 {
-  using boost::geometry::return_centroid;
   using boost::geometry::within;
   using lanelet::geometry::distance2d;
   using lanelet::geometry::toArcCoordinates;
   using lanelet::utils::to2D;
+  using lanelet::utils::conversion::toLaneletPoint;
 
   // assume that there are no parked vehicles in intersection.
   std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else");
@@ -498,13 +499,9 @@ bool isObjectOnRoadShoulder(
   // +: object position
   // o: nearest point on centerline
 
-  lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y());
-
   const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
-  const auto centerline_pose =
-    lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position);
-  lanelet::BasicPoint3d centerline_point(
-    centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z);
+  const auto centerline_pos =
+    lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position).position;
 
   bool is_left_side_parked_vehicle = false;
   if (!isOnRight(object)) {
@@ -526,8 +523,9 @@ bool isObjectOnRoadShoulder(
         }
       }
 
-      const auto center_to_left_boundary =
-        distance2d(to2D(most_left_lanelet.leftBound().basicLineString()), to2D(centerline_point));
+      const auto center_to_left_boundary = distance2d(
+        to2D(most_left_lanelet.leftBound().basicLineString()),
+        to2D(toLaneletPoint(centerline_pos)).basicPoint());
 
       return std::make_pair(
         center_to_left_boundary - 0.5 * object.object.shape.dimensions.y, sub_type);
@@ -538,7 +536,8 @@ bool isObjectOnRoadShoulder(
     }
 
     const auto arc_coordinates = toArcCoordinates(
-      to2D(object.overhang_lanelet.centerline().basicLineString()), object_centroid);
+      to2D(object.overhang_lanelet.centerline().basicLineString()),
+      to2D(toLaneletPoint(object_pose.position)).basicPoint());
     object.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance;
 
     is_left_side_parked_vehicle = object.shiftable_ratio > parameters->object_check_shiftable_ratio;
@@ -564,8 +563,9 @@ bool isObjectOnRoadShoulder(
         }
       }
 
-      const auto center_to_right_boundary =
-        distance2d(to2D(most_right_lanelet.rightBound().basicLineString()), to2D(centerline_point));
+      const auto center_to_right_boundary = distance2d(
+        to2D(most_right_lanelet.rightBound().basicLineString()),
+        to2D(toLaneletPoint(centerline_pos)).basicPoint());
 
       return std::make_pair(
         center_to_right_boundary - 0.5 * object.object.shape.dimensions.y, sub_type);
@@ -576,7 +576,8 @@ bool isObjectOnRoadShoulder(
     }
 
     const auto arc_coordinates = toArcCoordinates(
-      to2D(object.overhang_lanelet.centerline().basicLineString()), object_centroid);
+      to2D(object.overhang_lanelet.centerline().basicLineString()),
+      to2D(toLaneletPoint(object_pose.position)).basicPoint());
     object.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance;
 
     is_right_side_parked_vehicle =
@@ -754,6 +755,8 @@ bool isSatisfiedWithVehicleCondition(
   const std::shared_ptr<AvoidanceParameters> & parameters)
 {
   using boost::geometry::within;
+  using lanelet::utils::to2D;
+  using lanelet::utils::conversion::toLaneletPoint;
 
   object.behavior = getObjectBehavior(object, parameters);
   object.is_within_intersection = isWithinIntersection(object, planner_data->route_handler);
@@ -764,9 +767,10 @@ bool isSatisfiedWithVehicleCondition(
   }
 
   // check vehicle shift ratio
-  lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y());
-  const auto on_ego_driving_lane =
-    within(object_centroid, object.overhang_lanelet.polygon2d().basicPolygon());
+  const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
+  const auto on_ego_driving_lane = within(
+    to2D(toLaneletPoint(object_pos)).basicPoint(),
+    object.overhang_lanelet.polygon2d().basicPolygon());
   if (on_ego_driving_lane) {
     if (isObjectOnRoadShoulder(object, planner_data->route_handler, parameters)) {
       return true;