fix(trajectory_safety_filter, boundary_departure): cherry-pick merged commit to e2e branch

common/autoware_boundary_departure_checker/CMakeLists.txt

@@ -4,20 +4,45 @@ project(autoware_boundary_departure_checker)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-ament_auto_add_library(${PROJECT_NAME} SHARED
-  src/boundary_departure_checker.cpp
-  src/boundary_departure_checker_abnormality.cpp
-  src/utils.cpp
+ament_auto_add_library(boundary_departure_checker_utils SHARED
   src/conversion.cpp
   src/steering_abnormality_utils.cpp
+  src/utils.cpp
+  src/debug.cpp
+  src/footprint_generator/normal_footprint.cpp
+  src/footprint_generator/localization_footprint.cpp
+  src/footprint_generator/longitudinal_footprint.cpp
+  src/footprint_generator/steering_footprint.cpp
+  src/footprint_generator/footprint_manager.cpp
+)
+
+ament_auto_add_library(uncrossable_boundary_departure_checker SHARED
+  src/uncrossable_boundary_departure_checker.cpp
+)
+
+target_link_libraries(uncrossable_boundary_departure_checker
+  boundary_departure_checker_utils
+)
+
+ament_auto_add_library(boundary_departure_checker SHARED
+  src/boundary_departure_checker.cpp
+)
+
+target_link_libraries(boundary_departure_checker
+  boundary_departure_checker_utils
 )
 
 if(BUILD_TESTING)
   file(GLOB_RECURSE TEST_SOURCES test/*.cpp)
   ament_add_gtest(test_${PROJECT_NAME}
     ${TEST_SOURCES}
   )
-target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME})
+
+  target_link_libraries(test_${PROJECT_NAME}
+    boundary_departure_checker_utils
+    boundary_departure_checker
+    uncrossable_boundary_departure_checker
+  )
 endif()
 
 ament_auto_package()

common/autoware_boundary_departure_checker/include/autoware/boundary_departure_checker/boundary_departure_checker.hpp

@@ -17,7 +17,7 @@
 
 #include "autoware/boundary_departure_checker/parameters.hpp"
 
-#include <autoware_utils/system/time_keeper.hpp>
+#include <autoware_utils_debug/time_keeper.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 #include <rosidl_runtime_cpp/message_initialization.hpp>
 #include <tl_expected/expected.hpp>
@@ -38,9 +38,9 @@
 #include <lanelet2_core/geometry/LineString.h>
 #include <lanelet2_core/geometry/Polygon.h>
 
-#include <map>
 #include <memory>
 #include <string>
+#include <unordered_set>
 #include <utility>
 #include <vector>
 
@@ -52,16 +52,16 @@ class BoundaryDepartureChecker
 {
 public:
   explicit BoundaryDepartureChecker(
-    std::shared_ptr<autoware_utils::TimeKeeper> time_keeper =
-      std::make_shared<autoware_utils::TimeKeeper>())
+    std::shared_ptr<autoware_utils_debug::TimeKeeper> time_keeper =
+      std::make_shared<autoware_utils_debug::TimeKeeper>())
   : time_keeper_(std::move(time_keeper))
   {
   }
 
   BoundaryDepartureChecker(
     Param param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info,
-    std::shared_ptr<autoware_utils::TimeKeeper> time_keeper =
-      std::make_shared<autoware_utils::TimeKeeper>())
+    std::shared_ptr<autoware_utils_debug::TimeKeeper> time_keeper =
+      std::make_shared<autoware_utils_debug::TimeKeeper>())
   : param_(std::move(param)),
     vehicle_info_ptr_(std::make_shared<autoware::vehicle_info_utils::VehicleInfo>(vehicle_info)),
     time_keeper_(std::move(time_keeper))
@@ -70,145 +70,41 @@ class BoundaryDepartureChecker
   Output update(const Input & input);
 
   void setParam(const Param & param) { param_ = param; }
-  BoundaryDepartureChecker(
-    lanelet::LaneletMapPtr lanelet_map_ptr, const VehicleInfo & vehicle_info,
-    const Param & param = Param{},
-    std::shared_ptr<autoware_utils::TimeKeeper> time_keeper =
-      std::make_shared<autoware_utils::TimeKeeper>());
 
   bool checkPathWillLeaveLane(
     const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const;
 
   std::vector<std::pair<double, lanelet::Lanelet>> getLaneletsFromPath(
     const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
 
-  std::optional<autoware_utils::Polygon2d> getFusedLaneletPolygonForPath(
+  std::optional<autoware_utils_geometry::Polygon2d> getFusedLaneletPolygonForPath(
     const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
 
   bool updateFusedLaneletPolygonForPath(
     const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path,
     std::vector<lanelet::Id> & fused_lanelets_id,
-    std::optional<autoware_utils::Polygon2d> & fused_lanelets_polygon) const;
+    std::optional<autoware_utils_geometry::Polygon2d> & fused_lanelets_polygon) const;
 
   bool checkPathWillLeaveLane(
     const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
 
   bool checkPathWillLeaveLane(
     const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path,
     std::vector<lanelet::Id> & fused_lanelets_id,
-    std::optional<autoware_utils::Polygon2d> & fused_lanelets_polygon) const;
+    std::optional<autoware_utils_geometry::Polygon2d> & fused_lanelets_polygon) const;
 
   PathWithLaneId cropPointsOutsideOfLanes(
     const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path,
     const size_t end_index, std::vector<lanelet::Id> & fused_lanelets_id,
-    std::optional<autoware_utils::Polygon2d> & fused_lanelets_polygon);
+    std::optional<autoware_utils_geometry::Polygon2d> & fused_lanelets_polygon);
 
   static bool isOutOfLane(
     const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint);
 
-  // ==== abnormalities ===
-  /**
-   * @brief Build an R-tree of uncrossable boundaries (e.g., road_border) from a lanelet map.
-   *
-   * Filters the map's line strings by type and constructs a spatial index used to detect boundary
-   * violations.
-   *
-   * @param lanelet_map_ptr Shared pointer to the lanelet map.
-   * @return Constructed R-tree or an error message if map or parameters are invalid.
-   */
-  tl::expected<UncrossableBoundRTree, std::string> build_uncrossable_boundaries_tree(
-    const lanelet::LaneletMapPtr & lanelet_map_ptr);
-
-  /**
-   * @brief Generate data structure with embedded abnormality information based on the
-   * predicted trajectory and current ego state.
-   *
-   * This function creates extended ego footprints for various abnormality types (e.g.,
-   * localization, steering) and computes their corresponding closest boundary projections and
-   * segments.
-   *
-   * @param predicted_traj         Ego's predicted trajectory (from MPC or trajectory follower).
-   * @param aw_raw_traj            Raw Autoware trajectory to extract underlying path information.
-   * @param curr_pose_with_cov     Ego pose with covariance for uncertainty margin calculation.
-   * @param current_steering       Current steering angle report.
-   * @return AbnormalitiesData containing footprints, their left/right sides, and projections to
-   * boundaries. Returns an error message string on failure.
-   */
-  tl::expected<AbnormalitiesData, std::string> get_abnormalities_data(
-    const TrajectoryPoints & predicted_traj,
-    const trajectory::Trajectory<TrajectoryPoint> & aw_raw_traj,
-    const geometry_msgs::msg::PoseWithCovariance & curr_pose_with_cov,
-    const SteeringReport & current_steering);
-
-  /**
-   * @brief Find closest uncrossable boundary segments for the left and right sides of the ego
-   * vehicle.
-   *
-   * Queries an R-tree to find the nearest segments to the ego's left and right footprints,
-   * ensuring no duplicate segments are recorded for a given lanelet line string.
-   *
-   * @param ego_sides_from_footprints Left and right polygonal side representations of ego
-   * footprints.
-   * @return BoundarySideWithIdx containing nearest segments on each side or error string if
-   * prerequisites fail.
-   */
-  tl::expected<BoundarySideWithIdx, std::string> get_boundary_segments_from_side(
-    const EgoSides & ego_sides_from_footprints);
-
-  /**
-   * @brief Select the closest projections to road boundaries for a specific side.
-   *
-   * Evaluates multiple abnormality-aware projections (e.g., NORMAL, LOCALIZATION) for each
-   * trajectory index, and selects the best candidate based on lateral distance and classification
-   * logic (CRITICAL/NEAR).
-   *
-   * @param projections_to_bound Abnormality-aware projections to boundaries.
-   * @param side_key             Side to process (left or right).
-   * @return Vector of closest projections with departure classification, or an error message on
-   * failure.
-   */
-  tl::expected<std::vector<ClosestProjectionToBound>, std::string>
-  get_closest_projections_to_boundaries_side(
-    const Abnormalities<ProjectionsToBound> & projections_to_bound, const double min_braking_dist,
-    const double max_braking_dist, const SideKey side_key);
-
-  /**
-   * @brief Select the closest projections to boundaries for both sides based on all abnormality
-   * types.
-   *
-   * Invokes `get_closest_projections_to_boundaries_side` for each side and updates the departure
-   * type based on braking feasibility (APPROACHING_DEPARTURE) using trajectory spacing and braking
-   * model.
-   *
-   * @param projections_to_bound Abnormality-wise projections to boundaries.
-   * @return ClosestProjectionsToBound structure containing selected points for both sides, or error
-   * string.
-   */
-  tl::expected<ClosestProjectionsToBound, std::string> get_closest_projections_to_boundaries(
-    const Abnormalities<ProjectionsToBound> & projections_to_bound, const double curr_vel,
-    const double curr_acc);
-
-  /**
-   * @brief Generate filtered departure points for both left and right sides.
-   *
-   * Converts closest projections into structured `DeparturePoint`s for each side,
-   * filtering based on hysteresis and distance, and grouping results using side keys.
-   *
-   * @param projections_to_bound Closest projections to road boundaries for each side.
-   * @param pred_traj_idx_to_ref_traj_lon_dist mapping from an index of the predicted trajectory to
-   * the corresponding arc length on the reference trajectory
-   * @return Side-keyed container of filtered departure points.
-   */
-  Side<DeparturePoints> get_departure_points(
-    const ClosestProjectionsToBound & projections_to_bound,
-    const std::vector<double> & pred_traj_idx_to_ref_traj_lon_dist);
-  // === Abnormalities
-
 private:
   Param param_;
   lanelet::LaneletMapPtr lanelet_map_ptr_;
   std::shared_ptr<VehicleInfo> vehicle_info_ptr_;
-  std::unique_ptr<UncrossableBoundRTree> uncrossable_boundaries_rtree_ptr_;
 
   bool willLeaveLane(
     const lanelet::ConstLanelets & candidate_lanelets,
@@ -223,12 +119,9 @@ class BoundaryDepartureChecker
     const std::vector<LinearRing2d> & vehicle_footprints,
     const SegmentRtree & uncrossable_segments) const;
 
-  autoware_utils::Polygon2d toPolygon2D(const lanelet::BasicPolygon2d & poly) const;
-
-  mutable std::shared_ptr<autoware_utils::TimeKeeper> time_keeper_;
+  autoware_utils_geometry::Polygon2d toPolygon2D(const lanelet::BasicPolygon2d & poly) const;
 
-  Footprint get_ego_footprints(
-    const AbnormalityType abnormality_type, const FootprintMargin uncertainty_fp_margin);
+  mutable std::shared_ptr<autoware_utils_debug::TimeKeeper> time_keeper_;
 };
 }  // namespace autoware::boundary_departure_checker
 

common/autoware_boundary_departure_checker/include/autoware/boundary_departure_checker/conversion.hpp

@@ -35,7 +35,7 @@ namespace autoware::boundary_departure_checker::utils
  */
 template <
   typename E,
-  std::enable_if_t<std::is_same_v<E, DepartureType> || std::is_same_v<E, AbnormalityType>, int> = 0>
+  std::enable_if_t<std::is_same_v<E, DepartureType> || std::is_same_v<E, FootprintType>, int> = 0>
 std::string to_enum_str(const E & value, const bool to_lower_case = true)
 {
   auto value_str = magic_enum::enum_name(value);
@@ -77,6 +77,14 @@ Point2d to_point_2d(const Eigen::Matrix<double, 3, 1> & ll_pt);
 Segment2d to_segment_2d(
   const Eigen::Matrix<double, 3, 1> & ll_pt1, const Eigen::Matrix<double, 3, 1> & ll_pt2);
 
+/**
+ * @brief Converts a 3D segment into its 2D representation by discarding the Z-coordinates.
+ *
+ * @param segment The input 3D segment to convert.
+ * @return A new 2D segment with the Z-coordinates of the original segment's endpoints removed.
+ */
+Segment2d to_segment_2d(const Segment3d & segment);
+
 /**
  * @brief Convert a 2D point and a z value into a 3D ROS geometry_msgs Point.
  * @param point A 2D point.