Merge pull request #785 from tier4/sync-upstream

chore: sync upstream

common/motion_utils/include/motion_utils/marker/marker_helper.hpp

@@ -16,7 +16,8 @@
 #define MOTION_UTILS__MARKER__MARKER_HELPER_HPP_
 
 #include "motion_utils/resample/resample_utils.hpp"
-#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
+
+#include <visualization_msgs/msg/marker_array.hpp>
 
 #include <functional>
 #include <string>

common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp

@@ -18,8 +18,6 @@
 #include "motion_utils/marker/marker_helper.hpp"
 
 #include <geometry_msgs/msg/pose.hpp>
-#include <visualization_msgs/msg/marker.hpp>
-#include <visualization_msgs/msg/marker_array.hpp>
 
 #include <string>
 #include <unordered_map>

common/motion_utils/src/marker/marker_helper.cpp

@@ -14,6 +14,10 @@
 
 #include "motion_utils/marker/marker_helper.hpp"
 
+#include "tier4_autoware_utils/ros/marker_helper.hpp"
+
+#include <visualization_msgs/msg/marker_array.hpp>
+
 #include <string>
 
 using tier4_autoware_utils::appendMarkerArray;

common/motion_utils/src/marker/virtual_wall_marker_creator.cpp

@@ -14,6 +14,8 @@
 
 #include "motion_utils/marker/virtual_wall_marker_creator.hpp"
 
+#include <visualization_msgs/msg/marker_array.hpp>
+
 namespace motion_utils
 {
 

common/path_distance_calculator/src/path_distance_calculator.cpp

@@ -15,7 +15,6 @@
 #include "path_distance_calculator.hpp"
 
 #include <motion_utils/trajectory/trajectory.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <algorithm>
 #include <chrono>

common/tier4_target_object_type_rviz_plugin/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 3.14)
+project(tier4_target_object_type_rviz_plugin)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+find_package(OpenCV REQUIRED)
+find_package(Qt5 REQUIRED Core Widgets)
+set(QT_LIBRARIES Qt5::Widgets)
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/target_object_type_panel.hpp
+  src/target_object_type_panel.cpp
+)
+target_link_libraries(${PROJECT_NAME}
+  ${QT_LIBRARIES}
+  ${OpenCV_LIBRARIES}
+)
+pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    plugins
+)

common/tier4_target_object_type_rviz_plugin/README.md

@@ -0,0 +1,9 @@
+# tier4_target_object_type_rviz_plugin
+
+This plugin allows you to check which types of the dynamic object is being used by each planner.
+
+![window](./image/window.png)
+
+## Limitations
+
+Currently, which parameters of which module to check are hardcoded. In the future, this will be parameterized using YAML.

common/tier4_target_object_type_rviz_plugin/package.xml

@@ -0,0 +1,27 @@
+<?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_target_object_type_rviz_plugin</name>
+  <version>0.0.1</version>
+  <description>The tier4_target_object_type_rviz_plugin package</description>
+  <maintainer email="[email protected]">Takamasa Horibe</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>rviz_rendering</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_target_object_type_rviz_plugin/plugins/plugin_description.xml

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

common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp

@@ -0,0 +1,248 @@
+//  Copyright 2023 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 "target_object_type_panel.hpp"
+
+#include <QColor>
+#include <QVBoxLayout>
+#include <pluginlib/class_list_macros.hpp>
+#include <rclcpp/parameter_client.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+TargetObjectTypePanel::TargetObjectTypePanel(QWidget * parent) : rviz_common::Panel(parent)
+{
+  node_ = std::make_shared<rclcpp::Node>("matrix_display_node");
+
+  setParameters();
+
+  matrix_widget_ = new QTableWidget(modules_.size(), targets_.size(), this);
+  for (size_t i = 0; i < modules_.size(); i++) {
+    matrix_widget_->setVerticalHeaderItem(
+      i, new QTableWidgetItem(QString::fromStdString(modules_[i])));
+  }
+  for (size_t j = 0; j < targets_.size(); j++) {
+    matrix_widget_->setHorizontalHeaderItem(
+      j, new QTableWidgetItem(QString::fromStdString(targets_[j])));
+  }
+
+  updateMatrix();
+
+  reload_button_ = new QPushButton("Reload", this);
+  connect(
+    reload_button_, &QPushButton::clicked, this, &TargetObjectTypePanel::onReloadButtonClicked);
+
+  QVBoxLayout * layout = new QVBoxLayout;
+  layout->addWidget(matrix_widget_);
+  layout->addWidget(reload_button_);
+  setLayout(layout);
+}
+
+void TargetObjectTypePanel::onReloadButtonClicked()
+{
+  RCLCPP_INFO(node_->get_logger(), "Reload button clicked. Update parameter data.");
+  updateMatrix();
+}
+
+void TargetObjectTypePanel::setParameters()
+{
+  // Parameter will be investigated for these modules:
+  modules_ = {
+    "avoidance",
+    "avoidance_by_lane_change",
+    "lane_change",
+    "obstacle_cruise (inside)",
+    "obstacle_cruise (outside)",
+    "obstacle_stop",
+    "obstacle_slowdown"};
+
+  // Parameter will be investigated for targets in each module
+  targets_ = {"car", "truck", "bus", "trailer", "unknown", "bicycle", "motorcycle", "pedestrian"};
+
+  // TODO(Horibe): If the param naming strategy is aligned, this should be done automatically based
+  // on the modules_ and targets_.
+
+  // default
+  ParamNameEnableObject default_param_name;
+  default_param_name.name.emplace("car", "car");
+  default_param_name.name.emplace("truck", "truck");
+  default_param_name.name.emplace("bus", "bus");
+  default_param_name.name.emplace("trailer", "trailer");
+  default_param_name.name.emplace("unknown", "unknown");
+  default_param_name.name.emplace("bicycle", "bicycle");
+  default_param_name.name.emplace("motorcycle", "motorcycle");
+  default_param_name.name.emplace("pedestrian", "pedestrian");
+
+  // avoidance
+  {
+    const auto module = "avoidance";
+    ParamNameEnableObject param_name;
+    param_name.node =
+      "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner";
+    param_name.ns = "avoidance.target_object";
+    param_name.name.emplace("car", "car.is_target");
+    param_name.name.emplace("truck", "truck.is_target");
+    param_name.name.emplace("bus", "bus.is_target");
+    param_name.name.emplace("trailer", "trailer.is_target");
+    param_name.name.emplace("unknown", "unknown.is_target");
+    param_name.name.emplace("bicycle", "bicycle.is_target");
+    param_name.name.emplace("motorcycle", "motorcycle.is_target");
+    param_name.name.emplace("pedestrian", "pedestrian.is_target");
+    param_names_.emplace(module, param_name);
+  }
+
+  // avoidance_by_lane_change
+  {
+    const auto module = "avoidance_by_lane_change";
+    ParamNameEnableObject param_name;
+    param_name.node =
+      "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner";
+    param_name.ns = "avoidance_by_lane_change.target_object";
+    param_name.name.emplace("car", "car.is_target");
+    param_name.name.emplace("truck", "truck.is_target");
+    param_name.name.emplace("bus", "bus.is_target");
+    param_name.name.emplace("trailer", "trailer.is_target");
+    param_name.name.emplace("unknown", "unknown.is_target");
+    param_name.name.emplace("bicycle", "bicycle.is_target");
+    param_name.name.emplace("motorcycle", "motorcycle.is_target");
+    param_name.name.emplace("pedestrian", "pedestrian.is_target");
+    param_names_.emplace(module, param_name);
+  }
+
+  // lane_change
+  {
+    const auto module = "lane_change";
+    ParamNameEnableObject param_name;
+    param_name.node =
+      "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner";
+    param_name.ns = "lane_change.target_object";
+    param_name.name = default_param_name.name;
+    param_names_.emplace(module, param_name);
+  }
+
+  // obstacle cruise (inside)
+  {
+    const auto module = "obstacle_cruise (inside)";
+    ParamNameEnableObject param_name;
+    param_name.node =
+      "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner";
+    param_name.ns = "common.cruise_obstacle_type.inside";
+    param_name.name = default_param_name.name;
+    param_names_.emplace(module, param_name);
+  }
+
+  // obstacle cruise (outside)
+  {
+    const auto module = "obstacle_cruise (outside)";
+    ParamNameEnableObject param_name;
+    param_name.node =
+      "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner";
+    param_name.ns = "common.cruise_obstacle_type.outside";
+    param_name.name = default_param_name.name;
+    param_names_.emplace(module, param_name);
+  }
+
+  // obstacle stop
+  {
+    const auto module = "obstacle_stop";
+    ParamNameEnableObject param_name;
+    param_name.node =
+      "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner";
+    param_name.ns = "common.stop_obstacle_type";
+    param_name.name = default_param_name.name;
+    param_names_.emplace(module, param_name);
+  }
+
+  // obstacle slowdown
+  {
+    const auto module = "obstacle_slowdown";
+    ParamNameEnableObject param_name;
+    param_name.node =
+      "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner";
+    param_name.ns = "common.slow_down_obstacle_type";
+    param_name.name = default_param_name.name;
+    param_names_.emplace(module, param_name);
+  }
+}
+
+void TargetObjectTypePanel::updateMatrix()
+{
+  // blue base
+  // const QColor color_in_use("#6eb6cc");
+  // const QColor color_no_use("#1d3e48");
+  // const QColor color_undefined("#9e9e9e");
+
+  // green base
+  const QColor color_in_use("#afff70");
+  const QColor color_no_use("#44642b");
+  const QColor color_undefined("#9e9e9e");
+
+  const auto set_undefined = [&](const auto i, const auto j) {
+    QTableWidgetItem * item = new QTableWidgetItem("N/A");
+    item->setForeground(QBrush(Qt::black));  // set the text color to black
+    item->setBackground(color_undefined);
+    matrix_widget_->setItem(i, j, item);
+  };
+
+  for (size_t i = 0; i < modules_.size(); i++) {
+    const auto & module = modules_[i];
+
+    // Check if module exists in param_names_
+    if (param_names_.find(module) == param_names_.end()) {
+      RCLCPP_WARN_STREAM(node_->get_logger(), module << " is not in the param names");
+      continue;
+    }
+
+    const auto & module_params = param_names_.at(module);
+    auto parameters_client =
+      std::make_shared<rclcpp::SyncParametersClient>(node_, module_params.node);
+    if (!parameters_client->wait_for_service(std::chrono::microseconds(500))) {
+      RCLCPP_WARN_STREAM(
+        node_->get_logger(), "Failed to find parameter service for node: " << module_params.node);
+      for (size_t j = 0; j < targets_.size(); j++) {
+        set_undefined(i, j);
+      }
+      continue;
+    }
+
+    for (size_t j = 0; j < targets_.size(); j++) {
+      const auto & target = targets_[j];
+
+      // Check if target exists in module's name map
+      if (module_params.name.find(target) == module_params.name.end()) {
+        RCLCPP_WARN_STREAM(
+          node_->get_logger(), target << " parameter is not set in the " << module);
+        continue;
+      }
+
+      std::string param_name = module_params.ns + "." + module_params.name.at(target);
+      auto parameter_result = parameters_client->get_parameters({param_name});
+
+      if (!parameter_result.empty()) {
+        bool value = parameter_result[0].as_bool();
+        QTableWidgetItem * item = new QTableWidgetItem(value ? "O" : "X");
+        item->setForeground(QBrush(value ? Qt::black : Qt::black));  // set the text color to black
+        item->setBackground(QBrush(value ? color_in_use : color_no_use));
+        matrix_widget_->setItem(i, j, item);
+      } else {
+        RCLCPP_WARN_STREAM(
+          node_->get_logger(),
+          "Failed to get parameter " << module_params.node << " " << param_name);
+
+        set_undefined(i, j);
+      }
+    }
+  }
+}
+
+PLUGINLIB_EXPORT_CLASS(TargetObjectTypePanel, rviz_common::Panel)