Merge pull request #1308 from tier4/hotfix/v0.26.1/cherry-pick-avoida…

…nce-pr

fix: cherry pick avoidance PRs

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -166,7 +166,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/config/avoidance.param.yaml

@@ -293,4 +293,11 @@
 
       # for debug
       debug:
-        marker: false
+        enable_other_objects_marker: false
+        enable_other_objects_info: false
+        enable_detection_area_marker: false
+        enable_drivable_bound_marker: false
+        enable_safety_check_marker: false
+        enable_shift_line_marker: false
+        enable_lane_marker: false
+        enable_misc_marker: false

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -15,16 +15,12 @@
 #ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_
 #define BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_
 
+#include "behavior_path_avoidance_module/type_alias.hpp"
 #include "behavior_path_planner_common/data_manager.hpp"
 #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"
 
 #include <rclcpp/time.hpp>
-#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
-
-#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
-#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
-#include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
 
 #include <lanelet2_core/primitives/Lanelet.h>
 #include <lanelet2_core/primitives/LineString.h>
@@ -38,21 +34,38 @@
 
 namespace behavior_path_planner
 {
-using autoware_auto_perception_msgs::msg::PredictedObject;
-using autoware_auto_perception_msgs::msg::PredictedPath;
-using autoware_auto_planning_msgs::msg::PathWithLaneId;
-
-using tier4_autoware_utils::Point2d;
-using tier4_autoware_utils::Polygon2d;
-using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
-
-using geometry_msgs::msg::Point;
-using geometry_msgs::msg::Pose;
 
 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};
@@ -324,7 +337,14 @@ struct AvoidanceParameters
   bool enable_bound_clipping{false};
 
   // debug
-  bool publish_debug_marker = false;
+  bool enable_other_objects_marker{false};
+  bool enable_other_objects_info{false};
+  bool enable_detection_area_marker{false};
+  bool enable_drivable_bound_marker{false};
+  bool enable_safety_check_marker{false};
+  bool enable_shift_line_marker{false};
+  bool enable_lane_marker{false};
+  bool enable_misc_marker{false};
 };
 
 struct ObjectData  // avoidance target
@@ -423,8 +443,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

@@ -16,25 +16,22 @@
 #define BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_
 
 #include "behavior_path_avoidance_module/data_structs.hpp"
+#include "behavior_path_avoidance_module/type_alias.hpp"
 
-#include <tier4_autoware_utils/ros/marker_helper.hpp>
-
-#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
-#include <visualization_msgs/msg/marker_array.hpp>
-
+#include <memory>
 #include <string>
 
-namespace marker_utils::avoidance_marker
+namespace behavior_path_planner::utils::avoidance
 {
-using autoware_auto_planning_msgs::msg::PathWithLaneId;
+
+using behavior_path_planner::AvoidanceParameters;
 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;
-using visualization_msgs::msg::MarkerArray;
 
 MarkerArray createEgoStatusMarkerArray(
   const AvoidancePlanningData & data, const Pose & p_ego, std::string && ns);
@@ -47,11 +44,13 @@ 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, const bool verbose);
 
 MarkerArray createDebugMarkerArray(
-  const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug);
-}  // namespace marker_utils::avoidance_marker
+  const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug,
+  const std::shared_ptr<AvoidanceParameters> & parameters);
+}  // namespace behavior_path_planner::utils::avoidance
 
 std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr);
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

@@ -16,11 +16,10 @@
 #define BEHAVIOR_PATH_AVOIDANCE_MODULE__HELPER_HPP_
 
 #include "behavior_path_avoidance_module/data_structs.hpp"
+#include "behavior_path_avoidance_module/type_alias.hpp"
 #include "behavior_path_avoidance_module/utils.hpp"
 #include "behavior_path_planner_common/utils/utils.hpp"
 
-#include <motion_utils/distance/distance.hpp>
-
 #include <algorithm>
 #include <limits>
 #include <memory>
@@ -32,11 +31,6 @@ namespace behavior_path_planner::helper::avoidance
 
 using behavior_path_planner::PathShifter;
 using behavior_path_planner::PlannerData;
-using motion_utils::calcDecelDistWithJerkAndAccConstraints;
-using motion_utils::calcLongitudinalOffsetPose;
-using motion_utils::calcSignedArcLength;
-using motion_utils::findNearestIndex;
-using tier4_autoware_utils::getPose;
 
 class AvoidanceHelper
 {
@@ -181,14 +175,14 @@ class AvoidanceHelper
   double getShift(const Point & p) const
   {
     validate();
-    const auto idx = findNearestIndex(prev_reference_path_.points, p);
+    const auto idx = motion_utils::findNearestIndex(prev_reference_path_.points, p);
     return prev_spline_shift_path_.shift_length.at(idx);
   }
 
   double getLinearShift(const Point & p) const
   {
     validate();
-    const auto idx = findNearestIndex(prev_reference_path_.points, p);
+    const auto idx = motion_utils::findNearestIndex(prev_reference_path_.points, p);
     return prev_linear_shift_path_.shift_length.at(idx);
   }
 
@@ -282,8 +276,8 @@ class AvoidanceHelper
     }
 
     const auto start_idx = data_->findEgoIndex(path.points);
-    const auto distance =
-      calcSignedArcLength(path.points, start_idx, max_v_point_.value().first.position);
+    const auto distance = motion_utils::calcSignedArcLength(
+      path.points, start_idx, max_v_point_.value().first.position);
     return std::make_pair(distance, max_v_point_.value().second);
   }
 
@@ -294,7 +288,7 @@ class AvoidanceHelper
     const auto & a_now = data_->self_acceleration->accel.accel.linear.x;
     const auto & a_lim = use_hard_constraints ? p->max_deceleration : p->nominal_deceleration;
     const auto & j_lim = use_hard_constraints ? p->max_jerk : p->nominal_jerk;
-    const auto ret = calcDecelDistWithJerkAndAccConstraints(
+    const auto ret = motion_utils::calcDecelDistWithJerkAndAccConstraints(
       getEgoSpeed(), target_velocity, a_now, a_lim, j_lim, -1.0 * j_lim);
 
     if (!!ret) {
@@ -343,6 +337,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) {
@@ -427,14 +435,15 @@ class AvoidanceHelper
     const auto x_max_accel =
       v_neg_jerk * t_max_accel + p->max_acceleration * std::pow(t_max_accel, 2.0) / 2.0;
 
-    const auto point =
-      calcLongitudinalOffsetPose(path.points, getEgoPosition(), x_neg_jerk + x_max_accel);
+    const auto point = motion_utils::calcLongitudinalOffsetPose(
+      path.points, getEgoPosition(), x_neg_jerk + x_max_accel);
     if (point.has_value()) {
       max_v_point_ = std::make_pair(point.value(), v_max);
       return;
     }
 
-    const auto x_end = calcSignedArcLength(path.points, getEgoPosition(), path.points.size() - 1);
+    const auto x_end =
+      motion_utils::calcSignedArcLength(path.points, getEgoPosition(), path.points.size() - 1);
     const auto t_end =
       (std::sqrt(v0 * v0 + 2.0 * p->max_acceleration * x_end) - v0) / p->max_acceleration;
     const auto v_end = v0 + p->max_acceleration * t_end;

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

@@ -383,7 +383,20 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
   // debug
   {
     const std::string ns = "avoidance.debug.";
-    p.publish_debug_marker = getOrDeclareParameter<bool>(*node, ns + "marker");
+    p.enable_other_objects_marker =
+      getOrDeclareParameter<bool>(*node, ns + "enable_other_objects_marker");
+    p.enable_other_objects_info =
+      getOrDeclareParameter<bool>(*node, ns + "enable_other_objects_info");
+    p.enable_detection_area_marker =
+      getOrDeclareParameter<bool>(*node, ns + "enable_detection_area_marker");
+    p.enable_drivable_bound_marker =
+      getOrDeclareParameter<bool>(*node, ns + "enable_drivable_bound_marker");
+    p.enable_safety_check_marker =
+      getOrDeclareParameter<bool>(*node, ns + "enable_safety_check_marker");
+    p.enable_shift_line_marker =
+      getOrDeclareParameter<bool>(*node, ns + "enable_shift_line_marker");
+    p.enable_lane_marker = getOrDeclareParameter<bool>(*node, ns + "enable_lane_marker");
+    p.enable_misc_marker = getOrDeclareParameter<bool>(*node, ns + "enable_misc_marker");
   }
 
   return p;

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

@@ -18,19 +18,14 @@
 #include "behavior_path_avoidance_module/data_structs.hpp"
 #include "behavior_path_avoidance_module/helper.hpp"
 #include "behavior_path_avoidance_module/shift_line_generator.hpp"
+#include "behavior_path_avoidance_module/type_alias.hpp"
 #include "behavior_path_planner_common/interface/scene_module_interface.hpp"
 #include "behavior_path_planner_common/interface/scene_module_visitor.hpp"
 
 #include <rclcpp/logging.hpp>
 #include <rclcpp/node.hpp>
 #include <rclcpp/time.hpp>
 
-#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
-#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
-#include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
-#include <tier4_planning_msgs/msg/avoidance_debug_msg.hpp>
-#include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
-
 #include <memory>
 #include <string>
 #include <unordered_map>
@@ -39,12 +34,8 @@
 namespace behavior_path_planner
 {
 
-using motion_utils::calcSignedArcLength;
-using motion_utils::findNearestIndex;
-
-using tier4_planning_msgs::msg::AvoidanceDebugMsg;
-
 using helper::avoidance::AvoidanceHelper;
+using tier4_planning_msgs::msg::AvoidanceDebugMsg;
 
 class AvoidanceModule : public SceneModuleInterface
 {
@@ -114,7 +105,7 @@ class AvoidanceModule : public SceneModuleInterface
 
     for (const auto & left_shift : left_shift_array_) {
       const double start_distance =
-        calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position);
+        motion_utils::calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position);
       const double finish_distance = start_distance + left_shift.relative_longitudinal;
       rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
         left_shift.uuid, true, start_distance, finish_distance, clock_->now());
@@ -127,7 +118,7 @@ class AvoidanceModule : public SceneModuleInterface
 
     for (const auto & right_shift : right_shift_array_) {
       const double start_distance =
-        calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position);
+        motion_utils::calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position);
       const double finish_distance = start_distance + right_shift.relative_longitudinal;
       rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
         right_shift.uuid, true, start_distance, finish_distance, clock_->now());

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp

@@ -17,6 +17,7 @@
 
 #include "behavior_path_avoidance_module/data_structs.hpp"
 #include "behavior_path_avoidance_module/helper.hpp"
+#include "behavior_path_avoidance_module/type_alias.hpp"
 #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"
 
 #include <memory>