output diag safety msg

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

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/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) {

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>
@@ -136,6 +136,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 +157,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

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -224,7 +224,7 @@ 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);
   if (is_permanent_go_) {
     if (has_collision) {
@@ -236,7 +236,12 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
 
   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.
+  // ==========================================================================================
   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) {

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,
@@ -680,7 +698,7 @@ class IntersectionModule : public SceneModuleInterface
   /**
    * @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(

planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp

@@ -367,7 +367,7 @@ IntersectionModule::isGreenPseudoCollisionStatus(
   return std::nullopt;
 }
 
-std::pair<bool, intersection::CollisionInterval::LanePosition> IntersectionModule::detectCollision(
+IntersectionModule::CollisionStatus IntersectionModule::detectCollision(
   const bool is_over_1st_pass_judge_line, const std::optional<bool> is_over_2nd_pass_judge_line)
 {
   // ==========================================================================================
@@ -387,9 +387,16 @@ std::pair<bool, intersection::CollisionInterval::LanePosition> IntersectionModul
   // expected to have been detected in the prior iteration because ego could have judged as UNSAFE
   // in the prior iteration
   // ==========================================================================================
-  std::vector<std::shared_ptr<intersection::ObjectInfo>> misjudge_objects;
-  std::vector<std::shared_ptr<intersection::ObjectInfo>> too_late_detect_objects;
+  std::vector<std::pair<CollisionStatus::BlameType, std::shared_ptr<intersection::ObjectInfo>>>
+    misjudge_objects;
+  std::vector<std::pair<CollisionStatus::BlameType, std::shared_ptr<intersection::ObjectInfo>>>
+    too_late_detect_objects;
   for (const auto & object_info : object_info_manager_.attentionObjects()) {
+    if (object_info->is_safe_under_traffic_control()) {
+      debug_data_.safe_under_traffic_control_targets.objects.push_back(
+        object_info->predicted_object());
+      continue;
+    }
     if (!object_info->is_unsafe()) {
       continue;
     }
@@ -416,17 +423,20 @@ std::pair<bool, intersection::CollisionInterval::LanePosition> IntersectionModul
       const auto & decision_at_1st_pass_judge_opt =
         object_info->decision_at_1st_pass_judge_line_passage();
       if (!decision_at_1st_pass_judge_opt) {
-        too_late_detect_objects.push_back(object_info);
+        too_late_detect_objects.emplace_back(
+          CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info);
         debug_container = &debug_data_.too_late_detect_targets.objects;
       } else {
         const auto & decision_at_1st_pass_judge = decision_at_1st_pass_judge_opt.value();
         if (
           decision_at_1st_pass_judge.safe ||
           decision_at_1st_pass_judge.safe_under_traffic_control) {
-          misjudge_objects.push_back(object_info);
+          misjudge_objects.emplace_back(
+            CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info);
           debug_container = &debug_data_.misjudge_targets.objects;
         } else {
-          too_late_detect_objects.push_back(object_info);
+          too_late_detect_objects.emplace_back(
+            CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info);
           debug_container = &debug_data_.too_late_detect_targets.objects;
         }
       }
@@ -435,29 +445,33 @@ std::pair<bool, intersection::CollisionInterval::LanePosition> IntersectionModul
       const auto & decision_at_2nd_pass_judge_opt =
         object_info->decision_at_2nd_pass_judge_line_passage();
       if (!decision_at_2nd_pass_judge_opt) {
-        too_late_detect_objects.push_back(object_info);
+        too_late_detect_objects.emplace_back(
+          CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info);
         debug_container = &debug_data_.too_late_detect_targets.objects;
       } else {
         const auto & decision_at_2nd_pass_judge = decision_at_2nd_pass_judge_opt.value();
         if (
           decision_at_2nd_pass_judge.safe ||
           decision_at_2nd_pass_judge.safe_under_traffic_control) {
-          misjudge_objects.push_back(object_info);
+          misjudge_objects.emplace_back(
+            CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info);
           debug_container = &debug_data_.misjudge_targets.objects;
         } else {
-          too_late_detect_objects.push_back(object_info);
+          too_late_detect_objects.emplace_back(
+            CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info);
           debug_container = &debug_data_.too_late_detect_targets.objects;
         }
       }
     }
-    debug_container->push_back(object_info->predicted_object());
+    debug_container->emplace_back(object_info->predicted_object());
   }
   if (collision_at_first_lane) {
-    return {true, intersection::CollisionInterval::FIRST};
+    return {
+      true, intersection::CollisionInterval::FIRST, too_late_detect_objects, misjudge_objects};
   } else if (collision_at_non_first_lane) {
-    return {true, intersection::CollisionInterval::ELSE};
+    return {true, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects};
   }
-  return {false, intersection::CollisionInterval::ELSE};
+  return {false, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects};
 }
 
 /*