diff --git a/common/rtc_manager_rviz_plugin/CMakeLists.txt b/common/rtc_manager_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..f2fad9e24665f --- /dev/null +++ b/common/rtc_manager_rviz_plugin/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.14) +project(rtc_manager_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/rtc_manager_panel.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + icons + plugins +) diff --git a/common/rtc_manager_rviz_plugin/README.md b/common/rtc_manager_rviz_plugin/README.md new file mode 100644 index 0000000000000..a4542849e4ce5 --- /dev/null +++ b/common/rtc_manager_rviz_plugin/README.md @@ -0,0 +1,20 @@ +# rtc_manager_rviz_plugin + +## Purpose + +This plugin displays each content of RTC status and switches each module of RTC auto mode. + +## Inputs / Outputs + +### Input + +tbd. + +### Output + +tbd. + +## HowToUse + +1. Start rviz and select panels/Add new panel. + ![select_panel](./images/select_panels.png) diff --git a/common/rtc_manager_rviz_plugin/icons/classes/RTCManagerPanel.png b/common/rtc_manager_rviz_plugin/icons/classes/RTCManagerPanel.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/rtc_manager_rviz_plugin/icons/classes/RTCManagerPanel.png differ diff --git a/common/rtc_manager_rviz_plugin/images/select_panels.png b/common/rtc_manager_rviz_plugin/images/select_panels.png new file mode 100644 index 0000000000000..a691602c42c3c Binary files /dev/null and b/common/rtc_manager_rviz_plugin/images/select_panels.png differ diff --git a/common/rtc_manager_rviz_plugin/package.xml b/common/rtc_manager_rviz_plugin/package.xml new file mode 100644 index 0000000000000..8af550e05ac69 --- /dev/null +++ b/common/rtc_manager_rviz_plugin/package.xml @@ -0,0 +1,32 @@ + + + + rtc_manager_rviz_plugin + 0.0.0 + The rtc manager rviz plugin package + Taiki Tanaka + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + tier4_external_api_msgs + tier4_planning_msgs + tier4_rtc_msgs + unique_identifier_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/rtc_manager_rviz_plugin/plugins/plugin_description.xml b/common/rtc_manager_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..001ae153e6762 --- /dev/null +++ b/common/rtc_manager_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + RTCManagerPanel + + + diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp new file mode 100644 index 0000000000000..363eca7b17058 --- /dev/null +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp @@ -0,0 +1,219 @@ +// +// 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 "rtc_manager_panel.hpp" + +#include +#include +#include +#include +#include + +#include + +namespace rviz_plugins +{ +inline std::string Bool2String(const bool var) { return var ? "True" : "False"; } +using std::placeholders::_1; +using std::placeholders::_2; +Module getModuleType(const std::string & module_name) +{ + Module module; + if (module_name == "blind_spot") { + module.type = Module::BLIND_SPOT; + } else if (module_name == "crosswalk") { + module.type = Module::CROSSWALK; + } else if (module_name == "detection_area") { + module.type = Module::DETECTION_AREA; + } else if (module_name == "intersection") { + module.type = Module::INTERSECTION; + } else if (module_name == "no_stopping_area") { + module.type = Module::NO_STOPPING_AREA; + } else if (module_name == "occlusion_spot") { + module.type = Module::OCCLUSION_SPOT; + } else if (module_name == "stop_line") { + module.type = Module::NONE; + } else if (module_name == "traffic_light") { + module.type = Module::TRAFFIC_LIGHT; + } else if (module_name == "virtual_traffic_light") { + module.type = Module::TRAFFIC_LIGHT; + } else if (module_name == "lane_change_left") { + module.type = Module::LANE_CHANGE_LEFT; + } else if (module_name == "lane_change_right") { + module.type = Module::LANE_CHANGE_RIGHT; + } else if (module_name == "avoidance_left") { + module.type = Module::AVOIDANCE_LEFT; + } else if (module_name == "avoidance_right") { + module.type = Module::AVOIDANCE_RIGHT; + } else if (module_name == "pull_over") { + module.type = Module::PULL_OVER; + } else if (module_name == "pull_out") { + module.type = Module::PULL_OUT; + } + return module; +} + +std::string getModuleName(const uint8_t module_type) +{ + switch (module_type) { + case Module::LANE_CHANGE_LEFT: { + return "lane_change_left"; + } + case Module::LANE_CHANGE_RIGHT: { + return "lane_change_right"; + } + case Module::AVOIDANCE_LEFT: { + return "avoidance_left"; + } + case Module::AVOIDANCE_RIGHT: { + return "avoidance_right"; + } + case Module::PULL_OVER: { + return "pull_over"; + } + case Module::PULL_OUT: { + return "pull_out"; + } + case Module::TRAFFIC_LIGHT: { + return "traffic_light"; + } + case Module::INTERSECTION: { + return "intersection"; + } + case Module::CROSSWALK: { + return "crosswalk"; + } + case Module::BLIND_SPOT: { + return "blind_spot"; + } + case Module::DETECTION_AREA: { + return "detection_area"; + } + case Module::NO_STOPPING_AREA: { + return "no_stopping_area"; + } + case Module::OCCLUSION_SPOT: { + return "occlusion_spot"; + } + } + return "NONE"; +} + +RTCManagerPanel::RTCManagerPanel(QWidget * parent) : rviz_common::Panel(parent) +{ + // TODO(tanaka): replace this magic number to Module::SIZE + auto * v_layout = new QVBoxLayout; + // statuses + auto * rtc_table_layout = new QHBoxLayout; + { + auto vertical_header = new QHeaderView(Qt::Vertical); + vertical_header->hide(); + auto horizontal_header = new QHeaderView(Qt::Horizontal); + horizontal_header->setSectionResizeMode(QHeaderView::Stretch); + rtc_table_ = new QTableWidget(); + rtc_table_->setColumnCount(column_size_); + rtc_table_->setHorizontalHeaderLabels({"ID", "Module", "AW Safe", "Received Cmd", "Distance"}); + rtc_table_->setVerticalHeader(vertical_header); + rtc_table_->setHorizontalHeader(horizontal_header); + rtc_table_layout->addWidget(rtc_table_); + v_layout->addLayout(rtc_table_layout); + } + setLayout(v_layout); +} + +void RTCManagerPanel::onInitialize() +{ + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + sub_rtc_status_ = raw_node_->create_subscription( + "/api/external/get/rtc_status", 1, std::bind(&RTCManagerPanel::onRTCStatus, this, _1)); +} + +void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg) +{ + rtc_table_->clearContents(); + if (msg->statuses.empty()) return; + // this is to stable rtc display not to occupy too much + size_t min_display_size{5}; + size_t max_display_size{10}; + rtc_table_->setRowCount( + std::max(min_display_size, std::min(msg->statuses.size(), max_display_size))); + int cnt = 0; + for (auto status : msg->statuses) { + if (static_cast(cnt) >= max_display_size) return; + // uuid + { + std::stringstream uuid; + uuid << std::setw(4) << std::setfill('0') << static_cast(status.uuid.uuid.at(0)); + auto label = new QLabel(QString::fromStdString(uuid.str())); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(uuid.str())); + rtc_table_->setCellWidget(cnt, 0, label); + } + + // module name + { + std::string module_name = getModuleName(status.module.type); + auto label = new QLabel(QString::fromStdString(module_name)); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(module_name)); + rtc_table_->setCellWidget(cnt, 1, label); + } + + // is aw safe + bool is_aw_safe = status.safe; + { + std::string is_safe = Bool2String(is_aw_safe); + auto label = new QLabel(QString::fromStdString(is_safe)); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(is_safe)); + rtc_table_->setCellWidget(cnt, 2, label); + } + + // is operator safe + const bool is_execute = status.command_status.type; + { + std::string text = is_execute ? "EXECUTE" : "WAIT"; + auto label = new QLabel(QString::fromStdString(text)); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(text)); + rtc_table_->setCellWidget(cnt, 3, label); + } + + // start distance + { + std::string start_distance = std::to_string(status.distance); + auto label = new QLabel(QString::fromStdString(start_distance)); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(start_distance)); + rtc_table_->setCellWidget(cnt, 4, label); + } + + if (is_aw_safe && is_execute) { + rtc_table_->cellWidget(cnt, 1)->setStyleSheet("background-color: #00FF00;"); + } else if (is_aw_safe || is_execute) { + rtc_table_->cellWidget(cnt, 1)->setStyleSheet("background-color: #FFFF00;"); + } else { + rtc_table_->cellWidget(cnt, 1)->setStyleSheet("background-color: #FF0000;"); + } + + cnt++; + } +} +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::RTCManagerPanel, rviz_common::Panel) diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp new file mode 100644 index 0000000000000..6a49fc82a4db5 --- /dev/null +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp @@ -0,0 +1,75 @@ +// +// 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 RTC_MANAGER_PANEL_HPP_ +#define RTC_MANAGER_PANEL_HPP_ + +#include +#include +#include +#include + +#ifndef Q_MOC_RUN +// cpp +#include +#include +#include +#include +// ros +#include +#include + +#include +// autoware +#include +#include +#include +#include +#include +#include +#endif + +namespace rviz_plugins +{ +using tier4_rtc_msgs::msg::Command; +using tier4_rtc_msgs::msg::CooperateCommand; +using tier4_rtc_msgs::msg::CooperateResponse; +using tier4_rtc_msgs::msg::CooperateStatus; +using tier4_rtc_msgs::msg::CooperateStatusArray; +using tier4_rtc_msgs::msg::Module; +using tier4_rtc_msgs::srv::CooperateCommands; +using unique_identifier_msgs::msg::UUID; + +class RTCManagerPanel : public rviz_common::Panel +{ + Q_OBJECT +public: + explicit RTCManagerPanel(QWidget * parent = nullptr); + void onInitialize() override; + +protected: + void onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg); + + rclcpp::Node::SharedPtr raw_node_; + rclcpp::Subscription::SharedPtr sub_rtc_status_; + rclcpp::Client::SharedPtr client_rtc_commands_; + QTableWidget * rtc_table_; + size_t column_size_ = {5}; +}; + +} // namespace rviz_plugins + +#endif // RTC_MANAGER_PANEL_HPP_