Skip to content

Commit 0ab6a3b

Browse files
TomohitoAndopre-commit-ci[bot]
TomohitoAndo
and
pre-commit-ci[bot]
authored
feat(behavior_velocity): support new traffic signal interface (autowarefoundation#4133)
* feat(behavior_velocity): support new traffic signal interface Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * style(pre-commit): autofix * add missing dependency Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * style(pre-commit): autofix * remove the external signal input source in behavior_planning_launch.py Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * replace TrafficLightElement with TrafficSignalElement Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * style(pre-commit): autofix * use the regulatory element id instead of traffic light id Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * change the input of traffic signal to traffic light arbiter Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * style(pre-commit): autofix * do not return until the all regulatory elements are checked Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * change input topic of the traffic signals Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix the traffic signal type in perception reproducer Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * add debug log when the signal data is outdated 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 b1e6fcf commit 0ab6a3b

File tree

27 files changed

+188
-309
lines changed

27 files changed

+188
-309
lines changed

‎common/tier4_traffic_light_rviz_plugin/README.md

+3-3
Original file line numberDiff line numberDiff line change
@@ -8,9 +8,9 @@ This plugin panel publishes dummy traffic light signals.
88

99
### Output
1010

11-
| Name | Type | Description |
12-
| ------------------------------------------------------- | -------------------------------------------------------- | ----------------------------- |
13-
| `/perception/traffic_light_recognition/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | Publish traffic light signals |
11+
| Name | Type | Description |
12+
| ------------------------------------------------------- | --------------------------------------------------- | ----------------------------- |
13+
| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | Publish traffic light signals |
1414

1515
## HowToUse
1616

‎common/tier4_traffic_light_rviz_plugin/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@
1111
<buildtool_depend>autoware_cmake</buildtool_depend>
1212

1313
<depend>autoware_auto_mapping_msgs</depend>
14-
<depend>autoware_auto_perception_msgs</depend>
14+
<depend>autoware_perception_msgs</depend>
1515
<depend>lanelet2_extension</depend>
1616
<depend>libqt5-core</depend>
1717
<depend>libqt5-gui</depend>

‎common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp

+46-50
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@
3434
#include <utility>
3535
#include <vector>
3636

37+
#undef signals
3738
namespace rviz_plugins
3839
{
3940
TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_common::Panel(parent)
@@ -138,55 +139,55 @@ void TrafficLightPublishPanel::onSetTrafficLightState()
138139
const auto shape = light_shape_combo_->currentText();
139140
const auto status = light_status_combo_->currentText();
140141

141-
TrafficLight traffic_light;
142+
TrafficSignalElement traffic_light;
142143
traffic_light.confidence = traffic_light_confidence_input_->value();
143144

144145
if (color == "RED") {
145-
traffic_light.color = TrafficLight::RED;
146+
traffic_light.color = TrafficSignalElement::RED;
146147
} else if (color == "AMBER") {
147-
traffic_light.color = TrafficLight::AMBER;
148+
traffic_light.color = TrafficSignalElement::AMBER;
148149
} else if (color == "GREEN") {
149-
traffic_light.color = TrafficLight::GREEN;
150+
traffic_light.color = TrafficSignalElement::GREEN;
150151
} else if (color == "WHITE") {
151-
traffic_light.color = TrafficLight::WHITE;
152+
traffic_light.color = TrafficSignalElement::WHITE;
152153
} else if (color == "UNKNOWN") {
153-
traffic_light.color = TrafficLight::UNKNOWN;
154+
traffic_light.color = TrafficSignalElement::UNKNOWN;
154155
}
155156

156157
if (shape == "CIRCLE") {
157-
traffic_light.shape = TrafficLight::CIRCLE;
158+
traffic_light.shape = TrafficSignalElement::CIRCLE;
158159
} else if (shape == "LEFT_ARROW") {
159-
traffic_light.shape = TrafficLight::LEFT_ARROW;
160+
traffic_light.shape = TrafficSignalElement::LEFT_ARROW;
160161
} else if (shape == "RIGHT_ARROW") {
161-
traffic_light.shape = TrafficLight::RIGHT_ARROW;
162+
traffic_light.shape = TrafficSignalElement::RIGHT_ARROW;
162163
} else if (shape == "UP_ARROW") {
163-
traffic_light.shape = TrafficLight::UP_ARROW;
164+
traffic_light.shape = TrafficSignalElement::UP_ARROW;
164165
} else if (shape == "DOWN_ARROW") {
165-
traffic_light.shape = TrafficLight::DOWN_ARROW;
166+
traffic_light.shape = TrafficSignalElement::DOWN_ARROW;
166167
} else if (shape == "DOWN_LEFT_ARROW") {
167-
traffic_light.shape = TrafficLight::DOWN_LEFT_ARROW;
168+
traffic_light.shape = TrafficSignalElement::DOWN_LEFT_ARROW;
168169
} else if (shape == "DOWN_RIGHT_ARROW") {
169-
traffic_light.shape = TrafficLight::DOWN_RIGHT_ARROW;
170+
traffic_light.shape = TrafficSignalElement::DOWN_RIGHT_ARROW;
170171
} else if (shape == "UNKNOWN") {
171-
traffic_light.shape = TrafficLight::UNKNOWN;
172+
traffic_light.shape = TrafficSignalElement::UNKNOWN;
172173
}
173174

174175
if (status == "SOLID_OFF") {
175-
traffic_light.status = TrafficLight::SOLID_OFF;
176+
traffic_light.status = TrafficSignalElement::SOLID_OFF;
176177
} else if (status == "SOLID_ON") {
177-
traffic_light.status = TrafficLight::SOLID_ON;
178+
traffic_light.status = TrafficSignalElement::SOLID_ON;
178179
} else if (status == "FLASHING") {
179-
traffic_light.status = TrafficLight::FLASHING;
180+
traffic_light.status = TrafficSignalElement::FLASHING;
180181
} else if (status == "UNKNOWN") {
181-
traffic_light.status = TrafficLight::UNKNOWN;
182+
traffic_light.status = TrafficSignalElement::UNKNOWN;
182183
}
183184

184185
TrafficSignal traffic_signal;
185-
traffic_signal.lights.push_back(traffic_light);
186-
traffic_signal.map_primitive_id = traffic_light_id;
186+
traffic_signal.elements.push_back(traffic_light);
187+
traffic_signal.traffic_signal_id = traffic_light_id;
187188

188189
for (auto & signal : extra_traffic_signals_.signals) {
189-
if (signal.map_primitive_id == traffic_light_id) {
190+
if (signal.traffic_signal_id == traffic_light_id) {
190191
signal = traffic_signal;
191192
return;
192193
}
@@ -247,7 +248,7 @@ void TrafficLightPublishPanel::createWallTimer()
247248
void TrafficLightPublishPanel::onTimer()
248249
{
249250
if (enable_publish_) {
250-
extra_traffic_signals_.header.stamp = rclcpp::Clock().now();
251+
extra_traffic_signals_.stamp = rclcpp::Clock().now();
251252
pub_traffic_signals_->publish(extra_traffic_signals_);
252253
}
253254

@@ -260,35 +261,35 @@ void TrafficLightPublishPanel::onTimer()
260261
for (size_t i = 0; i < extra_traffic_signals_.signals.size(); ++i) {
261262
const auto & signal = extra_traffic_signals_.signals.at(i);
262263

263-
if (signal.lights.empty()) {
264+
if (signal.elements.empty()) {
264265
continue;
265266
}
266267

267-
auto id_label = new QLabel(QString::number(signal.map_primitive_id));
268+
auto id_label = new QLabel(QString::number(signal.traffic_signal_id));
268269
id_label->setAlignment(Qt::AlignCenter);
269270

270271
auto color_label = new QLabel();
271272
color_label->setAlignment(Qt::AlignCenter);
272273

273-
const auto & light = signal.lights.front();
274+
const auto & light = signal.elements.front();
274275
switch (light.color) {
275-
case TrafficLight::RED:
276+
case TrafficSignalElement::RED:
276277
color_label->setText("RED");
277278
color_label->setStyleSheet("background-color: #FF0000;");
278279
break;
279-
case TrafficLight::AMBER:
280+
case TrafficSignalElement::AMBER:
280281
color_label->setText("AMBER");
281282
color_label->setStyleSheet("background-color: #FFBF00;");
282283
break;
283-
case TrafficLight::GREEN:
284+
case TrafficSignalElement::GREEN:
284285
color_label->setText("GREEN");
285286
color_label->setStyleSheet("background-color: #7CFC00;");
286287
break;
287-
case TrafficLight::WHITE:
288+
case TrafficSignalElement::WHITE:
288289
color_label->setText("WHITE");
289290
color_label->setStyleSheet("background-color: #FFFFFF;");
290291
break;
291-
case TrafficLight::UNKNOWN:
292+
case TrafficSignalElement::UNKNOWN:
292293
color_label->setText("UNKNOWN");
293294
color_label->setStyleSheet("background-color: #808080;");
294295
break;
@@ -300,31 +301,28 @@ void TrafficLightPublishPanel::onTimer()
300301
shape_label->setAlignment(Qt::AlignCenter);
301302

302303
switch (light.shape) {
303-
case TrafficLight::CIRCLE:
304+
case TrafficSignalElement::CIRCLE:
304305
shape_label->setText("CIRCLE");
305306
break;
306-
case TrafficLight::LEFT_ARROW:
307+
case TrafficSignalElement::LEFT_ARROW:
307308
shape_label->setText("LEFT_ARROW");
308309
break;
309-
case TrafficLight::RIGHT_ARROW:
310+
case TrafficSignalElement::RIGHT_ARROW:
310311
shape_label->setText("RIGHT_ARROW");
311312
break;
312-
case TrafficLight::UP_ARROW:
313+
case TrafficSignalElement::UP_ARROW:
313314
shape_label->setText("UP_ARROW");
314315
break;
315-
case TrafficLight::DOWN_ARROW:
316+
case TrafficSignalElement::DOWN_ARROW:
316317
shape_label->setText("DOWN_ARROW");
317318
break;
318-
case TrafficLight::DOWN_LEFT_ARROW:
319+
case TrafficSignalElement::DOWN_LEFT_ARROW:
319320
shape_label->setText("DOWN_LEFT_ARROW");
320321
break;
321-
case TrafficLight::DOWN_RIGHT_ARROW:
322+
case TrafficSignalElement::DOWN_RIGHT_ARROW:
322323
shape_label->setText("DOWN_RIGHT_ARROW");
323324
break;
324-
case TrafficLight::FLASHING:
325-
shape_label->setText("FLASHING");
326-
break;
327-
case TrafficLight::UNKNOWN:
325+
case TrafficSignalElement::UNKNOWN:
328326
shape_label->setText("UNKNOWN");
329327
break;
330328
default:
@@ -335,16 +333,16 @@ void TrafficLightPublishPanel::onTimer()
335333
status_label->setAlignment(Qt::AlignCenter);
336334

337335
switch (light.status) {
338-
case TrafficLight::SOLID_OFF:
336+
case TrafficSignalElement::SOLID_OFF:
339337
status_label->setText("SOLID_OFF");
340338
break;
341-
case TrafficLight::SOLID_ON:
339+
case TrafficSignalElement::SOLID_ON:
342340
status_label->setText("SOLID_ON");
343341
break;
344-
case TrafficLight::FLASHING:
342+
case TrafficSignalElement::FLASHING:
345343
status_label->setText("FLASHING");
346344
break;
347-
case TrafficLight::UNKNOWN:
345+
case TrafficSignalElement::UNKNOWN:
348346
status_label->setText("UNKNOWN");
349347
break;
350348
default:
@@ -375,11 +373,9 @@ void TrafficLightPublishPanel::onVectorMap(const HADMapBin::ConstSharedPtr msg)
375373
std::string info = "Fetching traffic lights :";
376374
std::string delim = " ";
377375
for (auto && tl_reg_elem_ptr : tl_reg_elems) {
378-
for (auto && traffic_light : tl_reg_elem_ptr->trafficLights()) {
379-
auto id = static_cast<int>(traffic_light.id());
380-
info += (std::exchange(delim, ", ") + std::to_string(id));
381-
traffic_light_ids_.insert(id);
382-
}
376+
auto id = static_cast<int>(tl_reg_elem_ptr->id());
377+
info += (std::exchange(delim, ", ") + std::to_string(id));
378+
traffic_light_ids_.insert(id);
383379
}
384380
RCLCPP_INFO_STREAM(raw_node_->get_logger(), info);
385381
received_vector_map_ = true;

‎common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp

+4-5
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,7 @@
2727
#include <rviz_common/panel.hpp>
2828

2929
#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
30-
#include <autoware_auto_perception_msgs/msg/traffic_signal_array.hpp>
30+
#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
3131
#endif
3232

3333
#include <set>
@@ -36,10 +36,9 @@ namespace rviz_plugins
3636
{
3737

3838
using autoware_auto_mapping_msgs::msg::HADMapBin;
39-
using autoware_auto_perception_msgs::msg::TrafficLight;
40-
using autoware_auto_perception_msgs::msg::TrafficSignal;
41-
using autoware_auto_perception_msgs::msg::TrafficSignalArray;
42-
39+
using autoware_perception_msgs::msg::TrafficSignal;
40+
using autoware_perception_msgs::msg::TrafficSignalArray;
41+
using autoware_perception_msgs::msg::TrafficSignalElement;
4342
class TrafficLightPublishPanel : public rviz_common::Panel
4443
{
4544
Q_OBJECT

‎launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py

-4
Original file line numberDiff line numberDiff line change
@@ -177,10 +177,6 @@ def launch_setup(context, *args, **kwargs):
177177
"~/input/traffic_signals",
178178
"/perception/traffic_light_recognition/traffic_signals",
179179
),
180-
(
181-
"~/input/external_traffic_signals",
182-
"/external/traffic_light_recognition/traffic_signals",
183-
),
184180
(
185181
"~/input/external_velocity_limit_mps",
186182
"/planning/scenario_planning/max_velocity_default",

‎planning/behavior_velocity_crosswalk_module/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
<depend>autoware_auto_perception_msgs</depend>
2222
<depend>autoware_auto_planning_msgs</depend>
2323
<depend>autoware_auto_tf2</depend>
24+
<depend>autoware_perception_msgs</depend>
2425
<depend>behavior_velocity_planner_common</depend>
2526
<depend>eigen</depend>
2627
<depend>geometry_msgs</depend>

‎planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

+16-19
Original file line numberDiff line numberDiff line change
@@ -938,28 +938,25 @@ bool CrosswalkModule::isRedSignalForPedestrians() const
938938
crosswalk_.regulatoryElementsAs<const lanelet::TrafficLight>();
939939

940940
for (const auto & traffic_lights_reg_elem : traffic_lights_reg_elems) {
941-
lanelet::ConstLineStringsOrPolygons3d traffic_lights = traffic_lights_reg_elem->trafficLights();
942-
for (const auto & traffic_light : traffic_lights) {
943-
const auto ll_traffic_light = static_cast<lanelet::ConstLineString3d>(traffic_light);
944-
const auto traffic_signal_stamped = planner_data_->getTrafficSignal(ll_traffic_light.id());
945-
if (!traffic_signal_stamped) {
946-
continue;
947-
}
941+
const auto traffic_signal_stamped =
942+
planner_data_->getTrafficSignal(traffic_lights_reg_elem->id());
943+
if (!traffic_signal_stamped) {
944+
continue;
945+
}
948946

949-
if (
950-
planner_param_.traffic_light_state_timeout <
951-
(clock_->now() - traffic_signal_stamped->header.stamp).seconds()) {
952-
continue;
953-
}
947+
if (
948+
planner_param_.traffic_light_state_timeout <
949+
(clock_->now() - traffic_signal_stamped->stamp).seconds()) {
950+
continue;
951+
}
954952

955-
const auto & lights = traffic_signal_stamped->signal.lights;
956-
if (lights.empty()) {
957-
continue;
958-
}
953+
const auto & lights = traffic_signal_stamped->signal.elements;
954+
if (lights.empty()) {
955+
continue;
956+
}
959957

960-
if (lights.front().color == TrafficLight::RED) {
961-
return true;
962-
}
958+
if (lights.front().color == TrafficSignalElement::RED) {
959+
return true;
963960
}
964961
}
965962

‎planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -50,8 +50,8 @@ namespace behavior_velocity_planner
5050
using autoware_auto_perception_msgs::msg::ObjectClassification;
5151
using autoware_auto_perception_msgs::msg::PredictedObject;
5252
using autoware_auto_perception_msgs::msg::PredictedObjects;
53-
using autoware_auto_perception_msgs::msg::TrafficLight;
5453
using autoware_auto_planning_msgs::msg::PathWithLaneId;
54+
using autoware_perception_msgs::msg::TrafficSignalElement;
5555
using lanelet::autoware::Crosswalk;
5656
using tier4_api_msgs::msg::CrosswalkStatus;
5757
using tier4_autoware_utils::Polygon2d;

‎planning/behavior_velocity_intersection_module/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919

2020
<depend>autoware_auto_perception_msgs</depend>
2121
<depend>autoware_auto_planning_msgs</depend>
22+
<depend>autoware_perception_msgs</depend>
2223
<depend>behavior_velocity_planner_common</depend>
2324
<depend>geometry_msgs</depend>
2425
<depend>interpolation</depend>

‎planning/behavior_velocity_intersection_module/src/util.cpp

+8-10
Original file line numberDiff line numberDiff line change
@@ -704,9 +704,10 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane)
704704
}
705705

706706
bool isTrafficLightArrowActivated(
707-
lanelet::ConstLanelet lane,
708-
const std::map<int, autoware_auto_perception_msgs::msg::TrafficSignalStamped> & tl_infos)
707+
lanelet::ConstLanelet lane, const std::map<int, TrafficSignalStamped> & tl_infos)
709708
{
709+
using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
710+
710711
const auto & turn_direction = lane.attributeOr("turn_direction", "else");
711712
std::optional<int> tl_id = std::nullopt;
712713
for (auto && tl_reg_elem : lane.regulatoryElementsAs<lanelet::TrafficLight>()) {
@@ -723,16 +724,13 @@ bool isTrafficLightArrowActivated(
723724
return false;
724725
}
725726
const auto & tl_info = tl_info_it->second;
726-
for (auto && tl_light : tl_info.signal.lights) {
727-
if (tl_light.color != autoware_auto_perception_msgs::msg::TrafficLight::GREEN) continue;
728-
if (tl_light.status != autoware_auto_perception_msgs::msg::TrafficLight::SOLID_ON) continue;
729-
if (
730-
turn_direction == std::string("left") &&
731-
tl_light.shape == autoware_auto_perception_msgs::msg::TrafficLight::LEFT_ARROW)
727+
for (auto && tl_light : tl_info.signal.elements) {
728+
if (tl_light.color != TrafficSignalElement::GREEN) continue;
729+
if (tl_light.status != TrafficSignalElement::SOLID_ON) continue;
730+
if (turn_direction == std::string("left") && tl_light.shape == TrafficSignalElement::LEFT_ARROW)
732731
return true;
733732
if (
734-
turn_direction == std::string("right") &&
735-
tl_light.shape == autoware_auto_perception_msgs::msg::TrafficLight::RIGHT_ARROW)
733+
turn_direction == std::string("right") && tl_light.shape == TrafficSignalElement::RIGHT_ARROW)
736734
return true;
737735
}
738736
return false;

‎planning/behavior_velocity_intersection_module/src/util.hpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -112,8 +112,7 @@ std::optional<Polygon2d> getIntersectionArea(
112112

113113
bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane);
114114
bool isTrafficLightArrowActivated(
115-
lanelet::ConstLanelet lane,
116-
const std::map<int, autoware_auto_perception_msgs::msg::TrafficSignalStamped> & tl_infos);
115+
lanelet::ConstLanelet lane, const std::map<int, TrafficSignalStamped> & tl_infos);
117116

118117
std::vector<DiscretizedLane> generateDetectionLaneDivisions(
119118
lanelet::ConstLanelets detection_lanelets,

0 commit comments

Comments
 (0)