diff --git a/visualization/tier4_planning_factor_rviz_plugin/CMakeLists.txt b/visualization/tier4_planning_factor_rviz_plugin/CMakeLists.txt
new file mode 100644
index 0000000000000..db1fa63c128fb
--- /dev/null
+++ b/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
+)
diff --git a/visualization/tier4_planning_factor_rviz_plugin/README.md b/visualization/tier4_planning_factor_rviz_plugin/README.md
new file mode 100644
index 0000000000000..7b65c89d5c22d
--- /dev/null
+++ b/visualization/tier4_planning_factor_rviz_plugin/README.md
@@ -0,0 +1 @@
+# tier4_planning_factor_rviz_plugin
diff --git a/visualization/tier4_planning_factor_rviz_plugin/icons/classes/PlanningFactorRvizPlugin.png b/visualization/tier4_planning_factor_rviz_plugin/icons/classes/PlanningFactorRvizPlugin.png
new file mode 100644
index 0000000000000..6a67573717ae1
Binary files /dev/null and b/visualization/tier4_planning_factor_rviz_plugin/icons/classes/PlanningFactorRvizPlugin.png differ
diff --git a/visualization/tier4_planning_factor_rviz_plugin/package.xml b/visualization/tier4_planning_factor_rviz_plugin/package.xml
new file mode 100644
index 0000000000000..f1d2ce7df6ef4
--- /dev/null
+++ b/visualization/tier4_planning_factor_rviz_plugin/package.xml
@@ -0,0 +1,33 @@
+
+
+
+ tier4_planning_factor_rviz_plugin
+ 0.40.0
+ The tier4_planning_factor_rviz_plugin package
+ Satoshi Ota
+ Mamoru Sobue
+ Shumpei Wakabayashi
+ Apache 2.0
+
+ ament_cmake
+ autoware_cmake
+
+ qtbase5-dev
+
+ autoware_motion_utils
+ autoware_universe_utils
+ autoware_vehicle_info_utils
+ rviz_common
+ rviz_default_plugins
+ tier4_planning_msgs
+
+ libqt5-widgets
+ rviz2
+
+ ament_lint_auto
+ autoware_lint_common
+
+
+ ament_cmake
+
+
diff --git a/visualization/tier4_planning_factor_rviz_plugin/plugins.xml b/visualization/tier4_planning_factor_rviz_plugin/plugins.xml
new file mode 100644
index 0000000000000..7e5b53d2b1d61
--- /dev/null
+++ b/visualization/tier4_planning_factor_rviz_plugin/plugins.xml
@@ -0,0 +1,8 @@
+
+
+
+ tier4_planning_factor_rviz_plugin
+ tier4_planning_msgs/msg/PlanningFactorArray
+
+
+
diff --git a/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.cpp b/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.cpp
new file mode 100644
index 0000000000000..9530368421596
--- /dev/null
+++ b/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
+#include
+#include
+#include
+
+#include
+
+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(code >> 16) / 255.0;
+ const float g = static_cast((code << 48) >> 56) / 255.0;
+ const float b = static_cast((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(i) / static_cast(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(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(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(safety_factor_marker));
+ }
+ }
+ }
+}
+} // namespace autoware::rviz_plugins
+
+// Export the plugin
+#include // NOLINT
+PLUGINLIB_EXPORT_CLASS(autoware::rviz_plugins::PlanningFactorRvizPlugin, rviz_common::Display)
diff --git a/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.hpp b/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.hpp
new file mode 100644
index 0000000000000..f2af16e3e06b1
--- /dev/null
+++ b/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
+#include
+#include
+#include
+#include
+
+#include
+
+#include
+
+namespace autoware::rviz_plugins
+{
+
+using RosTopicDisplay = rviz_common::RosTopicDisplay;
+
+class PlanningFactorRvizPlugin
+: public rviz_common::RosTopicDisplay
+{
+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());
+ 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_