add OberPassJudge state

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

planning/behavior_velocity_intersection_module/CMakeLists.txt

@@ -13,6 +13,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/scene_intersection.cpp
   src/intersection_lanelets.cpp
   src/object_manager.cpp
+  src/decision_result.cpp
   src/scene_intersection_prepare_data.cpp
   src/scene_intersection_stuck.cpp
   src/scene_intersection_occlusion.cpp

planning/behavior_velocity_intersection_module/src/decision_result.cpp

@@ -0,0 +1,60 @@
+// Copyright 2024 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "decision_result.hpp"
+
+namespace behavior_velocity_planner::intersection
+{
+std::string formatDecisionResult(const DecisionResult & decision_result)
+{
+  if (std::holds_alternative<InternalError>(decision_result)) {
+    const auto state = std::get<InternalError>(decision_result);
+    return "InternalError because " + state.error;
+  }
+  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";
+  }
+  if (std::holds_alternative<StuckStop>(decision_result)) {
+    return "StuckStop";
+  }
+  if (std::holds_alternative<YieldStuckStop>(decision_result)) {
+    return "YieldStuckStop";
+  }
+  if (std::holds_alternative<NonOccludedCollisionStop>(decision_result)) {
+    return "NonOccludedCollisionStop";
+  }
+  if (std::holds_alternative<FirstWaitBeforeOcclusion>(decision_result)) {
+    return "FirstWaitBeforeOcclusion";
+  }
+  if (std::holds_alternative<PeekingTowardOcclusion>(decision_result)) {
+    return "PeekingTowardOcclusion";
+  }
+  if (std::holds_alternative<OccludedCollisionStop>(decision_result)) {
+    return "OccludedCollisionStop";
+  }
+  if (std::holds_alternative<OccludedAbsenceTrafficLight>(decision_result)) {
+    return "OccludedAbsenceTrafficLight";
+  }
+  if (std::holds_alternative<Safe>(decision_result)) {
+    return "Safe";
+  }
+  if (std::holds_alternative<FullyPrioritized>(decision_result)) {
+    return "FullyPrioritized";
+  }
+  return "";
+}
+
+}  // namespace behavior_velocity_planner::intersection

planning/behavior_velocity_intersection_module/src/decision_result.hpp

@@ -25,11 +25,20 @@ namespace behavior_velocity_planner::intersection
 /**
  * @brief internal error
  */
-struct Indecisive
+struct InternalError
 {
   std::string error;
 };
 
+/**
+ * @brief
+ */
+struct OverPassJudge
+{
+  std::string safety_report;
+  std::string evasive_report;
+};
+
 /**
  * @brief detected stuck vehicle
  */
@@ -47,6 +56,7 @@ struct YieldStuckStop
 {
   size_t closest_idx{0};
   size_t stuck_stopline_idx{0};
+  std::string safety_report;
 };
 
 /**
@@ -57,6 +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;
 };
 
 /**
@@ -103,6 +114,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;
 };
 
 /**
@@ -116,6 +128,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;
 };
 
 /**
@@ -137,10 +150,12 @@ struct FullyPrioritized
   size_t closest_idx{0};
   size_t collision_stopline_idx{0};
   size_t occlusion_stopline_idx{0};
+  std::string safety_report;
 };
 
 using DecisionResult = std::variant<
-  Indecisive,                   //! internal process error, or over the pass judge line
+  InternalError,                //! internal process error, or over the pass judge line
+  OverPassJudge,                //! over the pass judge lines
   StuckStop,                    //! detected stuck vehicle
   YieldStuckStop,               //! detected yield stuck vehicle
   NonOccludedCollisionStop,     //! detected collision while FOV is clear
@@ -152,41 +167,7 @@ using DecisionResult = std::variant<
   FullyPrioritized              //! only detect vehicles violating traffic rules
   >;
 
-inline std::string formatDecisionResult(const DecisionResult & decision_result)
-{
-  if (std::holds_alternative<Indecisive>(decision_result)) {
-    const auto indecisive = std::get<Indecisive>(decision_result);
-    return "Indecisive because " + indecisive.error;
-  }
-  if (std::holds_alternative<StuckStop>(decision_result)) {
-    return "StuckStop";
-  }
-  if (std::holds_alternative<YieldStuckStop>(decision_result)) {
-    return "YieldStuckStop";
-  }
-  if (std::holds_alternative<NonOccludedCollisionStop>(decision_result)) {
-    return "NonOccludedCollisionStop";
-  }
-  if (std::holds_alternative<FirstWaitBeforeOcclusion>(decision_result)) {
-    return "FirstWaitBeforeOcclusion";
-  }
-  if (std::holds_alternative<PeekingTowardOcclusion>(decision_result)) {
-    return "PeekingTowardOcclusion";
-  }
-  if (std::holds_alternative<OccludedCollisionStop>(decision_result)) {
-    return "OccludedCollisionStop";
-  }
-  if (std::holds_alternative<OccludedAbsenceTrafficLight>(decision_result)) {
-    return "OccludedAbsenceTrafficLight";
-  }
-  if (std::holds_alternative<Safe>(decision_result)) {
-    return "Safe";
-  }
-  if (std::holds_alternative<FullyPrioritized>(decision_result)) {
-    return "FullyPrioritized";
-  }
-  return "";
-}
+std::string formatDecisionResult(const DecisionResult & decision_result);
 
 }  // namespace behavior_velocity_planner::intersection
 

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -167,18 +167,18 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   // ego path has just entered the entry of this intersection
   // ==========================================================================================
   if (!intersection_lanelets.first_attention_area()) {
-    return intersection::Indecisive{"attention area is empty"};
+    return intersection::InternalError{"attention area is empty"};
   }
   const auto first_attention_area = intersection_lanelets.first_attention_area().value();
   const auto default_stopline_idx_opt = intersection_stoplines.default_stopline;
   if (!default_stopline_idx_opt) {
-    return intersection::Indecisive{"default stop line is null"};
+    return intersection::InternalError{"default stop line is null"};
   }
   const auto default_stopline_idx = default_stopline_idx_opt.value();
   const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline;
   const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline;
   if (!first_attention_stopline_idx_opt || !occlusion_peeking_stopline_idx_opt) {
-    return intersection::Indecisive{"occlusion stop line is null"};
+    return intersection::InternalError{"occlusion stop line is null"};
   }
   const auto first_attention_stopline_idx = first_attention_stopline_idx_opt.value();
   const auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value();
@@ -228,25 +228,20 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     detectCollision(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line);
   if (is_permanent_go_) {
     if (has_collision) {
-      // TODO(Mamoru Sobue): diagnosis
-      return intersection::Indecisive{
-        "ego is over the pass judge lines and collision is detected. need acceleration to keep "
-        "safe."};
+      return intersection::OverPassJudge{"TODO", "ego needs acceleration to keep safe"};
     }
-    return intersection::Indecisive{"over pass judge lines and collision is not detected"};
+    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;
   if (
-    is_over_1st_pass_judge_line && !is_over_2nd_pass_judge_line &&
-    collision_on_1st_attention_lane) {
-    // TODO(Mamoru Sobue): diagnosis
-    return intersection::Indecisive{
-      "ego is already over the 1st pass judge line although still before the 2nd pass judge line, "
-      "but collision is detected on the first attention lane"};
+    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";
   }
-  */
 
   const auto closest_idx = intersection_stoplines.closest_idx;
   const bool is_over_default_stopline = util::isOverTargetIndex(
@@ -268,7 +263,9 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   const auto is_yield_stuck_status =
     isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines);
   if (is_yield_stuck_status) {
-    return is_yield_stuck_status.value();
+    auto yield_stuck = is_yield_stuck_status.value();
+    yield_stuck.safety_report = safety_report;
+    return yield_stuck;
   }
 
   collision_state_machine_.setStateWithMarginTime(
@@ -279,7 +276,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
 
   if (is_prioritized) {
     return intersection::FullyPrioritized{
-      has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx};
+      has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx,
+      safety_report};
   }
 
   // Safe
@@ -289,7 +287,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   // Only collision
   if (!is_occlusion_state && has_collision_with_margin) {
     return intersection::NonOccludedCollisionStop{
-      closest_idx, collision_stopline_idx, occlusion_stopline_idx};
+      closest_idx, collision_stopline_idx, occlusion_stopline_idx, safety_report};
   }
   // Occluded
   // utility functions
@@ -343,7 +341,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
         : false;
     if (!has_traffic_light_) {
       if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) {
-        return intersection::Indecisive{
+        return intersection::InternalError{
           "already passed maximum peeking line in the absence of traffic light"};
       }
       return intersection::OccludedAbsenceTrafficLight{
@@ -352,7 +350,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
         temporal_stop_before_attention_required,
         closest_idx,
         first_attention_stopline_idx,
-        occlusion_wo_tl_pass_judge_line_idx};
+        occlusion_wo_tl_pass_judge_line_idx,
+        safety_report};
     }
 
     // ==========================================================================================
@@ -395,7 +394,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
         collision_stopline_idx,
         first_attention_stopline_idx,
         occlusion_stopline_idx,
-        static_occlusion_timeout};
+        static_occlusion_timeout,
+        safety_report};
     } else {
       return intersection::PeekingTowardOcclusion{
         is_occlusion_cleared_with_margin,
@@ -438,7 +438,17 @@ void prepareRTCByDecisionResult(
 
 template <>
 void prepareRTCByDecisionResult(
-  [[maybe_unused]] const intersection::Indecisive & result,
+  [[maybe_unused]] const intersection::InternalError & result,
+  [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance,
+  [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance)
+{
+  return;
+}
+
+template <>
+void prepareRTCByDecisionResult(
+  [[maybe_unused]] const intersection::OverPassJudge & result,
   [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance,
   [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance)
@@ -646,7 +656,22 @@ template <>
 void reactRTCApprovalByDecisionResult(
   [[maybe_unused]] const bool rtc_default_approved,
   [[maybe_unused]] const bool rtc_occlusion_approved,
-  [[maybe_unused]] const intersection::Indecisive & decision_result,
+  [[maybe_unused]] const intersection::InternalError & decision_result,
+  [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param,
+  [[maybe_unused]] const double baselink2front,
+  [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path,
+  [[maybe_unused]] StopReason * stop_reason,
+  [[maybe_unused]] VelocityFactorInterface * velocity_factor,
+  [[maybe_unused]] IntersectionModule::DebugData * debug_data)
+{
+  return;
+}
+
+template <>
+void reactRTCApprovalByDecisionResult(
+  [[maybe_unused]] const bool rtc_default_approved,
+  [[maybe_unused]] const bool rtc_occlusion_approved,
+  [[maybe_unused]] const intersection::OverPassJudge & decision_result,
   [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param,
   [[maybe_unused]] const double baselink2front,
   [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path,

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -336,7 +336,7 @@ class IntersectionModule : public SceneModuleInterface
   bool is_permanent_go_{false};
 
   //! for checking if ego is over the pass judge lines because previously the situation was SAFE
-  intersection::DecisionResult prev_decision_result_{intersection::Indecisive{""}};
+  intersection::DecisionResult prev_decision_result_{intersection::InternalError{""}};
 
   //! flag if ego passed the 1st_pass_judge_line while peeking. If this is true, 1st_pass_judge_line
   //! is treated as the same position as occlusion_peeking_stopline
@@ -461,13 +461,13 @@ class IntersectionModule : public SceneModuleInterface
 
   /**
    * @brief prepare basic data structure
-   * @return return IntersectionStopLines if all data is valid, otherwise Indecisive
+   * @return return IntersectionStopLines if all data is valid, otherwise InternalError
    * @note if successful, it is ensure that intersection_lanelets_,
    * intersection_lanelets.first_conflicting_lane are not null
    *
    * To simplify modifyPathVelocityDetail(), this function is used at first
    */
-  intersection::Result<BasicData, intersection::Indecisive> prepareIntersectionData(
+  intersection::Result<BasicData, intersection::InternalError> prepareIntersectionData(
     const bool is_prioritized, PathWithLaneId * path);
 
   /**
@@ -625,7 +625,7 @@ class IntersectionModule : public SceneModuleInterface
    */
   /**
    * @brief check if ego is already over the pass judge line
-   * @return if ego is over both 1st/2nd pass judge lines, return Indecisive, else return
+   * @return if ego is over both 1st/2nd pass judge lines, return InternalError, else return
    * (is_over_1st_pass_judge, is_over_2nd_pass_judge)
    * @attention this function has access to value() of intersection_stoplines.default_stopline,
    * intersection_stoplines.occlusion_stopline

planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp

@@ -361,7 +361,7 @@ IntersectionModule::isGreenPseudoCollisionStatus(
     if (exist_close_vehicles) {
       const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value();
       return intersection::NonOccludedCollisionStop{
-        closest_idx, collision_stopline_idx, occlusion_stopline_idx};
+        closest_idx, collision_stopline_idx, occlusion_stopline_idx, std::string("")};
     }
   }
   return std::nullopt;