feat(traffic_light_arbiter): add signal match validator (#6423)

perception/traffic_light_arbiter/CMakeLists.txt

@@ -6,6 +6,7 @@ autoware_package()
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/traffic_light_arbiter.cpp
+  src/signal_match_validator.cpp
 )
 
 rclcpp_components_register_nodes(${PROJECT_NAME}

perception/traffic_light_arbiter/README.md

@@ -8,6 +8,19 @@ This package receives traffic signals from perception and external (e.g., V2X) c
 
 A node that merges traffic light/signal state from image recognition and external (e.g., V2X) systems to provide to a planning component.
 
+### Signal Match Validator
+
+When `enable_signal_matching` is set to true, this node validates the match between perception signals and external signals.
+The table below outlines how the matching process determines the output based on the combination of perception and external signal colors. Each cell represents the outcome when a specific color from a perception signal (columns) intersects with a color from an external signal (rows).
+
+| External \ Perception | RED     | AMBER   | GREEN   | UNKNOWN | Not Received |
+| --------------------- | ------- | ------- | ------- | ------- | ------------ |
+| RED                   | RED     | UNKNOWN | UNKNOWN | UNKNOWN | UNKNOWN      |
+| AMBER                 | UNKNOWN | AMBER   | UNKNOWN | UNKNOWN | UNKNOWN      |
+| GREEN                 | UNKNOWN | UNKNOWN | GREEN   | UNKNOWN | UNKNOWN      |
+| UNKNOWN               | UNKNOWN | UNKNOWN | UNKNOWN | UNKNOWN | UNKNOWN      |
+| Not Received          | UNKNOWN | UNKNOWN | UNKNOWN | UNKNOWN | UNKNOWN      |
+
 ### Inputs / Outputs
 
 #### Input
@@ -28,8 +41,9 @@ A node that merges traffic light/signal state from image recognition and externa
 
 ### Core Parameters
 
-| Name                        | Type   | Default Value | Description                                                                                                                      |
-| --------------------------- | ------ | ------------- | -------------------------------------------------------------------------------------------------------------------------------- |
-| `external_time_tolerance`   | double | 5.0           | The duration in seconds an external message is considered valid for merging                                                      |
-| `perception_time_tolerance` | double | 1.0           | The duration in seconds a perception message is considered valid for merging                                                     |
-| `external_priority`         | bool   | false         | Whether or not externals signals take precedence over perception-based ones. If false, the merging uses confidence as a criteria |
+| Name                        | Type   | Default Value | Description                                                                                                                                                                    |
+| --------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
+| `external_time_tolerance`   | double | 5.0           | The duration in seconds an external message is considered valid for merging                                                                                                    |
+| `perception_time_tolerance` | double | 1.0           | The duration in seconds a perception message is considered valid for merging                                                                                                   |
+| `external_priority`         | bool   | false         | Whether or not externals signals take precedence over perception-based ones. If false, the merging uses confidence as a criteria                                               |
+| `enable_signal_matching`    | bool   | false         | Decide whether to validate the match between perception signals and external signals. If set to true, verify that the colors match and only publish them if they are identical |

perception/traffic_light_arbiter/config/traffic_light_arbiter.param.yaml

@@ -3,3 +3,4 @@
     external_time_tolerance: 5.0
     perception_time_tolerance: 1.0
     external_priority: false
+    enable_signal_matching: false

perception/traffic_light_arbiter/include/traffic_light_arbiter/signal_match_validator.hpp

@@ -0,0 +1,103 @@
+// Copyright 2024 The Autoware Contributors
+//
+// 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 TRAFFIC_LIGHT_ARBITER__SIGNAL_MATCH_VALIDATOR_HPP_
+#define TRAFFIC_LIGHT_ARBITER__SIGNAL_MATCH_VALIDATOR_HPP_
+
+#include <lanelet2_extension/utility/query.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+
+#include <memory>
+#include <optional>
+#include <unordered_set>
+#include <vector>
+
+/**
+ * @class SignalMatchValidator
+ * @brief Validates and compares traffic signal data from different sources.
+ *
+ * This class is designed to compare and validate traffic signal information
+ * received from internal (e.g. camera perception) and external (e.g. V2I communication).
+ * It aims to ensure that the traffic signal information from both sources is consistent.
+ */
+class SignalMatchValidator
+{
+public:
+  using TrafficSignalArray = autoware_perception_msgs::msg::TrafficSignalArray;
+  using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal;
+  using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
+  using TrafficLightConstPtr = lanelet::TrafficLightConstPtr;
+
+  /**
+   * @brief Default constructor for SignalMatchValidator.
+   */
+  SignalMatchValidator() = default;
+
+  /**
+   * @brief Validates and compares traffic signal data from perception and external sources.
+   *
+   * This method compares traffic signal data obtained from perception results
+   * with data received from external source. It aims to find and return signals
+   * that are present and consistent in both datasets. If signals are not consistent,
+   * treat them as the unknown signal.
+   *
+   * @param perception_signals Traffic signal data from perception.
+   * @param external_signals Traffic signal data from external source.
+   * @return A validated TrafficSignalArray.
+   */
+  TrafficSignalArray validateSignals(
+    const TrafficSignalArray & perception_signals, const TrafficSignalArray & external_signals);
+
+  /**
+   * @brief Sets the pedestrian signals to be considered during validation.
+   *
+   * This method allows the specification of pedestrian signals, which are then
+   * used to adjust the validation logic, acknowledging their unique characteristics
+   * in traffic signal datasets.
+   *
+   * @param pedestrian_signals A vector of pedestrian signal pointers.
+   */
+  void setPedestrianSignals(const std::vector<TrafficLightConstPtr> & pedestrian_signals);
+
+  /**
+   * @brief Sets the priority flag for using external signal data over perception data.
+   *
+   * When set to true, this flag indicates that in cases of discrepancy between
+   * perception and external signal data, the external data should be prioritized.
+   *
+   * @param external_priority The priority flag for external signal data.
+   */
+  void setExternalPriority(const bool external_priority);
+
+private:
+  bool external_priority_;
+  std::unordered_set<lanelet::Id> map_pedestrian_signal_regulatory_elements_set_;
+
+  /**
+   * @brief Checks if a given signal ID corresponds to a pedestrian signal.
+   *
+   * This method determines whether a signal, identified by its ID, is classified
+   * as a pedestrian signal.
+   *
+   * @param signal_id The ID of the signal to check.
+   * @return True if the signal is a pedestrian signal, false otherwise.
+   */
+  bool isPedestrianSignal(const lanelet::Id & signal_id);
+};
+
+#endif  // TRAFFIC_LIGHT_ARBITER__SIGNAL_MATCH_VALIDATOR_HPP_

perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp

@@ -16,6 +16,7 @@
 #define TRAFFIC_LIGHT_ARBITER__TRAFFIC_LIGHT_ARBITER_HPP_
 
 #include <rclcpp/rclcpp.hpp>
+#include <traffic_light_arbiter/signal_match_validator.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
@@ -51,9 +52,11 @@ class TrafficLightArbiter : public rclcpp::Node
   double external_time_tolerance_;
   double perception_time_tolerance_;
   bool external_priority_;
+  bool enable_signal_matching_;
 
   TrafficSignalArray latest_perception_msg_;
   TrafficSignalArray latest_external_msg_;
+  std::unique_ptr<SignalMatchValidator> signal_match_validator_;
 };
 
 #endif  // TRAFFIC_LIGHT_ARBITER__TRAFFIC_LIGHT_ARBITER_HPP_