From b4449b9499a516e926701ab26829df53a03660cc Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Tue, 20 Feb 2024 19:30:00 +0900 Subject: [PATCH] feat(traffic_light_arbiter): add signal match validator (#6423) Signed-off-by: Tomohito Ando --- .../traffic_light_arbiter/CMakeLists.txt | 1 + perception/traffic_light_arbiter/README.md | 24 +- .../config/traffic_light_arbiter.param.yaml | 1 + .../signal_match_validator.hpp | 103 +++++++ .../traffic_light_arbiter.hpp | 3 + .../src/signal_match_validator.cpp | 287 ++++++++++++++++++ .../src/traffic_light_arbiter.cpp | 50 ++- 7 files changed, 459 insertions(+), 10 deletions(-) create mode 100644 perception/traffic_light_arbiter/include/traffic_light_arbiter/signal_match_validator.hpp create mode 100644 perception/traffic_light_arbiter/src/signal_match_validator.cpp diff --git a/perception/traffic_light_arbiter/CMakeLists.txt b/perception/traffic_light_arbiter/CMakeLists.txt index 181c2bec867d1..a350bdcf73cba 100644 --- a/perception/traffic_light_arbiter/CMakeLists.txt +++ b/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} diff --git a/perception/traffic_light_arbiter/README.md b/perception/traffic_light_arbiter/README.md index 952119020c215..ad25a6514ea1f 100644 --- a/perception/traffic_light_arbiter/README.md +++ b/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 | diff --git a/perception/traffic_light_arbiter/config/traffic_light_arbiter.param.yaml b/perception/traffic_light_arbiter/config/traffic_light_arbiter.param.yaml index 5dc2b62eaa446..dfe12ff352f16 100644 --- a/perception/traffic_light_arbiter/config/traffic_light_arbiter.param.yaml +++ b/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 diff --git a/perception/traffic_light_arbiter/include/traffic_light_arbiter/signal_match_validator.hpp b/perception/traffic_light_arbiter/include/traffic_light_arbiter/signal_match_validator.hpp new file mode 100644 index 0000000000000..68c9fb5ce9964 --- /dev/null +++ b/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 +#include + +#include + +#include + +#include +#include +#include +#include + +/** + * @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 & 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 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_ diff --git a/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp b/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp index 485ce84c5fea6..155e5e619062e 100644 --- a/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp +++ b/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 +#include #include #include @@ -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 signal_match_validator_; }; #endif // TRAFFIC_LIGHT_ARBITER__TRAFFIC_LIGHT_ARBITER_HPP_ diff --git a/perception/traffic_light_arbiter/src/signal_match_validator.cpp b/perception/traffic_light_arbiter/src/signal_match_validator.cpp new file mode 100644 index 0000000000000..312f2e596a493 --- /dev/null +++ b/perception/traffic_light_arbiter/src/signal_match_validator.cpp @@ -0,0 +1,287 @@ +// 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. + +#include "traffic_light_arbiter/signal_match_validator.hpp" + +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 find_signal_by_id( + const std::unordered_map & id_signal_map, int64_t signal_id) +{ + auto it = id_signal_map.find(signal_id); + if (it != id_signal_map.end()) { + return it->second; // Return the found signal + } else { + return std::nullopt; // Return an empty optional if not found + } +} + +// Creates a map from signal IDs to TrafficSignal objects. +std::unordered_map create_id_signal_map( + const TrafficSignalArray & traffic_signals) +{ + std::unordered_map id_signal_map; + for (const auto & traffic_signal : traffic_signals.signals) { + id_signal_map[traffic_signal.traffic_signal_id] = traffic_signal; + } + + return id_signal_map; +} + +// 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) +{ + Element signal_element; + signal_element.color = color; + signal_element.shape = shape; + signal_element.status = status; + signal_element.confidence = confidence; + + return signal_element; +} + +// Creates unknown elements for each unique shape from two element vectors +std::vector create_unknown_elements( + const std::vector & elements1, const std::vector & elements2) +{ + std::unordered_set shape_set; + for (const auto & element : elements1) { + shape_set.emplace(element.shape); + } + for (const auto & element : elements2) { + shape_set.emplace(element.shape); + } + + std::vector unknown_elements; + for (const auto & shape : shape_set) { + // 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 +TrafficSignal create_unknown_signal(const TrafficSignal & traffic_signal) +{ + TrafficSignal unknown_signal; + unknown_signal.traffic_signal_id = traffic_signal.traffic_signal_id; + for (const auto & element : traffic_signal.elements) { + // 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); + } + + 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; + + // 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); + 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 & signal1, const std::vector & signal2) +{ + // Returns false if vectors have different sizes + if (signal1.size() != signal2.size()) { + return false; + } + + // Sorts copies of the vectors by shape for comparison + std::vector sorted_signal1 = signal1; + std::vector 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 + 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 create_signal_id_set( + const std::vector & signals1, const std::vector & signals2) +{ + std::unordered_set signal_id_set; + for (const auto & traffic_signal : signals1) { + signal_id_set.emplace(traffic_signal.traffic_signal_id); + } + for (const auto & traffic_signal : signals2) { + signal_id_set.emplace(traffic_signal.traffic_signal_id); + } + + return signal_id_set; +} + +// Returns the signal with the highest confidence elements, considering a external priority +TrafficSignal get_highest_confidence_signal( + const std::optional & perception_signal, + const std::optional & external_signal, const bool external_priority) +{ + // 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 + if (external_priority) { + return *external_signal; + } + + // Compiles elements into a map by shape, to compare their confidences + using Key = Element::_shape_type; + std::map> shape_element_map; + for (const auto & element : perception_signal->elements) { + shape_element_map[element.shape].emplace_back(element); + } + for (const auto & element : external_signal->elements) { + shape_element_map[element.shape].emplace_back(element); + } + + TrafficSignal highest_confidence_signal; + + // 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 (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; }); + highest_confidence_signal.elements.emplace_back(*highest_confidence_element); + } + + return highest_confidence_signal; +} + +// 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 { + return stamp2; + } +} + +} // namespace util + +autoware_perception_msgs::msg::TrafficSignalArray SignalMatchValidator::validateSignals( + const TrafficSignalArray & perception_signals, const TrafficSignalArray & external_signals) +{ + TrafficSignalArray validated_signals; + + // Set newer stamp + validated_signals.stamp = util::get_newer_stamp(perception_signals.stamp, external_signals.stamp); + + // Create a map from signals to reduce the calculation cost + const auto perception_id_signal_map = util::create_id_signal_map(perception_signals); + const auto external_id_signal_map = util::create_id_signal_map(external_signals); + + // Create the unique set of the received id, + // then compare the signal element for each received signal id + const auto received_signal_id_set = + util::create_signal_id_set(perception_signals.signals, external_signals.signals); + + for (const auto & signal_id : received_signal_id_set) { + const auto perception_result = util::find_signal_by_id(perception_id_signal_map, signal_id); + const auto external_result = util::find_signal_by_id(external_id_signal_map, signal_id); + // Neither result exists + if (!perception_result && !external_result) { + continue; + } + + // We don't validate the pedestrian signals + // TODO(TomohitoAndo): Validate pedestrian signals + if (isPedestrianSignal(signal_id)) { + validated_signals.signals.emplace_back(util::get_highest_confidence_signal( + perception_result, external_result, external_priority_)); + + continue; + } + + // If either of the signal is not received, treat as unknown signal + if (!perception_result && external_result) { + const auto unknown_signal = util::create_unknown_signal(*external_result); + validated_signals.signals.emplace_back(unknown_signal); + continue; + } + if (!external_result && perception_result) { + const auto unknown_signal = util::create_unknown_signal(*perception_result); + validated_signals.signals.emplace_back(unknown_signal); + continue; + } + + // Check if they have the same elements + if (!util::are_all_elements_equivalent( + perception_result->elements, external_result->elements)) { + 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 + validated_signals.signals.emplace_back(*perception_result); + } + + return validated_signals; +} + +void SignalMatchValidator::setPedestrianSignals( + const std::vector & pedestrian_signals) +{ + for (const auto & pedestrian_signal : pedestrian_signals) { + map_pedestrian_signal_regulatory_elements_set_.emplace(pedestrian_signal->id()); + } +} + +void SignalMatchValidator::setExternalPriority(const bool external_priority) +{ + external_priority_ = external_priority; +} + +bool SignalMatchValidator::isPedestrianSignal(const lanelet::Id & signal_id) +{ + return map_pedestrian_signal_regulatory_elements_set_.find(signal_id) != + map_pedestrian_signal_regulatory_elements_set_.end(); +} diff --git a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp index 3838e93113066..f9a1888139071 100644 --- a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp +++ b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include @@ -40,6 +41,25 @@ std::vector filter_traffic_signals(const LaneletMapConstPt signals.push_back(signal); } } + + return signals; +} + +std::vector filter_pedestrian_signals(const LaneletMapConstPtr map) +{ + namespace query = lanelet::utils::query; + + const auto all_lanelets = query::laneletLayer(map); + const auto crosswalks = query::crosswalkLanelets(all_lanelets); + std::vector signals; + + for (const auto & crosswalk : crosswalks) { + const auto traffic_light_reg_elems = + crosswalk.regulatoryElementsAs(); + std::copy( + traffic_light_reg_elems.begin(), traffic_light_reg_elems.end(), std::back_inserter(signals)); + } + return signals; } @@ -51,6 +71,12 @@ TrafficLightArbiter::TrafficLightArbiter(const rclcpp::NodeOptions & options) external_time_tolerance_ = this->declare_parameter("external_time_tolerance", 5.0); perception_time_tolerance_ = this->declare_parameter("perception_time_tolerance", 1.0); external_priority_ = this->declare_parameter("external_priority", false); + enable_signal_matching_ = this->declare_parameter("enable_signal_matching", false); + + if (enable_signal_matching_) { + signal_match_validator_ = std::make_unique(); + signal_match_validator_->setExternalPriority(external_priority_); + } map_sub_ = create_subscription( "~/sub/vector_map", rclcpp::QoS(1).transient_local(), @@ -78,6 +104,12 @@ void TrafficLightArbiter::onMap(const LaneletMapBin::ConstSharedPtr msg) for (const auto & signal : signals) { map_regulatory_elements_set_->emplace(signal->id()); } + + if (enable_signal_matching_) { + // Filter only pedestrian signals to distinguish them in signal matching + const auto pedestrian_signals = lanelet::filter_pedestrian_signals(map); + signal_match_validator_->setPedestrianSignals(pedestrian_signals); + } } void TrafficLightArbiter::onPerceptionMsg(const TrafficSignalArray::ConstSharedPtr msg) @@ -140,12 +172,20 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim } }; - for (const auto & signal : latest_perception_msg_.signals) { - add_signal_function(signal, false); - } + if (enable_signal_matching_) { + const auto validated_signals = + signal_match_validator_->validateSignals(latest_perception_msg_, latest_external_msg_); + for (const auto & signal : validated_signals.signals) { + add_signal_function(signal, false); + } + } else { + for (const auto & signal : latest_perception_msg_.signals) { + add_signal_function(signal, false); + } - for (const auto & signal : latest_external_msg_.signals) { - add_signal_function(signal, external_priority_); + for (const auto & signal : latest_external_msg_.signals) { + add_signal_function(signal, external_priority_); + } } const auto get_highest_confidence_elements =