refactor(intersection): divide source files and modifyPathVelocity (a…

…utowarefoundation#6134)

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

planning/behavior_velocity_intersection_module/CMakeLists.txt

@@ -8,11 +8,16 @@ pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml)
 find_package(OpenCV REQUIRED)
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
-  src/debug.cpp
   src/manager.cpp
+  src/util.cpp
   src/scene_intersection.cpp
+  src/intersection_lanelets.cpp
+  src/scene_intersection_prepare_data.cpp
+  src/scene_intersection_stuck.cpp
+  src/scene_intersection_occlusion.cpp
+  src/scene_intersection_collision.cpp
   src/scene_merge_from_private_road.cpp
-  src/util.cpp
+  src/debug.cpp
 )
 
 target_link_libraries(${PROJECT_NAME}

planning/behavior_velocity_intersection_module/src/decision_result.hpp

@@ -0,0 +1,203 @@
+// 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.
+
+#ifndef DECISION_RESULT_HPP_
+#define DECISION_RESULT_HPP_
+
+#include <optional>
+#include <string>
+#include <variant>
+
+namespace behavior_velocity_planner::intersection
+{
+
+/**
+ * @struct
+ * @brief Internal error or ego already passed pass_judge_line
+ */
+struct Indecisive
+{
+  std::string error;
+};
+
+/**
+ * @struct
+ * @brief detected stuck vehicle
+ */
+struct StuckStop
+{
+  size_t closest_idx{0};
+  size_t stuck_stopline_idx{0};
+  std::optional<size_t> occlusion_stopline_idx{std::nullopt};
+};
+
+/**
+ * @struct
+ * @brief yielded by vehicle on the attention area
+ */
+struct YieldStuckStop
+{
+  size_t closest_idx{0};
+  size_t stuck_stopline_idx{0};
+};
+
+/**
+ * @struct
+ * @brief only collision is detected
+ */
+struct NonOccludedCollisionStop
+{
+  size_t closest_idx{0};
+  size_t collision_stopline_idx{0};
+  size_t occlusion_stopline_idx{0};
+};
+
+/**
+ * @struct
+ * @brief occlusion is detected so ego needs to stop at the default stop line position
+ */
+struct FirstWaitBeforeOcclusion
+{
+  bool is_actually_occlusion_cleared{false};
+  size_t closest_idx{0};
+  size_t first_stopline_idx{0};
+  size_t occlusion_stopline_idx{0};
+};
+
+/**
+ * @struct
+ * @brief ego is approaching the boundary of attention area in the presence of traffic light
+ */
+struct PeekingTowardOcclusion
+{
+  //! if intersection_occlusion is disapproved externally through RTC, it indicates
+  //! "is_forcefully_occluded"
+  bool is_actually_occlusion_cleared{false};
+  bool temporal_stop_before_attention_required{false};
+  size_t closest_idx{0};
+  size_t collision_stopline_idx{0};
+  size_t first_attention_stopline_idx{0};
+  size_t occlusion_stopline_idx{0};
+  //! 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 and shows up
+  //! intersection_occlusion(x.y)
+  std::optional<double> static_occlusion_timeout{std::nullopt};
+};
+
+/**
+ * @struct
+ * @brief both collision and occlusion are detected in the presence of traffic light
+ */
+struct OccludedCollisionStop
+{
+  bool is_actually_occlusion_cleared{false};
+  bool temporal_stop_before_attention_required{false};
+  size_t closest_idx{0};
+  size_t collision_stopline_idx{0};
+  size_t first_attention_stopline_idx{0};
+  size_t occlusion_stopline_idx{0};
+  //! 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};
+};
+
+/**
+ * @struct
+ * @brief at least occlusion is detected in the absence of traffic light
+ */
+struct OccludedAbsenceTrafficLight
+{
+  bool is_actually_occlusion_cleared{false};
+  bool collision_detected{false};
+  bool temporal_stop_before_attention_required{false};
+  size_t closest_idx{0};
+  size_t first_attention_area_stopline_idx{0};
+  size_t peeking_limit_line_idx{0};
+};
+
+/**
+ * @struct
+ * @brief both collision and occlusion are not detected
+ */
+struct Safe
+{
+  size_t closest_idx{0};
+  size_t collision_stopline_idx{0};
+  size_t occlusion_stopline_idx{0};
+};
+
+/**
+ * @struct
+ * @brief traffic light is red or arrow signal
+ */
+struct FullyPrioritized
+{
+  bool collision_detected{false};
+  size_t closest_idx{0};
+  size_t collision_stopline_idx{0};
+  size_t occlusion_stopline_idx{0};
+};
+
+using DecisionResult = std::variant<
+  Indecisive,                   //! internal process error, or over the pass judge line
+  StuckStop,                    //! detected stuck vehicle
+  YieldStuckStop,               //! detected yield stuck vehicle
+  NonOccludedCollisionStop,     //! detected collision while FOV is clear
+  FirstWaitBeforeOcclusion,     //! stop for a while before peeking to occlusion
+  PeekingTowardOcclusion,       //! peeking into occlusion while collision is not detected
+  OccludedCollisionStop,        //! occlusion and collision are both detected
+  OccludedAbsenceTrafficLight,  //! occlusion is detected in the absence of traffic light
+  Safe,                         //! judge as safe
+  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 "";
+}
+
+}  // namespace behavior_velocity_planner::intersection
+
+#endif  // DECISION_RESULT_HPP_

planning/behavior_velocity_intersection_module/src/util_type.hpp → planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp

@@ -12,26 +12,18 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef UTIL_TYPE_HPP_
-#define UTIL_TYPE_HPP_
+#ifndef INTERPOLATED_PATH_INFO_HPP_
+#define INTERPOLATED_PATH_INFO_HPP_
 
-#include <tier4_autoware_utils/geometry/geometry.hpp>
-
-#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
-#include <geometry_msgs/msg/point.hpp>
-#include <geometry_msgs/msg/polygon.hpp>
-#include <geometry_msgs/msg/pose.hpp>
 
-#include <lanelet2_core/primitives/CompoundPolygon.h>
-#include <lanelet2_core/primitives/Lanelet.h>
+#include <lanelet2_core/Forward.h>
 
 #include <optional>
 #include <set>
 #include <utility>
-#include <vector>
 
-namespace behavior_velocity_planner::util
+namespace behavior_velocity_planner::intersection
 {
 
 /**
@@ -52,6 +44,6 @@ struct InterpolatedPathInfo
   std::optional<std::pair<size_t, size_t>> lane_id_interval{std::nullopt};
 };
 
-}  // namespace behavior_velocity_planner::util
+}  // namespace behavior_velocity_planner::intersection
 
-#endif  // UTIL_TYPE_HPP_
+#endif  // INTERPOLATED_PATH_INFO_HPP_

planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp

@@ -0,0 +1,82 @@
+// 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 "intersection_lanelets.hpp"
+
+#include "util.hpp"
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_routing/RoutingGraph.h>
+
+#include <string>
+
+namespace behavior_velocity_planner::intersection
+{
+
+void IntersectionLanelets::update(
+  const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info,
+  const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length,
+  lanelet::routing::RoutingGraphPtr routing_graph_ptr)
+{
+  is_prioritized_ = is_prioritized;
+  // find the first conflicting/detection area polygon intersecting the path
+  if (!first_conflicting_area_) {
+    auto first = util::getFirstPointInsidePolygonsByFootprint(
+      conflicting_area_, interpolated_path_info, footprint, vehicle_length);
+    if (first) {
+      first_conflicting_lane_ = conflicting_.at(first.value().second);
+      first_conflicting_area_ = conflicting_area_.at(first.value().second);
+    }
+  }
+  if (!first_attention_area_) {
+    const auto first = util::getFirstPointInsidePolygonsByFootprint(
+      attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length);
+    if (first) {
+      first_attention_lane_ = attention_non_preceding_.at(first.value().second);
+      first_attention_area_ = attention_non_preceding_area_.at(first.value().second);
+    }
+  }
+  if (first_attention_lane_ && !second_attention_lane_ && !second_attention_lane_empty_) {
+    const auto first_attention_lane = first_attention_lane_.value();
+    // remove first_attention_area_ and non-straight lanelets from attention_non_preceding
+    lanelet::ConstLanelets attention_non_preceding_ex_first;
+    lanelet::ConstLanelets sibling_first_attention_lanelets;
+    for (const auto & previous : routing_graph_ptr->previous(first_attention_lane)) {
+      for (const auto & following : routing_graph_ptr->following(previous)) {
+        sibling_first_attention_lanelets.push_back(following);
+      }
+    }
+    for (const auto & ll : attention_non_preceding_) {
+      // the sibling lanelets of first_attention_lanelet are ruled out
+      if (lanelet::utils::contains(sibling_first_attention_lanelets, ll)) {
+        continue;
+      }
+      if (std::string(ll.attributeOr("turn_direction", "else")).compare("straight") == 0) {
+        attention_non_preceding_ex_first.push_back(ll);
+      }
+    }
+    if (attention_non_preceding_ex_first.empty()) {
+      second_attention_lane_empty_ = true;
+    }
+    const auto attention_non_preceding_ex_first_area =
+      util::getPolygon3dFromLanelets(attention_non_preceding_ex_first);
+    const auto second = util::getFirstPointInsidePolygonsByFootprint(
+      attention_non_preceding_ex_first_area, interpolated_path_info, footprint, vehicle_length);
+    if (second) {
+      second_attention_lane_ = attention_non_preceding_ex_first.at(second.value().second);
+      second_attention_area_ = second_attention_lane_.value().polygon3d();
+    }
+  }
+}
+}  // namespace behavior_velocity_planner::intersection