refactor(avoidance): rebuild object info list (#6913)

* refactor(avoidance): rebuild object info structure

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

* refactor(avoidance): add helper function to unify same process

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

* refactor(avoidance): remove unused header

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

---------

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

Author: satoshi-ota

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -164,7 +164,7 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects(
       [](const auto & object) {
         ObjectData other_object;
         other_object.object = object;
-        other_object.reason = "OutOfTargetArea";
+        other_object.info = ObjectInfo::OUT_OF_TARGET_AREA;
         return other_object;
       });
   }

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -53,6 +53,34 @@ using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug;
 
 using route_handler::Direction;
 
+enum class ObjectInfo {
+  NONE = 0,
+  // ignore reasons
+  OUT_OF_TARGET_AREA,
+  FURTHER_THAN_THRESHOLD,
+  FURTHER_THAN_GOAL,
+  IS_NOT_TARGET_OBJECT,
+  IS_NOT_PARKING_OBJECT,
+  TOO_NEAR_TO_CENTERLINE,
+  TOO_NEAR_TO_GOAL,
+  MOVING_OBJECT,
+  UNSTABLE_OBJECT,
+  CROSSWALK_USER,
+  ENOUGH_LATERAL_DISTANCE,
+  LESS_THAN_EXECUTION_THRESHOLD,
+  PARALLEL_TO_EGO_LANE,
+  MERGING_TO_EGO_LANE,
+  DEVIATING_FROM_EGO_LANE,
+  // unavoidable reasons
+  NEED_DECELERATION,
+  SAME_DIRECTION_SHIFT,
+  INSUFFICIENT_DRIVABLE_SPACE,
+  INSUFFICIENT_LONGITUDINAL_DISTANCE,
+  INVALID_SHIFT_LINE,
+  // others
+  AMBIGUOUS_STOPPED_VEHICLE,
+};
+
 struct ObjectParameter
 {
   bool is_avoidance_target{false};
@@ -423,8 +451,8 @@ struct ObjectData  // avoidance target
   // overhang points (sort by distance)
   std::vector<std::pair<double, Point>> overhang_points{};
 
-  // unavoidable reason
-  std::string reason{};
+  // object detail info
+  ObjectInfo info{ObjectInfo::NONE};
 
   // lateral avoid margin
   std::optional<double> avoid_margin{std::nullopt};

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp

@@ -31,6 +31,7 @@ using behavior_path_planner::AvoidancePlanningData;
 using behavior_path_planner::AvoidLineArray;
 using behavior_path_planner::DebugData;
 using behavior_path_planner::ObjectDataArray;
+using behavior_path_planner::ObjectInfo;
 using behavior_path_planner::PathShifter;
 using behavior_path_planner::ShiftLineArray;
 using geometry_msgs::msg::Pose;
@@ -47,7 +48,7 @@ MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::st
 
 MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns);
 
-MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns);
+MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const ObjectInfo & info);
 
 MarkerArray createDebugMarkerArray(
   const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug);

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

@@ -343,6 +343,20 @@ class AvoidanceHelper
              parameters_->closest_distance_to_wait_and_see_for_ambiguous_vehicle;
   }
 
+  static bool isAbsolutelyNotAvoidable(const ObjectData & object)
+  {
+    if (object.is_avoidable) {
+      return false;
+    }
+
+    // can avoid it after deceleration.
+    if (object.info == ObjectInfo::NEED_DECELERATION) {
+      return false;
+    }
+
+    return true;
+  }
+
   bool isReady(const AvoidLineArray & new_shift_lines, const double current_shift_length) const
   {
     if (std::abs(current_shift_length) < 1e-3) {

planning/behavior_path_avoidance_module/src/debug.cpp

@@ -21,8 +21,6 @@
 #include <magic_enum.hpp>
 #include <tier4_autoware_utils/ros/uuid_helper.hpp>
 
-#include <tier4_planning_msgs/msg/avoidance_debug_factor.hpp>
-
 #include <string>
 #include <vector>
 
@@ -176,7 +174,8 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st
       marker.id = uuidToInt32(object.object.object_id);
       marker.pose.position.z += 2.0;
       std::ostringstream string_stream;
-      string_stream << object.reason << (object.is_parked ? "(PARKED)" : "");
+      string_stream << magic_enum::enum_name(object.info) << (object.is_parked ? "(PARKED)" : "");
+      string_stream << (object.is_ambiguous ? "(WAIT AND SEE)" : "");
       marker.text = string_stream.str();
       marker.color = createMarkerColor(1.0, 1.0, 1.0, 0.999);
       marker.scale = createMarkerScale(0.6, 0.6, 0.6);
@@ -441,14 +440,12 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons
   return msg;
 }
 
-MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns)
+MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const ObjectInfo & info)
 {
-  using behavior_path_planner::utils::convertToSnakeCase;
-
-  const auto filtered_objects = [&objects, &ns]() {
+  const auto filtered_objects = [&objects, &info]() {
     ObjectDataArray ret{};
     for (const auto & o : objects) {
-      if (o.reason != ns) {
+      if (o.info != info) {
         continue;
       }
       ret.push_back(o);
@@ -460,18 +457,20 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const
   MarkerArray msg;
   msg.markers.reserve(filtered_objects.size() * 2);
 
+  std::ostringstream string_stream;
+  string_stream << magic_enum::enum_name(info);
+
+  std::string ns = string_stream.str();
+  transform(ns.begin(), ns.end(), ns.begin(), tolower);
+
   appendMarkerArray(
     createObjectsCubeMarkerArray(
-      filtered_objects, "others_" + convertToSnakeCase(ns) + "_cube",
-      createMarkerScale(3.0, 1.5, 1.5), createMarkerColor(0.0, 1.0, 0.0, 0.8)),
+      filtered_objects, "others_" + ns + "_cube", createMarkerScale(3.0, 1.5, 1.5),
+      createMarkerColor(0.0, 1.0, 0.0, 0.8)),
     &msg);
+  appendMarkerArray(createObjectInfoMarkerArray(filtered_objects, "others_" + ns + "_info"), &msg);
   appendMarkerArray(
-    createObjectInfoMarkerArray(filtered_objects, "others_" + convertToSnakeCase(ns) + "_info"),
-    &msg);
-  appendMarkerArray(
-    createOverhangLaneletMarkerArray(
-      filtered_objects, "others_" + convertToSnakeCase(ns) + "_overhang_lanelet"),
-    &msg);
+    createOverhangLaneletMarkerArray(filtered_objects, "others_" + ns + "_overhang_lanelet"), &msg);
 
   return msg;
 }
@@ -528,7 +527,6 @@ MarkerArray createDebugMarkerArray(
   using marker_utils::showPolygon;
   using marker_utils::showPredictedPath;
   using marker_utils::showSafetyCheckInfo;
-  using tier4_planning_msgs::msg::AvoidanceDebugFactor;
 
   const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now();
   MarkerArray msg;
@@ -564,24 +562,22 @@ MarkerArray createDebugMarkerArray(
 
   // ignore objects
   {
-    addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD);
-    addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD);
-    addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE);
-    addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL);
-    addObjects(data.other_objects, AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE);
-    addObjects(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT);
-    addObjects(data.other_objects, std::string("MovingObject"));
-    addObjects(data.other_objects, std::string("CrosswalkUser"));
-    addObjects(data.other_objects, std::string("OutOfTargetArea"));
-    addObjects(data.other_objects, std::string("NotNeedAvoidance"));
-    addObjects(data.other_objects, std::string("LessThanExecutionThreshold"));
-    addObjects(data.other_objects, std::string("TooNearToGoal"));
-    addObjects(data.other_objects, std::string("ParallelToEgoLane"));
-    addObjects(data.other_objects, std::string("MergingToEgoLane"));
-    addObjects(data.other_objects, std::string("DeviatingFromEgoLane"));
-    addObjects(data.other_objects, std::string("UnstableObject"));
-    addObjects(data.other_objects, std::string("AmbiguousStoppedVehicle"));
-    addObjects(data.other_objects, std::string("AmbiguousStoppedVehicle(wait-and-see)"));
+    addObjects(data.other_objects, ObjectInfo::FURTHER_THAN_THRESHOLD);
+    addObjects(data.other_objects, ObjectInfo::IS_NOT_TARGET_OBJECT);
+    addObjects(data.other_objects, ObjectInfo::FURTHER_THAN_GOAL);
+    addObjects(data.other_objects, ObjectInfo::TOO_NEAR_TO_CENTERLINE);
+    addObjects(data.other_objects, ObjectInfo::IS_NOT_PARKING_OBJECT);
+    addObjects(data.other_objects, ObjectInfo::MOVING_OBJECT);
+    addObjects(data.other_objects, ObjectInfo::CROSSWALK_USER);
+    addObjects(data.other_objects, ObjectInfo::OUT_OF_TARGET_AREA);
+    addObjects(data.other_objects, ObjectInfo::ENOUGH_LATERAL_DISTANCE);
+    addObjects(data.other_objects, ObjectInfo::LESS_THAN_EXECUTION_THRESHOLD);
+    addObjects(data.other_objects, ObjectInfo::TOO_NEAR_TO_GOAL);
+    addObjects(data.other_objects, ObjectInfo::PARALLEL_TO_EGO_LANE);
+    addObjects(data.other_objects, ObjectInfo::MERGING_TO_EGO_LANE);
+    addObjects(data.other_objects, ObjectInfo::DEVIATING_FROM_EGO_LANE);
+    addObjects(data.other_objects, ObjectInfo::UNSTABLE_OBJECT);
+    addObjects(data.other_objects, ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE);
   }
 
   // shift line pre-process

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -31,7 +31,6 @@
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 
 #include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp"
-#include <tier4_planning_msgs/msg/detail/avoidance_debug_factor__struct.hpp>
 
 #include <algorithm>
 #include <limits>
@@ -52,7 +51,6 @@ using tier4_autoware_utils::calcLateralDeviation;
 using tier4_autoware_utils::getPoint;
 using tier4_autoware_utils::getPose;
 using tier4_autoware_utils::toHexString;
-using tier4_planning_msgs::msg::AvoidanceDebugFactor;
 
 namespace
 {
@@ -119,9 +117,8 @@ bool AvoidanceModule::isExecutionRequested() const
   }
 
   return std::any_of(
-    avoid_data_.target_objects.begin(), avoid_data_.target_objects.end(), [](const auto & o) {
-      return o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK;
-    });
+    avoid_data_.target_objects.begin(), avoid_data_.target_objects.end(),
+    [this](const auto & o) { return !helper_->isAbsolutelyNotAvoidable(o); });
 }
 
 bool AvoidanceModule::isExecutionReady() const
@@ -136,10 +133,9 @@ bool AvoidanceModule::isExecutionReady() const
 
 bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const
 {
-  const bool has_avoidance_target =
-    std::any_of(data.target_objects.begin(), data.target_objects.end(), [](const auto & o) {
-      return o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK;
-    });
+  const bool has_avoidance_target = std::any_of(
+    data.target_objects.begin(), data.target_objects.end(),
+    [this](const auto & o) { return !helper_->isAbsolutelyNotAvoidable(o); });
 
   if (has_avoidance_target) {
     return false;
@@ -337,7 +333,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
 
   for (const auto & object : object_outside_target_lane.objects) {
     ObjectData other_object = createObjectData(data, object);
-    other_object.reason = "OutOfTargetArea";
+    other_object.info = ObjectInfo::OUT_OF_TARGET_AREA;
     data.other_objects.push_back(other_object);
   }
 
@@ -367,8 +363,6 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
       debug_info.object_id = toHexString(o.object.object_id);
       debug_info.longitudinal_distance = o.longitudinal;
       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);
     };
 
@@ -530,12 +524,11 @@ void AvoidanceModule::fillEgoStatus(
    * if the both of following two conditions are satisfied, the module surely avoid the object.
    * Condition1: there is risk to collide with object without avoidance.
    * Condition2: there is enough space to avoid.
-   * In TOO_LARGE_JERK condition, it is possible to avoid object by deceleration even if the flag
+   * In NEED_DECELERATION condition, it is possible to avoid object by deceleration even if the flag
    * is_avoidable is FALSE. So, the module inserts stop point for such a object.
    */
   for (const auto & o : data.target_objects) {
-    const auto enough_space = o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK;
-    if (o.avoid_required && enough_space) {
+    if (o.avoid_required && !helper_->isAbsolutelyNotAvoidable(o)) {
       data.avoid_required = true;
       data.stop_target_object = o;
       data.to_stop_line = o.to_stop_line;
@@ -1667,9 +1660,7 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
     return;
   }
 
-  const auto enough_space =
-    object.value().is_avoidable || object.value().reason == AvoidanceDebugFactor::TOO_LARGE_JERK;
-  if (!enough_space) {
+  if (helper_->isAbsolutelyNotAvoidable(object.value())) {
     return;
   }
 

planning/behavior_path_avoidance_module/src/shift_line_generator.cpp

@@ -19,14 +19,11 @@
 
 #include <tier4_autoware_utils/ros/uuid_helper.hpp>
 
-#include <tier4_planning_msgs/msg/detail/avoidance_debug_factor__struct.hpp>
-
 namespace behavior_path_planner::utils::avoidance
 {
 
 using tier4_autoware_utils::generateUUID;
 using tier4_autoware_utils::getPoint;
-using tier4_planning_msgs::msg::AvoidanceDebugFactor;
 
 namespace
 {
@@ -182,7 +179,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
 
     // prepare distance is not enough. unavoidable.
     if (avoidance_distance < 1e-3) {
-      object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO;
+      object.info = ObjectInfo::INSUFFICIENT_LONGITUDINAL_DISTANCE;
       return std::nullopt;
     }
 
@@ -200,10 +197,10 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
     // avoidance distance is not enough. unavoidable.
     if (!isBestEffort(parameters_->policy_deceleration)) {
       if (avoidance_distance < helper_->getMinAvoidanceDistance(avoiding_shift) + LON_DIST_BUFFER) {
-        object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO;
+        object.info = ObjectInfo::INSUFFICIENT_LONGITUDINAL_DISTANCE;
         return std::nullopt;
       } else {
-        object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
+        object.info = ObjectInfo::NEED_DECELERATION;
         return std::nullopt;
       }
     }
@@ -213,7 +210,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
       avoidance_distance, helper_->getLateralMaxJerkLimit(), helper_->getAvoidanceEgoSpeed());
 
     if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) {
-      object.reason = "LessThanExecutionThreshold";
+      object.info = ObjectInfo::LESS_THAN_EXECUTION_THRESHOLD;
       return std::nullopt;
     }
 
@@ -224,7 +221,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
     if (
       avoidance_distance <
       helper_->getMinAvoidanceDistance(feasible_shift_length) + LON_DIST_BUFFER) {
-      object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO;
+      object.info = ObjectInfo::INSUFFICIENT_LONGITUDINAL_DISTANCE;
       return std::nullopt;
     }
 
@@ -238,7 +235,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
       0.5 * data_->parameters.vehicle_width + lateral_hard_margin;
     if (infeasible) {
       RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. ");
-      object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
+      object.info = ObjectInfo::NEED_DECELERATION;
       return std::nullopt;
     }
 
@@ -254,7 +251,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
   AvoidOutlines outlines;
   for (auto & o : data.target_objects) {
     if (!o.avoid_margin.has_value()) {
-      o.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN;
+      o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE;
       if (o.avoid_required && is_forward_object(o)) {
         break;
       } else {
@@ -266,7 +263,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
     const auto desire_shift_length =
       helper_->getShiftLength(o, is_object_on_right, o.avoid_margin.value());
     if (utils::avoidance::isSameDirectionShift(is_object_on_right, desire_shift_length)) {
-      o.reason = AvoidanceDebugFactor::SAME_DIRECTION_SHIFT;
+      o.info = ObjectInfo::SAME_DIRECTION_SHIFT;
       if (o.avoid_required && is_forward_object(o)) {
         break;
       } else {
@@ -377,7 +374,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
     } else if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) {
       outlines.emplace_back(al_avoid, al_return);
     } else {
-      o.reason = "InvalidShiftLine";
+      o.info = ObjectInfo::INVALID_SHIFT_LINE;
       continue;
     }