Skip to content

Commit 9219335

Browse files
authored
Merge pull request #1608 from tier4/fix/traffic_light_utils_nullopt
fix(traffic_light_utils): prevent accessing nullopt (autowarefoundation#9163)
2 parents 3c27a1f + b0a5c36 commit 9219335

File tree

1 file changed

+8
-4
lines changed

1 file changed

+8
-4
lines changed

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp

+8-4
Original file line numberDiff line numberDiff line change
@@ -34,11 +34,13 @@ double getDistanceToNextTrafficLight(
3434
lanelet::utils::to2D(lanelet_point).basicPoint());
3535

3636
for (const auto & element : current_lanelet.regulatoryElementsAs<lanelet::TrafficLight>()) {
37-
lanelet::ConstLineString3d lanelet_stop_lines = element->stopLine().value();
37+
const auto lanelet_stop_lines = element->stopLine();
38+
39+
if (!lanelet_stop_lines.has_value()) continue;
3840

3941
const auto to_stop_line = lanelet::geometry::toArcCoordinates(
4042
lanelet::utils::to2D(current_lanelet.centerline()),
41-
lanelet::utils::to2D(lanelet_stop_lines).front().basicPoint());
43+
lanelet::utils::to2D(lanelet_stop_lines.value()).front().basicPoint());
4244

4345
const auto distance_object_to_stop_line = to_stop_line.length - to_object.length;
4446

@@ -61,11 +63,13 @@ double getDistanceToNextTrafficLight(
6163
}
6264

6365
for (const auto & element : llt.regulatoryElementsAs<lanelet::TrafficLight>()) {
64-
lanelet::ConstLineString3d lanelet_stop_lines = element->stopLine().value();
66+
const auto lanelet_stop_lines = element->stopLine();
67+
68+
if (!lanelet_stop_lines.has_value()) continue;
6569

6670
const auto to_stop_line = lanelet::geometry::toArcCoordinates(
6771
lanelet::utils::to2D(llt.centerline()),
68-
lanelet::utils::to2D(lanelet_stop_lines).front().basicPoint());
72+
lanelet::utils::to2D(lanelet_stop_lines.value()).front().basicPoint());
6973

7074
return distance + to_stop_line.length - to_object.length;
7175
}

0 commit comments

Comments
 (0)