diff --git a/common/tier4_adapi_rviz_plugin/CMakeLists.txt b/common/tier4_adapi_rviz_plugin/CMakeLists.txt index f06ab33823a3f..cfc3821561b7a 100644 --- a/common/tier4_adapi_rviz_plugin/CMakeLists.txt +++ b/common/tier4_adapi_rviz_plugin/CMakeLists.txt @@ -12,7 +12,7 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON) ament_auto_add_library(${PROJECT_NAME} SHARED src/route_tool.cpp src/route_panel.cpp - src/state_panel.cpp + src/operation_mode_panel.cpp src/door_panel.cpp ) diff --git a/common/tier4_adapi_rviz_plugin/icons/classes/StatePanel.png b/common/tier4_adapi_rviz_plugin/icons/classes/OperationModePanel.png similarity index 100% rename from common/tier4_adapi_rviz_plugin/icons/classes/StatePanel.png rename to common/tier4_adapi_rviz_plugin/icons/classes/OperationModePanel.png diff --git a/common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml b/common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml index 0b4f00bd56278..a96e7d73f5d75 100644 --- a/common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml @@ -8,8 +8,8 @@ RoutePanel - - StatePanel + + OperationModePanel diff --git a/common/tier4_adapi_rviz_plugin/src/operation_mode_panel.cpp b/common/tier4_adapi_rviz_plugin/src/operation_mode_panel.cpp new file mode 100644 index 0000000000000..fc8bee8b97758 --- /dev/null +++ b/common/tier4_adapi_rviz_plugin/src/operation_mode_panel.cpp @@ -0,0 +1,221 @@ +// +// Copyright 2020 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// + +#include "operation_mode_panel.hpp" + +#include +#include +#include + +#include +#include + +namespace tier4_adapi_rviz_plugins +{ + +OperationModePanel::OperationModePanel(QWidget * parent) : rviz_common::Panel(parent) +{ + auto * layout = new QGridLayout; + setLayout(layout); + + operation_mode_label_ptr_ = new QLabel("INIT"); + operation_mode_label_ptr_->setAlignment(Qt::AlignCenter); + operation_mode_label_ptr_->setStyleSheet("border:1px solid black;"); + layout->addWidget(operation_mode_label_ptr_, 0, 0, 2, 1); + + auto_button_ptr_ = new QPushButton("AUTO"); + connect(auto_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutonomous())); + layout->addWidget(auto_button_ptr_, 0, 1); + + stop_button_ptr_ = new QPushButton("STOP"); + connect(stop_button_ptr_, SIGNAL(clicked()), SLOT(onClickStop())); + layout->addWidget(stop_button_ptr_, 0, 2); + + local_button_ptr_ = new QPushButton("LOCAL"); + connect(local_button_ptr_, SIGNAL(clicked()), SLOT(onClickLocal())); + layout->addWidget(local_button_ptr_, 1, 1); + + remote_button_ptr_ = new QPushButton("REMOTE"); + connect(remote_button_ptr_, SIGNAL(clicked()), SLOT(onClickRemote())); + layout->addWidget(remote_button_ptr_, 1, 2); + + control_mode_label_ptr_ = new QLabel("INIT"); + control_mode_label_ptr_->setAlignment(Qt::AlignCenter); + control_mode_label_ptr_->setStyleSheet("border:1px solid black;"); + layout->addWidget(control_mode_label_ptr_, 2, 0); + + enable_button_ptr_ = new QPushButton("Enable"); + connect(enable_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutowareControl())); + layout->addWidget(enable_button_ptr_, 2, 1); + + disable_button_ptr_ = new QPushButton("Disable"); + connect(disable_button_ptr_, SIGNAL(clicked()), SLOT(onClickDirectControl())); + layout->addWidget(disable_button_ptr_, 2, 2); +} + +void OperationModePanel::onInitialize() +{ + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + sub_operation_mode_ = raw_node_->create_subscription( + "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), + std::bind(&OperationModePanel::onOperationMode, this, std::placeholders::_1)); + + client_change_to_autonomous_ = + raw_node_->create_client("/api/operation_mode/change_to_autonomous"); + + client_change_to_stop_ = + raw_node_->create_client("/api/operation_mode/change_to_stop"); + + client_change_to_local_ = + raw_node_->create_client("/api/operation_mode/change_to_local"); + + client_change_to_remote_ = + raw_node_->create_client("/api/operation_mode/change_to_remote"); + + client_enable_autoware_control_ = + raw_node_->create_client("/api/operation_mode/enable_autoware_control"); + + client_enable_direct_control_ = + raw_node_->create_client("/api/operation_mode/disable_autoware_control"); +} + +template +void callService(const rclcpp::Logger & logger, const typename rclcpp::Client::SharedPtr client) +{ + auto req = std::make_shared(); + + RCLCPP_DEBUG(logger, "client request"); + + if (!client->service_is_ready()) { + RCLCPP_DEBUG(logger, "client is unavailable"); + return; + } + + client->async_send_request(req, [logger](typename rclcpp::Client::SharedFuture result) { + RCLCPP_DEBUG( + logger, "Status: %d, %s", result.get()->status.code, result.get()->status.message.c_str()); + }); +} + +void OperationModePanel::onClickAutonomous() +{ + callService(raw_node_->get_logger(), client_change_to_autonomous_); +} + +void OperationModePanel::onClickStop() +{ + callService(raw_node_->get_logger(), client_change_to_stop_); +} + +void OperationModePanel::onClickLocal() +{ + callService(raw_node_->get_logger(), client_change_to_local_); +} + +void OperationModePanel::onClickRemote() +{ + callService(raw_node_->get_logger(), client_change_to_remote_); +} + +void OperationModePanel::onClickAutowareControl() +{ + callService(raw_node_->get_logger(), client_enable_autoware_control_); +} + +void OperationModePanel::onClickDirectControl() +{ + callService(raw_node_->get_logger(), client_enable_direct_control_); +} + +void OperationModePanel::onOperationMode(const OperationModeState::ConstSharedPtr msg) +{ + const auto updateLabel = [](QLabel * label, QString text, QString style) { + label->setText(text); + label->setStyleSheet(style); + }; + + const auto updateButton = [](QPushButton * button, bool available) { + if (available) { + button->setStyleSheet("color: black;"); + } else { + button->setStyleSheet("color: white;"); + } + }; + + // Update current operation mode. + + QString state = ""; + QString style = ""; + + switch (msg->mode) { + case OperationModeState::AUTONOMOUS: + state = "AUTONOMOUS"; + style = "background-color: #00FF00;"; // green + break; + + case OperationModeState::LOCAL: + state = "LOCAL"; + style = "background-color: #FFFF00;"; // yellow + break; + + case OperationModeState::REMOTE: + state = "REMOTE"; + style = "background-color: #FFFF00;"; // yellow + break; + + case OperationModeState::STOP: + state = "STOP"; + style = "background-color: #FFA500;"; // orange + break; + + default: + state = "UNKNOWN (" + QString::number(msg->mode) + ")"; + style = "background-color: #FF0000;"; // red + break; + } + + if (msg->is_in_transition) { + state += "\n(TRANSITION)"; + } + + updateLabel(operation_mode_label_ptr_, state, style); + + // Update current control mode. + + if (msg->is_autoware_control_enabled) { + updateLabel(control_mode_label_ptr_, "Enable", "background-color: #00FF00;"); // green + } else { + updateLabel(control_mode_label_ptr_, "Disable", "background-color: #FFFF00;"); // yellow + } + + // Update operation mode available. + + updateButton(auto_button_ptr_, msg->is_autonomous_mode_available); + updateButton(stop_button_ptr_, msg->is_stop_mode_available); + updateButton(local_button_ptr_, msg->is_local_mode_available); + updateButton(remote_button_ptr_, msg->is_remote_mode_available); + + // Update control mode available. + + updateButton(enable_button_ptr_, true); + updateButton(disable_button_ptr_, true); +} + +} // namespace tier4_adapi_rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(tier4_adapi_rviz_plugins::OperationModePanel, rviz_common::Panel) diff --git a/common/tier4_adapi_rviz_plugin/src/operation_mode_panel.hpp b/common/tier4_adapi_rviz_plugin/src/operation_mode_panel.hpp new file mode 100644 index 0000000000000..4f848295bbb4a --- /dev/null +++ b/common/tier4_adapi_rviz_plugin/src/operation_mode_panel.hpp @@ -0,0 +1,76 @@ +// +// Copyright 2020 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// + +#ifndef OPERATION_MODE_PANEL_HPP_ +#define OPERATION_MODE_PANEL_HPP_ + +#include +#include +#include +#include + +#include +#include + +namespace tier4_adapi_rviz_plugins +{ + +class OperationModePanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit OperationModePanel(QWidget * parent = nullptr); + void onInitialize() override; + +public Q_SLOTS: // NOLINT for Qt + void onClickAutonomous(); + void onClickStop(); + void onClickLocal(); + void onClickRemote(); + void onClickAutowareControl(); + void onClickDirectControl(); + +protected: + using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; + using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; + + QLabel * operation_mode_label_ptr_{nullptr}; + QPushButton * stop_button_ptr_{nullptr}; + QPushButton * auto_button_ptr_{nullptr}; + QPushButton * local_button_ptr_{nullptr}; + QPushButton * remote_button_ptr_{nullptr}; + + QLabel * control_mode_label_ptr_{nullptr}; + QPushButton * enable_button_ptr_{nullptr}; + QPushButton * disable_button_ptr_{nullptr}; + + rclcpp::Node::SharedPtr raw_node_; + rclcpp::Subscription::SharedPtr sub_operation_mode_; + rclcpp::Client::SharedPtr client_change_to_autonomous_; + rclcpp::Client::SharedPtr client_change_to_stop_; + rclcpp::Client::SharedPtr client_change_to_local_; + rclcpp::Client::SharedPtr client_change_to_remote_; + rclcpp::Client::SharedPtr client_enable_autoware_control_; + rclcpp::Client::SharedPtr client_enable_direct_control_; + + void onOperationMode(const OperationModeState::ConstSharedPtr msg); + void changeOperationMode(const rclcpp::Client::SharedPtr client); +}; + +} // namespace tier4_adapi_rviz_plugins + +#endif // OPERATION_MODE_PANEL_HPP_ diff --git a/common/tier4_adapi_rviz_plugin/src/state_panel.cpp b/common/tier4_adapi_rviz_plugin/src/state_panel.cpp deleted file mode 100644 index 6d2980a7f2c4a..0000000000000 --- a/common/tier4_adapi_rviz_plugin/src/state_panel.cpp +++ /dev/null @@ -1,633 +0,0 @@ -// -// Copyright 2020 TIER IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// - -#include "state_panel.hpp" - -#include -#include -#include -#include -#include -#include - -#include -#include - -namespace tier4_adapi_rviz_plugins -{ - -StatePanel::StatePanel(QWidget * parent) : rviz_common::Panel(parent) -{ - // Gear - auto * gear_prefix_label_ptr = new QLabel("GEAR: "); - gear_prefix_label_ptr->setAlignment(Qt::AlignRight | Qt::AlignVCenter); - gear_label_ptr_ = new QLabel("INIT"); - gear_label_ptr_->setAlignment(Qt::AlignCenter); - auto * gear_layout = new QHBoxLayout; - gear_layout->addWidget(gear_prefix_label_ptr); - gear_layout->addWidget(gear_label_ptr_); - - // Velocity Limit - velocity_limit_button_ptr_ = new QPushButton("Send Velocity Limit"); - pub_velocity_limit_input_ = new QSpinBox(); - pub_velocity_limit_input_->setRange(-100.0, 100.0); - pub_velocity_limit_input_->setValue(0.0); - pub_velocity_limit_input_->setSingleStep(5.0); - connect(velocity_limit_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickVelocityLimit())); - - // Emergency Button - emergency_button_ptr_ = new QPushButton("Set Emergency"); - connect(emergency_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickEmergencyButton())); - - // Layout - auto * v_layout = new QVBoxLayout; - auto * velocity_limit_layout = new QHBoxLayout(); - v_layout->addWidget(makeOperationModeGroup()); - v_layout->addWidget(makeControlModeGroup()); - { - auto * h_layout = new QHBoxLayout(); - h_layout->addWidget(makeRoutingGroup()); - h_layout->addWidget(makeLocalizationGroup()); - h_layout->addWidget(makeMotionGroup()); - h_layout->addWidget(makeFailSafeGroup()); - v_layout->addLayout(h_layout); - } - - v_layout->addLayout(gear_layout); - velocity_limit_layout->addWidget(velocity_limit_button_ptr_); - velocity_limit_layout->addWidget(pub_velocity_limit_input_); - velocity_limit_layout->addWidget(new QLabel(" [km/h]")); - velocity_limit_layout->addWidget(emergency_button_ptr_); - v_layout->addLayout(velocity_limit_layout); - setLayout(v_layout); -} - -QGroupBox * StatePanel::makeOperationModeGroup() -{ - auto * group = new QGroupBox("OperationMode"); - auto * grid = new QGridLayout; - - operation_mode_label_ptr_ = new QLabel("INIT"); - operation_mode_label_ptr_->setAlignment(Qt::AlignCenter); - operation_mode_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(operation_mode_label_ptr_, 0, 0, 0, 1); - - auto_button_ptr_ = new QPushButton("AUTO"); - auto_button_ptr_->setCheckable(true); - connect(auto_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutonomous())); - grid->addWidget(auto_button_ptr_, 0, 1); - - stop_button_ptr_ = new QPushButton("STOP"); - stop_button_ptr_->setCheckable(true); - connect(stop_button_ptr_, SIGNAL(clicked()), SLOT(onClickStop())); - grid->addWidget(stop_button_ptr_, 0, 2); - - local_button_ptr_ = new QPushButton("LOCAL"); - local_button_ptr_->setCheckable(true); - connect(local_button_ptr_, SIGNAL(clicked()), SLOT(onClickLocal())); - grid->addWidget(local_button_ptr_, 1, 1); - - remote_button_ptr_ = new QPushButton("REMOTE"); - remote_button_ptr_->setCheckable(true); - connect(remote_button_ptr_, SIGNAL(clicked()), SLOT(onClickRemote())); - grid->addWidget(remote_button_ptr_, 1, 2); - - group->setLayout(grid); - return group; -} - -QGroupBox * StatePanel::makeControlModeGroup() -{ - auto * group = new QGroupBox("AutowareControl"); - auto * grid = new QGridLayout; - - control_mode_label_ptr_ = new QLabel("INIT"); - control_mode_label_ptr_->setAlignment(Qt::AlignCenter); - control_mode_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(control_mode_label_ptr_, 0, 0); - - enable_button_ptr_ = new QPushButton("Enable"); - enable_button_ptr_->setCheckable(true); - connect(enable_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutowareControl())); - grid->addWidget(enable_button_ptr_, 0, 1); - - disable_button_ptr_ = new QPushButton("Disable"); - disable_button_ptr_->setCheckable(true); - connect(disable_button_ptr_, SIGNAL(clicked()), SLOT(onClickDirectControl())); - grid->addWidget(disable_button_ptr_, 0, 2); - - group->setLayout(grid); - return group; -} - -QGroupBox * StatePanel::makeRoutingGroup() -{ - auto * group = new QGroupBox("Routing"); - auto * grid = new QGridLayout; - - routing_label_ptr_ = new QLabel("INIT"); - routing_label_ptr_->setAlignment(Qt::AlignCenter); - routing_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(routing_label_ptr_, 0, 0); - - clear_route_button_ptr_ = new QPushButton("Clear Route"); - clear_route_button_ptr_->setCheckable(true); - connect(clear_route_button_ptr_, SIGNAL(clicked()), SLOT(onClickClearRoute())); - grid->addWidget(clear_route_button_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -QGroupBox * StatePanel::makeLocalizationGroup() -{ - auto * group = new QGroupBox("Localization"); - auto * grid = new QGridLayout; - - localization_label_ptr_ = new QLabel("INIT"); - localization_label_ptr_->setAlignment(Qt::AlignCenter); - localization_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(localization_label_ptr_, 0, 0); - - init_by_gnss_button_ptr_ = new QPushButton("Init by GNSS"); - connect(init_by_gnss_button_ptr_, SIGNAL(clicked()), SLOT(onClickInitByGnss())); - grid->addWidget(init_by_gnss_button_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -QGroupBox * StatePanel::makeMotionGroup() -{ - auto * group = new QGroupBox("Motion"); - auto * grid = new QGridLayout; - - motion_label_ptr_ = new QLabel("INIT"); - motion_label_ptr_->setAlignment(Qt::AlignCenter); - motion_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(motion_label_ptr_, 0, 0); - - accept_start_button_ptr_ = new QPushButton("Accept Start"); - accept_start_button_ptr_->setCheckable(true); - connect(accept_start_button_ptr_, SIGNAL(clicked()), SLOT(onClickAcceptStart())); - grid->addWidget(accept_start_button_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -QGroupBox * StatePanel::makeFailSafeGroup() -{ - auto * group = new QGroupBox("FailSafe"); - auto * grid = new QGridLayout; - - mrm_state_label_ptr_ = new QLabel("INIT"); - mrm_state_label_ptr_->setAlignment(Qt::AlignCenter); - mrm_state_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(mrm_state_label_ptr_, 0, 0); - - mrm_behavior_label_ptr_ = new QLabel("INIT"); - mrm_behavior_label_ptr_->setAlignment(Qt::AlignCenter); - mrm_behavior_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(mrm_behavior_label_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -void StatePanel::onInitialize() -{ - using std::placeholders::_1; - - raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - - // Operation Mode - sub_operation_mode_ = raw_node_->create_subscription( - "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), - std::bind(&StatePanel::onOperationMode, this, _1)); - - client_change_to_autonomous_ = - raw_node_->create_client("/api/operation_mode/change_to_autonomous"); - - client_change_to_stop_ = - raw_node_->create_client("/api/operation_mode/change_to_stop"); - - client_change_to_local_ = - raw_node_->create_client("/api/operation_mode/change_to_local"); - - client_change_to_remote_ = - raw_node_->create_client("/api/operation_mode/change_to_remote"); - - client_enable_autoware_control_ = - raw_node_->create_client("/api/operation_mode/enable_autoware_control"); - - client_enable_direct_control_ = - raw_node_->create_client("/api/operation_mode/disable_autoware_control"); - - // Routing - sub_route_ = raw_node_->create_subscription( - "/api/routing/state", rclcpp::QoS{1}.transient_local(), - std::bind(&StatePanel::onRoute, this, _1)); - - client_clear_route_ = raw_node_->create_client("/api/routing/clear_route"); - - // Localization - sub_localization_ = raw_node_->create_subscription( - "/api/localization/initialization_state", rclcpp::QoS{1}.transient_local(), - std::bind(&StatePanel::onLocalization, this, _1)); - - client_init_by_gnss_ = - raw_node_->create_client("/api/localization/initialize"); - - // Motion - sub_motion_ = raw_node_->create_subscription( - "/api/motion/state", rclcpp::QoS{1}.transient_local(), - std::bind(&StatePanel::onMotion, this, _1)); - - client_accept_start_ = raw_node_->create_client("/api/motion/accept_start"); - - // FailSafe - sub_mrm_ = raw_node_->create_subscription( - "/api/fail_safe/mrm_state", rclcpp::QoS{1}.transient_local(), - std::bind(&StatePanel::onMRMState, this, _1)); - - // Others - sub_gear_ = raw_node_->create_subscription( - "/vehicle/status/gear_status", 10, std::bind(&StatePanel::onShift, this, _1)); - - sub_emergency_ = raw_node_->create_subscription( - "/api/autoware/get/emergency", 10, std::bind(&StatePanel::onEmergencyStatus, this, _1)); - - client_emergency_stop_ = raw_node_->create_client( - "/api/autoware/set/emergency"); - - pub_velocity_limit_ = raw_node_->create_publisher( - "/planning/scenario_planning/max_velocity_default", rclcpp::QoS{1}.transient_local()); -} - -void StatePanel::onOperationMode(const OperationModeState::ConstSharedPtr msg) -{ - auto changeButtonState = [this]( - QPushButton * button, const bool is_desired_mode_available, - const uint8_t current_mode = OperationModeState::UNKNOWN, - const uint8_t desired_mode = OperationModeState::STOP) { - if (is_desired_mode_available && current_mode != desired_mode) { - activateButton(button); - } else { - deactivateButton(button); - } - }; - - QString text = ""; - QString style_sheet = ""; - // Operation Mode - switch (msg->mode) { - case OperationModeState::AUTONOMOUS: - text = "AUTONOMOUS"; - style_sheet = "background-color: #00FF00;"; // green - break; - - case OperationModeState::LOCAL: - text = "LOCAL"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case OperationModeState::REMOTE: - text = "REMOTE"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case OperationModeState::STOP: - text = "STOP"; - style_sheet = "background-color: #FFA500;"; // orange - break; - - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } - - if (msg->is_in_transition) { - text += "\n(TRANSITION)"; - } - - updateLabel(operation_mode_label_ptr_, text, style_sheet); - - // Control Mode - if (msg->is_autoware_control_enabled) { - updateLabel(control_mode_label_ptr_, "Enable", "background-color: #00FF00;"); // green - } else { - updateLabel(control_mode_label_ptr_, "Disable", "background-color: #FFFF00;"); // yellow - } - - // Button - changeButtonState( - auto_button_ptr_, msg->is_autonomous_mode_available, msg->mode, OperationModeState::AUTONOMOUS); - changeButtonState( - stop_button_ptr_, msg->is_stop_mode_available, msg->mode, OperationModeState::STOP); - changeButtonState( - local_button_ptr_, msg->is_local_mode_available, msg->mode, OperationModeState::LOCAL); - changeButtonState( - remote_button_ptr_, msg->is_remote_mode_available, msg->mode, OperationModeState::REMOTE); - - changeButtonState(enable_button_ptr_, !msg->is_autoware_control_enabled); - changeButtonState(disable_button_ptr_, msg->is_autoware_control_enabled); -} - -void StatePanel::onRoute(const RouteState::ConstSharedPtr msg) -{ - QString text = ""; - QString style_sheet = ""; - switch (msg->state) { - case RouteState::UNSET: - text = "UNSET"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case RouteState::SET: - text = "SET"; - style_sheet = "background-color: #00FF00;"; // green - break; - - case RouteState::ARRIVED: - text = "ARRIVED"; - style_sheet = "background-color: #FFA500;"; // orange - break; - - case RouteState::CHANGING: - text = "CHANGING"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } - - updateLabel(routing_label_ptr_, text, style_sheet); - - if (msg->state == RouteState::SET) { - activateButton(clear_route_button_ptr_); - } else { - deactivateButton(clear_route_button_ptr_); - } -} - -void StatePanel::onLocalization(const LocalizationInitializationState::ConstSharedPtr msg) -{ - QString text = ""; - QString style_sheet = ""; - switch (msg->state) { - case LocalizationInitializationState::UNINITIALIZED: - text = "UNINITIALIZED"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case LocalizationInitializationState::INITIALIZING: - text = "INITIALIZING"; - style_sheet = "background-color: #FFA500;"; // orange - break; - - case LocalizationInitializationState::INITIALIZED: - text = "INITIALIZED"; - style_sheet = "background-color: #00FF00;"; // green - break; - - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } - - updateLabel(localization_label_ptr_, text, style_sheet); -} - -void StatePanel::onMotion(const MotionState::ConstSharedPtr msg) -{ - QString text = ""; - QString style_sheet = ""; - switch (msg->state) { - case MotionState::STARTING: - text = "STARTING"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case MotionState::STOPPED: - text = "STOPPED"; - style_sheet = "background-color: #FFA500;"; // orange - break; - - case MotionState::MOVING: - text = "MOVING"; - style_sheet = "background-color: #00FF00;"; // green - break; - - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } - - updateLabel(motion_label_ptr_, text, style_sheet); - - if (msg->state == MotionState::STARTING) { - activateButton(accept_start_button_ptr_); - } else { - deactivateButton(accept_start_button_ptr_); - } -} - -void StatePanel::onMRMState(const MRMState::ConstSharedPtr msg) -{ - // state - { - QString text = ""; - QString style_sheet = ""; - switch (msg->state) { - case MRMState::NONE: - text = "NONE"; - style_sheet = "background-color: #00FF00;"; // green - break; - - case MRMState::MRM_OPERATING: - text = "MRM_OPERATING"; - style_sheet = "background-color: #FFA500;"; // orange - break; - - case MRMState::MRM_SUCCEEDED: - text = "MRM_SUCCEEDED"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case MRMState::MRM_FAILED: - text = "MRM_FAILED"; - style_sheet = "background-color: #FF0000;"; // red - break; - - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } - - updateLabel(mrm_state_label_ptr_, text, style_sheet); - } - - // behavior - { - QString text = ""; - QString style_sheet = ""; - switch (msg->behavior) { - case MRMState::NONE: - text = "NONE"; - style_sheet = "background-color: #00FF00;"; // green - break; - - case MRMState::PULL_OVER: - text = "PULL_OVER"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case MRMState::COMFORTABLE_STOP: - text = "COMFORTABLE_STOP"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case MRMState::EMERGENCY_STOP: - text = "EMERGENCY_STOP"; - style_sheet = "background-color: #FFA500;"; // orange - break; - - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } - - updateLabel(mrm_behavior_label_ptr_, text, style_sheet); - } -} - -void StatePanel::onShift(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) -{ - switch (msg->report) { - case autoware_vehicle_msgs::msg::GearReport::PARK: - gear_label_ptr_->setText("PARKING"); - break; - case autoware_vehicle_msgs::msg::GearReport::REVERSE: - gear_label_ptr_->setText("REVERSE"); - break; - case autoware_vehicle_msgs::msg::GearReport::DRIVE: - gear_label_ptr_->setText("DRIVE"); - break; - case autoware_vehicle_msgs::msg::GearReport::NEUTRAL: - gear_label_ptr_->setText("NEUTRAL"); - break; - case autoware_vehicle_msgs::msg::GearReport::LOW: - gear_label_ptr_->setText("LOW"); - break; - } -} - -void StatePanel::onEmergencyStatus( - const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg) -{ - current_emergency_ = msg->emergency; - if (msg->emergency) { - emergency_button_ptr_->setText(QString::fromStdString("Clear Emergency")); - emergency_button_ptr_->setStyleSheet("background-color: #FF0000;"); - } else { - emergency_button_ptr_->setText(QString::fromStdString("Set Emergency")); - emergency_button_ptr_->setStyleSheet("background-color: #00FF00;"); - } -} - -void StatePanel::onClickVelocityLimit() -{ - auto velocity_limit = std::make_shared(); - velocity_limit->stamp = raw_node_->now(); - velocity_limit->max_velocity = pub_velocity_limit_input_->value() / 3.6; - pub_velocity_limit_->publish(*velocity_limit); -} - -void StatePanel::onClickAutonomous() -{ - callServiceWithoutResponse(client_change_to_autonomous_); -} -void StatePanel::onClickStop() -{ - callServiceWithoutResponse(client_change_to_stop_); -} -void StatePanel::onClickLocal() -{ - callServiceWithoutResponse(client_change_to_local_); -} -void StatePanel::onClickRemote() -{ - callServiceWithoutResponse(client_change_to_remote_); -} -void StatePanel::onClickAutowareControl() -{ - callServiceWithoutResponse(client_enable_autoware_control_); -} -void StatePanel::onClickDirectControl() -{ - callServiceWithoutResponse(client_enable_direct_control_); -} - -void StatePanel::onClickClearRoute() -{ - callServiceWithoutResponse(client_clear_route_); -} - -void StatePanel::onClickInitByGnss() -{ - callServiceWithoutResponse(client_init_by_gnss_); -} - -void StatePanel::onClickAcceptStart() -{ - callServiceWithoutResponse(client_accept_start_); -} - -void StatePanel::onClickEmergencyButton() -{ - using tier4_external_api_msgs::msg::ResponseStatus; - using tier4_external_api_msgs::srv::SetEmergency; - - auto request = std::make_shared(); - request->emergency = !current_emergency_; - - RCLCPP_INFO(raw_node_->get_logger(), request->emergency ? "Set Emergency" : "Clear Emergency"); - - client_emergency_stop_->async_send_request( - request, [this](rclcpp::Client::SharedFuture result) { - const auto & response = result.get(); - if (response->status.code == ResponseStatus::SUCCESS) { - RCLCPP_INFO(raw_node_->get_logger(), "service succeeded"); - } else { - RCLCPP_WARN( - raw_node_->get_logger(), "service failed: %s", response->status.message.c_str()); - } - }); -} - -} // namespace tier4_adapi_rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(tier4_adapi_rviz_plugins::StatePanel, rviz_common::Panel) diff --git a/common/tier4_adapi_rviz_plugin/src/state_panel.hpp b/common/tier4_adapi_rviz_plugin/src/state_panel.hpp deleted file mode 100644 index b30494772061e..0000000000000 --- a/common/tier4_adapi_rviz_plugin/src/state_panel.hpp +++ /dev/null @@ -1,206 +0,0 @@ -// -// Copyright 2020 TIER IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// - -#ifndef STATE_PANEL_HPP_ -#define STATE_PANEL_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace tier4_adapi_rviz_plugins -{ - -class StatePanel : public rviz_common::Panel -{ - using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; - using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; - using RouteState = autoware_adapi_v1_msgs::msg::RouteState; - using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute; - using LocalizationInitializationState = - autoware_adapi_v1_msgs::msg::LocalizationInitializationState; - using InitializeLocalization = autoware_adapi_v1_msgs::srv::InitializeLocalization; - using MotionState = autoware_adapi_v1_msgs::msg::MotionState; - using AcceptStart = autoware_adapi_v1_msgs::srv::AcceptStart; - using MRMState = autoware_adapi_v1_msgs::msg::MrmState; - - Q_OBJECT - -public: - explicit StatePanel(QWidget * parent = nullptr); - void onInitialize() override; - -public Q_SLOTS: // NOLINT for Qt - void onClickAutonomous(); - void onClickStop(); - void onClickLocal(); - void onClickRemote(); - void onClickAutowareControl(); - void onClickDirectControl(); - void onClickClearRoute(); - void onClickInitByGnss(); - void onClickAcceptStart(); - void onClickVelocityLimit(); - void onClickEmergencyButton(); - -protected: - // Layout - QGroupBox * makeOperationModeGroup(); - QGroupBox * makeControlModeGroup(); - QGroupBox * makeRoutingGroup(); - QGroupBox * makeLocalizationGroup(); - QGroupBox * makeMotionGroup(); - QGroupBox * makeFailSafeGroup(); - - void onShift(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); - void onEmergencyStatus(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg); - - rclcpp::Node::SharedPtr raw_node_; - rclcpp::Subscription::SharedPtr sub_gear_; - rclcpp::Client::SharedPtr client_emergency_stop_; - rclcpp::Subscription::SharedPtr sub_emergency_; - rclcpp::Publisher::SharedPtr pub_velocity_limit_; - - // Operation Mode - QLabel * operation_mode_label_ptr_{nullptr}; - QPushButton * stop_button_ptr_{nullptr}; - QPushButton * auto_button_ptr_{nullptr}; - QPushButton * local_button_ptr_{nullptr}; - QPushButton * remote_button_ptr_{nullptr}; - - rclcpp::Subscription::SharedPtr sub_operation_mode_; - rclcpp::Client::SharedPtr client_change_to_autonomous_; - rclcpp::Client::SharedPtr client_change_to_stop_; - rclcpp::Client::SharedPtr client_change_to_local_; - rclcpp::Client::SharedPtr client_change_to_remote_; - - // Control Mode - QLabel * control_mode_label_ptr_{nullptr}; - QPushButton * enable_button_ptr_{nullptr}; - QPushButton * disable_button_ptr_{nullptr}; - rclcpp::Client::SharedPtr client_enable_autoware_control_; - rclcpp::Client::SharedPtr client_enable_direct_control_; - - // Functions - void onOperationMode(const OperationModeState::ConstSharedPtr msg); - void changeOperationMode(const rclcpp::Client::SharedPtr client); - - // Routing - QLabel * routing_label_ptr_{nullptr}; - QPushButton * clear_route_button_ptr_{nullptr}; - - rclcpp::Subscription::SharedPtr sub_route_; - rclcpp::Client::SharedPtr client_clear_route_; - - void onRoute(const RouteState::ConstSharedPtr msg); - - // Localization - QLabel * localization_label_ptr_{nullptr}; - QPushButton * init_by_gnss_button_ptr_{nullptr}; - - rclcpp::Subscription::SharedPtr sub_localization_; - rclcpp::Client::SharedPtr client_init_by_gnss_; - - void onLocalization(const LocalizationInitializationState::ConstSharedPtr msg); - - // Motion - QLabel * motion_label_ptr_{nullptr}; - QPushButton * accept_start_button_ptr_{nullptr}; - - rclcpp::Subscription::SharedPtr sub_motion_; - rclcpp::Client::SharedPtr client_accept_start_; - - void onMotion(const MotionState::ConstSharedPtr msg); - - // FailSafe - QLabel * mrm_state_label_ptr_{nullptr}; - QLabel * mrm_behavior_label_ptr_{nullptr}; - - rclcpp::Subscription::SharedPtr sub_mrm_; - - void onMRMState(const MRMState::ConstSharedPtr msg); - - // Others - QPushButton * velocity_limit_button_ptr_; - QLabel * gear_label_ptr_; - - QSpinBox * pub_velocity_limit_input_; - QPushButton * emergency_button_ptr_; - - bool current_emergency_{false}; - - template - void callServiceWithoutResponse(const typename rclcpp::Client::SharedPtr client) - { - auto req = std::make_shared(); - - RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); - - if (!client->service_is_ready()) { - RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); - return; - } - - client->async_send_request(req, [this](typename rclcpp::Client::SharedFuture result) { - RCLCPP_DEBUG( - raw_node_->get_logger(), "Status: %d, %s", result.get()->status.code, - result.get()->status.message.c_str()); - }); - } - - static void updateLabel(QLabel * label, QString text, QString style_sheet) - { - label->setText(text); - label->setStyleSheet(style_sheet); - } - - static void activateButton(QAbstractButton * button) - { - button->setChecked(false); - button->setEnabled(true); - } - - static void deactivateButton(QAbstractButton * button) - { - button->setChecked(true); - button->setEnabled(false); - } -}; - -} // namespace tier4_adapi_rviz_plugins - -#endif // STATE_PANEL_HPP_