fix(avoidance): update logic to calculate centerline distance (#5855)

* fix(avoidance): use centerline of current lanelets in object filtering logic

Signed-off-by: satoshi-ota <[email protected]>

* fix(avoidance): fix test

Signed-off-by: satoshi-ota <[email protected]>

---------

Signed-off-by: satoshi-ota <[email protected]>

planning/behavior_path_avoidance_by_lane_change_module/package.xml

@@ -22,6 +22,7 @@
   <depend>behavior_path_lane_change_module</depend>
   <depend>behavior_path_planner</depend>
   <depend>behavior_path_planner_common</depend>
+  <depend>lanelet2_extension</depend>
   <depend>motion_utils</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -20,6 +20,7 @@
 #include "behavior_path_planner_common/utils/path_utils.hpp"
 #include "behavior_path_planner_common/utils/utils.hpp"
 
+#include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/logging.hpp>
 
 #include <boost/geometry/algorithms/centroid.hpp>
@@ -232,6 +233,7 @@ ObjectData AvoidanceByLaneChange::createObjectData(
   using boost::geometry::return_centroid;
   using motion_utils::findNearestIndex;
   using tier4_autoware_utils::calcDistance2d;
+  using tier4_autoware_utils::calcLateralDeviation;
 
   const auto p = std::dynamic_pointer_cast<AvoidanceParameters>(avoidance_parameters_);
 
@@ -263,8 +265,11 @@ ObjectData AvoidanceByLaneChange::createObjectData(
   utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, p);
 
   // Calc lateral deviation from path to target object.
-  object_data.lateral =
-    tier4_autoware_utils::calcLateralDeviation(object_closest_pose, object_pose.position);
+  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(

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -52,6 +52,8 @@ using geometry_msgs::msg::TransformStamped;
 
 using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug;
 
+using route_handler::Direction;
+
 struct ObjectParameter
 {
   bool is_avoidance_target{false};
@@ -327,14 +329,14 @@ struct ObjectData  // avoidance target
 {
   ObjectData() = default;
   ObjectData(const PredictedObject & obj, double lat, double lon, double len, double overhang)
-  : object(obj), lateral(lat), longitudinal(lon), length(len), overhang_dist(overhang)
+  : object(obj), to_centerline(lat), longitudinal(lon), length(len), overhang_dist(overhang)
   {
   }
 
   PredictedObject object;
 
   // lateral position of the CoM, in Frenet coordinate from ego-pose
-  double lateral;
+  double to_centerline;
 
   // longitudinal position of the CoM, in Frenet coordinate from ego-pose
   double longitudinal;
@@ -396,6 +398,9 @@ struct ObjectData  // avoidance target
   // is within intersection area
   bool is_within_intersection{false};
 
+  // object direction.
+  Direction direction{Direction::NONE};
+
   // unavoidable reason
   std::string reason{""};
 
@@ -566,8 +571,6 @@ struct ShiftLineData
  */
 struct DebugData
 {
-  std::shared_ptr<lanelet::ConstLanelets> current_lanelets;
-
   geometry_msgs::msg::Polygon detection_area;
 
   lanelet::ConstLineStrings3d bounds;

planning/behavior_path_avoidance_module/src/debug.cpp

@@ -16,6 +16,7 @@
 
 #include "behavior_path_planner_common/utils/utils.hpp"
 
+#include <lanelet2_extension/visualization/visualization.hpp>
 #include <magic_enum.hpp>
 #include <tier4_autoware_utils/ros/uuid_helper.hpp>
 
@@ -160,7 +161,7 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st
       std::ostringstream string_stream;
       string_stream << std::fixed << std::setprecision(2) << std::boolalpha;
       string_stream << "ratio:" << object.shiftable_ratio << " [-]\n"
-                    << "lateral:" << object.lateral << " [m]\n"
+                    << "lateral:" << object.to_centerline << " [m]\n"
                     << "necessity:" << object.avoid_required << " [-]\n"
                     << "stoppable:" << object.is_stoppable << " [-]\n"
                     << "stop_factor:" << object.to_stop_factor_distance << " [m]\n"
@@ -495,6 +496,8 @@ MarkerArray createDrivableBounds(
 MarkerArray createDebugMarkerArray(
   const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug)
 {
+  using behavior_path_planner::utils::transformToLanelets;
+  using lanelet::visualization::laneletsAsTriangleMarkerArray;
   using marker_utils::createLaneletsAreaMarkerArray;
   using marker_utils::createObjectsMarkerArray;
   using marker_utils::createPathMarkerArray;
@@ -608,9 +611,13 @@ MarkerArray createDebugMarkerArray(
   // misc
   {
     add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5));
-    add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0));
     add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1));
     add(createDrivableBounds(data, "drivable_bound", 1.0, 0.0, 0.42));
+    add(laneletsAsTriangleMarkerArray(
+      "drivable_lanes", transformToLanelets(data.drivable_lanes),
+      createMarkerColor(0.16, 1.0, 0.69, 0.2)));
+    add(laneletsAsTriangleMarkerArray(
+      "current_lanes", data.current_lanelets, createMarkerColor(1.0, 1.0, 1.0, 0.2)));
   }
 
   return msg;

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -328,14 +328,12 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
 
   // debug
   {
-    debug.current_lanelets = std::make_shared<lanelet::ConstLanelets>(data.current_lanelets);
-
     std::vector<AvoidanceDebugMsg> debug_info_array;
     const auto append = [&](const auto & o) {
       AvoidanceDebugMsg debug_info;
       debug_info.object_id = toHexString(o.object.object_id);
       debug_info.longitudinal_distance = o.longitudinal;
-      debug_info.lateral_distance_from_centerline = o.lateral;
+      debug_info.lateral_distance_from_centerline = o.to_centerline;
       debug_info.allow_avoidance = o.reason == "";
       debug_info.failed_reason = o.reason;
       debug_info_array.push_back(debug_info);
@@ -380,7 +378,11 @@ ObjectData AvoidanceModule::createObjectData(
   utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, parameters_);
 
   // Calc lateral deviation from path to target object.
-  object_data.lateral = calcLateralDeviation(object_closest_pose, object_pose.position);
+  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(

planning/behavior_path_avoidance_module/src/utils.cpp

@@ -594,7 +594,7 @@ bool isSatisfiedWithVehicleCondition(
   }
 
   // Object is on center line -> ignore.
-  if (std::abs(object.lateral) < parameters->threshold_distance_object_is_on_center) {
+  if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) {
     object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE;
     return false;
   }
@@ -678,7 +678,11 @@ std::optional<double> getAvoidMargin(
 
 bool isOnRight(const ObjectData & obj)
 {
-  return obj.lateral < 0.0;
+  if (obj.direction == Direction::NONE) {
+    throw std::logic_error("object direction is not initialized. something wrong.");
+  }
+
+  return obj.direction == Direction::RIGHT;
 }
 
 double calcShiftLength(
@@ -960,9 +964,8 @@ std::vector<DrivableAreaInfo::Obstacle> generateObstaclePolygonsForDrivableArea(
       object.avoid_margin.value() - object_parameter.envelope_buffer_margin - vehicle_width / 2.0;
     const auto obj_poly =
       tier4_autoware_utils::expandPolygon(object.envelope_poly, diff_poly_buffer);
-    const bool is_left = 0 < object.lateral;
     obstacles_for_drivable_area.push_back(
-      {object.object.kinematics.initial_pose_with_covariance.pose, obj_poly, is_left});
+      {object.object.kinematics.initial_pose_with_covariance.pose, obj_poly, !isOnRight(object)});
   }
   return obstacles_for_drivable_area;
 }

planning/behavior_path_avoidance_module/test/test_utils.cpp

@@ -25,19 +25,15 @@ using behavior_path_planner::utils::avoidance::isSameDirectionShift;
 TEST(BehaviorPathPlanningAvoidanceUtilsTest, shiftLengthDirectionTest)
 {
   ObjectData right_obj;
-  right_obj.lateral = -0.3;
+  right_obj.direction = route_handler::Direction::RIGHT;
   const double negative_shift_length = -1.0;
   const double positive_shift_length = 1.0;
 
   ASSERT_TRUE(isSameDirectionShift(isOnRight(right_obj), negative_shift_length));
   ASSERT_FALSE(isSameDirectionShift(isOnRight(right_obj), positive_shift_length));
 
   ObjectData left_obj;
-  left_obj.lateral = 0.3;
+  left_obj.direction = route_handler::Direction::LEFT;
   ASSERT_TRUE(isSameDirectionShift(isOnRight(left_obj), positive_shift_length));
   ASSERT_FALSE(isSameDirectionShift(isOnRight(left_obj), negative_shift_length));
-
-  const double zero_shift_length = 0.0;
-  ASSERT_TRUE(isSameDirectionShift(isOnRight(left_obj), zero_shift_length));
-  ASSERT_FALSE(isSameDirectionShift(isOnRight(right_obj), zero_shift_length));
 }