wip

Signed-off-by: Tomohito Ando <[email protected]>

Author: TomohitoAndo

perception/traffic_light_arbiter/include/traffic_light_arbiter/signal_match_validator.hpp

@@ -63,15 +63,41 @@ class SignalMatchValidator
   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);
 
-  bool isPedestrianSignal(const lanelet::Id & signal_id);
-
 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

perception/traffic_light_arbiter/src/signal_match_validator.cpp

@@ -19,20 +19,23 @@ namespace util
 using TrafficSignalArray = autoware_perception_msgs::msg::TrafficSignalArray;
 using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal;
 using Element = autoware_perception_msgs::msg::TrafficSignalElement;
+using Time = builtin_interfaces::msg::Time;
 
+// Finds a signal by its ID within a TrafficSignalArray.
 std::optional<TrafficSignal> find_signal_by_id(
   const TrafficSignalArray & signals, int64_t signal_id)
 {
   auto it = std::find_if(
     signals.signals.begin(), signals.signals.end(),
     [signal_id](const TrafficSignal & signal) { return signal.traffic_signal_id == signal_id; });
   if (it != signals.signals.end()) {
-    return *it;
+    return *it;  // Return the found signal.
   } else {
-    return std::nullopt;
+    return std::nullopt;  // Return an empty optional if not found.
   }
 }
 
+// Creates a TrafficSignalElement with specified attributes.
 Element create_element(
   const Element::_color_type & color, const Element::_shape_type & shape,
   const Element::_status_type & status, const Element::_confidence_type & confidence)
@@ -46,7 +49,7 @@ Element create_element(
   return signal_element;
 }
 
-// Create unknown elements for each shape
+// Creates unknown elements for each unique shape from two element vectors.
 std::vector<Element> create_unknown_elements(
   const std::vector<Element> & elements1, const std::vector<Element> & elements2)
 {
@@ -60,66 +63,68 @@ std::vector<Element> create_unknown_elements(
 
   std::vector<Element> unknown_elements;
   for (const auto & shape : shape_set) {
-    // Confidence doesn't matter because this is the unknown signal
+    // Confidence is set to a default value as it is not relevant for unknown signals.
     unknown_elements.emplace_back(
-      util::create_element(Element::UNKNOWN, shape, Element::UNKNOWN, /* confidence */ 1.0));
+      create_element(Element::UNKNOWN, shape, Element::UNKNOWN, /* confidence */ 1.0));
   }
 
   return unknown_elements;
 }
 
+// Creates a 'unknown' signal with elements matching the shapes of a given signal's elements.
 TrafficSignal create_unknown_signal(const TrafficSignal & signal)
 {
   TrafficSignal unknown_signal;
   unknown_signal.traffic_signal_id = signal.traffic_signal_id;
   for (const auto & element : signal.elements) {
-    // Confidence doesn't matter because this is the unknown signal
+    // Confidence is set to a default value as it is not relevant for unknown signals.
     const auto unknown_element =
-      util::create_element(Element::UNKNOWN, element.shape, Element::UNKNOWN, /* confidence */ 1.0);
+      create_element(Element::UNKNOWN, element.shape, Element::UNKNOWN, /* confidence */ 1.0);
     unknown_signal.elements.emplace_back(unknown_element);
   }
 
   return unknown_signal;
 }
 
+// Creates an 'unknown' signal by combining unique shapes from two signals' elements.
 TrafficSignal create_unknown_signal(const TrafficSignal & signal1, const TrafficSignal & signal2)
 {
   TrafficSignal unknown_signal;
 
-  // Assume that the both ids are same
+  // Assumes that both signals have the same traffic_signal_id.
   unknown_signal.traffic_signal_id = signal1.traffic_signal_id;
 
-  const auto unknown_elements = util::create_unknown_elements(signal1.elements, signal2.elements);
+  const auto unknown_elements = create_unknown_elements(signal1.elements, signal2.elements);
   for (const auto & element : unknown_elements) {
     unknown_signal.elements.emplace_back(element);
   }
 
   return unknown_signal;
 }
 
+// Checks if all elements in two vectors are equivalent
 bool are_all_elements_equivalent(
   const std::vector<Element> & signal1, const std::vector<Element> & signal2)
 {
-  // Check if the vectors have the same size
+  // Returns false if vectors have different sizes.
   if (signal1.size() != signal2.size()) {
     return false;
   }
 
-  // Create copies of the vectors
+  // Sorts copies of the vectors by shape for comparison.
   std::vector<Element> sorted_signal1 = signal1;
   std::vector<Element> sorted_signal2 = signal2;
-
-  // Sort based on the shape to ensure that they are same order
   auto compare_by_shape = [](const Element & a, const Element & b) { return a.shape < b.shape; };
   std::sort(sorted_signal1.begin(), sorted_signal1.end(), compare_by_shape);
   std::sort(sorted_signal2.begin(), sorted_signal2.end(), compare_by_shape);
 
-  // Compare the sorted vectors and return true if they have all the same elements
+  // Returns true if sorted vectors are equal.
   return std::equal(
     sorted_signal1.begin(), sorted_signal1.end(), sorted_signal2.begin(), sorted_signal2.end(),
     [](const Element & a, const Element & b) { return a.color == b.color && a.shape == b.shape; });
 }
 
+// Creates a set of unique signal IDs from two vectors of TrafficSignals.
 std::unordered_set<lanelet::Id> create_signal_id_set(
   const std::vector<TrafficSignal> & signals1, const std::vector<TrafficSignal> & signals2)
 {
@@ -134,24 +139,25 @@ std::unordered_set<lanelet::Id> create_signal_id_set(
   return signal_id_set;
 }
 
+// Returns the signal with the highest confidence elements, considering a external priority.
 TrafficSignal get_highest_confidence_signal(
   const std::optional<TrafficSignal> & perception_signal,
   const std::optional<TrafficSignal> & external_signal, const bool external_priority)
 {
-  // If the either of the signal doesn't exist, return the signal that exists
+  // Returns the existing signal if only one of them exists.
   if (!perception_signal) {
     return *external_signal;
   }
   if (!external_signal) {
     return *perception_signal;
   }
 
-  // If the external_priority is true, use the external results
+  // Gives priority to the external signal if external_priority is true.
   if (external_priority) {
     return *external_signal;
   }
 
-  // Create map using shape as key
+  // Compiles elements into a map by shape, to compare their confidences.
   using Key = Element::_shape_type;
   std::map<Key, std::vector<Element>> shape_element_map;
   for (const auto & element : perception_signal->elements) {
@@ -163,12 +169,11 @@ TrafficSignal get_highest_confidence_signal(
 
   TrafficSignal highest_confidence_signal;
 
-  // Assume that the both ids are same
+  // Assumes that both signals have the same traffic_signal_id.
   highest_confidence_signal.traffic_signal_id = perception_signal->traffic_signal_id;
 
-  // Find the highest confidence element and push it
-  for (const auto & shape_and_elements : shape_element_map) {
-    const auto & elements = shape_and_elements.second;
+  // For each shape, finds the element with the highest confidence and adds it to the signal.
+  for (const auto & [shape, elements] : shape_element_map) {
     const auto highest_confidence_element = std::max_element(
       elements.begin(), elements.end(),
       [](const Element & a, const Element & b) { return a.confidence < b.confidence; });
@@ -178,9 +183,10 @@ TrafficSignal get_highest_confidence_signal(
   return highest_confidence_signal;
 }
 
-using Time = builtin_interfaces::msg::Time;
+// Determines the newer of two Time stamps.
 Time get_newer_stamp(const Time & stamp1, const Time & stamp2)
 {
+  // Returns stamp1 if it is newer than stamp2, otherwise returns stamp2.
   if (stamp1.sec > stamp2.sec || (stamp1.sec == stamp2.sec && stamp1.nanosec > stamp2.nanosec)) {
     return stamp1;
   } else {