Feat/v4.4.0.1/e2e trajectory modifier

common/autoware_obstacle_proximity_checker/include/autoware/obstacle_proximity_checker/structs.hpp

@@ -37,6 +37,8 @@ struct ObstacleTypeParameters
   double surround_check_front_distance{0.0};
   double surround_check_side_distance{0.0};
   double surround_check_back_distance{0.0};
+  bool enable_bbox_check{true};
+  bool enable_polygon_check{true};
 };
 
 struct Parameters

common/autoware_obstacle_proximity_checker/src/obstacle_proximity_checker.cpp

@@ -187,6 +187,21 @@ std::optional<ProximityObstacle> ProximityChecker::getNearestObstacleByDynamicOb
     return std::nullopt;
   }
 
+  auto is_enabled = [&](const auto & object, const std::string & label) {
+    const auto enable_check_iter = parameters_.object_type_enable_check.find(label);
+    if (
+      enable_check_iter == parameters_.object_type_enable_check.end() ||
+      !enable_check_iter->second) {
+      return false;
+    }
+    const auto & object_param = parameters_.obstacle_types_map.at(label);
+    if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX)
+      return object_param.enable_bbox_check;
+    if (object.shape.type == autoware_perception_msgs::msg::Shape::POLYGON)
+      return object_param.enable_polygon_check;
+    return false;
+  };
+
   autoware_perception_msgs::msg::PredictedObject nearest_object;
   double minimum_distance = std::numeric_limits<double>::max();
   bool was_minimum_distance_updated = false;
@@ -199,12 +214,7 @@ std::optional<ProximityObstacle> ProximityChecker::getNearestObstacleByDynamicOb
     }
 
     const auto & str_label = label_iter->second;
-    const auto enable_check_iter = parameters_.object_type_enable_check.find(str_label);
-    if (
-      enable_check_iter == parameters_.object_type_enable_check.end() ||
-      !enable_check_iter->second) {
-      continue;
-    }
+    if (!is_enabled(object, str_label)) continue;
 
     const auto & object_param = parameters_.obstacle_types_map.at(str_label);
     const double front_margin = object_param.surround_check_front_distance;

common/autoware_traffic_light_compliance_checker/CMakeLists.txt

@@ -7,11 +7,18 @@ autoware_package()
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/traffic_light_compliance_checker.cpp
   src/traffic_light_status_tracker.cpp
+  src/utils.cpp
+  src/v2i_handling.cpp
 )
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_ros REQUIRED)
+  file(GLOB_RECURSE TEST_SOURCES test/*.cpp)
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${TEST_SOURCES})
+  target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME})
 endif()
 
 ament_auto_package()

common/autoware_traffic_light_compliance_checker/README.md

@@ -5,7 +5,6 @@ The `traffic_light_compliance_checker` package provides a deterministic validati
 ## Core Features
 
 1. **Signal State Tracking (`TrafficLightStatusTracker`)**
-
    - Eliminates perception jitter and signal flickering by maintaining a temporal state history for each traffic light group ID.
    - Leverages stable duration thresholds before validating a transition to `RED` or `AMBER`.
    - Utilizes a hysteresis buffer to sustain known states during transient object occlusions.
@@ -88,6 +87,13 @@ To prevent sudden, harsh emergency braking maneuvers caused by raw perception no
 - **Segment-by-Segment Geometric Scan:** The checker evaluates trajectory segments sequentially against mapped stop lines using a localized `boost::geometry::intersection` check. The loop breaks immediately upon finding the first chronological intersection point, calculating the dynamic distance-to-stop-line and interpolating the exact crossing timestamp (for amber light) using `autoware::interpolation::lerp`.
 - **Red Light Evaluation:** Generates a violation if an intersection occurs, unless the trajectory makes the ego come to a complete stop within the specified `stop_overshoot_margin`.
 - **Amber Light Evaluation:** Computes the dynamic stopping distance based on current velocity, acceleration, applied deceleration, and system response latency. If the vehicle can stop safely before the line, or if it cannot clear the intersection within the `crossing_time_limit`, an amber violation is recorded to enforce a stop.
+- **Arrow-aware amber passing:** On protected turn lanes with a separate direction-arrow bulb in the map, the circle signal often goes `GREEN → AMBER → RED` before `GREEN *_ARROW` appears. While the circle is amber after a green circle, requiring a stop would be overly strict. When `enable_arrow_aware_amber_passing` is true, the checker skips stop-line collection (no amber/red violation) if all of the following hold:
+  - ego route lane is a left or right turn lane (`turn_direction`)
+  - the traffic light has a static arrow in the map (`subtype` containing `arrow`, or light-bulb `arrow` attribute)
+  - the amber phase was reached from a green circle (`AmberState::kFromGreen`)
+  - the current signal still reports an amber circle
+
+  Transitions from red (or unknown) to amber still require a stop. A brief red circle before the green arrow is reported is also still treated as a stop (same scope as the behavior velocity traffic light module).
 
 ## Structs and Interface Definitions
 
@@ -133,5 +139,6 @@ The interfaces pass inputs and output results through the following standard dat
 | `ego_stopped_velocity_threshold`               | `double` | Velocity threshold beneath which the ego vehicle is considered completely stopped ($m/s$).                 |
 | `treat_amber_light_as_red_light`               | `bool`   | If true, disables amber passing logic and treats all amber states as strict red signals.                   |
 | `treat_unknown_light_as_red_light`             | `bool`   | If true, evaluates unclassified or blank signal states as strict red signals.                              |
+| `enable_arrow_aware_amber_passing`             | `bool`   | If true, allow Green Circle → Amber pass on turn lanes with a mapped static arrow.                         |
 | `checked_trajectory_length.deceleration_limit` | `double` | Comfortable stop deceleration limit ($m/s^2$) for computing trajectory checking length.                    |
 | `checked_trajectory_length.jerk_limit`         | `double` | Comfortable stop jerk limit ($m/s^3$) for computing trajectory checking length.                            |

common/autoware_traffic_light_compliance_checker/include/autoware/traffic_light_compliance_checker/structs.hpp

@@ -25,16 +25,11 @@
 #include <lanelet2_core/geometry/LineString.h>
 
 #include <cstdint>
-#include <optional>
-#include <unordered_map>
 #include <vector>
 
 namespace autoware::traffic_light_compliance_checker
 {
 
-/// @brief route lanelet id per traffic light id, for the traffic lights of the current route
-using RouteTrafficLightIndex = std::unordered_map<lanelet::Id, lanelet::Id>;
-
 /// @brief input data for traffic light compliance check
 struct Inputs
 {
@@ -52,7 +47,6 @@ struct StopLineInfo
 {
   lanelet::BasicLineString2d line;
   int64_t traffic_light_id;
-  int64_t traffic_light_lanelet_id;
 };
 
 /// @brief type of traffic light violation
@@ -91,53 +85,52 @@ struct ComplianceResult
   Violations violations;
 };
 
-/// @brief parameters for traffic light signal status tracking
-struct StatusTrackerParameters
-{
-  double stable_duration_threshold_red;
-  double stable_duration_threshold_amber;
-  double stable_duration_threshold_unknown;
+/// @brief how the current amber phase was reached
+enum class AmberState {
+  kNotAmber,      ///< not currently in an amber state
+  kFromGreen,     ///< amber reached from green circle
+  kFromNonGreen,  ///< amber reached from non-green
 };
 
-/// @brief part of the trajectory to check against stop lines
-struct PreparedTrajectory
+/// @brief parameters for traffic light signal status tracking
+struct StatusTrackerParameters
 {
-  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> points;
-  lanelet::BasicLineString2d linestring;
-  std::optional<lanelet::BasicPoint2d> stop_point;
-  double backward_length{0.0};
+  double stable_duration_threshold_red{0.0};
+  double stable_duration_threshold_amber{0.0};
+  double stable_duration_threshold_unknown{0.0};
 };
 
 /// @brief parameters for traffic light compliance check
 struct Parameters
 {
-  double deceleration_limit;
-  double jerk_limit;
-  double delay_response_time;
-  double crossing_time_limit;
-  bool treat_amber_light_as_red_light;
-  bool treat_unknown_light_as_red_light;
-  double stop_overshoot_margin;
-  double allow_if_cannot_stop_distance;
-  double min_lookahead_distance;
-  double ego_stopped_velocity_threshold;
+  double deceleration_limit{2.8};
+  double jerk_limit{5.0};
+  double delay_response_time{0.5};
+  double crossing_time_limit{2.75};
+  bool treat_amber_light_as_red_light{false};
+  bool treat_unknown_light_as_red_light{false};
+  bool enable_arrow_aware_amber_passing{true};
+  double stop_overshoot_margin{0.5};
+  double allow_if_cannot_stop_distance{0.0};
+  double min_lookahead_distance{0.0};
+  double ego_stopped_velocity_threshold{0.01};
   StatusTrackerParameters status_tracker_parameters;
   struct CheckedTrajectoryLength
   {
-    double deceleration_limit;
-    double jerk_limit;
+    double deceleration_limit{2.0};
+    double jerk_limit{2.0};
   } checked_trajectory_length;
   struct AmberRejection
   {
-    double hysteresis_duration;
-    bool reject_if_stop_detected;
+    double hysteresis_duration{0.0};
+    bool reject_if_stop_detected{false};
   } amber_rejection;
   struct V2IHandling
   {
-    bool get_last_time_allowed_to_pass_from_map;
-    double last_time_allowed_to_pass;
-    double velocity_threshold;
-    double required_time_to_departure;
+    bool get_last_time_allowed_to_pass_from_map{false};
+    double last_time_allowed_to_pass{4.0};
+    double velocity_threshold{0.5};
+    double required_time_to_departure{5.0};
   } v2i_handling;
 };
 

common/autoware_traffic_light_compliance_checker/include/autoware/traffic_light_compliance_checker/traffic_light_compliance_checker.hpp

@@ -37,6 +37,7 @@
 
 namespace autoware::traffic_light_compliance_checker
 {
+
 /// @brief class to check if a trajectory complies with traffic lights
 class TrafficLightComplianceChecker
 {
@@ -54,6 +55,8 @@ class TrafficLightComplianceChecker
   /**
    * @brief check if the trajectory complies with traffic lights
    * @param input input data for compliance check (raw signals are filtered internally)
+   * @param use_v2i_remaining_time also reject crossings of non-stop lights that V2I predicts to
+   * turn red before ego can pass
    * @return result of compliance check, or error message if check fails
    */
   [[nodiscard]] tl::expected<ComplianceResult, std::string> check(
@@ -75,7 +78,6 @@ class TrafficLightComplianceChecker
   [[nodiscard]] ComplianceResult check_with_filtered_signals(
     const Inputs & input,
     const autoware_perception_msgs::msg::TrafficLightGroupArray & filtered_signals,
-    const RouteTrafficLightIndex & route_traffic_light_index,
     const std::vector<int64_t> & force_reject_amber_ids, const bool check_red_lights,
     const bool check_amber_lights, const bool is_ego_stopped) const;
 
@@ -95,7 +97,7 @@ class TrafficLightComplianceChecker
   /// @brief return the red and amber stop lines related to the given traffic light groups
   [[nodiscard]] std::pair<std::vector<StopLineInfo>, std::vector<StopLineInfo>> get_stop_lines(
     const lanelet::LaneletMap & lanelet_map,
-    const RouteTrafficLightIndex & route_traffic_light_index,
+    const autoware_planning_msgs::msg::LaneletRoute & route,
     const autoware_perception_msgs::msg::TrafficLightGroupArray & traffic_lights) const;
 
   /// @brief return true if there is a stop point and it is within margin distance of the stop line
@@ -105,24 +107,29 @@ class TrafficLightComplianceChecker
 
   /// @brief return true if ego can safely pass an amber traffic light
   [[nodiscard]] bool can_pass_amber_light(
-    const int64_t traffic_light_id, const double distance_to_stop_line,
+    const int64_t traffic_light_id, const double distance_from_ego_front,
     const double current_velocity, const double current_acceleration,
     const double time_to_cross_stop_line) const;
 
-  bool is_allow_if_cannot_stop(const double distance_to_cross_point) const;
+  bool is_allow_if_cannot_stop(const double distance_from_ego_front) const;
 
-  /// @brief return the stop lines that will turn red before ego can pass them, based on the V2I
-  /// signal predictions
-  [[nodiscard]] ComplianceResult handle_v2i(
+  /// @brief return violations for the stop lines of non-stop lights that will turn red before ego
+  /// can pass them, based on the V2I signal predictions. Defined in v2i_handling.cpp.
+  [[nodiscard]] Violations get_v2i_violations(
     const Inputs & input,
     const autoware_perception_msgs::msg::TrafficLightGroupArray & filtered_signals,
-    const RouteTrafficLightIndex & route_traffic_light_index,
-    const bool use_v2i_remaining_time, const bool is_ego_stopped) const;
+    const bool is_ego_stopped) const;
+
+  void cleanup_violation_distance_history(
+    const std::vector<StopLineInfo> & red_stop_lines,
+    const std::vector<StopLineInfo> & amber_stop_lines, const bool check_red_lights,
+    const bool check_amber_lights) const;
 
   Parameters params_;
   vehicle_info_utils::VehicleInfo vehicle_info_;
   std::unique_ptr<TrafficLightStatusTracker> status_tracker_;
   mutable std::unordered_map<int64_t, rclcpp::Time> amber_rejection_history_;
+  mutable std::unordered_map<int64_t, double> violation_distance_history_;
   std::optional<double> ego_stopping_distance_;
 };
 

common/autoware_traffic_light_compliance_checker/include/autoware/traffic_light_compliance_checker/traffic_light_status_tracker.hpp

@@ -19,10 +19,13 @@
 
 #include <rclcpp/time.hpp>
 
+#include <autoware_perception_msgs/msg/traffic_light_element.hpp>
 #include <autoware_perception_msgs/msg/traffic_light_group_array.hpp>
 
 #include <cstdint>
+#include <optional>
 #include <unordered_map>
+#include <vector>
 
 namespace autoware::traffic_light_compliance_checker
 {
@@ -40,12 +43,19 @@ class TrafficLightStatusTracker
    * @param signals raw traffic light signals
    * @param current_time current time stamp
    * @param is_ego_stopped true if ego velocity is below the stopped threshold
-   * @return signals with unstable states cleared
+   * @return signals with stable states (or raw signals if ego is stopped)
    */
   [[nodiscard]] autoware_perception_msgs::msg::TrafficLightGroupArray filter_signals(
     const autoware_perception_msgs::msg::TrafficLightGroupArray & signals,
     const rclcpp::Time & current_time, bool is_ego_stopped);
 
+  /**
+   * @brief return how the current amber phase was reached for a traffic light group
+   * @param traffic_light_group_id traffic light regulatory element / group id
+   * @return amber transition state; kNotAmber if the id is unknown or not amber
+   */
+  [[nodiscard]] AmberState get_amber_transition_state(int64_t traffic_light_group_id) const;
+
   [[nodiscard]] double get_duration(const int64_t traffic_light_id) const
   {
     const auto it = signal_history_.find(traffic_light_id);
@@ -54,15 +64,25 @@ class TrafficLightStatusTracker
   }
 
 private:
-  void cleanup_signal_history(const rclcpp::Time & current_time);
-
   struct SignalStateHistory
   {
-    autoware_perception_msgs::msg::TrafficLightGroup msg;
+    autoware_perception_msgs::msg::TrafficLightGroup current_state;
+    std::optional<autoware_perception_msgs::msg::TrafficLightGroup> stable_state;
     rclcpp::Time first_seen_time;
     rclcpp::Time last_seen_time;
+    AmberState amber_transition_state{AmberState::kNotAmber};
   };
 
+  void cleanup_signal_history(const rclcpp::Time & current_time);
+
+  static void update_amber_transition_state(
+    SignalStateHistory & history,
+    const std::vector<autoware_perception_msgs::msg::TrafficLightElement> & previous_elements,
+    const std::vector<autoware_perception_msgs::msg::TrafficLightElement> & current_elements);
+
+  double required_stability_duration(
+    const autoware_perception_msgs::msg::TrafficLightGroup & signal) const;
+
   StatusTrackerParameters params_;
   std::unordered_map<int64_t, SignalStateHistory> signal_history_;
 };

common/autoware_traffic_light_compliance_checker/include/autoware/traffic_light_compliance_checker/utils.hpp

@@ -0,0 +1,63 @@
+// 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__TRAFFIC_LIGHT_COMPLIANCE_CHECKER__UTILS_HPP_
+#define AUTOWARE__TRAFFIC_LIGHT_COMPLIANCE_CHECKER__UTILS_HPP_
+
+#include "autoware/traffic_light_compliance_checker/structs.hpp"
+#include "autoware_lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp"
+
+#include <autoware_perception_msgs/msg/traffic_light_element.hpp>
+#include <autoware_perception_msgs/msg/traffic_light_group.hpp>
+
+#include <lanelet2_core/primitives/Lanelet.h>
+
+#include <memory>
+#include <vector>
+
+namespace autoware::traffic_light_compliance_checker
+{
+
+[[nodiscard]] bool is_equal(
+  const autoware_perception_msgs::msg::TrafficLightElement & a,
+  const autoware_perception_msgs::msg::TrafficLightElement & b);
+
+[[nodiscard]] bool is_equal(
+  const std::vector<autoware_perception_msgs::msg::TrafficLightElement> & a,
+  const std::vector<autoware_perception_msgs::msg::TrafficLightElement> & b);
+
+[[nodiscard]] bool has_amber_circle(
+  const std::vector<autoware_perception_msgs::msg::TrafficLightElement> & elements);
+
+[[nodiscard]] bool has_green_circle(
+  const std::vector<autoware_perception_msgs::msg::TrafficLightElement> & elements);
+
+[[nodiscard]] bool has_red_circle(
+  const std::vector<autoware_perception_msgs::msg::TrafficLightElement> & elements);
+
+[[nodiscard]] bool has_unknown(
+  const std::vector<autoware_perception_msgs::msg::TrafficLightElement> & elements);
+
+[[nodiscard]] bool has_static_arrow(
+  const std::shared_ptr<const lanelet::autoware::AutowareTrafficLight> & reg_elem);
+
+[[nodiscard]] bool is_arrow_aware_amber_pass(
+  const lanelet::ConstLanelet & lanelet,
+  const autoware_perception_msgs::msg::TrafficLightGroup & signal,
+  const std::shared_ptr<const lanelet::autoware::AutowareTrafficLight> & reg_elem,
+  const AmberState amber_transition_state);
+
+}  // namespace autoware::traffic_light_compliance_checker
+
+#endif  // AUTOWARE__TRAFFIC_LIGHT_COMPLIANCE_CHECKER__UTILS_HPP_