output diag safety msg

Signed-off-by: Mamoru Sobue <[email protected]>

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

@@ -31,10 +31,12 @@ std::string formatDecisionResult(const DecisionResult & 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/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

@@ -224,23 +224,32 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     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 = generateBlameDiagnosis(too_late_detect_objects, misjudge_objects);
   if (is_permanent_go_) {
     if (has_collision) {
-      return intersection::OverPassJudge{"TODO", "ego needs acceleration to keep safe"};
+      return intersection::OverPassJudge{
+        safety_diag.c_str(), "ego needs acceleration to keep safe"};
     }
     return intersection::OverPassJudge{
       "no collision is detected", "ego can safely pass the intersection at this rate"};
   }
 
-  std::string safety_report{""};
+  std::string safety_report = safety_diag;
   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.
+  // ==========================================================================================
   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 +1253,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 +1270,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) {

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -194,6 +194,7 @@ class IntersectionModule : public SceneModuleInterface
     std::optional<std::vector<lanelet::CompoundPolygon3d>> yield_stuck_detect_area{std::nullopt};
 
     std::optional<geometry_msgs::msg::Polygon> candidate_collision_ego_lane_polygon{std::nullopt};
+    autoware_auto_perception_msgs::msg::PredictedObjects safe_under_traffic_control_targets;
     autoware_auto_perception_msgs::msg::PredictedObjects unsafe_targets;
     autoware_auto_perception_msgs::msg::PredictedObjects misjudge_targets;
     autoware_auto_perception_msgs::msg::PredictedObjects too_late_detect_targets;
@@ -239,6 +240,23 @@ class IntersectionModule : public SceneModuleInterface
     const bool safely_passed_2nd_judge_line;
   };
 
+  /**
+   * @brief
+   */
+  struct CollisionStatus
+  {
+    enum BlameType {
+      BLAME_AT_FIRST_PASS_JUDGE,
+      BLAME_AT_SECOND_PASS_JUDGE,
+    };
+    const bool collision_detected;
+    const intersection::CollisionInterval::LanePosition collision_position;
+    const std::vector<std::pair<BlameType, std::shared_ptr<intersection::ObjectInfo>>>
+      too_late_detect_objects;
+    const std::vector<std::pair<BlameType, std::shared_ptr<intersection::ObjectInfo>>>
+      misjudge_objects;
+  };
+
   IntersectionModule(
     const int64_t module_id, const int64_t lane_id, std::shared_ptr<const PlannerData> planner_data,
     const PlannerParam & planner_param, const std::set<lanelet::Id> & associative_ids,
@@ -342,11 +360,13 @@ class IntersectionModule : public SceneModuleInterface
   //! is treated as the same position as occlusion_peeking_stopline
   bool passed_1st_judge_line_while_peeking_{false};
 
-  //! save the time when ego passed the 1st/2nd_pass_judge_line with safe decision. If collision is
-  //! expected after these variables are non-null, then it is the fault of past perception failure
-  //! at these time.
-  std::optional<rclcpp::Time> safely_passed_1st_judge_line_time_{std::nullopt};
-  std::optional<rclcpp::Time> safely_passed_2nd_judge_line_time_{std::nullopt};
+  //! save the time and ego position when ego passed the 1st/2nd_pass_judge_line with safe
+  //! decision. If collision is expected after these variables are non-null, then it is the fault of
+  //! past perception failure at these time.
+  std::optional<std::pair<rclcpp::Time, geometry_msgs::msg::Pose>>
+    safely_passed_1st_judge_line_time_{std::nullopt};
+  std::optional<std::pair<rclcpp::Time, geometry_msgs::msg::Pose>>
+    safely_passed_2nd_judge_line_time_{std::nullopt};
   /** @}*/
 
 private:
@@ -677,10 +697,22 @@ class IntersectionModule : public SceneModuleInterface
     const size_t closest_idx, const size_t collision_stopline_idx,
     const intersection::IntersectionStopLines & intersection_stoplines);
 
+  /**
+   * @brief generate the message explaining why too_late_detect_objects/misjudge_objects exist and
+   * blame past perception fault
+   */
+  std::string generateBlameDiagnosis(
+    const std::vector<
+      std::pair<CollisionStatus::BlameType, std::shared_ptr<intersection::ObjectInfo>>> &
+      too_late_detect_objects,
+    const std::vector<
+      std::pair<CollisionStatus::BlameType, std::shared_ptr<intersection::ObjectInfo>>> &
+      misjudge_objects) const;
+
   /**
    * @brief return if collision is detected and the collision position
    */
-  std::pair<bool, intersection::CollisionInterval::LanePosition> detectCollision(
+  CollisionStatus detectCollision(
     const bool is_over_1st_pass_judge_line, const std::optional<bool> is_over_2nd_pass_judge_line);
 
   std::optional<size_t> checkAngleForTargetLanelets(