output diag safety msg

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -59,6 +59,8 @@
           object_expected_deceleration: 2.0
         ignore_on_red_traffic_light:
           object_margin_to_path: 2.0
+        avoid_collision_by_acceleration:
+          object_time_margin_to_collision_point: 4.0
 
       occlusion:
         enable: false

planning/behavior_velocity_intersection_module/package.xml

@@ -22,18 +22,19 @@
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>behavior_velocity_planner_common</depend>
+  <depend>fmt</depend>
   <depend>geometry_msgs</depend>
   <depend>interpolation</depend>
   <depend>lanelet2_extension</depend>
   <depend>libopencv-dev</depend>
+  <depend>magic_enum</depend>
   <depend>motion_utils</depend>
   <depend>nav_msgs</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>route_handler</depend>
   <depend>rtc_interface</depend>
   <depend>tf2_geometry_msgs</depend>
-  <depend>tier4_api_msgs</depend>
   <depend>tier4_autoware_utils</depend>
   <depend>tier4_planning_msgs</depend>
   <depend>vehicle_info_util</depend>

planning/behavior_velocity_intersection_module/src/debug.cpp

@@ -192,6 +192,14 @@ constexpr std::tuple<float, float, float> red()
   return {r, g, b};
 }
 
+constexpr std::tuple<float, float, float> light_blue()
+{
+  constexpr uint64_t code = 0x96cde6;
+  constexpr float r = static_cast<int>(code >> 16) / 255.0;
+  constexpr float g = static_cast<int>((code << 48) >> 56) / 255.0;
+  constexpr float b = static_cast<int>((code << 56) >> 56) / 255.0;
+  return {r, g, b};
+}
 }  // namespace
 
 namespace behavior_velocity_planner
@@ -280,6 +288,13 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
   static constexpr auto green = ::green();
   static constexpr auto yellow = ::yellow();
   static constexpr auto red = ::red();
+  static constexpr auto light_blue = ::light_blue();
+  appendMarkerArray(
+    debug::createObjectsMarkerArray(
+      debug_data_.safe_under_traffic_control_targets, "safe_under_traffic_control_targets",
+      module_id_, now, std::get<0>(light_blue), std::get<1>(light_blue), std::get<2>(light_blue)),
+    &debug_marker_array, now);
+
   appendMarkerArray(
     debug::createObjectsMarkerArray(
       debug_data_.unsafe_targets, "unsafe_targets", module_id_, now, std::get<0>(green),

planning/behavior_velocity_intersection_module/src/decision_result.cpp

@@ -25,16 +25,18 @@ std::string formatDecisionResult(const DecisionResult & decision_result)
   if (std::holds_alternative<OverPassJudge>(decision_result)) {
     const auto state = std::get<OverPassJudge>(decision_result);
     return "OverPassJudge:\nsafety_report:" + state.safety_report + "\nevasive_report:\n" +
-           state.evasive_report + "\n";
+           state.evasive_report;
   }
   if (std::holds_alternative<StuckStop>(decision_result)) {
     return "StuckStop";
   }
   if (std::holds_alternative<YieldStuckStop>(decision_result)) {
-    return "YieldStuckStop";
+    const auto state = std::get<YieldStuckStop>(decision_result);
+    return "YieldStuckStop:\nsafety_report:" + state.safety_report;
   }
   if (std::holds_alternative<NonOccludedCollisionStop>(decision_result)) {
-    return "NonOccludedCollisionStop";
+    const auto state = std::get<NonOccludedCollisionStop>(decision_result);
+    return "NonOccludedCollisionStop\nsafety_report:" + state.safety_report;
   }
   if (std::holds_alternative<FirstWaitBeforeOcclusion>(decision_result)) {
     return "FirstWaitBeforeOcclusion";
@@ -43,16 +45,19 @@ std::string formatDecisionResult(const DecisionResult & decision_result)
     return "PeekingTowardOcclusion";
   }
   if (std::holds_alternative<OccludedCollisionStop>(decision_result)) {
-    return "OccludedCollisionStop";
+    const auto state = std::get<OccludedCollisionStop>(decision_result);
+    return "OccludedCollisionStop\nsafety_report:" + state.safety_report;
   }
   if (std::holds_alternative<OccludedAbsenceTrafficLight>(decision_result)) {
-    return "OccludedAbsenceTrafficLight";
+    const auto state = std::get<OccludedAbsenceTrafficLight>(decision_result);
+    return "OccludedAbsenceTrafficLight\nsafety_report:" + state.safety_report;
   }
   if (std::holds_alternative<Safe>(decision_result)) {
     return "Safe";
   }
   if (std::holds_alternative<FullyPrioritized>(decision_result)) {
-    return "FullyPrioritized";
+    const auto state = std::get<FullyPrioritized>(decision_result);
+    return "FullyPrioritized\nsafety_report:" + state.safety_report;
   }
   return "";
 }

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -133,6 +133,11 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   ip.collision_detection.ignore_on_red_traffic_light.object_margin_to_path =
     getOrDeclareParameter<double>(
       node, ns + ".collision_detection.ignore_on_red_traffic_light.object_margin_to_path");
+  ip.collision_detection.avoid_collision_by_acceleration
+    .object_time_margin_to_collision_point = getOrDeclareParameter<double>(
+    node,
+    ns +
+      ".collision_detection.avoid_collision_by_acceleration.object_time_margin_to_collision_point");
 
   ip.occlusion.enable = getOrDeclareParameter<bool>(node, ns + ".occlusion.enable");
   ip.occlusion.occlusion_attention_area_length =

planning/behavior_velocity_intersection_module/src/object_manager.cpp

@@ -93,6 +93,27 @@ void ObjectInfo::update_safety(
   safe_under_traffic_control_ = safe_under_traffic_control;
 }
 
+std::optional<geometry_msgs::msg::Point> ObjectInfo::estimated_past_position(
+  const double past_duration) const
+{
+  if (!attention_lanelet_opt) {
+    return std::nullopt;
+  }
+  const auto attention_lanelet = attention_lanelet_opt.value();
+  const auto current_arc_coords = lanelet::utils::getArcCoordinates(
+    {attention_lanelet}, predicted_object_.kinematics.initial_pose_with_covariance.pose);
+  const auto distance = current_arc_coords.distance;
+  const auto past_length =
+    current_arc_coords.length -
+    predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x * past_duration;
+  const auto past_point = lanelet::geometry::fromArcCoordinates(
+    attention_lanelet.centerline2d(), lanelet::ArcCoordinates{past_length, distance});
+  geometry_msgs::msg::Point past_position;
+  past_position.x = past_point.x();
+  past_position.y = past_point.y();
+  return std::make_optional(past_position);
+}
+
 void ObjectInfo::calc_dist_to_stopline()
 {
   if (!stopline_opt || !attention_lanelet_opt) {
@@ -233,7 +254,7 @@ std::optional<intersection::CollisionInterval> findPassageInterval(
   const autoware_auto_perception_msgs::msg::Shape & shape,
   const lanelet::BasicPolygon2d & ego_lane_poly,
   const std::optional<lanelet::ConstLanelet> & first_attention_lane_opt,
-  [[maybe_unused]] const std::optional<lanelet::ConstLanelet> & second_attention_lane_opt)
+  const std::optional<lanelet::ConstLanelet> & second_attention_lane_opt)
 {
   const auto first_itr = std::adjacent_find(
     predicted_path.path.cbegin(), predicted_path.path.cend(), [&](const auto & a, const auto & b) {
@@ -259,26 +280,30 @@ std::optional<intersection::CollisionInterval> findPassageInterval(
   const size_t exit_idx = std::distance(predicted_path.path.begin(), last_itr.base()) - 1;
   const double object_exit_time =
     static_cast<double>(exit_idx) * rclcpp::Duration(predicted_path.time_step).seconds();
-  const auto [lane_position, lane_id] =
-    [&]() -> std::pair<intersection::CollisionInterval::LanePosition, lanelet::Id> {
+  const auto lane_position = [&]() {
     if (first_attention_lane_opt) {
       if (lanelet::geometry::inside(
             first_attention_lane_opt.value(),
             lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) {
-        return std::make_pair(
-          intersection::CollisionInterval::LanePosition::FIRST,
-          first_attention_lane_opt.value().id());
+        return intersection::CollisionInterval::LanePosition::FIRST;
+      }
+    }
+    if (second_attention_lane_opt) {
+      if (lanelet::geometry::inside(
+            second_attention_lane_opt.value(),
+            lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) {
+        return intersection::CollisionInterval::LanePosition::SECOND;
       }
     }
-    return std::make_pair(intersection::CollisionInterval::LanePosition::ELSE, lanelet::InvalId);
+    return intersection::CollisionInterval::LanePosition::ELSE;
   }();
 
   std::vector<geometry_msgs::msg::Pose> path;
   for (const auto & pose : predicted_path.path) {
     path.push_back(pose);
   }
   return intersection::CollisionInterval{
-    lane_position, lane_id, path, {enter_idx, exit_idx}, {object_enter_time, object_exit_time}};
+    lane_position, path, {enter_idx, exit_idx}, {object_enter_time, object_exit_time}};
 }
 
 }  // namespace behavior_velocity_planner::intersection

planning/behavior_velocity_intersection_module/src/object_manager.hpp

@@ -21,7 +21,7 @@
 #include <geometry_msgs/msg/pose.hpp>
 #include <unique_identifier_msgs/msg/uuid.hpp>
 
-#include <boost/unordered_map.hpp>
+#include <boost/functional/hash.hpp>
 #include <boost/uuid/uuid.hpp>
 
 #include <lanelet2_core/primitives/Lanelet.h>
@@ -62,10 +62,10 @@ struct CollisionInterval
 {
   enum LanePosition {
     FIRST,
+    SECOND,
     ELSE,
   };
   LanePosition lane_position{LanePosition::ELSE};
-  lanelet::Id lane_id{lanelet::InvalId};
 
   //! original predicted path
   std::vector<geometry_msgs::msg::Pose> path;
@@ -82,34 +82,18 @@ struct CollisionKnowledge
   //! the time when the expected collision is judged
   rclcpp::Time stamp;
 
-  //! if judged as SAFE/UNSAFE
-  bool safe{false};
-
-  //! if judged as SAFE given traffic control
-  bool safe_under_traffic_control{false};
+  enum SafeType {
+    UNSAFE,
+    SAFE,
+    SAFE_UNDER_TRAFFIC_CONTROL,
+  };
+  SafeType safe_type{SafeType::UNSAFE};
 
   //! if !safe, this has value, and it safe, this maybe null if the predicted path does not
   //! intersect with ego path
   std::optional<CollisionInterval> interval{std::nullopt};
 
   double observed_velocity;
-  /**
-   * diag format:
-   * for objects with decision_at_1st_pass_judge_line_passage or
-   * decision_at_2nd_pass_judge_line_passage:
-   * [uuid] was detected as [safe] at [stamp] with the
-   * velocity of [observed_velocity] and the possible collision position was at
-   * [(path[interval_position.first])] on the Lanelet[interval.lane_id]  which was expected to
-   * happen after [interval_time.first] seconds. Now it is judged as unsafe with the velocity of
-   * [new observed_velocity] at [(new path[new interval_position.first])] on the Lanelet[new
-   * interval.lane_id]
-   * for objects without the above members:
-   * [uuid] was not detected when ego passed the 1st/2nd pass judge line at
-   * [manager.passed_1st/2nd_judge_line_first_time], so collision detection was impossible at that
-   * time. but now collision is expected on the 1st/2nd attention lanelet. This dangerous situation
-   * is because at time [manager.passed_1st/2nd_judge_line_first_time] ego could not detect [uuid]
-   * which was estimated to be xxx meter behind from yyy position
-   */
 };
 
 /**
@@ -136,6 +120,8 @@ class ObjectInfo
     return unsafe_interval_;
   }
 
+  bool is_safe_under_traffic_control() const { return safe_under_traffic_control_; }
+
   /**
    * @brief update predicted_object_, attention_lanelet, stopline, dist_to_stopline
    */
@@ -155,7 +141,8 @@ class ObjectInfo
   /**
    * @brief find the estimated position of the object in the past
    */
-  geometry_msgs::msg::Pose estimated_past_pose(const double past_duration) const;
+  std::optional<geometry_msgs::msg::Point> estimated_past_position(
+    const double past_duration) const;
 
   /**
    * @brief check if object can stop before stopline under the deceleration. return false if
@@ -186,6 +173,13 @@ class ObjectInfo
     decision_at_2nd_pass_judge_line_passage_ = knowledge;
   }
 
+  const std::optional<CollisionInterval> & unsafe_interval() const { return unsafe_interval_; }
+
+  double observed_velocity() const
+  {
+    return predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x;
+  }
+
   const std::optional<CollisionKnowledge> & decision_at_1st_pass_judge_line_passage() const
   {
     return decision_at_1st_pass_judge_line_passage_;
@@ -196,8 +190,9 @@ class ObjectInfo
     return decision_at_2nd_pass_judge_line_passage_;
   }
 
-private:
   const std::string uuid_str;
+
+private:
   autoware_auto_perception_msgs::msg::PredictedObject predicted_object_;
 
   //! null if the object in intersection_area but not in attention_area

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -218,29 +218,42 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
 
   // ==========================================================================================
   // run collision checking for each objects considering traffic light level. Also if ego just
-  // passed each pass judge line for the first time, save current collision status.
+  // passed each pass judge line for the first time, save current collision status for late
+  // diagnosis
   // ==========================================================================================
   updateObjectInfoManagerCollision(
     path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line,
     safely_passed_2nd_judge_line);
 
-  const auto [has_collision, collision_position] =
+  const auto [has_collision, collision_position, too_late_detect_objects, misjudge_objects] =
     detectCollision(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line);
+  const std::string safety_diag =
+    generateDetectionBlameDiagnosis(too_late_detect_objects, misjudge_objects);
   if (is_permanent_go_) {
     if (has_collision) {
-      return intersection::OverPassJudge{"TODO", "ego needs acceleration to keep safe"};
+      const auto closest_idx = intersection_stoplines.closest_idx;
+      const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis(
+        *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects);
+      return intersection::OverPassJudge{safety_diag, evasive_diag};
     }
     return intersection::OverPassJudge{
       "no collision is detected", "ego can safely pass the intersection at this rate"};
   }
 
-  std::string safety_report{""};
   const bool collision_on_1st_attention_lane =
-    has_collision && collision_position == intersection::CollisionInterval::LanePosition::FIRST;
+    has_collision && (collision_position == intersection::CollisionInterval::LanePosition::FIRST);
+  // ==========================================================================================
+  // this state is very dangerous because ego is very close/over the boundary of 1st attention lane
+  // and collision is detected on the 1st lane. Since the 2nd attention lane also exists in this
+  // case, possible another collision may be expected on the 2nd attention lane too.
+  // ==========================================================================================
+  std::string safety_report = safety_diag;
   if (
     is_over_1st_pass_judge_line && is_over_2nd_pass_judge_line &&
     is_over_2nd_pass_judge_line.value() && collision_on_1st_attention_lane) {
-    safety_report = "TODO";
+    safety_report +=
+      "\nego is between the 1st and 2nd pass judge line but collision is expected on the 1st "
+      "attention lane, which is dangerous.";
   }
 
   const auto closest_idx = intersection_stoplines.closest_idx;
@@ -1244,7 +1257,7 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat
     util::isOverTargetIndex(path, closest_idx, current_pose, pass_judge_line_idx);
   bool safely_passed_1st_judge_line_first_time = false;
   if (is_over_1st_pass_judge_line && was_safe && !safely_passed_1st_judge_line_time_) {
-    safely_passed_1st_judge_line_time_ = clock_->now();
+    safely_passed_1st_judge_line_time_ = std::make_pair(clock_->now(), current_pose);
     safely_passed_1st_judge_line_first_time = true;
   }
   const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
@@ -1261,7 +1274,7 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat
   if (
     is_over_2nd_pass_judge_line && is_over_2nd_pass_judge_line.value() && was_safe &&
     !safely_passed_2nd_judge_line_time_) {
-    safely_passed_2nd_judge_line_time_ = clock_->now();
+    safely_passed_2nd_judge_line_time_ = std::make_pair(clock_->now(), current_pose);
     safely_passed_2nd_judge_line_first_time = true;
   }
   if (second_pass_judge_line_idx_opt) {