feat(trajectory_modifier): add v2i handling for traffic_light_stop pl… (#3499)

feat(trajectory_modifier): add v2i handling for traffic_light_stop plugin

Signed-off-by: Mert Çolak <colak.mrt@outlook.com>
Co-authored-by: Mert Çolak <colak.mrt@outlook.com>

Author: N-Eiki

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

@@ -25,11 +25,16 @@
 #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
 {
@@ -47,10 +52,11 @@ struct StopLineInfo
 {
   lanelet::BasicLineString2d line;
   int64_t traffic_light_id;
+  int64_t traffic_light_lanelet_id;
 };
 
 /// @brief type of traffic light violation
-enum class ViolationType { RED_LIGHT, AMBER_LIGHT };
+enum class ViolationType { RED_LIGHT, AMBER_LIGHT, V2I };
 
 /// @brief violation detail
 struct Violation
@@ -93,6 +99,15 @@ struct StatusTrackerParameters
   double stable_duration_threshold_unknown;
 };
 
+/// @brief part of the trajectory to check against stop lines
+struct PreparedTrajectory
+{
+  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> points;
+  lanelet::BasicLineString2d linestring;
+  std::optional<lanelet::BasicPoint2d> stop_point;
+  double backward_length{0.0};
+};
+
 /// @brief parameters for traffic light compliance check
 struct Parameters
 {
@@ -117,6 +132,13 @@ struct Parameters
     double hysteresis_duration;
     bool reject_if_stop_detected;
   } 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;
+  } v2i_handling;
 };
 
 }  // namespace autoware::traffic_light_compliance_checker

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

@@ -37,7 +37,6 @@
 
 namespace autoware::traffic_light_compliance_checker
 {
-
 /// @brief class to check if a trajectory complies with traffic lights
 class TrafficLightComplianceChecker
 {
@@ -58,7 +57,8 @@ class TrafficLightComplianceChecker
    * @return result of compliance check, or error message if check fails
    */
   [[nodiscard]] tl::expected<ComplianceResult, std::string> check(
-    const Inputs & input, const bool check_red_lights = true, const bool check_amber_lights = true);
+    const Inputs & input, const bool check_red_lights = true, const bool check_amber_lights = true,
+    const bool use_v2i_remaining_time = false);
 
   /**
    * @brief update parameters
@@ -75,6 +75,7 @@ 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;
 
@@ -94,7 +95,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 autoware_planning_msgs::msg::LaneletRoute & route,
+    const RouteTrafficLightIndex & route_traffic_light_index,
     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
@@ -110,6 +111,14 @@ class TrafficLightComplianceChecker
 
   bool is_allow_if_cannot_stop(const double distance_to_cross_point) 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(
+    const Inputs & input,
+    const autoware_perception_msgs::msg::TrafficLightGroupArray & filtered_signals,
+    const RouteTrafficLightIndex & route_traffic_light_index,
+    const bool use_v2i_remaining_time) const;
+
   Parameters params_;
   vehicle_info_utils::VehicleInfo vehicle_info_;
   std::unique_ptr<TrafficLightStatusTracker> status_tracker_;