feat(rviz): add new plugin to show string stamped (#139)

Signed-off-by: satoshi-ota <[email protected]>

common/tier4_string_viewer_rviz_plugin/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.14)
+project(tier4_string_viewer_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/string_viewer_panel.hpp
+  src/string_viewer_panel.cpp
+)
+target_link_libraries(${PROJECT_NAME}
+  ${QT_LIBRARIES}
+)
+pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    icons
+    plugins
+)

common/tier4_string_viewer_rviz_plugin/README.md

@@ -0,0 +1,15 @@
+# tier4_string_viewer_rviz_plugin
+
+## Purpose
+
+This plugin displays the ROS message whose topic type is `tier4_debug_msgs::msg::StringStamped` in rviz.
+
+## Assumptions / Known limits
+
+TBD.
+
+## Usage
+
+1. Start rviz and select panels/Add new panel.
+   ![select_panel](./images/select_panels.png)
+2. Select tier4_string_viewer_rviz_plugin/StringViewerPanel and press OK.

common/tier4_string_viewer_rviz_plugin/package.xml

@@ -0,0 +1,28 @@
+<?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>tier4_string_viewer_rviz_plugin</name>
+  <version>0.0.0</version>
+  <description>The tier4_string_viewer_rviz_plugin package</description>
+  <maintainer email="[email protected]">Satoshi Ota</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_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_debug_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/tier4_string_viewer_rviz_plugin/plugins/plugin_description.xml

@@ -0,0 +1,7 @@
+<library path="tier4_string_viewer_rviz_plugin">
+  <class
+    type="tier4_string_viewer_rviz_plugin::StringViewerPanel"
+    base_class_type="rviz_common::Panel">
+    <description>StringViewerPanel</description>
+  </class>
+</library>

common/tier4_string_viewer_rviz_plugin/src/string_viewer_panel.cpp

@@ -0,0 +1,86 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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 "string_viewer_panel.hpp"
+
+#include <QVBoxLayout>
+#include <rclcpp/rclcpp.hpp>
+
+#include <ctime>
+
+namespace tier4_string_viewer_rviz_plugin
+{
+
+StringViewerPanel::StringViewerPanel(QWidget * parent) : rviz_common::Panel(parent)
+{
+  auto * layout = new QVBoxLayout(this);
+
+  topic_list_ = new QComboBox();
+  layout->addWidget(topic_list_);
+
+  contents_ = new QLabel;
+  layout->addWidget(contents_);
+
+  setLayout(layout);
+
+  connect(
+    topic_list_, SIGNAL(currentIndexChanged(const QString &)), this,
+    SLOT(on_topic_name(const QString &)));
+}
+
+void StringViewerPanel::onInitialize()
+{
+  raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
+
+  using namespace std::literals::chrono_literals;
+  timer_ = raw_node_->create_wall_timer(1000ms, [&]() { on_timer(); });
+}
+
+void StringViewerPanel::on_topic_name(const QString & topic)
+{
+  contents_->clear();
+  sub_string_.reset();
+  sub_string_ = raw_node_->create_subscription<StringStamped>(
+    topic.toStdString(), rclcpp::QoS{1},
+    std::bind(&StringViewerPanel::on_string, this, std::placeholders::_1));
+}
+
+void StringViewerPanel::on_string(const StringStamped::ConstSharedPtr msg)
+{
+  contents_->setText(msg->data.c_str());
+  contents_->setWordWrap(true);
+}
+
+void StringViewerPanel::on_timer()
+{
+  const auto std_message_type = rosidl_generator_traits::name<StringStamped>();
+  const auto published_topics = raw_node_->get_topic_names_and_types();
+
+  if (published_topics.size() == topic_num_) return;
+
+  for (const auto & topic : published_topics) {
+    // Only add topics whose type matches.
+    for (const auto & type : topic.second) {
+      if (type == std_message_type) {
+        topic_list_->addItem(QString::fromStdString(topic.first));
+      }
+    }
+  }
+
+  topic_num_ = published_topics.size();
+}
+}  // namespace tier4_string_viewer_rviz_plugin
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(tier4_string_viewer_rviz_plugin::StringViewerPanel, rviz_common::Panel)

common/tier4_string_viewer_rviz_plugin/src/string_viewer_panel.hpp

@@ -0,0 +1,65 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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 STRING_VIEWER_PANEL_HPP_
+#define STRING_VIEWER_PANEL_HPP_
+
+#include <QComboBox>
+#include <QLabel>
+#include <rviz_common/display_context.hpp>
+#include <rviz_common/panel.hpp>
+#include <rviz_common/ros_integration/ros_node_abstraction_iface.hpp>
+#include <rviz_common/ros_topic_display.hpp>
+
+#include <tier4_debug_msgs/msg/string_stamped.hpp>
+
+namespace tier4_string_viewer_rviz_plugin
+{
+
+using tier4_debug_msgs::msg::StringStamped;
+
+class QLineEdit;
+
+class StringViewerPanel : public rviz_common::Panel
+{
+  Q_OBJECT
+
+public:
+  explicit StringViewerPanel(QWidget * parent = nullptr);
+
+  void onInitialize() override;
+
+private Q_SLOTS:
+  void on_topic_name(const QString & topic);
+
+private:
+  void on_string(const StringStamped::ConstSharedPtr msg);
+
+  void on_timer();
+
+  QLabel * contents_;
+
+  QComboBox * topic_list_;
+
+  rclcpp::Node::SharedPtr raw_node_;
+
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  rclcpp::Subscription<StringStamped>::SharedPtr sub_string_;
+
+  size_t topic_num_{0L};
+};
+}  // namespace tier4_string_viewer_rviz_plugin
+
+#endif  // STRING_VIEWER_PANEL_HPP_