Skip to content

Commit c61b2be

Browse files
author
KhalilSelyan
committed
update state panel with latest design and custom components
Signed-off-by: KhalilSelyan <khalil@leodrive.ai>
1 parent 73ba212 commit c61b2be

File tree

2 files changed

+581
-363
lines changed

2 files changed

+581
-363
lines changed
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,264 @@
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 AUTOWARE_STATE_PANEL_HPP_
18+
#define AUTOWARE_STATE_PANEL_HPP_
19+
20+
#include "custom_button.hpp"
21+
#include "custom_container.hpp"
22+
#include "custom_icon_label.hpp"
23+
#include "custom_label.hpp"
24+
#include "custom_segmented_button.hpp"
25+
#include "custom_slider.hpp"
26+
#include "custom_toggle_switch.hpp"
27+
28+
#include <QChar>
29+
#include <QColor>
30+
#include <QGridLayout>
31+
#include <QHBoxLayout>
32+
#include <QHeaderView>
33+
#include <QLabel>
34+
#include <QLayout>
35+
#include <QPushButton>
36+
#include <QScrollArea>
37+
#include <QSlider>
38+
#include <QSpinBox>
39+
#include <QString>
40+
#include <QTableWidget>
41+
#include <QVBoxLayout>
42+
#include <ament_index_cpp/get_package_share_directory.hpp>
43+
#include <rclcpp/rclcpp.hpp>
44+
#include <rviz_common/panel.hpp>
45+
46+
#include <autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>
47+
#include <autoware_adapi_v1_msgs/msg/motion_state.hpp>
48+
#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
49+
#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
50+
#include <autoware_adapi_v1_msgs/msg/route_state.hpp>
51+
#include <autoware_adapi_v1_msgs/srv/accept_start.hpp>
52+
#include <autoware_adapi_v1_msgs/srv/change_operation_mode.hpp>
53+
#include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
54+
#include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
55+
#include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
56+
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
57+
#include <diagnostic_msgs/msg/diagnostic_status.hpp>
58+
#include <tier4_external_api_msgs/msg/emergency.hpp>
59+
#include <tier4_external_api_msgs/srv/set_emergency.hpp>
60+
#include <tier4_planning_msgs/msg/velocity_limit.hpp>
61+
62+
#include <qlabel.h>
63+
64+
#include <memory>
65+
#include <string>
66+
#include <unordered_map>
67+
68+
namespace rviz_plugins
69+
{
70+
class AutowareStatePanel : public rviz_common::Panel
71+
{
72+
using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState;
73+
using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode;
74+
using RouteState = autoware_adapi_v1_msgs::msg::RouteState;
75+
using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute;
76+
using LocalizationInitializationState =
77+
autoware_adapi_v1_msgs::msg::LocalizationInitializationState;
78+
using InitializeLocalization = autoware_adapi_v1_msgs::srv::InitializeLocalization;
79+
using MotionState = autoware_adapi_v1_msgs::msg::MotionState;
80+
using AcceptStart = autoware_adapi_v1_msgs::srv::AcceptStart;
81+
using MRMState = autoware_adapi_v1_msgs::msg::MrmState;
82+
using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray;
83+
using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus;
84+
85+
Q_OBJECT
86+
87+
public:
88+
explicit AutowareStatePanel(QWidget * parent = nullptr);
89+
void onInitialize() override;
90+
91+
public Q_SLOTS: // NOLINT for Qt
92+
void onClickAutonomous();
93+
void onClickStop();
94+
void onClickLocal();
95+
void onClickRemote();
96+
void onClickAutowareControl();
97+
void onClickDirectControl();
98+
void onClickClearRoute();
99+
void onClickInitByGnss();
100+
void onClickAcceptStart();
101+
void onClickVelocityLimit();
102+
void onClickEmergencyButton();
103+
void onSwitchStateChanged(int state);
104+
105+
protected:
106+
// Layout
107+
QVBoxLayout * makeVelocityLimitGroup();
108+
QVBoxLayout * makeOperationModeGroup();
109+
QVBoxLayout * makeRoutingGroup();
110+
QVBoxLayout * makeLocalizationGroup();
111+
QVBoxLayout * makeMotionGroup();
112+
QVBoxLayout * makeFailSafeGroup();
113+
// QVBoxLayout * makeDiagnosticGroup();
114+
115+
// void onShift(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg);
116+
void onEmergencyStatus(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg);
117+
118+
rclcpp::Node::SharedPtr raw_node_;
119+
120+
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::GearReport>::SharedPtr sub_gear_;
121+
122+
rclcpp::Client<tier4_external_api_msgs::srv::SetEmergency>::SharedPtr client_emergency_stop_;
123+
rclcpp::Subscription<tier4_external_api_msgs::msg::Emergency>::SharedPtr sub_emergency_;
124+
125+
rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr pub_velocity_limit_;
126+
127+
QLabel * velocity_limit_value_label_{nullptr};
128+
bool sliderIsDragging = false;
129+
130+
// Operation Mode
131+
QLabel * operation_mode_label_ptr_{nullptr};
132+
QPushButton * stop_button_ptr_{nullptr};
133+
QPushButton * auto_button_ptr_{nullptr};
134+
QPushButton * local_button_ptr_{nullptr};
135+
QPushButton * remote_button_ptr_{nullptr};
136+
137+
rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_;
138+
rclcpp::Client<ChangeOperationMode>::SharedPtr client_change_to_autonomous_;
139+
rclcpp::Client<ChangeOperationMode>::SharedPtr client_change_to_stop_;
140+
rclcpp::Client<ChangeOperationMode>::SharedPtr client_change_to_local_;
141+
rclcpp::Client<ChangeOperationMode>::SharedPtr client_change_to_remote_;
142+
143+
//// Control Mode
144+
CustomSegmentedButton * segmented_button;
145+
CustomToggleSwitch * control_mode_switch_ptr_{nullptr};
146+
QLabel * control_mode_label_ptr_{nullptr};
147+
QPushButton * enable_button_ptr_{nullptr};
148+
QPushButton * disable_button_ptr_{nullptr};
149+
rclcpp::Client<ChangeOperationMode>::SharedPtr client_enable_autoware_control_;
150+
rclcpp::Client<ChangeOperationMode>::SharedPtr client_enable_direct_control_;
151+
152+
//// Functions
153+
void onOperationMode(const OperationModeState::ConstSharedPtr msg);
154+
void changeOperationMode(const rclcpp::Client<ChangeOperationMode>::SharedPtr client);
155+
156+
// Routing
157+
CustomIconLabel * routing_icon{nullptr};
158+
CustomElevatedButton * clear_route_button_ptr_{nullptr};
159+
160+
rclcpp::Subscription<RouteState>::SharedPtr sub_route_;
161+
rclcpp::Client<ClearRoute>::SharedPtr client_clear_route_;
162+
163+
void onRoute(const RouteState::ConstSharedPtr msg);
164+
165+
// Localization
166+
CustomIconLabel * localization_icon{nullptr};
167+
CustomElevatedButton * init_by_gnss_button_ptr_{nullptr};
168+
169+
rclcpp::Subscription<LocalizationInitializationState>::SharedPtr sub_localization_;
170+
rclcpp::Client<InitializeLocalization>::SharedPtr client_init_by_gnss_;
171+
172+
void onLocalization(const LocalizationInitializationState::ConstSharedPtr msg);
173+
174+
// Motion
175+
CustomIconLabel * motion_icon{nullptr};
176+
CustomElevatedButton * accept_start_button_ptr_{nullptr};
177+
178+
rclcpp::Subscription<MotionState>::SharedPtr sub_motion_;
179+
rclcpp::Client<AcceptStart>::SharedPtr client_accept_start_;
180+
181+
void onMotion(const MotionState::ConstSharedPtr msg);
182+
183+
// FailSafe
184+
CustomIconLabel * mrm_state_icon{nullptr};
185+
QLabel * mrm_state_label_ptr_{nullptr};
186+
CustomIconLabel * mrm_behavior_icon{nullptr};
187+
QLabel * mrm_behavior_label_ptr_{nullptr};
188+
189+
rclcpp::Subscription<MRMState>::SharedPtr sub_mrm_;
190+
191+
void onMRMState(const MRMState::ConstSharedPtr msg);
192+
193+
// Diagnostic
194+
195+
// QLabel * diagnostic_label_ptr_{nullptr};
196+
// QVBoxLayout * diagnostic_layout_{nullptr};
197+
// std::unordered_map<std::string, QLabel *> statusLabels;
198+
199+
// rclcpp::Subscription<DiagnosticArray>::SharedPtr sub_diag_;
200+
// void onDiagnostics(const DiagnosticArray::ConstSharedPtr msg);
201+
// void addStatusLabel(const std::string & name, int level);
202+
// void updateStatusLabel(const std::string & name, int level);
203+
204+
// Others
205+
QLabel * velocity_limit_setter_ptr_;
206+
QLabel * gear_label_ptr_;
207+
208+
QSpinBox * pub_velocity_limit_input_;
209+
CustomElevatedButton * emergency_button_ptr_;
210+
211+
bool current_emergency_{false};
212+
213+
template <typename T>
214+
void callServiceWithoutResponse(const typename rclcpp::Client<T>::SharedPtr client)
215+
{
216+
auto req = std::make_shared<typename T::Request>();
217+
218+
RCLCPP_DEBUG(raw_node_->get_logger(), "client request");
219+
220+
if (!client->service_is_ready()) {
221+
RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable");
222+
return;
223+
}
224+
225+
client->async_send_request(req, [this](typename rclcpp::Client<T>::SharedFuture result) {
226+
RCLCPP_DEBUG(
227+
raw_node_->get_logger(), "Status: %d, %s", result.get()->status.code,
228+
result.get()->status.message.c_str());
229+
});
230+
}
231+
232+
static void updateLabel(QLabel * label, QString text, QString style_sheet)
233+
{
234+
label->setText(text);
235+
label->setStyleSheet(style_sheet);
236+
}
237+
static void updateCustomLabel(
238+
CustomLabel * label, QString text, QColor bg_color, QColor text_color)
239+
{
240+
label->updateStyle(text, bg_color, text_color);
241+
}
242+
243+
static void updateButton(QPushButton * button, QString text, QString style_sheet)
244+
{
245+
button->setText(text);
246+
button->setStyleSheet(style_sheet);
247+
}
248+
249+
static void activateButton(QAbstractButton * button)
250+
{
251+
button->setChecked(false);
252+
button->setDisabled(false);
253+
}
254+
255+
static void deactivateButton(QAbstractButton * button)
256+
{
257+
button->setChecked(true);
258+
button->setDisabled(true);
259+
}
260+
};
261+
262+
} // namespace rviz_plugins
263+
264+
#endif // AUTOWARE_STATE_PANEL_HPP_

0 commit comments

Comments
 (0)