feat(rviz): add new plugin to visualize planning factor (#9999)

* feat(rviz): add new plugin to visualize planning factor

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

* feat(rviz): show safety factors

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

* chore: add maintainer

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

* feat: show detail

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

---------

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

visualization/tier4_planning_factor_rviz_plugin/CMakeLists.txt

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

visualization/tier4_planning_factor_rviz_plugin/README.md

@@ -0,0 +1 @@
+# tier4_planning_factor_rviz_plugin

visualization/tier4_planning_factor_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_planning_factor_rviz_plugin</name>
+  <version>0.40.0</version>
+  <description>The tier4_planning_factor_rviz_plugin package</description>
+  <maintainer email="[email protected]">Satoshi Ota</maintainer>
+  <maintainer email="[email protected]">Mamoru Sobue</maintainer>
+  <maintainer email="[email protected]">Shumpei Wakabayashi</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <build_depend>qtbase5-dev</build_depend>
+
+  <depend>autoware_motion_utils</depend>
+  <depend>autoware_universe_utils</depend>
+  <depend>autoware_vehicle_info_utils</depend>
+  <depend>rviz_common</depend>
+  <depend>rviz_default_plugins</depend>
+  <depend>tier4_planning_msgs</depend>
+
+  <exec_depend>libqt5-widgets</exec_depend>
+  <exec_depend>rviz2</exec_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/tier4_planning_factor_rviz_plugin/plugins.xml

@@ -0,0 +1,8 @@
+<library path="tier4_planning_factor_rviz_plugin">
+
+  <class name="tier4_planning_factor_rviz_plugin/PlanningFactorRvizPlugin" type="autoware::rviz_plugins::PlanningFactorRvizPlugin" base_class_type="rviz_common::Display">
+    <description>tier4_planning_factor_rviz_plugin</description>
+    <message_type>tier4_planning_msgs/msg/PlanningFactorArray</message_type>
+  </class>
+
+</library>

visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.cpp

@@ -0,0 +1,189 @@
+// 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 "planning_factor_rviz_plugin.hpp"
+
+#include <autoware/motion_utils/marker/marker_helper.hpp>
+#include <autoware/universe_utils/math/constants.hpp>
+#include <autoware/universe_utils/math/trigonometry.hpp>
+#include <autoware/universe_utils/ros/marker_helper.hpp>
+
+#include <string>
+
+namespace autoware::rviz_plugins
+{
+
+using autoware::motion_utils::createDeadLineVirtualWallMarker;
+using autoware::motion_utils::createSlowDownVirtualWallMarker;
+using autoware::motion_utils::createStopVirtualWallMarker;
+using autoware::universe_utils::createDefaultMarker;
+using autoware::universe_utils::createMarkerScale;
+
+namespace
+{
+
+std_msgs::msg::ColorRGBA convertFromColorCode(const uint64_t code, const float alpha)
+{
+  const float r = static_cast<int>(code >> 16) / 255.0;
+  const float g = static_cast<int>((code << 48) >> 56) / 255.0;
+  const float b = static_cast<int>((code << 56) >> 56) / 255.0;
+
+  return autoware::universe_utils::createMarkerColor(r, g, b, alpha);
+}
+
+std_msgs::msg::ColorRGBA getGreen(const float alpha)
+{
+  constexpr uint64_t code = 0x00e676;
+  return convertFromColorCode(code, alpha);
+}
+
+std_msgs::msg::ColorRGBA getRed(const float alpha)
+{
+  constexpr uint64_t code = 0xff3d00;
+  return convertFromColorCode(code, alpha);
+}
+
+std_msgs::msg::ColorRGBA getGray(const float alpha)
+{
+  constexpr uint64_t code = 0xbdbdbd;
+  return convertFromColorCode(code, alpha);
+}
+
+visualization_msgs::msg::Marker createArrowMarker(
+  const size_t id, const geometry_msgs::msg::Point & position,
+  const std_msgs::msg::ColorRGBA & color, const std::string & name, const double height_offset,
+  const double arrow_length = 1.0)
+{
+  const double line_width = 0.25 * arrow_length;
+  const double head_width = 0.5 * arrow_length;
+  const double head_height = 0.5 * arrow_length;
+
+  auto marker = createDefaultMarker(
+    "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name + "_arrow", id,
+    visualization_msgs::msg::Marker::ARROW, createMarkerScale(line_width, head_width, head_height),
+    color);
+
+  geometry_msgs::msg::Point src, dst;
+  src = position;
+  src.z += height_offset + arrow_length;
+  dst = position;
+  dst.z += height_offset;
+
+  marker.points.push_back(src);
+  marker.points.push_back(dst);
+
+  return marker;
+}
+
+visualization_msgs::msg::Marker createCircleMarker(
+  const size_t id, const geometry_msgs::msg::Point & position,
+  const std_msgs::msg::ColorRGBA & color, const std::string & name, const double radius,
+  const double height_offset, const double line_width = 0.1)
+{
+  auto marker = createDefaultMarker(
+    "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name, id, visualization_msgs::msg::Marker::LINE_STRIP,
+    createMarkerScale(line_width, 0.0, 0.0), color);
+
+  constexpr size_t num_points = 20;
+  for (size_t i = 0; i < num_points; ++i) {
+    geometry_msgs::msg::Point point;
+    const double ratio = static_cast<double>(i) / static_cast<double>(num_points);
+    const double theta = 2 * autoware::universe_utils::pi * ratio;
+    point.x = position.x + radius * autoware::universe_utils::cos(theta);
+    point.y = position.y + radius * autoware::universe_utils::sin(theta);
+    point.z = position.z + height_offset;
+    marker.points.push_back(point);
+  }
+  marker.points.push_back(marker.points.front());
+
+  return marker;
+}
+
+visualization_msgs::msg::Marker createNameTextMarker(
+  const size_t id, const geometry_msgs::msg::Point & position, const std::string & name,
+  const double height_offset, const double text_size = 0.5)
+{
+  auto marker = createDefaultMarker(
+    "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name + "_name_text", id,
+    visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, text_size),
+    getGray(0.999));
+
+  marker.text = name;
+
+  marker.pose.position = position;
+  marker.pose.position.z += height_offset;
+
+  return marker;
+}
+
+visualization_msgs::msg::MarkerArray createTargetMarker(
+  const size_t id, const geometry_msgs::msg::Point & position,
+  const std_msgs::msg::ColorRGBA & color, const std::string & name,
+  const double height_offset = 2.0, const double arrow_length = 1.0, const double line_width = 0.1)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  marker_array.markers.push_back(
+    createArrowMarker(id, position, color, name, height_offset, arrow_length));
+  marker_array.markers.push_back(createCircleMarker(
+    id, position, color, name + "_circle1", 0.5 * arrow_length, height_offset + 0.75 * arrow_length,
+    line_width));
+  marker_array.markers.push_back(createCircleMarker(
+    id, position, color, name + "_circle2", 0.75 * arrow_length,
+    height_offset + 0.75 * arrow_length, line_width));
+  marker_array.markers.push_back(createNameTextMarker(
+    id, position, name, height_offset + 1.5 * arrow_length, 0.5 * arrow_length));
+
+  return marker_array;
+}
+}  // namespace
+
+void PlanningFactorRvizPlugin::processMessage(
+  const tier4_planning_msgs::msg::PlanningFactorArray::ConstSharedPtr msg)
+{
+  size_t i = 0L;
+  for (const auto & factor : msg->factors) {
+    const auto text = factor.module + (factor.detail.empty() ? "" : " (" + factor.detail + ")");
+
+    switch (factor.behavior) {
+      case tier4_planning_msgs::msg::PlanningFactor::STOP:
+        for (const auto & control_point : factor.control_points) {
+          const auto virtual_wall = createStopVirtualWallMarker(
+            control_point.pose, text, msg->header.stamp, i++, baselink2front_.getFloat());
+          add_marker(std::make_shared<visualization_msgs::msg::MarkerArray>(virtual_wall));
+        }
+        break;
+
+      case tier4_planning_msgs::msg::PlanningFactor::SLOW_DOWN:
+        for (const auto & control_point : factor.control_points) {
+          const auto virtual_wall = createSlowDownVirtualWallMarker(
+            control_point.pose, text, msg->header.stamp, i++, baselink2front_.getFloat());
+          add_marker(std::make_shared<visualization_msgs::msg::MarkerArray>(virtual_wall));
+        }
+        break;
+    }
+
+    for (const auto & safety_factor : factor.safety_factors.factors) {
+      const auto color = safety_factor.is_safe ? getGreen(0.999) : getRed(0.999);
+      for (const auto & point : safety_factor.points) {
+        const auto safety_factor_marker = createTargetMarker(i++, point, color, factor.module);
+        add_marker(std::make_shared<visualization_msgs::msg::MarkerArray>(safety_factor_marker));
+      }
+    }
+  }
+}
+}  // namespace autoware::rviz_plugins
+
+// Export the plugin
+#include <pluginlib/class_list_macros.hpp>  // NOLINT
+PLUGINLIB_EXPORT_CLASS(autoware::rviz_plugins::PlanningFactorRvizPlugin, rviz_common::Display)

visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.hpp

@@ -0,0 +1,103 @@
+// 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 PLANNING_FACTOR_RVIZ_PLUGIN_HPP_
+#define PLANNING_FACTOR_RVIZ_PLUGIN_HPP_
+
+#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
+#include <rviz_common/display.hpp>
+#include <rviz_common/properties/float_property.hpp>
+#include <rviz_default_plugins/displays/marker/marker_common.hpp>
+#include <rviz_default_plugins/displays/marker_array/marker_array_display.hpp>
+
+#include <tier4_planning_msgs/msg/planning_factor_array.hpp>
+
+#include <string>
+
+namespace autoware::rviz_plugins
+{
+
+using RosTopicDisplay = rviz_common::RosTopicDisplay<tier4_planning_msgs::msg::PlanningFactorArray>;
+
+class PlanningFactorRvizPlugin
+: public rviz_common::RosTopicDisplay<tier4_planning_msgs::msg::PlanningFactorArray>
+{
+public:
+  PlanningFactorRvizPlugin()
+  : marker_common_{this},
+    baselink2front_{"Baselink To Front", 0.0, "Length between base link to front.", this},
+    topic_name_{"planning_factors"}
+  {
+  }
+
+  void onInitialize() override
+  {
+    RosTopicDisplay::RTDClass::onInitialize();
+    marker_common_.initialize(this->context_, this->scene_node_);
+    QString message_type = QString::fromStdString(
+      rosidl_generator_traits::name<tier4_planning_msgs::msg::PlanningFactorArray>());
+    this->topic_property_->setMessageType(message_type);
+    this->topic_property_->setValue(topic_name_.c_str());
+    this->topic_property_->setDescription("Topic to subscribe to.");
+
+    const auto vehicle_info =
+      autoware::vehicle_info_utils::VehicleInfoUtils(*rviz_ros_node_.lock()->get_raw_node())
+        .getVehicleInfo();
+    baselink2front_.setValue(vehicle_info.max_longitudinal_offset_m);
+  }
+
+  void load(const rviz_common::Config & config) override
+  {
+    RosTopicDisplay::Display::load(config);
+    marker_common_.load(config);
+  }
+
+  void update(float wall_dt, float ros_dt) override { marker_common_.update(wall_dt, ros_dt); }
+
+  void reset() override
+  {
+    RosTopicDisplay::reset();
+    marker_common_.clearMarkers();
+  }
+
+  void clear_markers() { marker_common_.clearMarkers(); }
+
+  void add_marker(visualization_msgs::msg::Marker::ConstSharedPtr marker_ptr)
+  {
+    marker_common_.addMessage(marker_ptr);
+  }
+
+  void add_marker(visualization_msgs::msg::MarkerArray::ConstSharedPtr markers_ptr)
+  {
+    marker_common_.addMessage(markers_ptr);
+  }
+
+  void delete_marker(rviz_default_plugins::displays::MarkerID marker_id)
+  {
+    marker_common_.deleteMarker(marker_id);
+  }
+
+private:
+  void processMessage(
+    const tier4_planning_msgs::msg::PlanningFactorArray::ConstSharedPtr msg) override;
+
+  rviz_default_plugins::displays::MarkerCommon marker_common_;
+
+  rviz_common::properties::FloatProperty baselink2front_;
+
+  std::string topic_name_;
+};
+}  // namespace autoware::rviz_plugins
+
+#endif  // PLANNING_FACTOR_RVIZ_PLUGIN_HPP_