Skip to content

Commit c8b5347

Browse files
TomohitoAndopre-commit-ci[bot]
authored and
karishma1911
committed
feat(traffic_light_arbiter): add signal match validator (autowarefoundation#6423)
* feat(traffic_light_arbiter): add signal match validator Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix comments Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * add an explanation of signal match validator to readme Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * update readme Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * style(pre-commit): autofix * fix comments Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * use std::copy instead of std::transform Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * use unordered_map to reduce calculation cost Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * avoid using `signal` because it is used in std::signal Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> --------- Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent e607aba commit c8b5347

File tree

7 files changed

+459
-10
lines changed

7 files changed

+459
-10
lines changed

‎perception/traffic_light_arbiter/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ autoware_package()
66

77
ament_auto_add_library(${PROJECT_NAME} SHARED
88
src/traffic_light_arbiter.cpp
9+
src/signal_match_validator.cpp
910
)
1011

1112
rclcpp_components_register_nodes(${PROJECT_NAME}

‎perception/traffic_light_arbiter/README.md

+19-5
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,19 @@ This package receives traffic signals from perception and external (e.g., V2X) c
88

99
A node that merges traffic light/signal state from image recognition and external (e.g., V2X) systems to provide to a planning component.
1010

11+
### Signal Match Validator
12+
13+
When `enable_signal_matching` is set to true, this node validates the match between perception signals and external signals.
14+
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).
15+
16+
| External \ Perception | RED | AMBER | GREEN | UNKNOWN | Not Received |
17+
| --------------------- | ------- | ------- | ------- | ------- | ------------ |
18+
| RED | RED | UNKNOWN | UNKNOWN | UNKNOWN | UNKNOWN |
19+
| AMBER | UNKNOWN | AMBER | UNKNOWN | UNKNOWN | UNKNOWN |
20+
| GREEN | UNKNOWN | UNKNOWN | GREEN | UNKNOWN | UNKNOWN |
21+
| UNKNOWN | UNKNOWN | UNKNOWN | UNKNOWN | UNKNOWN | UNKNOWN |
22+
| Not Received | UNKNOWN | UNKNOWN | UNKNOWN | UNKNOWN | UNKNOWN |
23+
1124
### Inputs / Outputs
1225

1326
#### Input
@@ -28,8 +41,9 @@ A node that merges traffic light/signal state from image recognition and externa
2841

2942
### Core Parameters
3043

31-
| Name | Type | Default Value | Description |
32-
| --------------------------- | ------ | ------------- | -------------------------------------------------------------------------------------------------------------------------------- |
33-
| `external_time_tolerance` | double | 5.0 | The duration in seconds an external message is considered valid for merging |
34-
| `perception_time_tolerance` | double | 1.0 | The duration in seconds a perception message is considered valid for merging |
35-
| `external_priority` | bool | false | Whether or not externals signals take precedence over perception-based ones. If false, the merging uses confidence as a criteria |
44+
| Name | Type | Default Value | Description |
45+
| --------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
46+
| `external_time_tolerance` | double | 5.0 | The duration in seconds an external message is considered valid for merging |
47+
| `perception_time_tolerance` | double | 1.0 | The duration in seconds a perception message is considered valid for merging |
48+
| `external_priority` | bool | false | Whether or not externals signals take precedence over perception-based ones. If false, the merging uses confidence as a criteria |
49+
| `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

+1
Original file line numberDiff line numberDiff line change
@@ -3,3 +3,4 @@
33
external_time_tolerance: 5.0
44
perception_time_tolerance: 1.0
55
external_priority: false
6+
enable_signal_matching: false

‎perception/traffic_light_arbiter/include/traffic_light_arbiter/signal_match_validator.hpp

+103
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,103 @@
1+
// Copyright 2024 The Autoware Contributors
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef TRAFFIC_LIGHT_ARBITER__SIGNAL_MATCH_VALIDATOR_HPP_
16+
#define TRAFFIC_LIGHT_ARBITER__SIGNAL_MATCH_VALIDATOR_HPP_
17+
18+
#include <lanelet2_extension/utility/query.hpp>
19+
#include <rclcpp/rclcpp.hpp>
20+
21+
#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
22+
23+
#include <lanelet2_core/LaneletMap.h>
24+
25+
#include <memory>
26+
#include <optional>
27+
#include <unordered_set>
28+
#include <vector>
29+
30+
/**
31+
* @class SignalMatchValidator
32+
* @brief Validates and compares traffic signal data from different sources.
33+
*
34+
* This class is designed to compare and validate traffic signal information
35+
* received from internal (e.g. camera perception) and external (e.g. V2I communication).
36+
* It aims to ensure that the traffic signal information from both sources is consistent.
37+
*/
38+
class SignalMatchValidator
39+
{
40+
public:
41+
using TrafficSignalArray = autoware_perception_msgs::msg::TrafficSignalArray;
42+
using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal;
43+
using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
44+
using TrafficLightConstPtr = lanelet::TrafficLightConstPtr;
45+
46+
/**
47+
* @brief Default constructor for SignalMatchValidator.
48+
*/
49+
SignalMatchValidator() = default;
50+
51+
/**
52+
* @brief Validates and compares traffic signal data from perception and external sources.
53+
*
54+
* This method compares traffic signal data obtained from perception results
55+
* with data received from external source. It aims to find and return signals
56+
* that are present and consistent in both datasets. If signals are not consistent,
57+
* treat them as the unknown signal.
58+
*
59+
* @param perception_signals Traffic signal data from perception.
60+
* @param external_signals Traffic signal data from external source.
61+
* @return A validated TrafficSignalArray.
62+
*/
63+
TrafficSignalArray validateSignals(
64+
const TrafficSignalArray & perception_signals, const TrafficSignalArray & external_signals);
65+
66+
/**
67+
* @brief Sets the pedestrian signals to be considered during validation.
68+
*
69+
* This method allows the specification of pedestrian signals, which are then
70+
* used to adjust the validation logic, acknowledging their unique characteristics
71+
* in traffic signal datasets.
72+
*
73+
* @param pedestrian_signals A vector of pedestrian signal pointers.
74+
*/
75+
void setPedestrianSignals(const std::vector<TrafficLightConstPtr> & pedestrian_signals);
76+
77+
/**
78+
* @brief Sets the priority flag for using external signal data over perception data.
79+
*
80+
* When set to true, this flag indicates that in cases of discrepancy between
81+
* perception and external signal data, the external data should be prioritized.
82+
*
83+
* @param external_priority The priority flag for external signal data.
84+
*/
85+
void setExternalPriority(const bool external_priority);
86+
87+
private:
88+
bool external_priority_;
89+
std::unordered_set<lanelet::Id> map_pedestrian_signal_regulatory_elements_set_;
90+
91+
/**
92+
* @brief Checks if a given signal ID corresponds to a pedestrian signal.
93+
*
94+
* This method determines whether a signal, identified by its ID, is classified
95+
* as a pedestrian signal.
96+
*
97+
* @param signal_id The ID of the signal to check.
98+
* @return True if the signal is a pedestrian signal, false otherwise.
99+
*/
100+
bool isPedestrianSignal(const lanelet::Id & signal_id);
101+
};
102+
103+
#endif // TRAFFIC_LIGHT_ARBITER__SIGNAL_MATCH_VALIDATOR_HPP_

‎perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp

+3
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
#define TRAFFIC_LIGHT_ARBITER__TRAFFIC_LIGHT_ARBITER_HPP_
1717

1818
#include <rclcpp/rclcpp.hpp>
19+
#include <traffic_light_arbiter/signal_match_validator.hpp>
1920

2021
#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
2122
#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
@@ -51,9 +52,11 @@ class TrafficLightArbiter : public rclcpp::Node
5152
double external_time_tolerance_;
5253
double perception_time_tolerance_;
5354
bool external_priority_;
55+
bool enable_signal_matching_;
5456

5557
TrafficSignalArray latest_perception_msg_;
5658
TrafficSignalArray latest_external_msg_;
59+
std::unique_ptr<SignalMatchValidator> signal_match_validator_;
5760
};
5861

5962
#endif // TRAFFIC_LIGHT_ARBITER__TRAFFIC_LIGHT_ARBITER_HPP_

0 commit comments

Comments
 (0)