@@ -23,18 +23,28 @@ using Time = builtin_interfaces::msg::Time;
23
23
24
24
// Finds a signal by its ID within a TrafficSignalArray
25
25
std::optional<TrafficSignal> find_signal_by_id (
26
- const TrafficSignalArray & signals , int64_t signal_id)
26
+ const std::unordered_map<lanelet::Id, TrafficSignal> & id_signal_map , int64_t signal_id)
27
27
{
28
- auto it = std::find_if (
29
- signals.signals .begin (), signals.signals .end (),
30
- [signal_id](const TrafficSignal & signal ) { return signal .traffic_signal_id == signal_id; });
31
- if (it != signals.signals .end ()) {
32
- return *it; // Return the found signal
28
+ auto it = id_signal_map.find (signal_id);
29
+ if (it != id_signal_map.end ()) {
30
+ return it->second ; // Return the found signal
33
31
} else {
34
32
return std::nullopt; // Return an empty optional if not found
35
33
}
36
34
}
37
35
36
+ // Creates a map from signal IDs to TrafficSignal objects.
37
+ std::unordered_map<lanelet::Id, TrafficSignal> create_id_signal_map (
38
+ const TrafficSignalArray & signals)
39
+ {
40
+ std::unordered_map<lanelet::Id, TrafficSignal> id_signal_map;
41
+ for (const auto & signal : signals.signals ) {
42
+ id_signal_map[signal .traffic_signal_id ] = signal ;
43
+ }
44
+
45
+ return id_signal_map;
46
+ }
47
+
38
48
// Creates a TrafficSignalElement with specified attributes
39
49
Element create_element (
40
50
const Element::_color_type & color, const Element::_shape_type & shape,
@@ -204,15 +214,18 @@ autoware_perception_msgs::msg::TrafficSignalArray SignalMatchValidator::validate
204
214
// Set newer stamp
205
215
validated_signals.stamp = util::get_newer_stamp (perception_signals.stamp , external_signals.stamp );
206
216
217
+ // Create a map from signals to reduce the calculation cost
218
+ const auto perception_id_signal_map = util::create_id_signal_map (perception_signals);
219
+ const auto external_id_signal_map = util::create_id_signal_map (external_signals);
220
+
207
221
// Create the unique set of the received id,
208
222
// then compare the signal element for each received signal id
209
223
const auto received_signal_id_set =
210
224
util::create_signal_id_set (perception_signals.signals , external_signals.signals );
211
225
212
226
for (const auto & signal_id : received_signal_id_set) {
213
- const auto perception_result = util::find_signal_by_id (perception_signals, signal_id);
214
- const auto external_result = util::find_signal_by_id (external_signals, signal_id);
215
-
227
+ const auto perception_result = util::find_signal_by_id (perception_id_signal_map, signal_id);
228
+ const auto external_result = util::find_signal_by_id (external_id_signal_map, signal_id);
216
229
// Neither result exists
217
230
if (!perception_result && !external_result) {
218
231
continue ;
0 commit comments