|
| 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