Skip to content

Commit 0a708da

Browse files
isamu-takagiisamu-takagi
authored
feat: operation mode debug panel (autowarefoundation#8933)
Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
1 parent 7fd0259 commit 0a708da

File tree

8 files changed

+302
-842
lines changed

8 files changed

+302
-842
lines changed

‎visualization/tier4_adapi_rviz_plugin/CMakeLists.txt

+1-1
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON)
1212
ament_auto_add_library(${PROJECT_NAME} SHARED
1313
src/route_tool.cpp
1414
src/route_panel.cpp
15-
src/state_panel.cpp
15+
src/operation_mode_debug_panel.cpp
1616
src/door_panel.cpp
1717
)
1818

‎visualization/tier4_adapi_rviz_plugin/README.md

+2
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
# tier4_adapi_rviz_plugin
22

3+
This package contains tools for testing AD API. For general AD API usage, we recommend using [tier4_state_rviz_plugin](../tier4_state_rviz_plugin/README.md).
4+
35
## RoutePanel
46

57
To use the panel, set the topic name from 2D Goal Pose Tool to `/rviz/routing/pose`.

‎visualization/tier4_adapi_rviz_plugin/icons/classes/StatePanel.png

-18.4 KB
Binary file not shown.

‎visualization/tier4_adapi_rviz_plugin/plugins/plugin_description.xml

+2-2
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,8 @@
88
<description>RoutePanel</description>
99
</class>
1010

11-
<class type="tier4_adapi_rviz_plugins::StatePanel" base_class_type="rviz_common::Panel">
12-
<description>StatePanel</description>
11+
<class type="tier4_adapi_rviz_plugins::OperationModeDebugPanel" base_class_type="rviz_common::Panel">
12+
<description>OperationModeDebugPanel</description>
1313
</class>
1414

1515
<class type="tier4_adapi_rviz_plugins::DoorPanel" base_class_type="rviz_common::Panel">

‎visualization/tier4_adapi_rviz_plugin/src/operation_mode_debug_panel.cpp

+221
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,221 @@
1+
//
2+
// Copyright 2020 TIER IV, Inc. All rights reserved.
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
//
16+
17+
#include "operation_mode_debug_panel.hpp"
18+
19+
#include <QGridLayout>
20+
#include <QString>
21+
#include <rviz_common/display_context.hpp>
22+
23+
#include <memory>
24+
#include <string>
25+
26+
namespace tier4_adapi_rviz_plugins
27+
{
28+
29+
OperationModeDebugPanel::OperationModeDebugPanel(QWidget * parent) : rviz_common::Panel(parent)
30+
{
31+
auto * layout = new QGridLayout;
32+
setLayout(layout);
33+
34+
operation_mode_label_ptr_ = new QLabel("INIT");
35+
operation_mode_label_ptr_->setAlignment(Qt::AlignCenter);
36+
operation_mode_label_ptr_->setStyleSheet("border:1px solid black;");
37+
layout->addWidget(operation_mode_label_ptr_, 0, 0, 2, 1);
38+
39+
auto_button_ptr_ = new QPushButton("AUTO");
40+
connect(auto_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutonomous()));
41+
layout->addWidget(auto_button_ptr_, 0, 1);
42+
43+
stop_button_ptr_ = new QPushButton("STOP");
44+
connect(stop_button_ptr_, SIGNAL(clicked()), SLOT(onClickStop()));
45+
layout->addWidget(stop_button_ptr_, 0, 2);
46+
47+
local_button_ptr_ = new QPushButton("LOCAL");
48+
connect(local_button_ptr_, SIGNAL(clicked()), SLOT(onClickLocal()));
49+
layout->addWidget(local_button_ptr_, 1, 1);
50+
51+
remote_button_ptr_ = new QPushButton("REMOTE");
52+
connect(remote_button_ptr_, SIGNAL(clicked()), SLOT(onClickRemote()));
53+
layout->addWidget(remote_button_ptr_, 1, 2);
54+
55+
control_mode_label_ptr_ = new QLabel("INIT");
56+
control_mode_label_ptr_->setAlignment(Qt::AlignCenter);
57+
control_mode_label_ptr_->setStyleSheet("border:1px solid black;");
58+
layout->addWidget(control_mode_label_ptr_, 2, 0);
59+
60+
enable_button_ptr_ = new QPushButton("Enable");
61+
connect(enable_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutowareControl()));
62+
layout->addWidget(enable_button_ptr_, 2, 1);
63+
64+
disable_button_ptr_ = new QPushButton("Disable");
65+
connect(disable_button_ptr_, SIGNAL(clicked()), SLOT(onClickDirectControl()));
66+
layout->addWidget(disable_button_ptr_, 2, 2);
67+
}
68+
69+
void OperationModeDebugPanel::onInitialize()
70+
{
71+
raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
72+
73+
sub_operation_mode_ = raw_node_->create_subscription<OperationModeState>(
74+
"/api/operation_mode/state", rclcpp::QoS{1}.transient_local(),
75+
std::bind(&OperationModeDebugPanel::onOperationMode, this, std::placeholders::_1));
76+
77+
client_change_to_autonomous_ =
78+
raw_node_->create_client<ChangeOperationMode>("/api/operation_mode/change_to_autonomous");
79+
80+
client_change_to_stop_ =
81+
raw_node_->create_client<ChangeOperationMode>("/api/operation_mode/change_to_stop");
82+
83+
client_change_to_local_ =
84+
raw_node_->create_client<ChangeOperationMode>("/api/operation_mode/change_to_local");
85+
86+
client_change_to_remote_ =
87+
raw_node_->create_client<ChangeOperationMode>("/api/operation_mode/change_to_remote");
88+
89+
client_enable_autoware_control_ =
90+
raw_node_->create_client<ChangeOperationMode>("/api/operation_mode/enable_autoware_control");
91+
92+
client_enable_direct_control_ =
93+
raw_node_->create_client<ChangeOperationMode>("/api/operation_mode/disable_autoware_control");
94+
}
95+
96+
template <typename T>
97+
void callService(const rclcpp::Logger & logger, const typename rclcpp::Client<T>::SharedPtr client)
98+
{
99+
auto req = std::make_shared<typename T::Request>();
100+
101+
RCLCPP_DEBUG(logger, "client request");
102+
103+
if (!client->service_is_ready()) {
104+
RCLCPP_DEBUG(logger, "client is unavailable");
105+
return;
106+
}
107+
108+
client->async_send_request(req, [logger](typename rclcpp::Client<T>::SharedFuture result) {
109+
RCLCPP_DEBUG(
110+
logger, "Status: %d, %s", result.get()->status.code, result.get()->status.message.c_str());
111+
});
112+
}
113+
114+
void OperationModeDebugPanel::onClickAutonomous()
115+
{
116+
callService<ChangeOperationMode>(raw_node_->get_logger(), client_change_to_autonomous_);
117+
}
118+
119+
void OperationModeDebugPanel::onClickStop()
120+
{
121+
callService<ChangeOperationMode>(raw_node_->get_logger(), client_change_to_stop_);
122+
}
123+
124+
void OperationModeDebugPanel::onClickLocal()
125+
{
126+
callService<ChangeOperationMode>(raw_node_->get_logger(), client_change_to_local_);
127+
}
128+
129+
void OperationModeDebugPanel::onClickRemote()
130+
{
131+
callService<ChangeOperationMode>(raw_node_->get_logger(), client_change_to_remote_);
132+
}
133+
134+
void OperationModeDebugPanel::onClickAutowareControl()
135+
{
136+
callService<ChangeOperationMode>(raw_node_->get_logger(), client_enable_autoware_control_);
137+
}
138+
139+
void OperationModeDebugPanel::onClickDirectControl()
140+
{
141+
callService<ChangeOperationMode>(raw_node_->get_logger(), client_enable_direct_control_);
142+
}
143+
144+
void OperationModeDebugPanel::onOperationMode(const OperationModeState::ConstSharedPtr msg)
145+
{
146+
const auto updateLabel = [](QLabel * label, QString text, QString style) {
147+
label->setText(text);
148+
label->setStyleSheet(style);
149+
};
150+
151+
const auto updateButton = [](QPushButton * button, bool available) {
152+
if (available) {
153+
button->setStyleSheet("color: black;");
154+
} else {
155+
button->setStyleSheet("color: white;");
156+
}
157+
};
158+
159+
// Update current operation mode.
160+
161+
QString state = "";
162+
QString style = "";
163+
164+
switch (msg->mode) {
165+
case OperationModeState::AUTONOMOUS:
166+
state = "AUTONOMOUS";
167+
style = "background-color: #00FF00;"; // green
168+
break;
169+
170+
case OperationModeState::LOCAL:
171+
state = "LOCAL";
172+
style = "background-color: #FFFF00;"; // yellow
173+
break;
174+
175+
case OperationModeState::REMOTE:
176+
state = "REMOTE";
177+
style = "background-color: #FFFF00;"; // yellow
178+
break;
179+
180+
case OperationModeState::STOP:
181+
state = "STOP";
182+
style = "background-color: #FFA500;"; // orange
183+
break;
184+
185+
default:
186+
state = "UNKNOWN (" + QString::number(msg->mode) + ")";
187+
style = "background-color: #FF0000;"; // red
188+
break;
189+
}
190+
191+
if (msg->is_in_transition) {
192+
state += "\n(TRANSITION)";
193+
}
194+
195+
updateLabel(operation_mode_label_ptr_, state, style);
196+
197+
// Update current control mode.
198+
199+
if (msg->is_autoware_control_enabled) {
200+
updateLabel(control_mode_label_ptr_, "Enable", "background-color: #00FF00;"); // green
201+
} else {
202+
updateLabel(control_mode_label_ptr_, "Disable", "background-color: #FFFF00;"); // yellow
203+
}
204+
205+
// Update operation mode available.
206+
207+
updateButton(auto_button_ptr_, msg->is_autonomous_mode_available);
208+
updateButton(stop_button_ptr_, msg->is_stop_mode_available);
209+
updateButton(local_button_ptr_, msg->is_local_mode_available);
210+
updateButton(remote_button_ptr_, msg->is_remote_mode_available);
211+
212+
// Update control mode available.
213+
214+
updateButton(enable_button_ptr_, true);
215+
updateButton(disable_button_ptr_, true);
216+
}
217+
218+
} // namespace tier4_adapi_rviz_plugins
219+
220+
#include <pluginlib/class_list_macros.hpp>
221+
PLUGINLIB_EXPORT_CLASS(tier4_adapi_rviz_plugins::OperationModeDebugPanel, rviz_common::Panel)

‎visualization/tier4_adapi_rviz_plugin/src/operation_mode_debug_panel.hpp

+76
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
//
2+
// Copyright 2020 TIER IV, Inc. All rights reserved.
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
//
16+
17+
#ifndef OPERATION_MODE_DEBUG_PANEL_HPP_
18+
#define OPERATION_MODE_DEBUG_PANEL_HPP_
19+
20+
#include <QLabel>
21+
#include <QPushButton>
22+
#include <rclcpp/rclcpp.hpp>
23+
#include <rviz_common/panel.hpp>
24+
25+
#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
26+
#include <autoware_adapi_v1_msgs/srv/change_operation_mode.hpp>
27+
28+
namespace tier4_adapi_rviz_plugins
29+
{
30+
31+
class OperationModeDebugPanel : public rviz_common::Panel
32+
{
33+
Q_OBJECT
34+
35+
public:
36+
explicit OperationModeDebugPanel(QWidget * parent = nullptr);
37+
void onInitialize() override;
38+
39+
public Q_SLOTS: // NOLINT for Qt
40+
void onClickAutonomous();
41+
void onClickStop();
42+
void onClickLocal();
43+
void onClickRemote();
44+
void onClickAutowareControl();
45+
void onClickDirectControl();
46+
47+
protected:
48+
using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState;
49+
using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode;
50+
51+
QLabel * operation_mode_label_ptr_{nullptr};
52+
QPushButton * stop_button_ptr_{nullptr};
53+
QPushButton * auto_button_ptr_{nullptr};
54+
QPushButton * local_button_ptr_{nullptr};
55+
QPushButton * remote_button_ptr_{nullptr};
56+
57+
QLabel * control_mode_label_ptr_{nullptr};
58+
QPushButton * enable_button_ptr_{nullptr};
59+
QPushButton * disable_button_ptr_{nullptr};
60+
61+
rclcpp::Node::SharedPtr raw_node_;
62+
rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_;
63+
rclcpp::Client<ChangeOperationMode>::SharedPtr client_change_to_autonomous_;
64+
rclcpp::Client<ChangeOperationMode>::SharedPtr client_change_to_stop_;
65+
rclcpp::Client<ChangeOperationMode>::SharedPtr client_change_to_local_;
66+
rclcpp::Client<ChangeOperationMode>::SharedPtr client_change_to_remote_;
67+
rclcpp::Client<ChangeOperationMode>::SharedPtr client_enable_autoware_control_;
68+
rclcpp::Client<ChangeOperationMode>::SharedPtr client_enable_direct_control_;
69+
70+
void onOperationMode(const OperationModeState::ConstSharedPtr msg);
71+
void changeOperationMode(const rclcpp::Client<ChangeOperationMode>::SharedPtr client);
72+
};
73+
74+
} // namespace tier4_adapi_rviz_plugins
75+
76+
#endif // OPERATION_MODE_DEBUG_PANEL_HPP_

0 commit comments

Comments
 (0)