Skip to content

Commit 6dbbaee

Browse files
satoshi-otasatoshi-ota
authored
feat(tier4_traffic_light_rviz_plugin): selectable light shape, status and confidence (#663)
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent 973cc80 commit 6dbbaee

File tree

2 files changed

+121
-43
lines changed

2 files changed

+121
-43
lines changed

‎common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp

+118-43
Original file line numberDiff line numberDiff line change
@@ -43,11 +43,25 @@ TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_comm
4343
traffic_light_id_input_->setRange(0, 999999);
4444
traffic_light_id_input_->setValue(0);
4545

46+
// Traffic Light Confidence
47+
traffic_light_confidence_input_ = new QDoubleSpinBox();
48+
traffic_light_confidence_input_->setRange(0.0, 1.0);
49+
traffic_light_confidence_input_->setSingleStep(0.1);
50+
traffic_light_confidence_input_->setValue(1.0);
51+
4652
// Traffic Light Color
4753
light_color_combo_ = new QComboBox();
48-
light_color_combo_->addItems(
49-
{"RED", "AMBER", "GREEN", "WHITE", "LEFT_ARROW", "RIGHT_ARROW", "UP_ARROW", "DOWN_ARROW",
50-
"DOWN_LEFT_ARROW", "DOWN_RIGHT_ARROW", "FLASHING", "UNKNOWN"});
54+
light_color_combo_->addItems({"RED", "AMBER", "GREEN", "WHITE", "UNKNOWN"});
55+
56+
// Traffic Light Shape
57+
light_shape_combo_ = new QComboBox();
58+
light_shape_combo_->addItems(
59+
{"CIRCLE", "LEFT_ARROW", "RIGHT_ARROW", "UP_ARROW", "DOWN_ARROW", "DOWN_LEFT_ARROW",
60+
"DOWN_RIGHT_ARROW", "CROSS", "UNKNOWN"});
61+
62+
// Traffic Light Status
63+
light_status_combo_ = new QComboBox();
64+
light_status_combo_->addItems({"SOLID_ON", "SOLID_OFF", "FLASHING", "UNKNOWN"});
5165

5266
// Set Traffic Signals Button
5367
set_button_ = new QPushButton("SET");
@@ -64,8 +78,8 @@ TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_comm
6478
horizontal_header->setSectionResizeMode(QHeaderView::Stretch);
6579

6680
traffic_table_ = new QTableWidget();
67-
traffic_table_->setColumnCount(2);
68-
traffic_table_->setHorizontalHeaderLabels({"ID", "Status"});
81+
traffic_table_->setColumnCount(5);
82+
traffic_table_->setHorizontalHeaderLabels({"ID", "Color", "Shape", "Status", "Confidence"});
6983
traffic_table_->setVerticalHeader(vertical_header);
7084
traffic_table_->setHorizontalHeader(horizontal_header);
7185

@@ -77,34 +91,48 @@ TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_comm
7791
auto * h_layout_1 = new QHBoxLayout;
7892
h_layout_1->addWidget(new QLabel("Rate: "));
7993
h_layout_1->addWidget(publishing_rate_input_);
80-
h_layout_1->addWidget(new QLabel("Traffic Light ID: "));
94+
h_layout_1->addWidget(new QLabel("ID: "));
8195
h_layout_1->addWidget(traffic_light_id_input_);
96+
h_layout_1->addWidget(new QLabel("Confidence: "));
97+
h_layout_1->addWidget(traffic_light_confidence_input_);
8298

8399
auto * h_layout_2 = new QHBoxLayout;
84-
h_layout_2->addWidget(new QLabel("Traffic Light Status: "));
85-
h_layout_2->addWidget(light_color_combo_);
100+
h_layout_2->addWidget(new QLabel("Traffic Light Color: "), 40);
101+
h_layout_2->addWidget(light_color_combo_, 60);
102+
103+
auto * h_layout_3 = new QHBoxLayout;
104+
h_layout_3->addWidget(new QLabel("Traffic Light Shape: "), 40);
105+
h_layout_3->addWidget(light_shape_combo_, 60);
106+
107+
auto * h_layout_4 = new QHBoxLayout;
108+
h_layout_4->addWidget(new QLabel("Traffic Light Status: "), 40);
109+
h_layout_4->addWidget(light_status_combo_, 60);
86110

87111
auto * v_layout = new QVBoxLayout;
88112
v_layout->addLayout(h_layout_1);
89113
v_layout->addLayout(h_layout_2);
114+
v_layout->addLayout(h_layout_3);
115+
v_layout->addLayout(h_layout_4);
90116
v_layout->addWidget(set_button_);
91117
v_layout->addWidget(reset_button_);
92118
v_layout->addWidget(publish_button_);
93119

94-
auto * h_layout_3 = new QHBoxLayout;
95-
h_layout_3->addLayout(v_layout);
96-
h_layout_3->addWidget(traffic_table_);
120+
auto * h_layout_5 = new QHBoxLayout;
121+
h_layout_5->addLayout(v_layout);
122+
h_layout_5->addWidget(traffic_table_);
97123

98-
setLayout(h_layout_3);
124+
setLayout(h_layout_5);
99125
}
100126

101127
void TrafficLightPublishPanel::onSetTrafficLightState()
102128
{
103129
const auto traffic_light_id = traffic_light_id_input_->value();
104130
const auto color = light_color_combo_->currentText();
131+
const auto shape = light_shape_combo_->currentText();
132+
const auto status = light_status_combo_->currentText();
105133

106134
TrafficLight traffic_light;
107-
traffic_light.confidence = 1.0;
135+
traffic_light.confidence = traffic_light_confidence_input_->value();
108136

109137
if (color == "RED") {
110138
traffic_light.color = TrafficLight::RED;
@@ -114,24 +142,38 @@ void TrafficLightPublishPanel::onSetTrafficLightState()
114142
traffic_light.color = TrafficLight::GREEN;
115143
} else if (color == "WHITE") {
116144
traffic_light.color = TrafficLight::WHITE;
117-
} else if (color == "LEFT_ARROW") {
118-
traffic_light.color = TrafficLight::LEFT_ARROW;
119-
} else if (color == "RIGHT_ARROW") {
120-
traffic_light.color = TrafficLight::RIGHT_ARROW;
121-
} else if (color == "UP_ARROW") {
122-
traffic_light.color = TrafficLight::UP_ARROW;
123-
} else if (color == "DOWN_ARROW") {
124-
traffic_light.color = TrafficLight::DOWN_ARROW;
125-
} else if (color == "DOWN_LEFT_ARROW") {
126-
traffic_light.color = TrafficLight::DOWN_LEFT_ARROW;
127-
} else if (color == "DOWN_RIGHT_ARROW") {
128-
traffic_light.color = TrafficLight::DOWN_RIGHT_ARROW;
129-
} else if (color == "FLASHING") {
130-
traffic_light.color = TrafficLight::FLASHING;
131145
} else if (color == "UNKNOWN") {
132146
traffic_light.color = TrafficLight::UNKNOWN;
133147
}
134148

149+
if (shape == "CIRCLE") {
150+
traffic_light.shape = TrafficLight::CIRCLE;
151+
} else if (shape == "LEFT_ARROW") {
152+
traffic_light.shape = TrafficLight::LEFT_ARROW;
153+
} else if (shape == "RIGHT_ARROW") {
154+
traffic_light.shape = TrafficLight::RIGHT_ARROW;
155+
} else if (shape == "UP_ARROW") {
156+
traffic_light.shape = TrafficLight::UP_ARROW;
157+
} else if (shape == "DOWN_ARROW") {
158+
traffic_light.shape = TrafficLight::DOWN_ARROW;
159+
} else if (shape == "DOWN_LEFT_ARROW") {
160+
traffic_light.shape = TrafficLight::DOWN_LEFT_ARROW;
161+
} else if (shape == "DOWN_RIGHT_ARROW") {
162+
traffic_light.shape = TrafficLight::DOWN_RIGHT_ARROW;
163+
} else if (shape == "UNKNOWN") {
164+
traffic_light.shape = TrafficLight::UNKNOWN;
165+
}
166+
167+
if (status == "SOLID_OFF") {
168+
traffic_light.status = TrafficLight::SOLID_OFF;
169+
} else if (status == "SOLID_ON") {
170+
traffic_light.status = TrafficLight::SOLID_ON;
171+
} else if (status == "FLASHING") {
172+
traffic_light.status = TrafficLight::FLASHING;
173+
} else if (status == "UNKNOWN") {
174+
traffic_light.status = TrafficLight::UNKNOWN;
175+
}
176+
135177
TrafficSignal traffic_signal;
136178
traffic_signal.lights.push_back(traffic_light);
137179
traffic_signal.map_primitive_id = traffic_light_id;
@@ -234,44 +276,77 @@ void TrafficLightPublishPanel::onTimer()
234276
color_label->setText("WHITE");
235277
color_label->setStyleSheet("background-color: #FFFFFF;");
236278
break;
279+
case TrafficLight::UNKNOWN:
280+
color_label->setText("UNKNOWN");
281+
color_label->setStyleSheet("background-color: #808080;");
282+
break;
283+
default:
284+
break;
285+
}
286+
287+
auto shape_label = new QLabel();
288+
shape_label->setAlignment(Qt::AlignCenter);
289+
290+
switch (light.shape) {
291+
case TrafficLight::CIRCLE:
292+
shape_label->setText("CIRCLE");
293+
break;
237294
case TrafficLight::LEFT_ARROW:
238-
color_label->setText("LEFT_ARROW");
239-
color_label->setStyleSheet("background-color: #7CFC00;");
295+
shape_label->setText("LEFT_ARROW");
240296
break;
241297
case TrafficLight::RIGHT_ARROW:
242-
color_label->setText("RIGHT_ARROW");
243-
color_label->setStyleSheet("background-color: #7CFC00;");
298+
shape_label->setText("RIGHT_ARROW");
244299
break;
245300
case TrafficLight::UP_ARROW:
246-
color_label->setText("UP_ARROW");
247-
color_label->setStyleSheet("background-color: #7CFC00;");
301+
shape_label->setText("UP_ARROW");
248302
break;
249303
case TrafficLight::DOWN_ARROW:
250-
color_label->setText("DOWN_ARROW");
251-
color_label->setStyleSheet("background-color: #7CFC00;");
304+
shape_label->setText("DOWN_ARROW");
252305
break;
253306
case TrafficLight::DOWN_LEFT_ARROW:
254-
color_label->setText("DOWN_LEFT_ARROW");
255-
color_label->setStyleSheet("background-color: #7CFC00;");
307+
shape_label->setText("DOWN_LEFT_ARROW");
256308
break;
257309
case TrafficLight::DOWN_RIGHT_ARROW:
258-
color_label->setText("DOWN_RIGHT_ARROW");
259-
color_label->setStyleSheet("background-color: #7CFC00;");
310+
shape_label->setText("DOWN_RIGHT_ARROW");
260311
break;
261312
case TrafficLight::FLASHING:
262-
color_label->setText("FLASHING");
263-
color_label->setStyleSheet("background-color: #7CFC00;");
313+
shape_label->setText("FLASHING");
264314
break;
265315
case TrafficLight::UNKNOWN:
266-
color_label->setText("UNKNOWN");
267-
color_label->setStyleSheet("background-color: #808080;");
316+
shape_label->setText("UNKNOWN");
268317
break;
269318
default:
270319
break;
271320
}
272321

322+
auto status_label = new QLabel();
323+
status_label->setAlignment(Qt::AlignCenter);
324+
325+
switch (light.status) {
326+
case TrafficLight::SOLID_OFF:
327+
status_label->setText("SOLID_OFF");
328+
break;
329+
case TrafficLight::SOLID_ON:
330+
status_label->setText("SOLID_ON");
331+
break;
332+
case TrafficLight::FLASHING:
333+
status_label->setText("FLASHING");
334+
break;
335+
case TrafficLight::UNKNOWN:
336+
status_label->setText("UNKNOWN");
337+
break;
338+
default:
339+
break;
340+
}
341+
342+
auto confidence_label = new QLabel(QString::number(light.confidence));
343+
confidence_label->setAlignment(Qt::AlignCenter);
344+
273345
traffic_table_->setCellWidget(i, 0, id_label);
274346
traffic_table_->setCellWidget(i, 1, color_label);
347+
traffic_table_->setCellWidget(i, 2, shape_label);
348+
traffic_table_->setCellWidget(i, 3, status_label);
349+
traffic_table_->setCellWidget(i, 4, confidence_label);
275350
}
276351
}
277352

‎common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp

+3
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,10 @@ public Q_SLOTS:
6060

6161
QSpinBox * publishing_rate_input_;
6262
QSpinBox * traffic_light_id_input_;
63+
QDoubleSpinBox * traffic_light_confidence_input_;
6364
QComboBox * light_color_combo_;
65+
QComboBox * light_shape_combo_;
66+
QComboBox * light_status_combo_;
6467
QPushButton * set_button_;
6568
QPushButton * reset_button_;
6669
QPushButton * publish_button_;

0 commit comments

Comments
 (0)