chore(intersection): add const to member functions, replace enum with class tag for occlusion (autowarefoundation#6299)

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

Author: soblin

planning/behavior_velocity_intersection_module/src/object_manager.cpp

@@ -240,7 +240,7 @@ void ObjectInfoManager::clearObjects()
   parked_objects_.clear();
 };
 
-std::vector<std::shared_ptr<ObjectInfo>> ObjectInfoManager::allObjects()
+std::vector<std::shared_ptr<ObjectInfo>> ObjectInfoManager::allObjects() const
 {
   std::vector<std::shared_ptr<ObjectInfo>> all_objects = attention_area_objects_;
   all_objects.insert(

planning/behavior_velocity_intersection_module/src/object_manager.hpp

@@ -109,7 +109,7 @@ class ObjectInfo
     return predicted_object_;
   };
 
-  std::optional<CollisionInterval> is_unsafe() const
+  std::optional<CollisionInterval> unsafe_info() const
   {
     if (safe_under_traffic_control_) {
       return std::nullopt;
@@ -246,7 +246,7 @@ class ObjectInfoManager
 
   const std::vector<std::shared_ptr<ObjectInfo>> & parkedObjects() const { return parked_objects_; }
 
-  std::vector<std::shared_ptr<ObjectInfo>> allObjects();
+  std::vector<std::shared_ptr<ObjectInfo>> allObjects() const;
 
   const std::unordered_map<unique_identifier_msgs::msg::UUID, std::shared_ptr<ObjectInfo>> &
   getObjectsMap()

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -51,7 +51,6 @@ 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),
-  node_(node),
   lane_id_(lane_id),
   associative_ids_(associative_ids),
   turn_direction_(turn_direction),
@@ -87,10 +86,10 @@ IntersectionModule::IntersectionModule(
   }
 
   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>(
+    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>(
+  object_ttc_pub_ = node.create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
     "~/debug/intersection/object_ttc", 1);
 }
 
@@ -104,11 +103,13 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   const auto decision_result = modifyPathVelocityDetail(path, stop_reason);
   prev_decision_result_ = decision_result;
 
-  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);
+  {
+    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);
+  }
 
   prepareRTCStatus(decision_result, *path);
 
@@ -224,6 +225,14 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   updateObjectInfoManagerCollision(
     path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line,
     safely_passed_2nd_judge_line);
+  for (const auto & object_info : object_info_manager_.attentionObjects()) {
+    if (!object_info->unsafe_info()) {
+      continue;
+    }
+    setObjectsOfInterestData(
+      object_info->predicted_object().kinematics.initial_pose_with_covariance.pose,
+      object_info->predicted_object().shape, ColorName::RED);
+  }
 
   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);
@@ -240,17 +249,17 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
       "no collision is detected", "ego can safely pass the intersection at this rate"};
   }
 
-  const bool collision_on_1st_attention_lane =
-    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) {
+  if (const bool collision_on_1st_attention_lane =
+        has_collision &&
+        (collision_position == intersection::CollisionInterval::LanePosition::FIRST);
+      is_over_1st_pass_judge_line && is_over_2nd_pass_judge_line.has_value() &&
+      !is_over_2nd_pass_judge_line.value() && collision_on_1st_attention_lane) {
     safety_report +=
       "\nego is between the 1st and 2nd pass judge line but collision is expected on the 1st "
       "attention lane, which is dangerous.";
@@ -374,15 +383,15 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
         closest_idx,
         first_attention_stopline_idx,
         occlusion_wo_tl_pass_judge_line_idx,
-        safety_diag};
+        safety_report};
     }
 
     // ==========================================================================================
     // following remaining block is "has_traffic_light_"
     //
     // if ego is stuck by static occlusion in the presence of traffic light, start timeout count
     // ==========================================================================================
-    const bool is_static_occlusion = occlusion_status == OcclusionType::STATICALLY_OCCLUDED;
+    const bool is_static_occlusion = std::holds_alternative<StaticallyOccluded>(occlusion_status);
     const bool is_stuck_by_static_occlusion =
       stoppedAtPosition(
         occlusion_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking) &&

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -41,6 +41,7 @@
 #include <string>
 #include <tuple>
 #include <utility>
+#include <variant>
 #include <vector>
 
 namespace behavior_velocity_planner
@@ -166,16 +167,25 @@ class IntersectionModule : public SceneModuleInterface
     } debug;
   };
 
-  enum OcclusionType {
-    //! occlusion is not detected
-    NOT_OCCLUDED,
-    //! occlusion is not caused by dynamic objects
-    STATICALLY_OCCLUDED,
-    //! occlusion is caused by dynamic objects
-    DYNAMICALLY_OCCLUDED,
-    //! actual occlusion does not exist, only disapproved by RTC
-    RTC_OCCLUDED,
+  //! occlusion is not detected
+  struct NotOccluded
+  {
+    double best_clearance_distance{-1.0};
+  };
+  //! occlusion is not caused by dynamic objects
+  struct StaticallyOccluded
+  {
+    double best_clearance_distance{0.0};
+  };
+  //! occlusion is caused by dynamic objects
+  struct DynamicallyOccluded
+  {
+    double best_clearance_distance{0.0};
   };
+  //! actual occlusion does not exist, only disapproved by RTC
+  using RTCOccluded = std::monostate;
+  using OcclusionType =
+    std::variant<NotOccluded, StaticallyOccluded, DynamicallyOccluded, RTCOccluded>;
 
   struct DebugData
   {
@@ -294,11 +304,9 @@ class IntersectionModule : public SceneModuleInterface
   bool getOcclusionSafety() const { return occlusion_safety_; }
   double getOcclusionDistance() const { return occlusion_stop_distance_; }
   void setOcclusionActivation(const bool activation) { occlusion_activated_ = activation; }
-  bool isOcclusionFirstStopRequired() { return occlusion_first_stop_required_; }
+  bool isOcclusionFirstStopRequired() const { return occlusion_first_stop_required_; }
 
 private:
-  rclcpp::Node & node_;
-
   /**
    ***********************************************************
    ***********************************************************
@@ -396,7 +404,7 @@ class IntersectionModule : public SceneModuleInterface
    * @defgroup occlusion-variables [var] occlusion detection variables
    * @{
    */
-  OcclusionType prev_occlusion_status_;
+  OcclusionType prev_occlusion_status_{NotOccluded{}};
 
   //! debouncing for the first brief stop at the default stopline
   StateMachine before_creep_state_machine_;
@@ -499,7 +507,7 @@ class IntersectionModule : public SceneModuleInterface
    */
   std::optional<size_t> getStopLineIndexFromMap(
     const intersection::InterpolatedPathInfo & interpolated_path_info,
-    lanelet::ConstLanelet assigned_lanelet);
+    lanelet::ConstLanelet assigned_lanelet) const;
 
   /**
    * @brief generate IntersectionStopLines
@@ -510,15 +518,15 @@ class IntersectionModule : public SceneModuleInterface
     const lanelet::ConstLanelet & first_attention_lane,
     const std::optional<lanelet::CompoundPolygon3d> & second_attention_area_opt,
     const intersection::InterpolatedPathInfo & interpolated_path_info,
-    autoware_auto_planning_msgs::msg::PathWithLaneId * original_path);
+    autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) const;
 
   /**
    * @brief generate IntersectionLanelets
    */
   intersection::IntersectionLanelets generateObjectiveLanelets(
     lanelet::LaneletMapConstPtr lanelet_map_ptr,
     lanelet::routing::RoutingGraphPtr routing_graph_ptr,
-    const lanelet::ConstLanelet assigned_lanelet);
+    const lanelet::ConstLanelet assigned_lanelet) const;
 
   /**
    * @brief generate PathLanelets
@@ -529,14 +537,15 @@ class IntersectionModule : public SceneModuleInterface
     const lanelet::CompoundPolygon3d & first_conflicting_area,
     const std::vector<lanelet::CompoundPolygon3d> & conflicting_areas,
     const std::optional<lanelet::CompoundPolygon3d> & first_attention_area,
-    const std::vector<lanelet::CompoundPolygon3d> & attention_areas, const size_t closest_idx);
+    const std::vector<lanelet::CompoundPolygon3d> & attention_areas,
+    const size_t closest_idx) const;
 
   /**
    * @brief generate discretized detection lane linestring.
    */
   std::vector<lanelet::ConstLineString3d> generateDetectionLaneDivisions(
     lanelet::ConstLanelets detection_lanelets,
-    const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution);
+    const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) const;
   /** @} */
 
 private:
@@ -636,7 +645,8 @@ class IntersectionModule : public SceneModuleInterface
    * @attention this function has access to value() of intersection_lanelets_,
    * intersection_lanelets.first_attention_area(), occlusion_attention_divisions_
    */
-  OcclusionType detectOcclusion(const intersection::InterpolatedPathInfo & interpolated_path_info);
+  OcclusionType detectOcclusion(
+    const intersection::InterpolatedPathInfo & interpolated_path_info) const;
   /** @} */
 
 private:
@@ -699,7 +709,7 @@ class IntersectionModule : public SceneModuleInterface
    */
   std::optional<intersection::NonOccludedCollisionStop> isGreenPseudoCollisionStatus(
     const size_t closest_idx, const size_t collision_stopline_idx,
-    const intersection::IntersectionStopLines & intersection_stoplines);
+    const intersection::IntersectionStopLines & intersection_stoplines) const;
 
   /**
    * @brief generate the message explaining why too_late_detect_objects/misjudge_objects exist and
@@ -731,7 +741,8 @@ class IntersectionModule : public SceneModuleInterface
    * @brief return if collision is detected and the collision position
    */
   CollisionStatus detectCollision(
-    const bool is_over_1st_pass_judge_line, const std::optional<bool> is_over_2nd_pass_judge_line);
+    const bool is_over_1st_pass_judge_line,
+    const std::optional<bool> is_over_2nd_pass_judge_line) const;
 
   std::optional<size_t> checkAngleForTargetLanelets(
     const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets,
@@ -747,7 +758,7 @@ class IntersectionModule : public SceneModuleInterface
   TimeDistanceArray calcIntersectionPassingTime(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized,
     const intersection::IntersectionStopLines & intersection_stoplines,
-    tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array);
+    tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const;
   /** @} */
 
   mutable DebugData debug_data_;

planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp

@@ -346,7 +346,7 @@ void IntersectionModule::cutPredictPathWithinDuration(
 std::optional<intersection::NonOccludedCollisionStop>
 IntersectionModule::isGreenPseudoCollisionStatus(
   const size_t closest_idx, const size_t collision_stopline_idx,
-  const intersection::IntersectionStopLines & intersection_stoplines)
+  const intersection::IntersectionStopLines & intersection_stoplines) const
 {
   // ==========================================================================================
   // if there are any vehicles on the attention area when ego entered the intersection on green
@@ -588,7 +588,8 @@ std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis(
 }
 
 IntersectionModule::CollisionStatus IntersectionModule::detectCollision(
-  const bool is_over_1st_pass_judge_line, const std::optional<bool> is_over_2nd_pass_judge_line)
+  const bool is_over_1st_pass_judge_line,
+  const std::optional<bool> is_over_2nd_pass_judge_line) const
 {
   // ==========================================================================================
   // if collision is detected for multiple objects, we prioritize collision on the first
@@ -598,14 +599,18 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision(
   bool collision_at_non_first_lane = false;
 
   // ==========================================================================================
-  // find the objects which is judges as UNSAFE after ego passed pass judge lines.
+  // find the objects which are judged as UNSAFE after ego passed pass judge lines.
   //
   // misjudge_objects are those that were once judged as safe when ego passed the pass judge line
   //
-  // too_late_detect objects are those that (1) were not detected when ego passed the pass judge
-  // line (2) were judged as dangerous at the same time when ego passed the pass judge, which are
-  // expected to have been detected in the prior iteration because ego could have judged as UNSAFE
-  // in the prior iteration
+  // too_late_detect_objects are those that (1) were not detected when ego passed the pass judge
+  // line (2) were judged as dangerous at the same time when ego passed the pass judge line, which
+  // means they were expected to have been detected when ego passed the pass judge lines or in the
+  // prior iteration, because ego could have judged them as UNSAFE if their information was
+  // available at that time.
+  //
+  // that case is both "too late to stop" and "too late to go" for the planner. and basically
+  // detection side is responsible for this fault
   // ==========================================================================================
   std::vector<std::pair<CollisionStatus::BlameType, std::shared_ptr<intersection::ObjectInfo>>>
     misjudge_objects;
@@ -617,10 +622,10 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision(
         object_info->predicted_object());
       continue;
     }
-    if (!object_info->is_unsafe()) {
+    if (!object_info->unsafe_info()) {
       continue;
     }
-    const auto & unsafe_info = object_info->is_unsafe().value();
+    const auto & unsafe_info = object_info->unsafe_info().value();
     // ==========================================================================================
     // if ego is over the pass judge lines, then the visualization as "too_late_objects" or
     // "misjudge_objects" is more important than that for "unsafe"
@@ -986,7 +991,7 @@ std::optional<size_t> IntersectionModule::checkAngleForTargetLanelets(
 IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized,
   const intersection::IntersectionStopLines & intersection_stoplines,
-  tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array)
+  tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const
 {
   const double intersection_velocity =
     planner_param_.collision_detection.velocity_profile.default_velocity;