wip

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

Author: TomohitoAndo

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/traffic_light_arbiter.hpp

@@ -53,11 +53,12 @@ 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_;
 
-  SignalMatchValidator signal_match_validator_;
+  std::unique_ptr<SignalMatchValidator> signal_match_validator_;
 };
 
 #endif  // TRAFFIC_LIGHT_ARBITER__TRAFFIC_LIGHT_ARBITER_HPP_

perception/traffic_light_arbiter/src/signal_match_validator.cpp

@@ -21,21 +21,21 @@ 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.
+// 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 the found signal.
+    return *it;  // Return the found signal
   } else {
-    return std::nullopt;  // Return an empty optional if not found.
+    return std::nullopt;  // Return an empty optional if not found
   }
 }
 
-// Creates a TrafficSignalElement with specified attributes.
+// 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)
@@ -49,7 +49,7 @@ Element create_element(
   return signal_element;
 }
 
-// Creates unknown elements for each unique shape from two element vectors.
+// 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)
 {
@@ -63,21 +63,21 @@ std::vector<Element> create_unknown_elements(
 
   std::vector<Element> unknown_elements;
   for (const auto & shape : shape_set) {
-    // Confidence is set to a default value as it is not relevant for unknown signals.
+    // Confidence is set to a default value as it is not relevant for unknown signals
     unknown_elements.emplace_back(
       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.
+// 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 is set to a default value as it is not relevant for unknown signals.
+    // Confidence is set to a default value as it is not relevant for unknown signals
     const auto unknown_element =
       create_element(Element::UNKNOWN, element.shape, Element::UNKNOWN, /* confidence */ 1.0);
     unknown_signal.elements.emplace_back(unknown_element);
@@ -86,12 +86,12 @@ TrafficSignal create_unknown_signal(const TrafficSignal & signal)
   return unknown_signal;
 }
 
-// Creates an 'unknown' signal by combining unique shapes from two signals' elements.
+// 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;
 
-  // Assumes that both signals have the same traffic_signal_id.
+  // Assumes that both signals have the same traffic_signal_id
   unknown_signal.traffic_signal_id = signal1.traffic_signal_id;
 
   const auto unknown_elements = create_unknown_elements(signal1.elements, signal2.elements);
@@ -106,25 +106,25 @@ TrafficSignal create_unknown_signal(const TrafficSignal & signal1, const Traffic
 bool are_all_elements_equivalent(
   const std::vector<Element> & signal1, const std::vector<Element> & signal2)
 {
-  // Returns false if vectors have different sizes.
+  // Returns false if vectors have different sizes
   if (signal1.size() != signal2.size()) {
     return false;
   }
 
-  // Sorts copies of the vectors by shape for comparison.
+  // Sorts copies of the vectors by shape for comparison
   std::vector<Element> sorted_signal1 = signal1;
   std::vector<Element> sorted_signal2 = signal2;
   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);
 
-  // Returns true if sorted vectors are equal.
+  // 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.
+// 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)
 {
@@ -139,25 +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.
+// 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)
 {
-  // Returns the existing signal if only one of them exists.
+  // Returns the existing signal if only one of them exists
   if (!perception_signal) {
     return *external_signal;
   }
   if (!external_signal) {
     return *perception_signal;
   }
 
-  // Gives priority to the external signal if external_priority is true.
+  // Gives priority to the external signal if external_priority is true
   if (external_priority) {
     return *external_signal;
   }
 
-  // Compiles elements into a map by shape, to compare their confidences.
+  // 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) {
@@ -169,10 +169,10 @@ TrafficSignal get_highest_confidence_signal(
 
   TrafficSignal highest_confidence_signal;
 
-  // Assumes that both signals have the same traffic_signal_id.
+  // Assumes that both signals have the same traffic_signal_id
   highest_confidence_signal.traffic_signal_id = perception_signal->traffic_signal_id;
 
-  // For each shape, finds the element with the highest confidence and adds it to the signal.
+  // 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(),
@@ -183,10 +183,10 @@ TrafficSignal get_highest_confidence_signal(
   return highest_confidence_signal;
 }
 
-// Determines the newer of two Time stamps.
+// 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.
+  // 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 {
@@ -242,15 +242,12 @@ autoware_perception_msgs::msg::TrafficSignalArray SignalMatchValidator::validate
     // Check if they have the same elements
     if (!util::are_all_elements_equivalent(
           perception_result->elements, external_result->elements)) {
-      // RCLCPP_WARN_STREAM(rclcpp::get_logger("debug"), "Not the same signal");
-
       const auto unknown_signal = util::create_unknown_signal(*perception_result, *external_result);
       validated_signals.signals.emplace_back(unknown_signal);
       continue;
     }
 
     // Both results are same, then insert the received color
-    // RCLCPP_WARN_STREAM(rclcpp::get_logger("debug"), "Both results are same");
     validated_signals.signals.emplace_back(*perception_result);
   }
 

perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp

@@ -31,7 +31,6 @@ namespace lanelet
 {
 
 using TrafficLightConstPtr = std::shared_ptr<const TrafficLight>;
-using TrafficLightConstPtr = lanelet::TrafficLightConstPtr;
 
 std::vector<TrafficLightConstPtr> filter_traffic_signals(const LaneletMapConstPtr map)
 {
@@ -73,8 +72,12 @@ TrafficLightArbiter::TrafficLightArbiter(const rclcpp::NodeOptions & options)
   external_time_tolerance_ = this->declare_parameter<double>("external_time_tolerance", 5.0);
   perception_time_tolerance_ = this->declare_parameter<double>("perception_time_tolerance", 1.0);
   external_priority_ = this->declare_parameter<bool>("external_priority", false);
+  enable_signal_matching_ = this->declare_parameter<bool>("enable_signal_matching", false);
 
-  signal_match_validator_.setExternalPriority(external_priority_);
+  if (enable_signal_matching_) {
+    signal_match_validator_ = std::make_unique<SignalMatchValidator>();
+    signal_match_validator_->setExternalPriority(external_priority_);
+  }
 
   map_sub_ = create_subscription<LaneletMapBin>(
     "~/sub/vector_map", rclcpp::QoS(1).transient_local(),
@@ -103,9 +106,11 @@ void TrafficLightArbiter::onMap(const LaneletMapBin::ConstSharedPtr msg)
     map_regulatory_elements_set_->emplace(signal->id());
   }
 
-  // Filter only pedestrian signals to distinguish them in compare function
-  const auto pedestrian_signals = lanelet::filter_pedestrian_signals(map);
-  signal_match_validator_.setPedestrianSignals(pedestrian_signals);
+  if (enable_signal_matching_) {
+    // Filter only pedestrian signals to distinguish them in compare function
+    const auto pedestrian_signals = lanelet::filter_pedestrian_signals(map);
+    signal_match_validator_->setPedestrianSignals(pedestrian_signals);
+  }
 }
 
 void TrafficLightArbiter::onPerceptionMsg(const TrafficSignalArray::ConstSharedPtr msg)
@@ -168,11 +173,9 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim
     }
   };
 
-  // TODO: parameter
-  const bool validate_perception_and_external = true;
-  if (validate_perception_and_external) {
+  if (enable_signal_matching_) {
     const auto validated_signals =
-      signal_match_validator_.validateSignals(latest_perception_msg_, latest_external_msg_);
+      signal_match_validator_->validateSignals(latest_perception_msg_, latest_external_msg_);
     for (const auto & signal : validated_signals.signals) {
       add_signal_function(signal, false);
     }