Merge pull request #945 from tier4/x2-cp-logging-level-reconfigure

feat(logger-level-reconfigure): cherry-pick from awf/main

common/tier4_autoware_utils/CMakeLists.txt

@@ -12,6 +12,7 @@ ament_auto_add_library(tier4_autoware_utils SHARED
   src/math/sin_table.cpp
   src/math/trigonometry.cpp
   src/ros/msg_operation.cpp
+  src/ros/logger_level_configure.cpp
 )
 
 if(BUILD_TESTING)

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.hpp

@@ -0,0 +1,68 @@
+// Copyright 2023 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.
+
+// =============== Note ===============
+// This is a util class implementation of the logger_config_component provided by ROS 2
+// https://github.com/ros2/demos/blob/humble/logging_demo/src/logger_config_component.cpp
+//
+// When ROS 2 officially supports the set_logger_level option in release version, this class can be
+// removed.
+// https://github.com/ros2/ros2/issues/1355
+
+// =============== How to use ===============
+// ___In your_node.hpp___
+// #include "tier4_autoware_utils/ros/logger_level_configure.hpp"
+// class YourNode : public rclcpp::Node {
+//   ...
+//
+//   // Define logger_configure as a node class member variable
+//   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
+// }
+//
+// ___In your_node.cpp___
+// YourNode::YourNode() {
+//   ...
+//
+//   // Set up logger_configure
+//   logger_configure_ = std::make_unique<LoggerLevelConfigure>(this);
+// }
+
+#ifndef TIER4_AUTOWARE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_
+#define TIER4_AUTOWARE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_
+
+#include "logging_demo/srv/config_logger.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+namespace tier4_autoware_utils
+{
+class LoggerLevelConfigure
+{
+private:
+  using ConfigLogger = logging_demo::srv::ConfigLogger;
+
+public:
+  explicit LoggerLevelConfigure(rclcpp::Node * node);
+
+private:
+  rclcpp::Logger ros_logger_;
+  rclcpp::Service<ConfigLogger>::SharedPtr srv_config_logger_;
+
+  void onLoggerConfigService(
+    const ConfigLogger::Request::SharedPtr request,
+    const ConfigLogger::Response::SharedPtr response);
+};
+
+}  // namespace tier4_autoware_utils
+#endif  // TIER4_AUTOWARE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_

common/tier4_autoware_utils/package.xml

@@ -19,6 +19,7 @@
   <depend>diagnostic_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>libboost-dev</depend>
+  <depend>logging_demo</depend>
   <depend>pcl_conversions</depend>
   <depend>pcl_ros</depend>
   <depend>rclcpp</depend>

common/tier4_autoware_utils/src/ros/logger_level_configure.cpp

@@ -0,0 +1,61 @@
+// Copyright 2023 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 "tier4_autoware_utils/ros/logger_level_configure.hpp"
+
+#include <rcutils/logging.h>
+
+namespace tier4_autoware_utils
+{
+LoggerLevelConfigure::LoggerLevelConfigure(rclcpp::Node * node) : ros_logger_(node->get_logger())
+{
+  using std::placeholders::_1;
+  using std::placeholders::_2;
+
+  srv_config_logger_ = node->create_service<ConfigLogger>(
+    "~/config_logger", std::bind(&LoggerLevelConfigure::onLoggerConfigService, this, _1, _2));
+}
+
+void LoggerLevelConfigure::onLoggerConfigService(
+  const ConfigLogger::Request::SharedPtr request, const ConfigLogger::Response::SharedPtr response)
+{
+  int logging_severity;
+  const auto ret_level = rcutils_logging_severity_level_from_string(
+    request->level.c_str(), rcl_get_default_allocator(), &logging_severity);
+
+  if (ret_level != RCUTILS_RET_OK) {
+    response->success = false;
+    RCLCPP_WARN_STREAM(
+      ros_logger_, "Failed to change logger level for "
+                     << request->logger_name
+                     << " due to an invalid logging severity: " << request->level);
+    return;
+  }
+
+  const auto ret_set =
+    rcutils_logging_set_logger_level(request->logger_name.c_str(), logging_severity);
+
+  if (ret_set != RCUTILS_RET_OK) {
+    response->success = false;
+    RCLCPP_WARN_STREAM(ros_logger_, "Failed to set logger level for " << request->logger_name);
+    return;
+  }
+
+  response->success = true;
+  RCLCPP_INFO_STREAM(
+    ros_logger_, "Logger level [" << request->level << "] is set for " << request->logger_name);
+  return;
+}
+
+}  // namespace tier4_autoware_utils

common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt

@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.14)
+project(tier4_logging_level_configure_rviz_plugin)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(Qt5 REQUIRED Core Widgets)
+set(QT_LIBRARIES Qt5::Widgets)
+set(CMAKE_AUTOMOC ON)
+add_definitions(-DQT_NO_KEYWORDS)
+
+ament_auto_add_library(tier4_logging_level_configure_rviz_plugin SHARED
+  include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp
+  src/logging_level_configure.cpp
+)
+
+target_link_libraries(tier4_logging_level_configure_rviz_plugin
+  ${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
+  plugins
+  config
+)

common/tier4_logging_level_configure_rviz_plugin/README.md

@@ -0,0 +1,5 @@
+# tier4_logging_level_configure_rviz_plugin
+
+This package provides an rviz_plugin that can easily change the logger level of each node
+
+![tier4_logging_level_configure_rviz_plugin](tier4_logging_level_configure_rviz_plugin.png)

common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml

@@ -0,0 +1,59 @@
+# logger_config.yaml
+
+# ============================================================
+# planning
+# ============================================================
+
+behavior_path_planner:
+  - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
+    logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner
+  - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
+    logger_name: tier4_autoware_utils
+
+behavior_path_planner_avoidance:
+  - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
+    logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance
+
+behavior_velocity_planner:
+  - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner
+    logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner
+  - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner
+    logger_name: tier4_autoware_utils
+
+behavior_velocity_planner_intersection:
+  - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner
+    logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.intersection
+
+motion_obstacle_avoidance:
+  - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner
+    logger_name: planning.scenario_planning.lane_driving.motion_planning.obstacle_avoidance_planner
+  - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner
+    logger_name: tier4_autoware_utils
+
+motion_velocity_smoother:
+  - node_name: /planning/scenario_planning/motion_velocity_smoother
+    logger_name: planning.scenario_planning.motion_velocity_smoother
+  - node_name: /planning/scenario_planning/motion_velocity_smoother
+    logger_name: tier4_autoware_utils
+
+# ============================================================
+# control
+# ============================================================
+
+lateral_controller:
+  - node_name: /control/trajectory_follower/controller_node_exe
+    logger_name: control.trajectory_follower.controller_node_exe.lateral_controller
+  - node_name: /control/trajectory_follower/controller_node_exe
+    logger_name: tier4_autoware_utils
+
+longitudinal_controller:
+  - node_name: /control/trajectory_follower/controller_node_exe
+    logger_name: control.trajectory_follower.controller_node_exe.longitudinal_controller
+  - node_name: /control/trajectory_follower/controller_node_exe
+    logger_name: tier4_autoware_utils
+
+vehicle_cmd_gate:
+  - node_name: /control/vehicle_cmd_gate
+    logger_name: control.vehicle_cmd_gate
+  - node_name: /control/vehicle_cmd_gate
+    logger_name: tier4_autoware_utils

common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp

@@ -0,0 +1,74 @@
+// Copyright 2023 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 TIER4_LOGGING_LEVEL_CONFIGURE_RVIZ_PLUGIN__LOGGING_LEVEL_CONFIGURE_HPP_
+#define TIER4_LOGGING_LEVEL_CONFIGURE_RVIZ_PLUGIN__LOGGING_LEVEL_CONFIGURE_HPP_
+
+#include "logging_demo/srv/config_logger.hpp"
+
+#include <QButtonGroup>
+#include <QHBoxLayout>
+#include <QLabel>
+#include <QMap>
+#include <QPushButton>
+#include <QVBoxLayout>
+#include <rclcpp/rclcpp.hpp>
+#include <rviz_common/panel.hpp>
+
+#include <map>
+#include <string>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+namespace rviz_plugin
+{
+
+class LoggingLevelConfigureRvizPlugin : public rviz_common::Panel
+{
+  Q_OBJECT  // This macro is needed for Qt to handle slots and signals
+
+    public : LoggingLevelConfigureRvizPlugin(QWidget * parent = nullptr);
+  void onInitialize() override;
+  void save(rviz_common::Config config) const override;
+  void load(const rviz_common::Config & config) override;
+
+private:
+  QMap<QString, QButtonGroup *> buttonGroups_;
+  rclcpp::Node::SharedPtr raw_node_;
+
+  // node_logger_map_[button_name] = {node_name, logger_name}
+  std::map<QString, std::vector<std::pair<QString, QString>>> node_logger_map_;
+
+  // client_map_[node_name] = service_client
+  std::unordered_map<QString, rclcpp::Client<logging_demo::srv::ConfigLogger>::SharedPtr>
+    client_map_;
+
+  // button_map_[button_name][logging_level] = Q_button_pointer
+  std::unordered_map<QString, std::unordered_map<QString, QPushButton *>> button_map_;
+
+  QStringList getNodeList();
+  int getMaxModuleNameWidth(QLabel * containerLabel);
+  void setLoggerNodeMap();
+
+private Q_SLOTS:
+  void onButtonClick(QPushButton * button, const QString & name, const QString & level);
+  void updateButtonColors(
+    const QString & target_module_name, QPushButton * active_button, const QString & level);
+  void changeLogLevel(const QString & container, const QString & level);
+};
+
+}  // namespace rviz_plugin
+
+#endif  // TIER4_LOGGING_LEVEL_CONFIGURE_RVIZ_PLUGIN__LOGGING_LEVEL_CONFIGURE_HPP_

common/tier4_logging_level_configure_rviz_plugin/package.xml

@@ -0,0 +1,33 @@
+<?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_logging_level_configure_rviz_plugin</name>
+  <version>0.1.0</version>
+  <description>The tier4_logging_level_configure_rviz_plugin package</description>
+  <maintainer email="[email protected]">Takamasa Horibe</maintainer>
+  <maintainer email="[email protected]">Satoshi Ota</maintainer>
+  <maintainer email="[email protected]">Kosuke Takeuchi</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>libqt5-core</depend>
+  <depend>libqt5-gui</depend>
+  <depend>libqt5-widgets</depend>
+  <depend>logging_demo</depend>
+  <depend>qtbase5-dev</depend>
+  <depend>rclcpp</depend>
+  <depend>rviz_common</depend>
+  <depend>rviz_default_plugins</depend>
+  <depend>rviz_rendering</depend>
+  <depend>yaml-cpp</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_logging_level_configure_rviz_plugin/plugins/plugin_description.xml

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