feat: add safety gate for generator-selector framework by go-sakayori · Pull Request #11404 · autowarefoundation/autoware_universe

go-sakayori · 2025-09-24T03:27:24Z

Description

This PR completes the implementation of autoware_trajectory_safety_filter for the generator-selector framework. It validates candidate trajectories from the trajectory optimizer before they are sent to the vehicle control module. The filter checks for potential safety violations such as out-of-lane conditions and collision risks with
predicted objects, ensuring that only safe trajectories are executed by the autonomous vehicle.

How was this PR tested?

a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml
index ad7649c3bf..a3665986bf 100644
--- a/launch/tier4_planning_launch/launch/planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/planning.launch.xml
@@ -35,12 +35,46 @@
       </include>
     </group>
 
+    <!-- trajectory generator -->
+    <group>
+      <push-ros-namespace namespace="trajectory_generator"/>
+      <include file="$(find-pkg-share autoware_diffusion_planner)/launch/diffusion_planner.launch.xml">
+        <arg name="input_odometry" value="/localization/kinematic_state"/>
+        <arg name="input_acceleration" value="/localization/acceleration"/>
+        <arg name="input_route" value="/planning/mission_planning/route"/>
+        <arg name="input_traffic_signals" value="/perception/traffic_light_recognition/traffic_signals"/>
+        <arg name="input_tracked_objects" value="/perception/object_recognition/tracking/objects"/>
+        <arg name="input_vector_map" value="/map/vector_map"/>
+        <!-- <arg name="output_trajectory" value="/planning/trajectory"/> -->
+        <arg name="output_trajectories" value="/planning/generator/diffusion_planner/candidate_trajectories"/>
+        <arg name="output_turn_indicators" value="/planning/turn_indicators_cmd"/>
+      </include>
+      <include file="$(find-pkg-share autoware_trajectory_optimizer)/launch/trajectory_optimizer.launch.xml">
+        <arg name="input_trajectories" value="/planning/generator/diffusion_planner/candidate_trajectories"/>
+        <arg name="output_traj" value="/planning/trajectory"/>
+        <arg name="output_trajectories" value="/planning/generator/trajectory_optimizer/candidate_trajectories"/>
+      </include>
+    </group>

+    <!-- trajectory safety filter -->
+    <group>
+      <push-ros-namespace namespace="trajectory_safety_filter"/>
+      <include file="$(find-pkg-share autoware_trajectory_safety_filter)/launch/trajectory_safety_filter.launch.xml">
+        <arg name="input_trajectories" value="/planning/generator/trajectory_optimizer/candidate_trajectories"/>
+        <arg name="input_objects" value="/perception/object_recognition/objects"/>
+        <arg name="input_odometry" value="/localization/kinematic_state"/>
+        <arg name="input_vector_map" value="/map/vector_map"/>
+      </include>
+    </group>

Notes for reviewers

In this update, the nodes in this package will not be launched.

Interface changes

None.

Effects on system behavior

None.

Signed-off-by: Go Sakayori <gsakayori@gmail.com>

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

github-actions · 2025-09-24T03:27:38Z

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

You've checked our contribution guidelines.
Your PR follows our pull request guidelines.
All required CI checks pass before marking the PR ready for review.

Copilot

Pull Request Overview

This PR implements the trajectory safety filter component for the generator-selector framework, providing validation of candidate trajectories before they reach the vehicle control module. The safety filter checks trajectories for out-of-lane conditions and collision risks with predicted objects.

Adds new trajectory safety filter package with plugin-based architecture
Implements OutOfLaneFilter and CollisionFilter plugins for trajectory validation
Integrates with existing planning pipeline through launch configuration updates

Reviewed Changes

Copilot reviewed 15 out of 15 changed files in this pull request and generated 5 comments.

Show a summary per file

File	Description
trajectory_safety_filter_node.cpp	Main node implementation with plugin loading and trajectory filtering logic
out_of_lane_filter.cpp	Plugin that validates trajectories stay within lane boundaries
collision_filter.cpp	Plugin that checks time-to-collision with predicted objects
plugins.xml	Plugin registration configuration
parameter_struct.yaml	Parameter definitions for filter configuration
package.xml	Package dependencies and metadata
launch/trajectory_safety_filter.launch.xml	Launch file for the safety filter node
Header files	Interface definitions and class declarations
config/trajectory_safety_filter.param.yaml	Default parameter values
README.md	Documentation for the trajectory safety filter
CMakeLists.txt	Build configuration

_{Tip: Customize your code reviews with copilot-instructions.md. Create the file or learn how to get started.}

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

codecov · 2025-09-24T03:41:56Z

Codecov Report

❌ Patch coverage is 0% with 258 lines in your changes missing coverage. Please review.
✅ Project coverage is 20.18%. Comparing base (b5ef197) to head (a72e65a).
⚠️ Report is 1 commits behind head on main.

Files with missing lines	Patch %	Lines
...ory_safety_filter/src/filters/collision_filter.cpp	0.00%	120 Missing ⚠️
...afety_filter/src/trajectory_safety_filter_node.cpp	0.00%	92 Missing ⚠️
...y_safety_filter/src/filters/out_of_lane_filter.cpp	0.00%	37 Missing ⚠️
...ajectory_safety_filter/safety_filter_interface.hpp	0.00%	6 Missing ⚠️
...jectory_safety_filter/filters/collision_filter.hpp	0.00%	2 Missing ⚠️
...ctory_safety_filter/filters/out_of_lane_filter.hpp	0.00%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main   #11404      +/-   ##
==========================================
- Coverage   20.29%   20.18%   -0.12%     
==========================================
  Files        1588     1593       +5     
  Lines      114260   114596     +336     
  Branches    39005    38129     -876     
==========================================
- Hits        23190    23132      -58     
- Misses      73084    73519     +435     
+ Partials    17986    17945      -41

Flag	Coverage Δ		*Carryforward flag
daily	`22.22% <ø> (-0.01%)`	⬇️	Carriedforward from 1f766e9
daily-cuda	`20.27% <ø> (-0.02%)`	⬇️	Carriedforward from 1f766e9
differential	`0.00% <0.00%> (?)`
total-cuda	`20.27% <ø> (-0.02%)`	⬇️	Carriedforward from 1f766e9

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

danielsanchezaran

Thank you for this PR, I believe the general code is solid, however, I worry about the performance when we have big maps and many trajectories and many objects. I would like to get some measurements of time using the time_keeper feature. Likewise, I believe it might be better to pre-compute object trajectories so this task does not need to be repeated ( since TTC is currently calculated using Points, I think the performance cost is not high, but it will increase when we decide to refine the method).

danielsanchezaran · 2025-09-25T01:33:41Z

+# if(BUILD_TESTING)
+#   ament_add_ros_isolated_gmock(test_${PROJECT_NAME}
+#     test/test_util.cpp
+#   )
+
+#   target_link_libraries(test_${PROJECT_NAME}
+#     ${PROJECT_NAME}
+#   )
+
+#   ament_target_dependencies(test_${PROJECT_NAME}
+#     autoware_test_utils
+#   )
+# endif()
+


Maybe it is better to delete this is there are no tests atm?

fixed 6fc9d30

danielsanchezaran · 2025-09-25T01:34:45Z

+This node performs a feasibility check on each candidate trajectory before it enters the final ranking stage. It drops paths that are physically impossible for the ego vehicle or that create an obvious driving risk.
+


I feel this description is a little bit off, the trajectories are feasible, but dangerous no?

fixed 4b369dc

danielsanchezaran · 2025-09-25T01:47:38Z

+  for (const auto & object : objects.objects) {
+    const double ttc = time_to_collision(traj_point, duration, object);
+
+    // Check if TTC is below minimum threshold
+    if (ttc < params_.min_ttc && ttc >= 0) {
+      return true;  // Potential collision detected
+    }
+  }
+  return false;  // No collision
+}


Suggested change

for (const auto & object : objects.objects) {

const double ttc = time_to_collision(traj_point, duration, object);

// Check if TTC is below minimum threshold

if (ttc < params_.min_ttc && ttc >= 0) {

return true; // Potential collision detected

}

}

return false; // No collision

}

return std::any_of(objects.begin(),objects.end(),[&](const auto& object){

const double ttc = time_to_collision(traj_point, duration, object);

return (ttc < params_.min_ttc && ttc >= 0);

}

Fixed in 9d897b1, but eventually removed

danielsanchezaran · 2025-09-25T02:07:20Z

+float time_to_collision(
+  const TrajectoryPoint & ego_point, const rclcpp::Duration & duration,
+  const autoware_perception_msgs::msg::PredictedObject & object, const float max_ttc_value = 100.0f)
+{
+  // Find the path with highest confidence
+  const auto max_confidence_path = std::max_element(
+    object.kinematics.predicted_paths.begin(), object.kinematics.predicted_paths.end(),
+    [](const auto & a, const auto & b) { return a.confidence < b.confidence; });
+  if (max_confidence_path == object.kinematics.predicted_paths.end()) return max_ttc_value;
+
+  const auto & object_path = max_confidence_path->path;
+  if (object_path.size() < 2) {
+    if (duration.seconds() == 0.0f) {
+      TrajectoryPoint object_point;
+      object_point.pose = object.kinematics.initial_pose_with_covariance.pose;
+      object_point.longitudinal_velocity_mps =
+        object.kinematics.initial_twist_with_covariance.twist.linear.x;
+      object_point.lateral_velocity_mps =
+        object.kinematics.initial_twist_with_covariance.twist.linear.y;
+      float ttc = time_to_collision(ego_point, object_point);
+      return std::min(ttc, max_ttc_value);
+    }
+    return max_ttc_value;
+  }


For every trajectory point, we are checking the position of each object at a set interval of "time_from_start" based on the highest confidence trajectory. If there is only one input trajectory, this approach is ok, however, if you have several trajectories to evaluate, we are rapidly increasing the computations required to get the object's position. Since the object position and trajectory are independent, I suggest calculating all the future positions of every object (up to the ttc horizon time with 3 seconds as default) ONCE, and store them in memory, then you can query the position of every object at a given time.

fixed ad92866

danielsanchezaran · 2025-09-25T02:18:02Z

+  const float distance = displacement.length();
+  if (distance < eps) return 0.0f;
+
+  const auto dir = displacement.normalized();
+
+  const auto v1 = get_velocity_in_world_coordinate(point1);
+  const auto v2 = get_velocity_in_world_coordinate(point2);
+
+  const float relative_velocity = tf2::tf2Dot(dir, v1) - tf2::tf2Dot(dir, v2);
+
+  if (std::abs(relative_velocity) < eps) return std::numeric_limits<float>::infinity();
+  return distance / relative_velocity;
+}
+


Very nice. Please add somewhere in the description that the TTC is currently calculated using infinitesimal points and its not a full 2D collision check for the sake of simplicity.

fixed 4ea930f

danielsanchezaran · 2025-09-25T02:28:09Z

+  for (const auto & point : traj_points) {
+    if (rclcpp::Duration(point.time_from_start).seconds() > params_.max_check_time) {
+      break;
+    }
+    const auto nearest_lanelets = lanelet::geometry::findWithin2d(
+      context.lanelet_map->laneletLayer,
+      lanelet::BasicPoint2d(point.pose.position.x, point.pose.position.y), 0.0);
+    if (nearest_lanelets.empty()) {
+      return false;
+    }
+  }
+
+  return true;


I worry about the processing time when we have a big map and several trajectories. I suggest creating a set of polygons for each trajectory and checking if they are leaving the lane using the boundary departure checker

autoware_universe/common/autoware_boundary_departure_checker/src/boundary_departure_checker.cpp

Line 135 in 5b03ec7

bool BoundaryDepartureChecker::willLeaveLane(

Fixed 1f766e9

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

go-sakayori · 2025-09-25T05:50:31Z

@danielsanchezaran Thank you for the comment. I will leave the computational time as a future work, and would improve it once we start using the new planning framework. I have added a section in README about the future work. e8501e1

danielsanchezaran · 2025-09-25T05:50:11Z

+  const TrajectoryPoints & traj_points, const FilterContext & context)
+{
+  // Check required context data
+  if (!context.lanelet_map || !context.odometry || traj_points.empty()) {


why not add the !boundary_departure_checker_ check from below to this line?

fixed 1fc43e6

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

danielsanchezaran

Built and also tested on Psim, seems to work fine. Unit tests should be added in the future. LGTM

…ation#11404) * copy packages from new planning framework Signed-off-by: Go Sakayori <gsakayori@gmail.com> * introduce plugin Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * add/remove plugins Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * fix precommit Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * fix readme Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * fix include guard Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * fix precommit Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * remove test section in CMakeList Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * change definition in README Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * add comment for ttc calculation Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * use lambda function for check_collison function Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * calculate obstacle position only once Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * use boundary departure checker Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * add future work section to README Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * small fix for if condition Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> --------- Signed-off-by: Go Sakayori <gsakayori@gmail.com> Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> Signed-off-by: emuemuJP <k.matsumoto.0807@gmail.com>

…ation#11404) * copy packages from new planning framework Signed-off-by: Go Sakayori <gsakayori@gmail.com> * introduce plugin Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * add/remove plugins Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * fix precommit Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * fix readme Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * fix include guard Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * fix precommit Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * remove test section in CMakeList Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * change definition in README Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * add comment for ttc calculation Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * use lambda function for check_collison function Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * calculate obstacle position only once Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * use boundary departure checker Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * add future work section to README Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * small fix for if condition Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> --------- Signed-off-by: Go Sakayori <gsakayori@gmail.com> Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

…ation/autoware_universe#11404) * copy packages from new planning framework Signed-off-by: Go Sakayori <gsakayori@gmail.com> * introduce plugin Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * add/remove plugins Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * fix precommit Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * fix readme Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * fix include guard Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * fix precommit Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * remove test section in CMakeList Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * change definition in README Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * add comment for ttc calculation Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * use lambda function for check_collison function Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * calculate obstacle position only once Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * use boundary departure checker Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * add future work section to README Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * small fix for if condition Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> --------- Signed-off-by: Go Sakayori <gsakayori@gmail.com> Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

go-sakayori and others added 4 commits September 24, 2025 12:15

copy packages from new planning framework

c8f6545

Signed-off-by: Go Sakayori <gsakayori@gmail.com>

introduce plugin

8ec2532

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

add/remove plugins

a031fa9

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

fix precommit

22d1420

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

go-sakayori requested review from Copilot and danielsanchezaran September 24, 2025 03:27

github-project-automation Bot added this to Software Working Group Sep 24, 2025

github-project-automation Bot moved this to To Triage in Software Working Group Sep 24, 2025

github-actions Bot added type:documentation Creating or refining documentation. (auto-assigned) component:planning Route planning, decision-making, and navigation. (auto-assigned) labels Sep 24, 2025

Copilot AI reviewed Sep 24, 2025

View reviewed changes

fix readme

87a9d46

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

go-sakayori marked this pull request as ready for review September 24, 2025 03:34

go-sakayori added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Sep 24, 2025

go-sakayori added 2 commits September 24, 2025 12:51

fix include guard

807e349

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

fix precommit

f86e2f4

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

danielsanchezaran requested changes Sep 25, 2025

View reviewed changes

go-sakayori added 7 commits September 25, 2025 11:36

remove test section in CMakeList

6fc9d30

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

change definition in README

4b369dc

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

add comment for ttc calculation

4ea930f

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

use lambda function for check_collison function

9d897b1

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

calculate obstacle position only once

ad92866

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

use boundary departure checker

1f766e9

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

add future work section to README

e8501e1

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

danielsanchezaran reviewed Sep 25, 2025

View reviewed changes

small fix for if condition

1fc43e6

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

danielsanchezaran approved these changes Sep 25, 2025

View reviewed changes

Merge branch 'main' into feat/add_safety_gate

a72e65a

go-sakayori merged commit a13637d into autowarefoundation:main Sep 25, 2025

go-sakayori deleted the feat/add_safety_gate branch September 25, 2025 09:09

github-project-automation Bot moved this from To Triage to Done in Software Working Group Sep 25, 2025

yukkysaito mentioned this pull request Sep 29, 2025

Enable generator-selector planning framework autowarefoundation/autoware#6292

Open

11 of 13 tasks

Kotakku mentioned this pull request Feb 10, 2026

feat(modifiler, safety_filter, traffic_rule_filter, ranker, adapeter): cherry pick some modules tier4/autoware_universe#2668

Merged

		This node performs a feasibility check on each candidate trajectory before it enters the final ranking stage. It drops paths that are physically impossible for the ego vehicle or that create an obvious driving risk.

Conversation

go-sakayori commented Sep 24, 2025

Description

Related links

How was this PR tested?

Notes for reviewers

Interface changes

Effects on system behavior

Uh oh!

github-actions Bot commented Sep 24, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull Request Overview

Reviewed Changes

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

codecov Bot commented Sep 24, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

danielsanchezaran left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

go-sakayori commented Sep 25, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

danielsanchezaran left a comment

Choose a reason for hiding this comment

Uh oh!

Reviewers

Uh oh!

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

github-actions Bot commented Sep 24, 2025 •

edited

Loading

codecov Bot commented Sep 24, 2025 •

edited

Loading