Skip to content

Commit b891653

Browse files
XiaoyuWang0601soblinpre-commit-ci-lite[bot]
authored
feat(autoware_traffic_light_utils): rewrite hasTrafficLightCircleColor and hasTrafficLightShape into three functions to handle overseas color arrow traffic light (#12481)
* feat(autoware_traffic_light_utils): merge hasTrafficLightCirleColor and hasTrafficLightShape into a general function hasTrafficLightShapeColor to handle oversea color arrow traffic light Signed-off-by: XiaoyuWang0601 <xiaoyu.wang@tier4.jp> * feat(autoware_traffic_light_utils): merge hasTrafficLightCirleColor and hasTrafficLightShape into a general function hasTrafficLightShapeColor to handle oversea color arrow traffic light Signed-off-by: XiaoyuWang0601 <xiaoyu.wang@tier4.jp> * fix: modify default parameter for hasTrafficLightShapeColor Signed-off-by: XiaoyuWang0601 <xiaoyu.wang@tier4.jp> * fix: separate hasTrafficLightShapeColor into three functions Signed-off-by: XiaoyuWang0601 <xiaoyu.wang@tier4.jp> * chore(miscs): remove unused lanelet2 extension header (#12081) chore(miscs): remove unused header include for lanelet2_extension Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> Signed-off-by: XiaoyuWang0601 <xiaoyu.wang@tier4.jp> * fix: revert modification Signed-off-by: XiaoyuWang0601 <xiaoyu.wang@tier4.jp> * fix: revert modification Signed-off-by: XiaoyuWang0601 <xiaoyu.wang@tier4.jp> * style(pre-commit): autofix Signed-off-by: XiaoyuWang0601 <xiaoyu.wang@tier4.jp> * fix: change TrafficLightElement msg belonging Signed-off-by: XiaoyuWang0601 <xiaoyu.wang@tier4.jp> --------- Signed-off-by: XiaoyuWang0601 <xiaoyu.wang@tier4.jp> Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> Co-authored-by: Mamoru Sobue <hilo.soblin@gmail.com> Co-authored-by: pre-commit-ci-lite[bot] <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com>
1 parent d559cea commit b891653

5 files changed

Lines changed: 83 additions & 51 deletions

File tree

‎common/autoware_traffic_light_utils/include/autoware/traffic_light_utils/traffic_light_utils.hpp‎

Lines changed: 22 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -36,31 +36,38 @@ namespace autoware::traffic_light_utils
3636
void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float confidence);
3737

3838
/**
39-
* @brief Checks if a traffic light state includes a circle-shaped light with the specified color.
39+
* @brief Checks if a traffic light state includes a light with the specified shape.
4040
*
41-
* Iterates through the traffic light elements to find a circle-shaped light that matches the given
42-
* color.
41+
* @param elements The traffic light elements to check.
42+
* @param lamp_shape The shape to look for.
43+
* @return True if a light with the specified shape is found, false otherwise.
44+
*/
45+
bool hasTrafficLightShape(
46+
const std::vector<autoware_perception_msgs::msg::TrafficLightElement> & elements,
47+
const uint8_t & lamp_shape);
48+
49+
/**
50+
* @brief Checks if a traffic light state includes a light with the specified color.
4351
*
44-
* @param tl_state The traffic light state to check.
45-
* @param lamp_color The color to look for in the traffic light's circle-shaped lamps.
46-
* @return True if a circle-shaped light with the specified color is found, false otherwise.
52+
* @param elements The traffic light elements to check.
53+
* @param lamp_color The color to look for.
54+
* @return True if a light with the specified color is found, false otherwise.
4755
*/
48-
bool hasTrafficLightCircleColor(
56+
bool hasTrafficLightColor(
4957
const std::vector<autoware_perception_msgs::msg::TrafficLightElement> & elements,
5058
const uint8_t & lamp_color);
5159

5260
/**
53-
* @brief Checks if a traffic light state includes a light with the specified shape.
54-
*
55-
* Searches through the traffic light elements to find a light that matches the given shape.
61+
* @brief Checks if a traffic light state includes a light with the specified shape and color.
5662
*
57-
* @param tl_state The traffic light state to check.
58-
* @param shape The shape to look for in the traffic light's lights.
59-
* @return True if a light with the specified shape is found, false otherwise.
63+
* @param elements The traffic light elements to check.
64+
* @param lamp_shape The shape to look for.
65+
* @param lamp_color The color to look for.
66+
* @return True if a light matching both shape and color is found, false otherwise.
6067
*/
61-
bool hasTrafficLightShape(
68+
bool hasTrafficLightShapeAndColor(
6269
const std::vector<autoware_perception_msgs::msg::TrafficLightElement> & elements,
63-
const uint8_t & lamp_shape);
70+
const uint8_t & lamp_shape, const uint8_t & lamp_color);
6471

6572
/**
6673
* @brief Determines if a traffic signal indicates a stop for the given lanelet.

‎common/autoware_traffic_light_utils/src/traffic_light_utils.cpp‎

Lines changed: 32 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -29,26 +29,36 @@ void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float c
2929
signal.elements[0].color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN;
3030
signal.elements[0].confidence = confidence;
3131
}
32-
bool hasTrafficLightCircleColor(
32+
bool hasTrafficLightShape(
3333
const std::vector<autoware_perception_msgs::msg::TrafficLightElement> & elements,
34-
const uint8_t & lamp_color)
34+
const uint8_t & lamp_shape)
3535
{
36-
const auto it_lamp =
37-
std::find_if(elements.begin(), elements.end(), [&lamp_color](const auto & x) {
38-
return x.shape == autoware_perception_msgs::msg::TrafficLightElement::CIRCLE &&
39-
x.color == lamp_color;
40-
});
36+
const auto it_lamp = std::find_if(
37+
elements.begin(), elements.end(),
38+
[&lamp_shape](const auto & x) { return x.shape == lamp_shape; });
4139

4240
return it_lamp != elements.end();
4341
}
4442

45-
bool hasTrafficLightShape(
43+
bool hasTrafficLightColor(
4644
const std::vector<autoware_perception_msgs::msg::TrafficLightElement> & elements,
47-
const uint8_t & lamp_shape)
45+
const uint8_t & lamp_color)
4846
{
4947
const auto it_lamp = std::find_if(
5048
elements.begin(), elements.end(),
51-
[&lamp_shape](const auto & x) { return x.shape == lamp_shape; });
49+
[&lamp_color](const auto & x) { return x.color == lamp_color; });
50+
51+
return it_lamp != elements.end();
52+
}
53+
54+
bool hasTrafficLightShapeAndColor(
55+
const std::vector<autoware_perception_msgs::msg::TrafficLightElement> & elements,
56+
const uint8_t & lamp_shape, const uint8_t & lamp_color)
57+
{
58+
const auto it_lamp =
59+
std::find_if(elements.begin(), elements.end(), [&lamp_shape, &lamp_color](const auto & x) {
60+
return x.shape == lamp_shape && x.color == lamp_color;
61+
});
5262

5363
return it_lamp != elements.end();
5464
}
@@ -58,8 +68,9 @@ bool isTrafficSignalStop(
5868
const autoware_perception_msgs::msg::TrafficLightGroup & tl_state)
5969
{
6070
const auto & elements = tl_state.elements;
61-
if (hasTrafficLightCircleColor(
62-
elements, autoware_perception_msgs::msg::TrafficLightElement::GREEN)) {
71+
if (hasTrafficLightShapeAndColor(
72+
elements, autoware_perception_msgs::msg::TrafficLightElement::CIRCLE,
73+
autoware_perception_msgs::msg::TrafficLightElement::GREEN)) {
6374
return false;
6475
}
6576

@@ -70,19 +81,23 @@ bool isTrafficSignalStop(
7081
}
7182
if (
7283
turn_direction == "right" &&
73-
hasTrafficLightShape(
74-
elements, autoware_perception_msgs::msg::TrafficLightElement::RIGHT_ARROW)) {
84+
hasTrafficLightShapeAndColor(
85+
elements, autoware_perception_msgs::msg::TrafficLightElement::RIGHT_ARROW,
86+
autoware_perception_msgs::msg::TrafficLightElement::GREEN)) {
7587
return false;
7688
}
7789
if (
7890
turn_direction == "left" &&
79-
hasTrafficLightShape(
80-
elements, autoware_perception_msgs::msg::TrafficLightElement::LEFT_ARROW)) {
91+
hasTrafficLightShapeAndColor(
92+
elements, autoware_perception_msgs::msg::TrafficLightElement::LEFT_ARROW,
93+
autoware_perception_msgs::msg::TrafficLightElement::GREEN)) {
8194
return false;
8295
}
8396
if (
8497
turn_direction == "straight" &&
85-
hasTrafficLightShape(elements, autoware_perception_msgs::msg::TrafficLightElement::UP_ARROW)) {
98+
hasTrafficLightShapeAndColor(
99+
elements, autoware_perception_msgs::msg::TrafficLightElement::UP_ARROW,
100+
autoware_perception_msgs::msg::TrafficLightElement::GREEN)) {
86101
return false;
87102
}
88103

‎planning/autoware_trajectory_validator/src/filters/traffic_rule/traffic_light_filter.cpp‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -109,12 +109,14 @@ TrafficLightFilter::get_stop_lines(
109109
std::vector<lanelet::BasicLineString2d> amber_stop_lines;
110110
for (const auto & [stop_line, signal] :
111111
collect_stop_lines(lanelet_map, traffic_lights.traffic_light_groups)) {
112-
if (traffic_light_utils::hasTrafficLightCircleColor(
113-
signal.elements, tier4_perception_msgs::msg::TrafficLightElement::RED)) {
112+
if (traffic_light_utils::hasTrafficLightShapeAndColor(
113+
signal.elements, autoware_perception_msgs::msg::TrafficLightElement::CIRCLE,
114+
autoware_perception_msgs::msg::TrafficLightElement::RED)) {
114115
red_stop_lines.push_back(stop_line);
115116
}
116-
if (traffic_light_utils::hasTrafficLightCircleColor(
117-
signal.elements, tier4_perception_msgs::msg::TrafficLightElement::AMBER)) {
117+
if (traffic_light_utils::hasTrafficLightShapeAndColor(
118+
signal.elements, autoware_perception_msgs::msg::TrafficLightElement::CIRCLE,
119+
autoware_perception_msgs::msg::TrafficLightElement::AMBER)) {
118120
amber_stop_lines.push_back(stop_line);
119121
}
120122
}

‎planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.cpp‎

Lines changed: 14 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -172,11 +172,11 @@ bool isTrafficSignalRedStop(
172172
const lanelet::ConstLanelet & lanelet,
173173
const std::vector<autoware_perception_msgs::msg::TrafficLightElement> & elements)
174174
{
175-
using autoware::traffic_light_utils::hasTrafficLightCircleColor;
176-
using autoware::traffic_light_utils::hasTrafficLightShape;
175+
using autoware::traffic_light_utils::hasTrafficLightShapeAndColor;
177176

178-
if (!hasTrafficLightCircleColor(
179-
elements, autoware_perception_msgs::msg::TrafficLightElement::RED)) {
177+
if (!hasTrafficLightShapeAndColor(
178+
elements, autoware_perception_msgs::msg::TrafficLightElement::CIRCLE,
179+
autoware_perception_msgs::msg::TrafficLightElement::RED)) {
180180
return false;
181181
}
182182

@@ -188,21 +188,26 @@ bool isTrafficSignalRedStop(
188188

189189
if (
190190
autoware::experimental::lanelet2_utils::is_right_direction(lanelet) &&
191-
hasTrafficLightShape(
192-
elements, autoware_perception_msgs::msg::TrafficLightElement::RIGHT_ARROW)) {
191+
hasTrafficLightShapeAndColor(
192+
elements, autoware_perception_msgs::msg::TrafficLightElement::RIGHT_ARROW,
193+
autoware_perception_msgs::msg::TrafficLightElement::GREEN)) {
193194
return false;
194195
}
195196
if (
196197
autoware::experimental::lanelet2_utils::is_left_direction(lanelet) &&
197-
hasTrafficLightShape(
198-
elements, autoware_perception_msgs::msg::TrafficLightElement::LEFT_ARROW)) {
198+
hasTrafficLightShapeAndColor(
199+
elements, autoware_perception_msgs::msg::TrafficLightElement::LEFT_ARROW,
200+
autoware_perception_msgs::msg::TrafficLightElement::GREEN)) {
199201
return false;
200202
}
201203
if (
202204
autoware::experimental::lanelet2_utils::is_straight_direction(lanelet) &&
203-
hasTrafficLightShape(elements, autoware_perception_msgs::msg::TrafficLightElement::UP_ARROW)) {
205+
hasTrafficLightShapeAndColor(
206+
elements, autoware_perception_msgs::msg::TrafficLightElement::UP_ARROW,
207+
autoware_perception_msgs::msg::TrafficLightElement::GREEN)) {
204208
return false;
205209
}
210+
206211
return true;
207212
}
208213

‎planning/planning_validator/autoware_planning_validator_intersection_collision_checker/src/utils.cpp‎

Lines changed: 9 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -197,8 +197,9 @@ void set_right_turn_target_lanelets(
197197
const auto traffic_light_elements = context->get_traffic_signal(regulatory_element->id());
198198
if (!traffic_light_elements.has_value()) continue;
199199

200-
if (autoware::traffic_light_utils::hasTrafficLightShape(
201-
traffic_light_elements.value(), TrafficLightElement::RIGHT_ARROW)) {
200+
if (autoware::traffic_light_utils::hasTrafficLightShapeAndColor(
201+
traffic_light_elements.value(), TrafficLightElement::RIGHT_ARROW,
202+
TrafficLightElement::GREEN)) {
202203
return true;
203204
}
204205
}
@@ -295,13 +296,15 @@ void set_left_turn_target_lanelets(
295296
const auto traffic_light_elements = context->get_traffic_signal(regulatory_element->id());
296297
if (!traffic_light_elements.has_value()) continue;
297298

298-
if (autoware::traffic_light_utils::hasTrafficLightCircleColor(
299-
traffic_light_elements.value(), TrafficLightElement::GREEN)) {
299+
if (autoware::traffic_light_utils::hasTrafficLightShapeAndColor(
300+
traffic_light_elements.value(), TrafficLightElement::CIRCLE,
301+
TrafficLightElement::GREEN)) {
300302
return true;
301303
}
302304

303-
if (autoware::traffic_light_utils::hasTrafficLightCircleColor(
304-
traffic_light_elements.value(), TrafficLightElement::AMBER)) {
305+
if (autoware::traffic_light_utils::hasTrafficLightShapeAndColor(
306+
traffic_light_elements.value(), TrafficLightElement::CIRCLE,
307+
TrafficLightElement::AMBER)) {
305308
return true;
306309
}
307310
}

0 commit comments

Comments
 (0)