Merge branch 'main' into chore/radar_tracks_msgs_converter_param

Author: scepter914

common/tensorrt_common/include/tensorrt_common/logger.hpp

@@ -19,13 +19,15 @@
 
 #include "NvInferRuntimeCommon.h"
 
+#include <atomic>
 #include <cassert>
 #include <ctime>
 #include <iomanip>
 #include <iostream>
 #include <ostream>
 #include <sstream>
 #include <string>
+#include <thread>
 
 namespace tensorrt_common
 {
@@ -200,7 +202,15 @@ class Logger : public nvinfer1::ILogger  // NOLINT
 public:
   //  Logger(Severity severity = Severity::kWARNING)
   //  Logger(Severity severity = Severity::kVERBOSE)
-  explicit Logger(Severity severity = Severity::kINFO) : mReportableSeverity(severity) {}
+  explicit Logger(Severity severity = Severity::kINFO)
+  : mReportableSeverity(severity), mVerbose(true), mThrottleStopFlag(false)
+  {
+  }
+
+  explicit Logger(const bool verbose, Severity severity = Severity::kINFO)
+  : mReportableSeverity(severity), mVerbose(verbose), mThrottleStopFlag(false)
+  {
+  }
 
   //!
   //! \enum TestResult
@@ -234,7 +244,44 @@ class Logger : public nvinfer1::ILogger  // NOLINT
   //!
   void log(Severity severity, const char * msg) noexcept override
   {
-    LogStreamConsumer(mReportableSeverity, severity) << "[TRT] " << std::string(msg) << std::endl;
+    if (mVerbose) {
+      LogStreamConsumer(mReportableSeverity, severity) << "[TRT] " << std::string(msg) << std::endl;
+    }
+  }
+
+  /**
+   * @brief Logging with throttle.
+   *
+   * @example
+   * Logger logger();
+   * auto log_thread = logger.log_throttle(nvinfer1::ILogger::Severity::kINFO, "SOME MSG", 1);
+   * // some operation
+   * logger.stop_throttle(log_thread);
+   *
+   * @param severity
+   * @param msg
+   * @param duration
+   * @return std::thread
+   *
+   */
+  std::thread log_throttle(Severity severity, const char * msg, const int duration) noexcept
+  {
+    mThrottleStopFlag.store(false);
+    auto log_func = [this](Severity s, const char * m, const int d) {
+      while (!mThrottleStopFlag.load()) {
+        this->log(s, m);
+        std::this_thread::sleep_for(std::chrono::seconds(d));
+      }
+    };
+
+    std::thread log_thread(log_func, severity, msg, duration);
+    return log_thread;
+  }
+
+  void stop_throttle(std::thread & log_thread) noexcept
+  {
+    mThrottleStopFlag.store(true);
+    log_thread.join();
   }
 
   //!
@@ -430,6 +477,8 @@ class Logger : public nvinfer1::ILogger  // NOLINT
   }
 
   Severity mReportableSeverity;
+  bool mVerbose;
+  std::atomic<bool> mThrottleStopFlag;
 };
 
 namespace
@@ -444,7 +493,7 @@ namespace
 //!
 inline LogStreamConsumer LOG_VERBOSE(const Logger & logger)
 {
-  return LogStreamConsumer(logger.getReportableSeverity(), Severity::kVERBOSE);
+  return LogStreamConsumer(logger.getReportableSeverity(), Severity::kVERBOSE) << "[TRT] ";
 }
 
 //!
@@ -456,7 +505,7 @@ inline LogStreamConsumer LOG_VERBOSE(const Logger & logger)
 //!
 inline LogStreamConsumer LOG_INFO(const Logger & logger)
 {
-  return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINFO);
+  return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINFO) << "[TRT] ";
 }
 
 //!
@@ -468,7 +517,7 @@ inline LogStreamConsumer LOG_INFO(const Logger & logger)
 //!
 inline LogStreamConsumer LOG_WARN(const Logger & logger)
 {
-  return LogStreamConsumer(logger.getReportableSeverity(), Severity::kWARNING);
+  return LogStreamConsumer(logger.getReportableSeverity(), Severity::kWARNING) << "[TRT] ";
 }
 
 //!
@@ -480,7 +529,7 @@ inline LogStreamConsumer LOG_WARN(const Logger & logger)
 //!
 inline LogStreamConsumer LOG_ERROR(const Logger & logger)
 {
-  return LogStreamConsumer(logger.getReportableSeverity(), Severity::kERROR);
+  return LogStreamConsumer(logger.getReportableSeverity(), Severity::kERROR) << "[TRT] ";
 }
 
 //!
@@ -494,7 +543,7 @@ inline LogStreamConsumer LOG_ERROR(const Logger & logger)
 //!
 inline LogStreamConsumer LOG_FATAL(const Logger & logger)
 {
-  return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINTERNAL_ERROR);
+  return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINTERNAL_ERROR) << "[TRT] ";
 }
 
 }  // anonymous namespace

common/tensorrt_common/src/tensorrt_common.cpp

@@ -186,7 +186,11 @@ void TrtCommon::setup()
     } else {
       std::cout << "Building... " << cache_engine_path << std::endl;
       logger_.log(nvinfer1::ILogger::Severity::kINFO, "Start build engine");
+      auto log_thread = logger_.log_throttle(
+        nvinfer1::ILogger::Severity::kINFO,
+        "Applying optimizations and building TRT CUDA engine. Please wait for a few minutes...", 5);
       buildEngineFromOnnx(model_file_path_, cache_engine_path);
+      logger_.stop_throttle(log_thread);
       logger_.log(nvinfer1::ILogger::Severity::kINFO, "End build engine");
     }
     engine_path = cache_engine_path;

evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.14)
+project(tier4_metrics_rviz_plugin)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(Qt5 REQUIRED Core Widgets Charts)
+set(QT_WIDGETS_LIB Qt5::Widgets)
+set(QT_CHARTS_LIB Qt5::Charts)
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+add_definitions(-DQT_NO_KEYWORDS)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/metrics_visualize_panel.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${QT_WIDGETS_LIB}
+  ${QT_CHARTS_LIB}
+)
+
+target_compile_options(${PROJECT_NAME} PUBLIC -Wno-error=deprecated-copy -Wno-error=pedantic)
+# 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
+)

evaluator/tier4_metrics_rviz_plugin/README.md

@@ -0,0 +1,16 @@
+# tier4_metrics_rviz_plugin
+
+## Purpose
+
+This plugin panel to visualize `planning_evaluator` output.
+
+## Inputs / Outputs
+
+| Name                                     | Type                                    | Description                           |
+| ---------------------------------------- | --------------------------------------- | ------------------------------------- |
+| `/diagnostic/planning_evaluator/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | Subscribe `planning_evaluator` output |
+
+## HowToUse
+
+1. Start rviz and select panels/Add new panel.
+2. Select MetricsVisualizePanel and press OK.

evaluator/tier4_metrics_rviz_plugin/icons/classes/MetricsVisualizePanel.png

@@ -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>tier4_metrics_rviz_plugin</name>
+  <version>0.0.0</version>
+  <description>The tier4_metrics_rviz_plugin package</description>
+  <maintainer email="satoshi.ota@tier4.jp">Satoshi OTA</maintainer>
+  <maintainer email="kyoichi.sugahara@tier4.jp">Kyoichi Sugahara</maintainer>
+  <maintainer email="maxime.clement@tier4.jp">Maxime CLEMENT</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>diagnostic_msgs</depend>
+  <depend>libqt5-core</depend>
+  <depend>libqt5-gui</depend>
+  <depend>libqt5-widgets</depend>
+  <depend>qtbase5-dev</depend>
+  <depend>rclcpp</depend>
+  <depend>rviz_common</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>

evaluator/tier4_metrics_rviz_plugin/package.xml

@@ -0,0 +1,5 @@
+<library path="tier4_metrics_rviz_plugin">
+  <class type="rviz_plugins::MetricsVisualizePanel" base_class_type="rviz_common::Panel">
+    <description>MetricsVisualizePanel</description>
+  </class>
+</library>

evaluator/tier4_metrics_rviz_plugin/plugins/plugin_description.xml

@@ -0,0 +1,86 @@
+//  Copyright 2024 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 "metrics_visualize_panel.hpp"
+
+#include <rviz_common/display_context.hpp>
+
+#include <X11/Xlib.h>
+
+#include <algorithm>
+#include <optional>
+#include <string>
+#include <vector>
+
+namespace rviz_plugins
+{
+MetricsVisualizePanel::MetricsVisualizePanel(QWidget * parent)
+: rviz_common::Panel(parent), grid_(new QGridLayout())
+{
+  setLayout(grid_);
+}
+
+void MetricsVisualizePanel::onInitialize()
+{
+  using std::placeholders::_1;
+
+  raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
+
+  sub_ = raw_node_->create_subscription<DiagnosticArray>(
+    "/diagnostic/planning_evaluator/metrics", rclcpp::QoS{1},
+    std::bind(&MetricsVisualizePanel::onMetrics, this, _1));
+
+  const auto period = std::chrono::milliseconds(static_cast<int64_t>(1e3 / 10));
+  timer_ = raw_node_->create_wall_timer(period, [&]() { onTimer(); });
+}
+
+void MetricsVisualizePanel::onTimer()
+{
+  std::lock_guard<std::mutex> message_lock(mutex_);
+
+  for (auto & [name, metric] : metrics_) {
+    metric.updateGraph();
+    metric.updateTable();
+  }
+}
+
+void MetricsVisualizePanel::onMetrics(const DiagnosticArray::ConstSharedPtr msg)
+{
+  std::lock_guard<std::mutex> message_lock(mutex_);
+
+  const auto time = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9;
+
+  constexpr size_t GRAPH_COL_SIZE = 5;
+  for (size_t i = 0; i < msg->status.size(); ++i) {
+    const auto & status = msg->status.at(i);
+
+    if (metrics_.count(status.name) == 0) {
+      auto metric = Metric(status);
+      metrics_.emplace(status.name, metric);
+      grid_->addWidget(metric.getTable(), i / GRAPH_COL_SIZE * 2, i % GRAPH_COL_SIZE);
+      grid_->setRowStretch(i / GRAPH_COL_SIZE * 2, false);
+      grid_->addWidget(metric.getChartView(), i / GRAPH_COL_SIZE * 2 + 1, i % GRAPH_COL_SIZE);
+      grid_->setRowStretch(i / GRAPH_COL_SIZE * 2 + 1, true);
+      grid_->setColumnStretch(i % GRAPH_COL_SIZE, true);
+    }
+
+    metrics_.at(status.name).updateData(time, status);
+  }
+}
+
+}  // namespace rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(rviz_plugins::MetricsVisualizePanel, rviz_common::Panel)