feat: publish error diagnostic graph and visualize in rviz

system/diagnostic_graph_utils/launch/logging.launch.xml

@@ -6,5 +6,6 @@
     <param name="root_path" value="$(var root_path)"/>
     <param name="max_depth" value="$(var max_depth)"/>
     <param name="show_rate" value="$(var show_rate)"/>
+    <param name="enable_terminal_log" value="$(var enable_terminal_log)"/>
   </node>
 </launch>

system/diagnostic_graph_utils/package.xml

@@ -10,9 +10,11 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_internal_debug_msgs</depend>
   <depend>diagnostic_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>tier4_debug_msgs</depend>
   <depend>tier4_system_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>

system/diagnostic_graph_utils/src/node/logging.cpp

@@ -33,8 +33,13 @@ LoggingNode::LoggingNode(const rclcpp::NodeOptions & options) : Node("logging",
   sub_graph_.register_create_callback(std::bind(&LoggingNode::on_create, this, _1));
   sub_graph_.subscribe(*this, 1);
 
+  pub_error_graph_text_ = create_publisher<autoware_internal_debug_msgs::msg::StringStamped>(
+    "~/debug/error_graph_text", rclcpp::QoS(1));
+
   const auto period = rclcpp::Rate(declare_parameter<double>("show_rate")).period();
   timer_ = rclcpp::create_timer(this, get_clock(), period, [this]() { on_timer(); });
+
+  enable_terminal_log_ = declare_parameter<bool>("enable_terminal_log");
 }
 
 void LoggingNode::on_create(DiagGraph::ConstSharedPtr graph)
@@ -52,12 +57,24 @@ void LoggingNode::on_create(DiagGraph::ConstSharedPtr graph)
 
 void LoggingNode::on_timer()
 {
-  static const auto message = "The target mode is not available for the following reasons:";
+  static const auto prefix_message = "The target mode is not available for the following reasons:";
   if (root_unit_ && root_unit_->level() != DiagUnit::DiagnosticStatus::OK) {
     dump_text_.str("");
     dump_text_.clear(std::stringstream::goodbit);
-    dump_unit(root_unit_, 0, "  ");
-    RCLCPP_WARN_STREAM(get_logger(), message << std::endl << dump_text_.str());
+    dump_unit(root_unit_, 0, "");
+
+    if (enable_terminal_log_) {
+      RCLCPP_WARN_STREAM(get_logger(), prefix_message << std::endl << dump_text_.str());
+    }
+
+    autoware_internal_debug_msgs::msg::StringStamped message;
+    message.stamp = now();
+    message.data = dump_text_.str();
+    pub_error_graph_text_->publish(message);
+  } else {
+    autoware_internal_debug_msgs::msg::StringStamped message;
+    message.stamp = now();
+    pub_error_graph_text_->publish(message);
   }
 }
 
@@ -85,7 +102,7 @@ void LoggingNode::dump_unit(DiagUnit * unit, int depth, const std::string & inde
 
   dump_text_ << indent << "- " + path << " " << text_level(unit->level()) << std::endl;
   for (const auto & child : unit->children()) {
-    dump_unit(child.unit, depth + 1, indent + "  ");
+    dump_unit(child.unit, depth + 1, indent + "    ");
   }
 }
 

system/diagnostic_graph_utils/src/node/logging.hpp

@@ -19,6 +19,8 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include "autoware_internal_debug_msgs/msg/string_stamped.hpp"
+
 #include <sstream>
 #include <string>
 
@@ -35,12 +37,15 @@ class LoggingNode : public rclcpp::Node
   void on_timer();
   void dump_unit(DiagUnit * unit, int depth, const std::string & indent);
   DiagGraphSubscription sub_graph_;
+  rclcpp::Publisher<autoware_internal_debug_msgs::msg::StringStamped>::SharedPtr
+    pub_error_graph_text_;
   rclcpp::TimerBase::SharedPtr timer_;
 
   DiagUnit * root_unit_;
   int max_depth_;
   std::string root_path_;
   std::ostringstream dump_text_;
+  bool enable_terminal_log_;
 };
 
 }  // namespace diagnostic_graph_utils

visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/CMakeLists.txt

@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.8)
+project(autoware_string_stamped_rviz_plugin)
+
+# find dependencies
+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
+  DIRECTORY src
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${QT_LIBRARIES}
+)
+
+# Export the plugin to be imported by rviz2
+pluginlib_export_plugin_description_file(rviz_common plugins/plugins_description.xml)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  plugins
+)

visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/README.md

@@ -0,0 +1,7 @@
+# autoware_string_stamped_rviz_plugin
+
+Plugin for displaying 2D overlays over the RViz2 3D scene.
+
+## Purpose
+
+This plugin displays the ROS message whose topic type is `autoware_internal_debug_msgs::msg::StringStamped` in rviz.

visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/package.xml

@@ -0,0 +1,30 @@
+<?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>autoware_string_stamped_rviz_plugin</name>
+  <version>0.39.0</version>
+  <description>
+    RViz2 plugin for 2D overlays in the 3D view. Mainly a port of the JSK overlay plugin
+        (https://github.com/jsk-ros-pkg/jsk_visualization).
+  </description>
+  <maintainer email="[email protected]">Takayuki Murooka</maintainer>
+  <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>ament_index_cpp</depend>
+  <depend>autoware_internal_debug_msgs</depend>
+  <depend>rviz_common</depend>
+  <depend>rviz_ogre_vendor</depend>
+  <depend>rviz_rendering</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/plugins/plugins_description.xml

@@ -0,0 +1,5 @@
+<library path="autoware_string_stamped_rviz_plugin">
+    <class name="autoware_string_stamped_rviz_plugin/StringStampedOverlayDisplay" type="autoware::string_stamped_rviz_plugin::StringStampedOverlayDisplay" base_class_type="rviz_common::Display">
+        <description>String stamped overlay plugin for the 3D view.</description>
+    </class>
+</library>

visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.cpp

@@ -0,0 +1,225 @@
+// 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.
+
+// Copyright (c) 2014, JSK Lab
+// All rights reserved.
+//
+// Software License Agreement (BSD License)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above
+//    copyright notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of {copyright_holder} nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE
+//  POSSIBILITY OF SUCH DAMAGE.
+
+#include "jsk_overlay_utils.hpp"
+
+#include <string>
+
+namespace jsk_rviz_plugins
+{
+ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer)
+: pixel_buffer_(pixel_buffer)
+{
+  pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL);
+}
+
+ScopedPixelBuffer::~ScopedPixelBuffer()
+{
+  pixel_buffer_->unlock();
+}
+
+Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer()
+{
+  return pixel_buffer_;
+}
+
+QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height)
+{
+  const Ogre::PixelBox & pixelBox = pixel_buffer_->getCurrentLock();
+  Ogre::uint8 * pDest = static_cast<Ogre::uint8 *>(pixelBox.data);
+  memset(pDest, 0, width * height);
+  return QImage(pDest, width, height, QImage::Format_ARGB32);
+}
+
+QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height, QColor & bg_color)
+{
+  QImage Hud = getQImage(width, height);
+  for (unsigned int i = 0; i < width; i++) {
+    for (unsigned int j = 0; j < height; j++) {
+      Hud.setPixel(i, j, bg_color.rgba());
+    }
+  }
+  return Hud;
+}
+
+QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay)
+{
+  return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight());
+}
+
+QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay, QColor & bg_color)
+{
+  return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight(), bg_color);
+}
+
+OverlayObject::OverlayObject(
+  Ogre::SceneManager * manager, rclcpp::Logger logger, const std::string & name)
+: name_(name), logger_(logger)
+{
+  rviz_rendering::RenderSystem::get()->prepareOverlays(manager);
+  std::string material_name = name_ + "Material";
+  Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr();
+  overlay_ = mOverlayMgr->create(name_);
+  panel_ = static_cast<Ogre::PanelOverlayElement *>(
+    mOverlayMgr->createOverlayElement("Panel", name_ + "Panel"));
+  panel_->setMetricsMode(Ogre::GMM_PIXELS);
+
+  panel_material_ = Ogre::MaterialManager::getSingleton().create(
+    material_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+  panel_->setMaterialName(panel_material_->getName());
+  overlay_->add2D(panel_);
+}
+
+OverlayObject::~OverlayObject()
+{
+  hide();
+  panel_material_->unload();
+  Ogre::MaterialManager::getSingleton().remove(panel_material_->getName());
+  // Ogre::OverlayManager* mOverlayMgr = Ogre::OverlayManager::getSingletonPtr();
+  // mOverlayMgr->destroyOverlayElement(panel_);
+  // delete panel_;
+  // delete overlay_;
+}
+
+std::string OverlayObject::getName()
+{
+  return name_;
+}
+
+void OverlayObject::hide()
+{
+  if (overlay_->isVisible()) {
+    overlay_->hide();
+  }
+}
+
+void OverlayObject::show()
+{
+  if (!overlay_->isVisible()) {
+    overlay_->show();
+  }
+}
+
+bool OverlayObject::isTextureReady()
+{
+  return static_cast<bool>(texture_);
+}
+
+void OverlayObject::updateTextureSize(unsigned int width, unsigned int height)
+{
+  const std::string texture_name = name_ + "Texture";
+  if (width == 0) {
+    RCLCPP_WARN(logger_, "width=0 is specified as texture size");
+    width = 1;
+  }
+  if (height == 0) {
+    RCLCPP_WARN(logger_, "height=0 is specified as texture size");
+    height = 1;
+  }
+  if (!isTextureReady() || ((width != texture_->getWidth()) || (height != texture_->getHeight()))) {
+    if (isTextureReady()) {
+      Ogre::TextureManager::getSingleton().remove(texture_name);
+      panel_material_->getTechnique(0)->getPass(0)->removeAllTextureUnitStates();
+    }
+    texture_ = Ogre::TextureManager::getSingleton().createManual(
+      texture_name,  // name
+      Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
+      Ogre::TEX_TYPE_2D,  // type
+      width, height,      // width & height of the render window
+      0,                  // number of mipmaps
+      Ogre::PF_A8R8G8B8,  // pixel format chosen to match a format Qt can use
+      Ogre::TU_DEFAULT    // usage
+    );
+    panel_material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name);
+
+    panel_material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
+  }
+}
+
+ScopedPixelBuffer OverlayObject::getBuffer()
+{
+  if (isTextureReady()) {
+    return ScopedPixelBuffer(texture_->getBuffer());
+  } else {
+    return ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr());
+  }
+}
+
+void OverlayObject::setPosition(double left, double top)
+{
+  panel_->setPosition(left, top);
+}
+
+void OverlayObject::setDimensions(double width, double height)
+{
+  panel_->setDimensions(width, height);
+}
+
+bool OverlayObject::isVisible()
+{
+  return overlay_->isVisible();
+}
+
+unsigned int OverlayObject::getTextureWidth()
+{
+  if (isTextureReady()) {
+    return texture_->getWidth();
+  } else {
+    return 0;
+  }
+}
+
+unsigned int OverlayObject::getTextureHeight()
+{
+  if (isTextureReady()) {
+    return texture_->getHeight();
+  } else {
+    return 0;
+  }
+}
+
+}  // namespace jsk_rviz_plugins