Skip to content

Commit 95fd4dc

Browse files
committed
use unordered_map to reduce calculation cost
Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
1 parent ffaa00d commit 95fd4dc

File tree

1 file changed

+22
-9
lines changed

1 file changed

+22
-9
lines changed

perception/traffic_light_arbiter/src/signal_match_validator.cpp

+22-9
Original file line numberDiff line numberDiff line change
@@ -23,18 +23,28 @@ using Time = builtin_interfaces::msg::Time;
2323

2424
// Finds a signal by its ID within a TrafficSignalArray
2525
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)
2727
{
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
3331
} else {
3432
return std::nullopt; // Return an empty optional if not found
3533
}
3634
}
3735

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+
3848
// Creates a TrafficSignalElement with specified attributes
3949
Element create_element(
4050
const Element::_color_type & color, const Element::_shape_type & shape,
@@ -204,15 +214,18 @@ autoware_perception_msgs::msg::TrafficSignalArray SignalMatchValidator::validate
204214
// Set newer stamp
205215
validated_signals.stamp = util::get_newer_stamp(perception_signals.stamp, external_signals.stamp);
206216

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+
207221
// Create the unique set of the received id,
208222
// then compare the signal element for each received signal id
209223
const auto received_signal_id_set =
210224
util::create_signal_id_set(perception_signals.signals, external_signals.signals);
211225

212226
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);
216229
// Neither result exists
217230
if (!perception_result && !external_result) {
218231
continue;

0 commit comments

Comments
 (0)