feat(trajectory_safety_filter): boundary departure prevention filter with diagnostics

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

@@ -147,7 +147,7 @@ struct Param
   double critical_departure_on_time_buffer_s{0.15};
   double critical_departure_off_time_buffer_s{0.15};
   AbnormalitiesConfigs abnormality_configs;
-  std::vector<std::string> boundary_types_to_detect;
+  std::vector<std::string> boundary_types_to_detect{"road_border"};
   std::vector<FootprintType> footprint_types_to_check;
 
   template <typename ConfigType>

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

@@ -257,17 +257,13 @@ UncrossableBoundRTree build_uncrossable_boundaries_rtree(
  * If it is within the segment's bounds, it returns the projection point. If it's outside,
  * an error is returned indicating the relative position.
  *
- * The `swap_points` flag controls the order of the returned pair, useful when projection
- * direction matters (e.g., from ego to boundary vs. from boundary to ego).
- *
  * @param p            The point to be projected.
  * @param segment      The line segment to project onto.
- * @param swap_points  Whether to swap the order of the original and projected point in the result.
  * @return A tuple containing the original point, projection point, and the distance between them,
  *         or an error message if the point lies outside the segment.
  */
-tl::expected<std::tuple<Point2d, Point2d, double>, std::string> point_to_segment_projection(
-  const Point2d & p, const Segment2d & segment, const bool swap_points = false);
+tl::expected<std::pair<Point2d, double>, std::string> point_to_segment_projection(
+  const Point2d & p, const Segment2d & segment);
 
 /**
  * @brief Computes the nearest projection between two segments, used to assess lateral distance.

common/autoware_boundary_departure_checker/src/footprint_generator/steering_footprint.cpp

@@ -71,9 +71,8 @@ Footprints SteeringFootprintGenerator::generate(
   vehicle_footprints.reserve(pred_traj.size());
   const auto local_vehicle_footprint = info.createFootprint(0.0, 0.0, 0.0, 0.0, 0.0, true);
 
-  vehicle_footprints.push_back(
-    autoware_utils_geometry::transform_vector(
-      local_vehicle_footprint, autoware_utils_geometry::pose2transform(pred_traj.front().pose)));
+  vehicle_footprints.push_back(autoware_utils_geometry::transform_vector(
+    local_vehicle_footprint, autoware_utils_geometry::pose2transform(pred_traj.front().pose)));
 
   if (pred_traj.size() < 2) return vehicle_footprints;
 
@@ -89,9 +88,8 @@ Footprints SteeringFootprintGenerator::generate(
       if (!delayed_index) delayed_index = i;
       continue;
     }
-    vehicle_footprints.push_back(
-      autoware_utils_geometry::transform_vector(
-        local_vehicle_footprint, autoware_utils_geometry::pose2transform(pred_traj[i].pose)));
+    vehicle_footprints.push_back(autoware_utils_geometry::transform_vector(
+      local_vehicle_footprint, autoware_utils_geometry::pose2transform(pred_traj[i].pose)));
     const auto dt = rclcpp::Duration(pred_traj[i + 1].time_from_start) -
                     rclcpp::Duration(pred_traj[i].time_from_start);
     t += dt.seconds();
@@ -109,9 +107,8 @@ Footprints SteeringFootprintGenerator::generate(
     const auto v = (prev_p.longitudinal_velocity_mps + curr_p.longitudinal_velocity_mps) * 0.5;
     // simulate the ego vehicle motion
     pose = update_pose_with_bicycle_model(pose, steering_angle, v, dt.seconds(), info.wheel_base_m);
-    vehicle_footprints.push_back(
-      autoware_utils_geometry::transform_vector(
-        local_vehicle_footprint, autoware_utils_geometry::pose2transform(pose)));
+    vehicle_footprints.push_back(autoware_utils_geometry::transform_vector(
+      local_vehicle_footprint, autoware_utils_geometry::pose2transform(pose)));
 
     // update the simulated steering angle
     const auto steering_change = original_steering_changes[i - *delayed_index] * config.factor +

common/autoware_boundary_departure_checker/src/uncrossable_boundary_departure_checker.cpp

@@ -297,6 +297,12 @@ tl::expected<DepartureData, std::string> UncrossableBoundaryDepartureChecker::ge
     trimmed_pred_traj, *vehicle_info_ptr_, curr_pose_with_cov, param_);
   const auto & footprint_type_order = footprint_manager_->get_footprint_type_order();
 
+  if (
+    generated_footprints.empty() || footprint_type_order.empty() ||
+    generated_footprints.at(footprint_type_order.front()).empty()) {
+    return tl::make_unexpected("Failed to generate any footprints for abnormalities");
+  }
+
   if (generated_footprints.empty() || footprint_type_order.empty()) {
     return tl::make_unexpected("Failed to generate any footprints");
   }

common/autoware_boundary_departure_checker/src/utils.cpp

@@ -189,9 +189,8 @@ std::vector<LinearRing2d> createVehicleFootprints(
   // Create vehicle footprint on each TrajectoryPoint
   std::vector<LinearRing2d> vehicle_footprints;
   for (const auto & p : trajectory) {
-    vehicle_footprints.push_back(
-      autoware_utils_geometry::transform_vector(
-        local_vehicle_footprint, autoware_utils_geometry::pose2transform(p.pose)));
+    vehicle_footprints.push_back(autoware_utils_geometry::transform_vector(
+      local_vehicle_footprint, autoware_utils_geometry::pose2transform(p.pose)));
   }
 
   return vehicle_footprints;
@@ -207,9 +206,8 @@ std::vector<LinearRing2d> createVehicleFootprints(
   // Create vehicle footprint on each Path point
   std::vector<LinearRing2d> vehicle_footprints;
   for (const auto & p : path.points) {
-    vehicle_footprints.push_back(
-      autoware_utils_geometry::transform_vector(
-        local_vehicle_footprint, autoware_utils_geometry::pose2transform(p.point.pose)));
+    vehicle_footprints.push_back(autoware_utils_geometry::transform_vector(
+      local_vehicle_footprint, autoware_utils_geometry::pose2transform(p.point.pose)));
   }
 
   return vehicle_footprints;
@@ -420,35 +418,25 @@ UncrossableBoundRTree build_uncrossable_boundaries_rtree(
   return {segments.begin(), segments.end()};
 }
 
-tl::expected<std::tuple<Point2d, Point2d, double>, std::string> point_to_segment_projection(
-  const Point2d & p, const Segment2d & segment, const bool swap_points)
+tl::expected<std::pair<Point2d, double>, std::string> point_to_segment_projection(
+  const Point2d & p, const Segment2d & segment)
 {
   const auto & p1 = segment.first;
   const auto & p2 = segment.second;
 
   const Point2d p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()};
   const Point2d p_vec = {p.x() - p1.x(), p.y() - p1.y()};
 
-  const auto result = [&swap_points](
-                        const Point2d & orig,
-                        const Point2d & proj) -> std::tuple<Point2d, Point2d, double> {
-    return swap_points ? std::make_tuple(proj, orig, boost::geometry::distance(proj, orig))
-                       : std::make_tuple(orig, proj, boost::geometry::distance(orig, proj));
-  };
-
   const auto c1 = boost::geometry::dot_product(p_vec, p2_vec);
   if (c1 < 0.0) return tl::make_unexpected("Point before segment start");
-  if (c1 == 0.0) return result(p, p1);
 
   const auto c2 = boost::geometry::dot_product(p2_vec, p2_vec);
   if (c1 > c2) return tl::make_unexpected("Point after segment end");
 
-  if (c1 == c2) return result(p, p2);
-
   const auto projection = p1 + (p2_vec * c1 / c2);
   const auto projection_point = Point2d{projection.x(), projection.y()};
 
-  return result(p, projection_point);
+  return std::make_pair(projection_point, boost::geometry::distance(p, projection_point));
 }
 
 tl::expected<ProjectionToBound, std::string> segment_to_segment_nearest_projection(
@@ -467,29 +455,28 @@ tl::expected<ProjectionToBound, std::string> segment_to_segment_nearest_projecti
 
   ProjectionsToBound projections;
   projections.reserve(4);
-  constexpr bool swap_result = true;
-  if (const auto projection_opt = point_to_segment_projection(ego_f, lane_seg, swap_result)) {
-    const auto & [pt_ego, pt_lane, dist] = *projection_opt;
-    const auto lon_offset = boost::geometry::distance(pt_ego, ego_f);
-    projections.emplace_back(pt_ego, pt_lane, lane_seg, dist, lon_offset, ego_sides_idx);
+  if (const auto projection_opt = point_to_segment_projection(ego_f, lane_seg)) {
+    const auto & [proj, dist] = *projection_opt;
+    constexpr auto lon_offset = 0.0;
+    projections.emplace_back(ego_f, proj, lane_seg, dist, lon_offset, ego_sides_idx);
   }
 
-  if (const auto projection_opt = point_to_segment_projection(ego_b, lane_seg, swap_result)) {
-    const auto & [pt_ego, pt_lane, dist] = *projection_opt;
-    const auto lon_offset = boost::geometry::distance(pt_ego, ego_f);
-    projections.emplace_back(pt_ego, pt_lane, lane_seg, dist, lon_offset, ego_sides_idx);
+  if (const auto projection_opt = point_to_segment_projection(ego_b, lane_seg)) {
+    const auto & [proj, dist] = *projection_opt;
+    const auto lon_offset = boost::geometry::distance(ego_b, ego_f);
+    projections.emplace_back(ego_b, proj, lane_seg, dist, lon_offset, ego_sides_idx);
   }
 
-  if (const auto projection_opt = point_to_segment_projection(lane_pt1, ego_seg, !swap_result)) {
-    const auto & [pt_ego, pt_lane, dist] = *projection_opt;
-    const auto lon_offset = boost::geometry::distance(pt_ego, ego_f);
-    projections.emplace_back(pt_ego, pt_lane, lane_seg, dist, lon_offset, ego_sides_idx);
+  if (const auto projection_opt = point_to_segment_projection(lane_pt1, ego_seg)) {
+    const auto & [proj, dist] = *projection_opt;
+    const auto lon_offset = boost::geometry::distance(proj, ego_f);
+    projections.emplace_back(proj, lane_pt1, lane_seg, dist, lon_offset, ego_sides_idx);
   }
 
-  if (const auto projection_opt = point_to_segment_projection(lane_pt2, ego_seg, !swap_result)) {
-    const auto & [pt_ego, pt_lane, dist] = *projection_opt;
-    const auto lon_offset = boost::geometry::distance(pt_ego, ego_f);
-    projections.emplace_back(pt_ego, pt_lane, lane_seg, dist, lon_offset, ego_sides_idx);
+  if (const auto projection_opt = point_to_segment_projection(lane_pt2, ego_seg)) {
+    const auto & [proj, dist] = *projection_opt;
+    const auto lon_offset = boost::geometry::distance(proj, ego_f);
+    projections.emplace_back(proj, lane_pt2, lane_seg, dist, lon_offset, ego_sides_idx);
   }
 
   if (projections.empty())

planning/autoware_trajectory_safety_filter/CMakeLists.txt

@@ -33,6 +33,7 @@ rclcpp_components_register_node(${PROJECT_NAME}
 ament_auto_add_library(autoware_trajectory_safety_filter_plugins SHARED
   src/filters/out_of_lane_filter.cpp
   src/filters/collision_filter.cpp
+  src/filters/uncrossable_boundary_departure_filter.cpp
 )
 
 target_link_libraries(autoware_trajectory_safety_filter_plugins

planning/autoware_trajectory_safety_filter/config/trajectory_safety_filter.param.yaml

@@ -1,6 +1,6 @@
 /**:
   ros__parameters:
-    filter_names: ["autoware::trajectory_safety_filter::plugin::OutOfLaneFilter", "autoware::trajectory_safety_filter::plugin::CollisionFilter"]
+    filter_names: ["autoware::trajectory_safety_filter::plugin::OutOfLaneFilter", "autoware::trajectory_safety_filter::plugin::CollisionFilter", "autoware::trajectory_safety_filter::plugin::UncrossableBoundaryDepartureFilter"]
     # available filter: "autoware::trajectory_safety_filter::plugin::OutOfLaneFilter", "autoware::trajectory_safety_filter::plugin::CollisionFilter"
 
     out_of_lane:

planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filters/collision_filter.hpp

@@ -44,7 +44,8 @@ class CollisionFilter : public SafetyFilterInterface
 public:
   CollisionFilter() : SafetyFilterInterface("CollisionFilter") {}
 
-  bool is_feasible(const TrajectoryPoints & traj_points, const FilterContext & context) override;
+  tl::expected<void, std::string> is_feasible(
+    const TrajectoryPoints & traj_points, const FilterContext & context) final;
 
   void set_parameters(const std::unordered_map<std::string, std::any> & params) override;
 

planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filters/out_of_lane_filter.hpp

@@ -44,7 +44,8 @@ class OutOfLaneFilter : public SafetyFilterInterface
 public:
   OutOfLaneFilter();
 
-  bool is_feasible(const TrajectoryPoints & traj_points, const FilterContext & context) override;
+  tl::expected<void, std::string> is_feasible(
+    const TrajectoryPoints & traj_points, const FilterContext & context) final;
 
   void set_parameters(const std::unordered_map<std::string, std::any> & params) override;
 

planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filters/uncrossable_boundary_departure_filter.hpp

@@ -0,0 +1,66 @@
+// Copyright 2026 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 AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTERS__UNCROSSABLE_BOUNDARY_DEPARTURE_FILTER_HPP_
+#define AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTERS__UNCROSSABLE_BOUNDARY_DEPARTURE_FILTER_HPP_
+
+#include "autoware/trajectory_safety_filter/safety_filter_interface.hpp"
+
+#include <autoware/boundary_departure_checker/uncrossable_boundary_departure_checker.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <any>
+#include <memory>
+#include <string>
+#include <unordered_map>
+namespace autoware::trajectory_safety_filter::plugin
+{
+class UncrossableBoundaryDepartureFilter : public SafetyFilterInterface
+{
+public:
+  UncrossableBoundaryDepartureFilter() : SafetyFilterInterface("UncrossableBoundaryDepartureFilter")
+  {
+  }
+
+  tl::expected<void, std::string> is_feasible(
+    const TrajectoryPoints & traj_points, const FilterContext & context) final;
+  void set_parameters(
+    [[maybe_unused]] const std::unordered_map<std::string, std::any> & params) final {};
+
+private:
+  std::unique_ptr<autoware::boundary_departure_checker::UncrossableBoundaryDepartureChecker>
+    uncrossable_boundary_departure_checker_ptr_;
+  rclcpp::Logger log_ = rclcpp::get_logger(name_);
+  std::shared_ptr<rclcpp::Clock> clock_ = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
+
+  [[nodiscard]] std::optional<std::string> is_invalid_input(
+    const TrajectoryPoints & traj_points, const FilterContext & context) const;
+
+  [[nodiscard]] bool is_debug_mode() const final { return true; }
+
+  template <typename... Args>
+  void warn_throttle(const char * fmt)
+  {
+    RCLCPP_WARN_THROTTLE(log_, *clock_, 5000, fmt);
+  }
+
+  template <typename... Args>
+  void warn_throttle(const char * fmt, Args... args)
+  {
+    RCLCPP_WARN_THROTTLE(log_, *clock_, 5000, fmt, args...);
+  }
+};
+}  // namespace autoware::trajectory_safety_filter::plugin
+
+#endif  // AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTERS__UNCROSSABLE_BOUNDARY_DEPARTURE_FILTER_HPP_

planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/safety_filter_interface.hpp

@@ -18,6 +18,7 @@
 #include "autoware/trajectory_safety_filter/filter_context.hpp"
 
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
+#include <tl_expected/expected.hpp>
 
 #include <autoware_planning_msgs/msg/trajectory_point.hpp>
 
@@ -38,18 +39,16 @@ class SafetyFilterInterface
 {
 public:
   explicit SafetyFilterInterface(std::string name) : name_(std::move(name)) {}
-  SafetyFilterInterface(std::string name, const VehicleInfo & vehicle_info)
-  : name_(std::move(name)), vehicle_info_ptr_(std::make_shared<VehicleInfo>(vehicle_info))
-  {
-  }
+
   virtual ~SafetyFilterInterface() = default;
   SafetyFilterInterface(const SafetyFilterInterface &) = delete;
   SafetyFilterInterface & operator=(const SafetyFilterInterface &) = delete;
   SafetyFilterInterface(SafetyFilterInterface &&) = delete;
   SafetyFilterInterface & operator=(SafetyFilterInterface &&) = delete;
 
   // Main filter method with context for plugin-specific data
-  virtual bool is_feasible(const TrajectoryPoints & traj_points, const FilterContext & context) = 0;
+  virtual tl::expected<void, std::string> is_feasible(
+    const TrajectoryPoints & traj_points, const FilterContext & context) = 0;
 
   // Set parameters directly (for testing and runtime configuration)
   virtual void set_parameters(const std::unordered_map<std::string, std::any> & params) = 0;
@@ -60,7 +59,9 @@ class SafetyFilterInterface
     vehicle_info_ptr_ = std::make_shared<VehicleInfo>(vehicle_info);
   }
 
-  std::string get_name() const { return name_; }
+  [[nodiscard]] std::string get_name() const { return name_; }
+
+  [[nodiscard]] virtual bool is_debug_mode() const { return false; }
 
 protected:
   std::string name_;

planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/trajectory_safety_filter_node.hpp

@@ -20,6 +20,7 @@
 #include <autoware_lanelet2_extension/utility/message_conversion.hpp>
 #include <autoware_trajectory_safety_filter_param.hpp>
 #include <autoware_utils_debug/time_keeper.hpp>
+#include <autoware_utils_diagnostics/diagnostics_interface.hpp>
 #include <autoware_utils_rclcpp/polling_subscriber.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 #include <pluginlib/class_loader.hpp>
@@ -45,6 +46,7 @@ using autoware_internal_planning_msgs::msg::CandidateTrajectory;
 using autoware_map_msgs::msg::LaneletMapBin;
 using autoware_perception_msgs::msg::PredictedObjects;
 using autoware_planning_msgs::msg::TrajectoryPoint;
+using autoware_utils_diagnostics::DiagnosticsInterface;
 using geometry_msgs::msg::AccelWithCovarianceStamped;
 using nav_msgs::msg::Odometry;
 
@@ -68,6 +70,7 @@ class TrajectorySafetyFilter : public rclcpp::Node
    * @param name Metric plugin name to unload
    */
   void unload_metric(const std::string & name);
+  void update_diagnostic(const CandidateTrajectories & filtered_trajectories);
 
   std::unique_ptr<safety_filter::ParamListener> listener_;
 
@@ -92,6 +95,8 @@ class TrajectorySafetyFilter : public rclcpp::Node
   pluginlib::ClassLoader<plugin::SafetyFilterInterface> plugin_loader_;
   std::vector<std::shared_ptr<plugin::SafetyFilterInterface>> plugins_;
   autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
+  std::unique_ptr<DiagnosticsInterface> diagnostics_interface_ptr_ =
+    std::make_unique<DiagnosticsInterface>(this, "trajectory_safety_filter");
 };
 
 }  // namespace autoware::trajectory_safety_filter

planning/autoware_trajectory_safety_filter/launch/trajectory_safety_filter.launch.xml

@@ -3,7 +3,7 @@
   <arg name="trajectory_safety_filter_param_path" default="$(find-pkg-share autoware_trajectory_safety_filter)/config/trajectory_safety_filter.param.yaml"/>
   <arg name="input_trajectories" default="~/input/trajectories"/>
   <arg name="output_trajectories" default="~/output/trajectories"/>
-  <arg name="input_vector_map" default="~/input/vector_map"/>
+  <arg name="input_vector_map" default="~/input/lanelet2_map"/>
   <arg name="input_odometry" default="~/input/odometry"/>
   <arg name="input_objects" default="~/input/objects"/>
   <arg name="input_acceleration" default="~/input/acceleration"/>

planning/autoware_trajectory_safety_filter/package.xml

@@ -33,6 +33,7 @@
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>tl_expected</depend>
 
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>

planning/autoware_trajectory_safety_filter/plugins.xml

@@ -12,4 +12,10 @@
       Filters trajectories based on Time To Collision (TTC) with predicted objects
     </description>
   </class>
+  <class type="autoware::trajectory_safety_filter::plugin::UncrossableBoundaryDepartureFilter"
+         base_class_type="autoware::trajectory_safety_filter::plugin::SafetyFilterInterface">
+    <description>
+      Filters trajectories that cross uncrossable boundaries
+    </description>
+  </class>
 </library>