feat(rtc_manager_rviz_plugin): add rtc manager viewer only

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

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)

common/rtc_manager_rviz_plugin/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rtc_manager_rviz_plugin</name>
+  <version>0.0.0</version>
+  <description>The rtc manager rviz plugin package</description>
+  <maintainer email="[email protected]">Taiki Tanaka</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>libqt5-core</depend>
+  <depend>libqt5-gui</depend>
+  <depend>libqt5-widgets</depend>
+  <depend>qtbase5-dev</depend>
+  <depend>rclcpp</depend>
+  <depend>rviz_common</depend>
+  <depend>tier4_external_api_msgs</depend>
+  <depend>tier4_planning_msgs</depend>
+  <depend>tier4_rtc_msgs</depend>
+  <depend>unique_identifier_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <rviz plugin="${prefix}/plugins/plugin_description.xml"/>
+  </export>
+</package>

common/rtc_manager_rviz_plugin/plugins/plugin_description.xml

@@ -0,0 +1,9 @@
+<library path="rtc_manager_rviz_plugin">
+
+  <class
+    type="rviz_plugins::RTCManagerPanel"
+    base_class_type="rviz_common::Panel">
+    <description>RTCManagerPanel</description>
+  </class>
+
+</library>

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 <QHBoxLayout>
+#include <QHeaderView>
+#include <QString>
+#include <QVBoxLayout>
+#include <rviz_common/display_context.hpp>
+
+#include <unique_identifier_msgs/msg/uuid.hpp>
+
+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<CooperateStatusArray>(
+    "/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<size_t>(cnt) >= max_display_size) return;
+    // uuid
+    {
+      std::stringstream uuid;
+      uuid << std::setw(4) << std::setfill('0') << static_cast<int>(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/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(rviz_plugins::RTCManagerPanel, rviz_common::Panel)

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 <QLabel>
+#include <QPushButton>
+#include <QSpinBox>
+#include <QTableWidget>
+
+#ifndef Q_MOC_RUN
+// cpp
+#include <algorithm>
+#include <memory>
+#include <string>
+#include <vector>
+// ros
+#include <rclcpp/rclcpp.hpp>
+#include <rviz_common/panel.hpp>
+
+#include <unique_identifier_msgs/msg/uuid.hpp>
+// autoware
+#include <tier4_rtc_msgs/msg/command.hpp>
+#include <tier4_rtc_msgs/msg/cooperate_command.hpp>
+#include <tier4_rtc_msgs/msg/cooperate_status.hpp>
+#include <tier4_rtc_msgs/msg/cooperate_status_array.hpp>
+#include <tier4_rtc_msgs/msg/module.hpp>
+#include <tier4_rtc_msgs/srv/cooperate_commands.hpp>
+#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<CooperateStatusArray>::SharedPtr sub_rtc_status_;
+  rclcpp::Client<CooperateCommands>::SharedPtr client_rtc_commands_;
+  QTableWidget * rtc_table_;
+  size_t column_size_ = {5};
+};
+
+}  // namespace rviz_plugins
+
+#endif  // RTC_MANAGER_PANEL_HPP_