feat(traffic_light_filter): add support for amber light

common/autoware_boundary_departure_checker/src/utils.cpp

@@ -689,10 +689,10 @@ double calc_judge_line_dist_with_jerk_limit(
                                  (2.0 * max_stop_jerk);
 
   if (v2 <= 0.0) {
-    const double t2 = -1.0 *
-                      (max_stop_acceleration +
-                       std::sqrt(acceleration * acceleration - 2.0 * max_stop_jerk * velocity)) /
-                      max_stop_jerk;
+    const double t2 =
+      -1.0 *
+      (acceleration + std::sqrt(acceleration * acceleration - 2.0 * max_stop_jerk * velocity)) /
+      max_stop_jerk;
     const double x2 =
       velocity * t2 + acceleration * std::pow(t2, 2) / 2.0 + max_stop_jerk * std::pow(t2, 3) / 6.0;
     return std::max(0.0, x1 + x2);

planning/autoware_trajectory_traffic_rule_filter/README.md

@@ -18,10 +18,14 @@ The package uses a plugin architecture that allows for flexible and extensible t
 
 #### TrafficLightFilter
 
-- Validates trajectory compliance with traffic signals
-- Monitors traffic light states from perception system
-- Checks if trajectory would pass through red lights
-- Allows trajectories only when traffic lights permit passage
+- Validates trajectory compliance with traffic signals.
+- Monitors traffic light states from the perception system.
+- **Limitation**: Currently, only circle light signals (RED, AMBER) are handled; arrow signals are ignored by this filter.
+- Reject trajectories when:
+  - the vehicle's footprint (front-end extension) crosses a red traffic light stop line.
+  - the vehicle's footprint crosses an amber traffic light stop line and it is determined that either:
+    1. ego can safely stop at the stop line without breaking the deceleration/jerk limits, considering a `delay_response_time`.
+    2. ego's `base_link` point cannot cross the stop line within the `crossing_time_limit`.
 
 ## Interface
 
@@ -61,3 +65,13 @@ The active filters are specified in `config/trajectory_traffic_rule_filter.param
     filter_names:
       - "autoware::trajectory_traffic_rule_filter::plugin::TrafficLightFilter"
 ```
+
+#### Traffic Light Filter parameters
+
+| Name                                                  | Type   | Description                                                                                                                                                        | Default Value |
+| ----------------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------ | ------------- |
+| `traffic_light_filter.deceleration_limit`             | double | Trajectories crossing an amber light are rejected if ego can stop at the stop line without breaking this limit                                                     | 2.8           |
+| `traffic_light_filter.jerk_limit`                     | double | Trajectories crossing an amber light are rejected if ego can stop at the stop line without breaking this limit                                                     | 5.0           |
+| `traffic_light_filter.delay_response_time`            | double | Delay response time (reaction time) used to estimate the minimum ego stopping distance from its current state                                                      | 0.5           |
+| `traffic_light_filter.crossing_time_limit`            | double | Trajectories crossing an amber light are rejected if they cannot cross (reach the stop line with `base_link`) before this time                                     | 2.75          |
+| `traffic_light_filter.treat_amber_light_as_red_light` | bool   | When true, amber lights are handled like red lights (using vehicle footprint crossing). If false, amber lights use the `base_link` crossing logic described above. | true          |

planning/autoware_trajectory_traffic_rule_filter/config/trajectory_traffic_rule_filter.param.yaml

@@ -1,3 +1,9 @@
 /**:
   ros__parameters:
     filter_names: ["autoware::trajectory_traffic_rule_filter::plugin::TrafficLightFilter"]
+    traffic_light_filter:
+      deceleration_limit: 2.8
+      jerk_limit: 5.0
+      delay_response_time: 0.5
+      crossing_time_limit: 2.75
+      treat_amber_light_as_red_light: true

planning/autoware_trajectory_traffic_rule_filter/include/autoware/trajectory_traffic_rule_filter/filters/traffic_light_filter.hpp

@@ -1,4 +1,4 @@
-// Copyright 2025 TIER IV, Inc.
+// 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.
@@ -22,6 +22,7 @@
 #include <lanelet2_core/Forward.h>
 
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace autoware::trajectory_traffic_rule_filter::plugin
@@ -37,12 +38,21 @@ class TrafficLightFilter : public TrafficRuleFilterInterface
     const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr & traffic_lights)
     override;
 
+  void set_parameters(const traffic_rule_filter::Params & params) override { params_ = params; }
+
+  /// @brief return true if ego can safely pass an amber traffic light
+  [[nodiscard]] bool can_pass_amber_light(
+    const double distance_to_stop_line, const double current_velocity,
+    const double current_acceleration, const double time_to_cross_stop_line) const;
+
 private:
-  /// @brief return the stop lines with red traffic lights for the given lanelet
-  [[nodiscard]] std::vector<lanelet::BasicLineString2d> get_red_stop_lines(
-    const lanelet::ConstLanelet & lanelet) const;
+  /// @brief return the red and amber stop lines related to the given lanelets
+  [[nodiscard]] std::pair<
+    std::vector<lanelet::BasicLineString2d>, std::vector<lanelet::BasicLineString2d>>
+  get_stop_lines(const lanelet::Lanelets & lanelets) const;
 
   autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr traffic_lights_;
+  traffic_rule_filter::Params params_;
 };
 
 }  // namespace autoware::trajectory_traffic_rule_filter::plugin

planning/autoware_trajectory_traffic_rule_filter/include/autoware/trajectory_traffic_rule_filter/traffic_rule_filter_interface.hpp

@@ -15,6 +15,7 @@
 #ifndef AUTOWARE__TRAJECTORY_TRAFFIC_RULE_FILTER__TRAFFIC_RULE_FILTER_INTERFACE_HPP_
 #define AUTOWARE__TRAJECTORY_TRAFFIC_RULE_FILTER__TRAFFIC_RULE_FILTER_INTERFACE_HPP_
 
+#include <autoware_trajectory_traffic_rule_filter/autoware_trajectory_traffic_rule_filter_param.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 #include <tl_expected/expected.hpp>
 
@@ -55,6 +56,10 @@ class TrafficRuleFilterInterface
     vehicle_info_ptr_ = std::make_shared<VehicleInfo>(vehicle_info);
   }
 
+  virtual void set_parameters(const traffic_rule_filter::Params & params) { (void)params; }
+
+  virtual void set_logger(rclcpp::Logger && logger) { logger_ = logger; }
+
   virtual void set_lanelet_map(
     const std::shared_ptr<lanelet::LaneletMap> & lanelet_map,
     const std::shared_ptr<lanelet::routing::RoutingGraph> & routing_graph,
@@ -75,6 +80,7 @@ class TrafficRuleFilterInterface
 
 protected:
   std::string name_;
+  std::optional<rclcpp::Logger> logger_;
   std::shared_ptr<VehicleInfo> vehicle_info_ptr_;
   std::shared_ptr<lanelet::LaneletMap> lanelet_map_;
   std::shared_ptr<lanelet::routing::RoutingGraph> routing_graph_;

planning/autoware_trajectory_traffic_rule_filter/include/autoware/trajectory_traffic_rule_filter/trajectory_traffic_rule_filter_node.hpp

@@ -17,7 +17,7 @@
 
 #include "autoware/trajectory_traffic_rule_filter/traffic_rule_filter_interface.hpp"
 
-#include <autoware_trajectory_traffic_rule_filter_param.hpp>
+#include <autoware_trajectory_traffic_rule_filter/autoware_trajectory_traffic_rule_filter_param.hpp>
 #include <autoware_utils_debug/time_keeper.hpp>
 #include <autoware_utils_diagnostics/diagnostics_interface.hpp>
 #include <autoware_utils_rclcpp/polling_subscriber.hpp>

planning/autoware_trajectory_traffic_rule_filter/package.xml

@@ -19,16 +19,19 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
-  <depend>autoware_boundary_departure_checker</depend>
   <depend>autoware_internal_planning_msgs</depend>
+  <depend>autoware_interpolation</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_lanelet2_utils</depend>
+  <depend>autoware_motion_utils</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_test_utils</depend>
   <depend>autoware_traffic_light_utils</depend>
   <depend>autoware_utils_debug</depend>
+  <depend>autoware_utils_diagnostics</depend>
   <depend>autoware_utils_rclcpp</depend>
+  <depend>autoware_utils_uuid</depend>
   <depend>autoware_vehicle_info_utils</depend>
   <depend>generate_parameter_library</depend>
   <depend>lanelet2_core</depend>

planning/autoware_trajectory_traffic_rule_filter/param/parameter_struct.yaml

@@ -2,5 +2,31 @@ traffic_rule_filter:
   filter_names:
     type: string_array
     default_value: []
-    description: metrics.
+    description: list of filter names
     read_only: false
+  traffic_light_filter:
+    deceleration_limit:
+      type: double
+      default_value: 2.8
+      description: trajectories crossing an amber light are rejected if ego can stop at the stop line without breaking this limit
+      read_only: false
+    jerk_limit:
+      type: double
+      default_value: 5.0
+      description: trajectories crossing an amber light are rejected if ego can stop at the stop line without breaking this limit
+      read_only: false
+    delay_response_time:
+      type: double
+      default_value: 0.5
+      description: delay response time used to estimate the minimum ego stopping distance
+      read_only: false
+    crossing_time_limit:
+      type: double
+      default_value: 2.75
+      description: trajectories crossing an amber light are rejected if they cannot cross before this time
+      read_only: false
+    treat_amber_light_as_red_light:
+      type: bool
+      default_value: true
+      description: when true, amber lights are handled like red lights
+      read_only: false

planning/autoware_trajectory_traffic_rule_filter/plugins.xml

@@ -4,7 +4,7 @@
          base_class_type="autoware::trajectory_traffic_rule_filter::plugin::TrafficRuleFilterInterface">
     <description>
       Filter for checking traffic light violations.
-      Rejects trajectories that would pass through red or yellow traffic lights.
+      Rejects trajectories that would pass through red or amber traffic lights.
     </description>
   </class>
 </library>

planning/autoware_trajectory_traffic_rule_filter/src/filters/traffic_light_filter.cpp

@@ -1,4 +1,4 @@
-// Copyright 2025 TIER IV, Inc.
+// 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.
@@ -14,20 +14,53 @@
 
 #include "autoware/trajectory_traffic_rule_filter/filters/traffic_light_filter.hpp"
 
+#include <autoware/interpolation/linear_interpolation.hpp>
+#include <autoware/motion_utils/distance/distance.hpp>
 #include <autoware/traffic_light_utils/traffic_light_utils.hpp>
+#include <rclcpp/logger.hpp>
+#include <rclcpp/logging.hpp>
 #include <tl_expected/expected.hpp>
 
 #include <boost/geometry.hpp>
+#include <boost/geometry/algorithms/for_each.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/geometry/Lanelet.h>
 #include <lanelet2_core/primitives/BasicRegulatoryElements.h>
 #include <lanelet2_core/primitives/BoundingBox.h>
 #include <lanelet2_core/primitives/LineString.h>
 
+#include <ctime>
 #include <string>
+#include <utility>
 #include <vector>
 
+namespace
+{
+/// @brief get stop lines where ego need to stop, and corresponding signal matching the given
+/// lanelet
+std::vector<std::pair<lanelet::BasicLineString2d, autoware_perception_msgs::msg::TrafficLightGroup>>
+get_matching_stop_lines(
+  const lanelet::Lanelet & lanelet,
+  const std::vector<autoware_perception_msgs::msg::TrafficLightGroup> & traffic_light_groups)
+{
+  std::vector<
+    std::pair<lanelet::BasicLineString2d, autoware_perception_msgs::msg::TrafficLightGroup>>
+    matching_stop_lines;
+  for (const auto & element : lanelet.regulatoryElementsAs<lanelet::TrafficLight>()) {
+    for (const auto & signal : traffic_light_groups) {
+      const auto is_matching =
+        signal.traffic_light_group_id == element->id() && element->stopLine().has_value();
+      if (is_matching && autoware::traffic_light_utils::isTrafficSignalStop(lanelet, signal)) {
+        matching_stop_lines.emplace_back(
+          lanelet::utils::to2D(element->stopLine()->basicLineString()), signal);
+      }
+    }
+  }
+  return matching_stop_lines;
+}
+}  // namespace
+
 namespace autoware::trajectory_traffic_rule_filter::plugin
 {
 TrafficLightFilter::TrafficLightFilter() : TrafficRuleFilterInterface("TrafficLightFilter")
@@ -40,34 +73,54 @@
   traffic_lights_ = traffic_lights;
 }
 
-std::vector<lanelet::BasicLineString2d> TrafficLightFilter::get_red_stop_lines(
-  const lanelet::ConstLanelet & lanelet) const
+std::pair<std::vector<lanelet::BasicLineString2d>, std::vector<lanelet::BasicLineString2d>>
+TrafficLightFilter::get_stop_lines(const lanelet::Lanelets & lanelets) const
 {
-  std::vector<lanelet::BasicLineString2d> stop_lines;
-  for (const auto & element : lanelet.regulatoryElementsAs<lanelet::TrafficLight>()) {
-    for (const auto & signal : traffic_lights_->traffic_light_groups) {
-      if (
-        signal.traffic_light_group_id == element->id() && element->stopLine().has_value() &&
-        autoware::traffic_light_utils::isTrafficSignalStop(lanelet, signal)) {
-        stop_lines.push_back(lanelet::utils::to2D(element->stopLine()->basicLineString()));
+  std::vector<lanelet::BasicLineString2d> red_stop_lines;
+  std::vector<lanelet::BasicLineString2d> amber_stop_lines;
+  for (const auto & lanelet : lanelets) {
+    for (const auto & [stop_line, signal] :
+         get_matching_stop_lines(lanelet, traffic_lights_->traffic_light_groups)) {
+      if (traffic_light_utils::hasTrafficLightCircleColor(
+            signal.elements, tier4_perception_msgs::msg::TrafficLightElement::RED)) {
+        red_stop_lines.push_back(stop_line);
+      }
+      if (traffic_light_utils::hasTrafficLightCircleColor(
+            signal.elements, tier4_perception_msgs::msg::TrafficLightElement::AMBER)) {
+        amber_stop_lines.push_back(stop_line);
       }
     }
   }
-  return stop_lines;
+  if (params_.traffic_light_filter.treat_amber_light_as_red_light) {
+    red_stop_lines.insert(red_stop_lines.end(), amber_stop_lines.begin(), amber_stop_lines.end());
+    amber_stop_lines.clear();
+  }
+  return {red_stop_lines, amber_stop_lines};
 }
 
 tl::expected<void, std::string> TrafficLightFilter::is_feasible(
   const TrajectoryPoints & trajectory_points)
 {
-  if (!lanelet_map_ || !traffic_lights_ || trajectory_points.empty() || !vehicle_info_ptr_) {
-    return {};  // Allow if no data available
-  }
-
+  TrajectoryPoints trajectory;
   lanelet::BasicLineString2d trajectory_ls;
   for (const auto & p : trajectory_points) {
+    // skip points behind ego
+    if (rclcpp::Duration(p.time_from_start).seconds() < 0.0) {
+      continue;
+    }
+    // skip points beyond the first stop
+    if (p.longitudinal_velocity_mps <= 0.0) {
+      break;
+    }
+    trajectory.push_back(p);
     trajectory_ls.emplace_back(p.pose.position.x, p.pose.position.y);
   }
-  if (vehicle_info_ptr_->max_longitudinal_offset_m > 0.0 && trajectory_ls.size() > 1) {
+
+  if (!lanelet_map_ || !traffic_lights_ || trajectory_ls.size() < 2 || !vehicle_info_ptr_) {
+    return tl::make_unexpected("No data available");  // Reject if no data available
+  }
+
+  if (vehicle_info_ptr_->max_longitudinal_offset_m > 0.0) {
     // extend the trajectory linestring by the vehicle's longitudinal offset
     const lanelet::BasicSegment2d last_segment(
       trajectory_ls[trajectory_ls.size() - 2], trajectory_ls.back());
@@ -81,14 +134,61 @@
   }
 
   const auto bbox = boost::geometry::return_envelope<lanelet::BoundingBox2d>(trajectory_ls);
-  for (const auto & ll : lanelet_map_->laneletLayer.search(bbox)) {
-    for (const auto & stop_line : get_red_stop_lines(ll)) {
-      if (boost::geometry::intersects(trajectory_ls, stop_line)) {
-        return tl::make_unexpected("crosses stop line of lanelet " + std::to_string(ll.id()));
+  const lanelet::Lanelets candidate_lanelets = lanelet_map_->laneletLayer.search(bbox);
+  const auto [red_stop_lines, amber_stop_lines] = get_stop_lines(candidate_lanelets);
+  for (const auto & red_stop_line : red_stop_lines) {
+    if (boost::geometry::intersects(trajectory_ls, red_stop_line)) {
+      return tl::make_unexpected("crosses red light");  // Reject trajectory (cross red light)
+    }
+  }
+  for (const auto & amber_stop_line : amber_stop_lines) {
+    auto distance_to_stop_line = 0.0;
+    std::optional<double> amber_stop_line_crossing_time;
+    for (size_t i = 0; i + 1 < trajectory.size(); ++i) {
+      lanelet::BasicPoints2d intersection_points;
+      const lanelet::BasicLineString2d segment{trajectory_ls[i], trajectory_ls[i + 1]};
+      const auto segment_length = static_cast<double>(boost::geometry::length(segment));
+      boost::geometry::intersection(segment, amber_stop_line, intersection_points);
+      if (!intersection_points.empty()) {
+        const auto distance_to_intersection =
+          boost::geometry::distance(segment.front(), intersection_points.front());
+        distance_to_stop_line += distance_to_intersection;
+        const auto ratio = distance_to_intersection / segment_length;
+        amber_stop_line_crossing_time = interpolation::lerp(
+          rclcpp::Duration(trajectory[i].time_from_start).seconds(),
+          rclcpp::Duration(trajectory[i + 1].time_from_start).seconds(), ratio);
+        break;
       }
+      distance_to_stop_line += segment_length;
+    }
+    const auto current_velocity = trajectory.front().longitudinal_velocity_mps;
+    const auto current_acceleration = trajectory.front().acceleration_mps2;
+    if (
+      amber_stop_line_crossing_time && !can_pass_amber_light(
+                                         distance_to_stop_line, current_velocity,
+                                         current_acceleration, *amber_stop_line_crossing_time)) {
+      return tl::make_unexpected("crosses amber light");  // Reject trajectory (cross amber light)
     }
   }
-  return {};  // Allow if no red lights found
+  return {};  // Allow trajectory
+}
+
+bool TrafficLightFilter::can_pass_amber_light(
+  const double distance_to_stop_line, const double current_velocity,
+  const double current_acceleration, const double time_to_cross_stop_line) const
+{
+  const double decel_limit = params_.traffic_light_filter.deceleration_limit;
+  const double jerk_limit = params_.traffic_light_filter.jerk_limit;
+  const double delay_response_time = params_.traffic_light_filter.delay_response_time;
+  const auto distance_for_ego_to_stop = motion_utils::calculate_stop_distance(
+    current_velocity, current_acceleration, decel_limit, jerk_limit, delay_response_time);
+
+  const bool can_stop =
+    distance_for_ego_to_stop.has_value() && *distance_for_ego_to_stop <= distance_to_stop_line;
+  const bool can_pass_in_time =
+    time_to_cross_stop_line <= params_.traffic_light_filter.crossing_time_limit;
+  const bool can_pass = !can_stop && can_pass_in_time;
+  return can_pass;
 }
 }  // namespace autoware::trajectory_traffic_rule_filter::plugin