Merge pull request #1164 from tier4/cherry-pick/pr6491

feat(intersection): add traffic signal info publisher and detailed occlusion diagnosis  (autowarefoundation#6491)

planning/behavior_velocity_intersection_module/src/decision_result.cpp

@@ -24,36 +24,39 @@ 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;
+    return "OverPassJudge:\nsafety_report:" + state.safety_report +
+           "\nevasive_report:" + state.evasive_report;
   }
   if (std::holds_alternative<StuckStop>(decision_result)) {
     return "StuckStop";
   }
   if (std::holds_alternative<YieldStuckStop>(decision_result)) {
     const auto & state = std::get<YieldStuckStop>(decision_result);
-    return "YieldStuckStop:\nsafety_report:" + state.safety_report;
+    return "YieldStuckStop:\nocclusion_report:" + state.occlusion_report;
   }
   if (std::holds_alternative<NonOccludedCollisionStop>(decision_result)) {
     const auto & state = std::get<NonOccludedCollisionStop>(decision_result);
-    return "NonOccludedCollisionStop\nsafety_report:" + state.safety_report;
+    return "NonOccludedCollisionStop:\nocclusion_report:" + state.occlusion_report;
   }
   if (std::holds_alternative<FirstWaitBeforeOcclusion>(decision_result)) {
-    return "FirstWaitBeforeOcclusion";
+    const auto & state = std::get<FirstWaitBeforeOcclusion>(decision_result);
+    return "FirstWaitBeforeOcclusion:\nocclusion_report:" + state.occlusion_report;
   }
   if (std::holds_alternative<PeekingTowardOcclusion>(decision_result)) {
-    return "PeekingTowardOcclusion";
+    const auto & state = std::get<PeekingTowardOcclusion>(decision_result);
+    return "PeekingTowardOcclusion:\nocclusion_report:" + state.occlusion_report;
   }
   if (std::holds_alternative<OccludedCollisionStop>(decision_result)) {
     const auto & state = std::get<OccludedCollisionStop>(decision_result);
-    return "OccludedCollisionStop\nsafety_report:" + state.safety_report;
+    return "OccludedCollisionStop:\nocclusion_report:" + state.occlusion_report;
   }
   if (std::holds_alternative<OccludedAbsenceTrafficLight>(decision_result)) {
     const auto & state = std::get<OccludedAbsenceTrafficLight>(decision_result);
-    return "OccludedAbsenceTrafficLight\nsafety_report:" + state.safety_report;
+    return "OccludedAbsenceTrafficLight:\nocclusion_report:" + state.occlusion_report;
   }
   if (std::holds_alternative<Safe>(decision_result)) {
-    return "Safe";
+    const auto & state = std::get<Safe>(decision_result);
+    return "Safe:\nocclusion_report:" + state.occlusion_report;
   }
   if (std::holds_alternative<FullyPrioritized>(decision_result)) {
     const auto & state = std::get<FullyPrioritized>(decision_result);

planning/behavior_velocity_intersection_module/src/decision_result.hpp

@@ -56,7 +56,7 @@ struct YieldStuckStop
 {
   size_t closest_idx{0};
   size_t stuck_stopline_idx{0};
-  std::string safety_report;
+  std::string occlusion_report;
 };
 
 /**
@@ -67,7 +67,7 @@ struct NonOccludedCollisionStop
   size_t closest_idx{0};
   size_t collision_stopline_idx{0};
   size_t occlusion_stopline_idx{0};
-  std::string safety_report;
+  std::string occlusion_report;
 };
 
 /**
@@ -79,6 +79,7 @@ struct FirstWaitBeforeOcclusion
   size_t closest_idx{0};
   size_t first_stopline_idx{0};
   size_t occlusion_stopline_idx{0};
+  std::string occlusion_report;
 };
 
 /**
@@ -98,6 +99,7 @@ struct PeekingTowardOcclusion
   //! contains the remaining time to release the static occlusion stuck and shows up
   //! intersection_occlusion(x.y)
   std::optional<double> static_occlusion_timeout{std::nullopt};
+  std::string occlusion_report;
 };
 
 /**
@@ -114,7 +116,7 @@ struct OccludedCollisionStop
   //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it
   //! contains the remaining time to release the static occlusion stuck
   std::optional<double> static_occlusion_timeout{std::nullopt};
-  std::string safety_report;
+  std::string occlusion_report;
 };
 
 /**
@@ -128,7 +130,7 @@ struct OccludedAbsenceTrafficLight
   size_t closest_idx{0};
   size_t first_attention_area_stopline_idx{0};
   size_t peeking_limit_line_idx{0};
-  std::string safety_report;
+  std::string occlusion_report;
 };
 
 /**
@@ -139,6 +141,7 @@ struct Safe
   size_t closest_idx{0};
   size_t collision_stopline_idx{0};
   size_t occlusion_stopline_idx{0};
+  std::string occlusion_report;
 };
 
 /**
@@ -154,7 +157,7 @@ struct FullyPrioritized
 };
 
 using DecisionResult = std::variant<
-  InternalError,                //! internal process error, or over the pass judge line
+  InternalError,                //! internal process error
   OverPassJudge,                //! over the pass judge lines
   StuckStop,                    //! detected stuck vehicle
   YieldStuckStop,               //! detected yield stuck vehicle

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -294,6 +294,11 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   }
 
   ip.debug.ttc = getOrDeclareParameter<std::vector<int64_t>>(node, ns + ".debug.ttc");
+
+  decision_state_pub_ =
+    node.create_publisher<std_msgs::msg::String>("~/debug/intersection/decision_state", 1);
+  tl_observation_pub_ = node.create_publisher<autoware_perception_msgs::msg::TrafficSignal>(
+    "~/debug/intersection_traffic_signal", 1);
 }
 
 void IntersectionModuleManager::launchNewModules(
@@ -390,6 +395,10 @@ bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegister
 
 void IntersectionModuleManager::sendRTC(const Time & stamp)
 {
+  double min_distance = std::numeric_limits<double>::infinity();
+  std::optional<TrafficSignalStamped> nearest_tl_observation{std::nullopt};
+  std_msgs::msg::String decision_type;
+
   for (const auto & scene_module : scene_modules_) {
     const auto intersection_module = std::dynamic_pointer_cast<IntersectionModule>(scene_module);
     const UUID uuid = getUUID(scene_module->getModuleId());
@@ -401,9 +410,27 @@ void IntersectionModuleManager::sendRTC(const Time & stamp)
     const auto occlusion_safety = intersection_module->getOcclusionSafety();
     occlusion_rtc_interface_.updateCooperateStatus(
       occlusion_uuid, occlusion_safety, occlusion_distance, occlusion_distance, stamp);
+
+    // ==========================================================================================
+    // module debug data
+    // ==========================================================================================
+    const auto internal_debug_data = intersection_module->getInternalDebugData();
+    if (internal_debug_data.distance < min_distance) {
+      min_distance = internal_debug_data.distance;
+      nearest_tl_observation = internal_debug_data.tl_observation;
+    }
+    decision_type.data += (internal_debug_data.decision_type + "\n");
   }
   rtc_interface_.publishCooperateStatus(stamp);  // publishRTCStatus()
   occlusion_rtc_interface_.publishCooperateStatus(stamp);
+
+  // ==========================================================================================
+  // publish module debug data
+  // ==========================================================================================
+  decision_state_pub_->publish(decision_type);
+  if (nearest_tl_observation) {
+    tl_observation_pub_->publish(nearest_tl_observation.value().signal);
+  }
 }
 
 void IntersectionModuleManager::setActivation()

planning/behavior_velocity_intersection_module/src/manager.hpp

@@ -24,6 +24,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <std_msgs/msg/string.hpp>
 #include <tier4_api_msgs/msg/intersection_status.hpp>
 
 #include <functional>
@@ -57,6 +58,9 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC
   void setActivation() override;
   /* called from SceneModuleInterface::updateSceneModuleInstances */
   void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
+
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr decision_state_pub_;
+  rclcpp::Publisher<autoware_perception_msgs::msg::TrafficSignal>::SharedPtr tl_observation_pub_;
 };
 
 class MergeFromPrivateModuleManager : public SceneModuleManagerInterface

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -52,14 +52,14 @@ IntersectionModule::IntersectionModule(
   const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node,
   const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock)
 : SceneModuleInterface(module_id, logger, clock),
+  planner_param_(planner_param),
   lane_id_(lane_id),
   associative_ids_(associative_ids),
   turn_direction_(turn_direction),
   has_traffic_light_(has_traffic_light),
   occlusion_uuid_(tier4_autoware_utils::generateUUID())
 {
   velocity_factor_.init(PlanningBehavior::INTERSECTION);
-  planner_param_ = planner_param;
 
   {
     collision_state_machine_.setMarginTime(
@@ -86,8 +86,6 @@ IntersectionModule::IntersectionModule(
     static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP);
   }
 
-  decision_state_pub_ =
-    node.create_publisher<std_msgs::msg::String>("~/debug/intersection/decision_state", 1);
   ego_ttc_pub_ = node.create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
     "~/debug/intersection/ego_ttc", 1);
   object_ttc_pub_ = node.create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
@@ -107,9 +105,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   {
     const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " +
                                       intersection::formatDecisionResult(decision_result);
-    std_msgs::msg::String decision_result_msg;
-    decision_result_msg.data = decision_type;
-    decision_state_pub_->publish(decision_result_msg);
+    internal_debug_data_.decision_type = decision_type;
   }
 
   prepareRTCStatus(decision_result, *path);
@@ -130,6 +126,25 @@ void IntersectionModule::initializeRTCStatus()
   // activated_ and occlusion_activated_ must be set from manager's RTC callback
 }
 
+static std::string formatOcclusionType(const IntersectionModule::OcclusionType & type)
+{
+  if (std::holds_alternative<IntersectionModule::NotOccluded>(type)) {
+    return "NotOccluded and the best occlusion clearance is " +
+           std::to_string(std::get<IntersectionModule::NotOccluded>(type).best_clearance_distance);
+  }
+  if (std::holds_alternative<IntersectionModule::StaticallyOccluded>(type)) {
+    return "StaticallyOccluded and the best occlusion clearance is " +
+           std::to_string(
+             std::get<IntersectionModule::StaticallyOccluded>(type).best_clearance_distance);
+  }
+  if (std::holds_alternative<IntersectionModule::DynamicallyOccluded>(type)) {
+    return "DynamicallyOccluded and the best occlusion clearance is " +
+           std::to_string(
+             std::get<IntersectionModule::DynamicallyOccluded>(type).best_clearance_distance);
+  }
+  return "RTCOccluded";
+}
+
 intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason)
 {
@@ -239,6 +254,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     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);
+  const std::string occlusion_diag = formatOcclusionType(occlusion_status);
+
   if (is_permanent_go_) {
     if (has_collision) {
       const auto closest_idx = intersection_stoplines.closest_idx;
@@ -287,7 +304,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines);
   if (is_yield_stuck_status) {
     auto yield_stuck = is_yield_stuck_status.value();
-    yield_stuck.safety_report = safety_report;
+    yield_stuck.occlusion_report = occlusion_diag;
     return yield_stuck;
   }
 
@@ -305,12 +322,13 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
 
   // Safe
   if (!is_occlusion_state && !has_collision_with_margin) {
-    return intersection::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx};
+    return intersection::Safe{
+      closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag};
   }
   // Only collision
   if (!is_occlusion_state && has_collision_with_margin) {
     return intersection::NonOccludedCollisionStop{
-      closest_idx, collision_stopline_idx, occlusion_stopline_idx, safety_report};
+      closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag};
   }
   // Occluded
   // utility functions
@@ -384,7 +402,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
         closest_idx,
         first_attention_stopline_idx,
         occlusion_wo_tl_pass_judge_line_idx,
-        safety_report};
+        occlusion_diag};
     }
 
     // ==========================================================================================
@@ -407,7 +425,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     const bool release_static_occlusion_stuck =
       (static_occlusion_timeout_state_machine_.getState() == StateMachine::State::GO);
     if (!has_collision_with_margin && release_static_occlusion_stuck) {
-      return intersection::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx};
+      return intersection::Safe{
+        closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag};
     }
     // occlusion_status is either STATICALLY_OCCLUDED or DYNAMICALLY_OCCLUDED
     const double max_timeout =
@@ -428,7 +447,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
         first_attention_stopline_idx,
         occlusion_stopline_idx,
         static_occlusion_timeout,
-        safety_report};
+        occlusion_diag};
     } else {
       return intersection::PeekingTowardOcclusion{
         is_occlusion_cleared_with_margin,
@@ -437,15 +456,17 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
         collision_stopline_idx,
         first_attention_stopline_idx,
         occlusion_stopline_idx,
-        static_occlusion_timeout};
+        static_occlusion_timeout,
+        occlusion_diag};
     }
   } else {
     const auto occlusion_stopline =
       (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_)
         ? first_attention_stopline_idx
         : occlusion_stopline_idx;
     return intersection::FirstWaitBeforeOcclusion{
-      is_occlusion_cleared_with_margin, closest_idx, default_stopline_idx, occlusion_stopline};
+      is_occlusion_cleared_with_margin, closest_idx, default_stopline_idx, occlusion_stopline,
+      occlusion_diag};
   }
 }
 
@@ -1256,6 +1277,7 @@ void IntersectionModule::updateTrafficSignalObservation()
     return;
   }
   last_tl_valid_observation_ = tl_info_opt.value();
+  internal_debug_data_.tl_observation = tl_info_opt.value();
 }
 
 IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus(

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -28,7 +28,6 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
-#include <std_msgs/msg/string.hpp>
 #include <tier4_debug_msgs/msg/float64_multi_array_stamped.hpp>
 
 #include <lanelet2_core/Forward.h>
@@ -242,6 +241,13 @@ class IntersectionModule : public SceneModuleInterface
       traffic_light_observation{std::nullopt};
   };
 
+  struct InternalDebugData
+  {
+    double distance{0.0};
+    std::string decision_type{};
+    std::optional<TrafficSignalStamped> tl_observation{std::nullopt};
+  };
+
   using TimeDistanceArray = std::vector<std::pair<double /* time*/, double /* distance*/>>;
 
   /**
@@ -326,6 +332,7 @@ class IntersectionModule : public SceneModuleInterface
   double getOcclusionDistance() const { return occlusion_stop_distance_; }
   void setOcclusionActivation(const bool activation) { occlusion_activated_ = activation; }
   bool isOcclusionFirstStopRequired() const { return occlusion_first_stop_required_; }
+  InternalDebugData & getInternalDebugData() const { return internal_debug_data_; }
 
 private:
   /**
@@ -336,6 +343,9 @@ class IntersectionModule : public SceneModuleInterface
    * following variables are unique to this intersection lanelet or to this module
    * @{
    */
+
+  const PlannerParam planner_param_;
+
   //! lanelet of this intersection
   const lanelet::Id lane_id_;
 
@@ -361,7 +371,6 @@ class IntersectionModule : public SceneModuleInterface
    * following variables are immutable once initialized
    * @{
    */
-  PlannerParam planner_param_;
 
   //! cache IntersectionLanelets struct
   std::optional<intersection::IntersectionLanelets> intersection_lanelets_{std::nullopt};
@@ -807,7 +816,7 @@ class IntersectionModule : public SceneModuleInterface
   /** @} */
 
   mutable DebugData debug_data_;
-  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr decision_state_pub_;
+  mutable InternalDebugData internal_debug_data_{};
   rclcpp::Publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>::SharedPtr ego_ttc_pub_;
   rclcpp::Publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>::SharedPtr object_ttc_pub_;
 };