diff --git a/common/tier4_debug_rviz_plugin/CMakeLists.txt b/common/tier4_debug_rviz_plugin/CMakeLists.txt
new file mode 100644
index 000000000000..05cf35b93ef9
--- /dev/null
+++ b/common/tier4_debug_rviz_plugin/CMakeLists.txt
@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.14)
+project(tier4_debug_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_debug_rviz_plugin SHARED
+  include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp
+  include/tier4_debug_rviz_plugin/jsk_overlay_utils.hpp
+  src/float32_multi_array_stamped_pie_chart.cpp
+  src/jsk_overlay_utils.cpp
+)
+
+target_link_libraries(tier4_debug_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
+  icons
+  plugins
+)
diff --git a/common/tier4_debug_rviz_plugin/README.md b/common/tier4_debug_rviz_plugin/README.md
new file mode 100644
index 000000000000..918981614163
--- /dev/null
+++ b/common/tier4_debug_rviz_plugin/README.md
@@ -0,0 +1,12 @@
+# tier4_debug_rviz_plugin
+
+This package is including jsk code.
+Note that jsk_overlay_utils.cpp and jsk_overlay_utils.hpp are BSD license.
+
+## Plugins
+
+### Float32MultiArrayStampedPieChart
+
+Pie chart from `tier4_debug_msgs::msg::Float32MultiArrayStamped`.
+
+![float32_multi_array_stamped_pie_chart](./images/float32_multi_array_stamped_pie_chart.png)
diff --git a/common/tier4_debug_rviz_plugin/icons/classes/Float32MultiArrayStampedPieChart.png b/common/tier4_debug_rviz_plugin/icons/classes/Float32MultiArrayStampedPieChart.png
new file mode 100644
index 000000000000..6a67573717ae
Binary files /dev/null and b/common/tier4_debug_rviz_plugin/icons/classes/Float32MultiArrayStampedPieChart.png differ
diff --git a/common/tier4_debug_rviz_plugin/images/float32_multi_array_stamped_pie_chart.png b/common/tier4_debug_rviz_plugin/images/float32_multi_array_stamped_pie_chart.png
new file mode 100644
index 000000000000..7cd7ca753f38
Binary files /dev/null and b/common/tier4_debug_rviz_plugin/images/float32_multi_array_stamped_pie_chart.png differ
diff --git a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp
new file mode 100644
index 000000000000..c8267c7051d9
--- /dev/null
+++ b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp
@@ -0,0 +1,172 @@
+// Copyright 2022 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.
+
+// Copyright (c) 2014, JSK Lab
+// All rights reserved.
+//
+// Software License Agreement (BSD License)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above
+//    copyright notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of {copyright_holder} nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE
+//  POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef TIER4_DEBUG_RVIZ_PLUGIN__FLOAT32_MULTI_ARRAY_STAMPED_PIE_CHART_HPP_
+#define TIER4_DEBUG_RVIZ_PLUGIN__FLOAT32_MULTI_ARRAY_STAMPED_PIE_CHART_HPP_
+
+#include <rviz_common/display_context.hpp>
+#include <rviz_common/frame_manager_iface.hpp>
+#include <rviz_common/message_filter_display.hpp>
+#include <rviz_common/properties/bool_property.hpp>
+#include <rviz_common/properties/color_property.hpp>
+#include <rviz_common/properties/float_property.hpp>
+#include <rviz_common/properties/int_property.hpp>
+#include <rviz_common/properties/parse_color.hpp>
+#include <rviz_common/properties/string_property.hpp>
+#include <rviz_common/validate_floats.hpp>
+#include <tier4_debug_rviz_plugin/jsk_overlay_utils.hpp>
+
+#include <tier4_debug_msgs/msg/float32_multi_array_stamped.hpp>
+
+#include <mutex>
+
+namespace rviz_plugins
+{
+class Float32MultiArrayStampedPieChartDisplay : public rviz_common::Display
+{
+  Q_OBJECT
+public:
+  Float32MultiArrayStampedPieChartDisplay();
+  virtual ~Float32MultiArrayStampedPieChartDisplay();
+
+  // methods for OverlayPickerTool
+  virtual bool isInRegion(int x, int y);
+  virtual void movePosition(int x, int y);
+  virtual void setPosition(int x, int y);
+  virtual int getX() { return left_; }
+  virtual int getY() { return top_; }
+
+protected:
+  virtual void subscribe();
+  virtual void unsubscribe();
+  virtual void onEnable();
+  virtual void onDisable();
+  virtual void onInitialize();
+  virtual void processMessage(
+    const tier4_debug_msgs::msg::Float32MultiArrayStamped::ConstSharedPtr msg);
+  virtual void drawPlot(double val);
+  virtual void update(float wall_dt, float ros_dt);
+  // properties
+  rviz_common::properties::StringProperty * update_topic_property_;
+  rviz_common::properties::IntProperty * data_index_property_;
+  rviz_common::properties::IntProperty * size_property_;
+  rviz_common::properties::IntProperty * left_property_;
+  rviz_common::properties::IntProperty * top_property_;
+  rviz_common::properties::ColorProperty * fg_color_property_;
+  rviz_common::properties::ColorProperty * bg_color_property_;
+  rviz_common::properties::ColorProperty * text_color_property_;
+  rviz_common::properties::FloatProperty * fg_alpha_property_;
+  rviz_common::properties::FloatProperty * fg_alpha2_property_;
+  rviz_common::properties::FloatProperty * bg_alpha_property_;
+  rviz_common::properties::FloatProperty * text_alpha_property_;
+  rviz_common::properties::IntProperty * text_size_property_;
+  rviz_common::properties::FloatProperty * max_value_property_;
+  rviz_common::properties::FloatProperty * min_value_property_;
+  rviz_common::properties::BoolProperty * show_caption_property_;
+  rviz_common::properties::BoolProperty * auto_color_change_property_;
+  rviz_common::properties::ColorProperty * max_color_property_;
+  rviz_common::properties::ColorProperty * med_color_property_;
+  rviz_common::properties::FloatProperty * max_color_threshold_property_;
+  rviz_common::properties::FloatProperty * med_color_threshold_property_;
+  rviz_common::properties::BoolProperty * clockwise_rotate_property_;
+
+  rclcpp::Subscription<tier4_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr sub_;
+  int left_;
+  int top_;
+  uint16_t texture_size_;
+  QColor fg_color_;
+  QColor bg_color_;
+  QColor max_color_;
+  QColor med_color_;
+  int text_size_;
+  bool show_caption_;
+  bool auto_color_change_;
+  int caption_offset_;
+  double fg_alpha_;
+  double fg_alpha2_;
+  double bg_alpha_;
+  double max_value_;
+  double min_value_;
+  double max_color_threshold_;
+  double med_color_threshold_;
+  bool update_required_;
+  bool first_time_;
+  float data_;
+  int data_index_{0};
+  jsk_rviz_plugins::OverlayObject::Ptr overlay_;
+  bool clockwise_rotate_;
+
+  std::mutex mutex_;
+
+protected Q_SLOTS:
+  void updateTopic();
+  void updateDataIndex();
+  void updateSize();
+  void updateTop();
+  void updateLeft();
+  void updateBGColor();
+  void updateTextSize();
+  void updateFGColor();
+  void updateFGAlpha();
+  void updateFGAlpha2();
+  void updateBGAlpha();
+  void updateMinValue();
+  void updateMaxValue();
+  void updateShowCaption();
+  void updateAutoColorChange();
+  void updateMaxColor();
+  void updateMedColor();
+  void updateMaxColorThreshold();
+  void updateMedColorThreshold();
+  void updateClockwiseRotate();
+
+private:
+};
+
+}  // namespace rviz_plugins
+
+#endif  // TIER4_DEBUG_RVIZ_PLUGIN__FLOAT32_MULTI_ARRAY_STAMPED_PIE_CHART_HPP_
diff --git a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/jsk_overlay_utils.hpp b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/jsk_overlay_utils.hpp
new file mode 100644
index 000000000000..37bf743e35f6
--- /dev/null
+++ b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/jsk_overlay_utils.hpp
@@ -0,0 +1,143 @@
+// Copyright 2022 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.
+
+// Copyright (c) 2014, JSK Lab
+// All rights reserved.
+//
+// Software License Agreement (BSD License)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above
+//    copyright notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of {copyright_holder} nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE
+//  POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef TIER4_DEBUG_RVIZ_PLUGIN__JSK_OVERLAY_UTILS_HPP_
+#define TIER4_DEBUG_RVIZ_PLUGIN__JSK_OVERLAY_UTILS_HPP_
+
+#include <OgreHardwarePixelBuffer.h>
+#include <OgreMaterialManager.h>
+#include <OgreSceneManager.h>
+#include <OgreTechnique.h>
+#include <OgreTexture.h>
+#include <OgreTextureManager.h>
+
+#include <memory>
+#include <string>
+// see OGRE/OgrePrerequisites.h
+// #define OGRE_VERSION
+// ((OGRE_VERSION_MAJOR << 16) | (OGRE_VERSION_MINOR << 8) | OGRE_VERSION_PATCH)
+#if OGRE_VERSION < ((1 << 16) | (9 << 8) | 0)
+#include <OGRE/OgreOverlayContainer.h>
+#include <OGRE/OgreOverlayElement.h>
+#include <OGRE/OgreOverlayManager.h>
+#include <OGRE/OgrePanelOverlayElement.h>
+#else
+#include <Overlay/OgreOverlay.h>
+#include <Overlay/OgreOverlayContainer.h>
+#include <Overlay/OgreOverlayElement.h>
+#include <Overlay/OgreOverlayManager.h>
+#include <Overlay/OgrePanelOverlayElement.h>
+#endif
+
+#include <QColor>
+#include <QCursor>
+#include <QImage>
+#include <QVariant>
+#include <rclcpp/rclcpp.hpp>
+#include <rviz_rendering/render_system.hpp>
+
+namespace jsk_rviz_plugins
+{
+class OverlayObject;
+
+class ScopedPixelBuffer
+{
+public:
+  explicit ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer);
+  virtual ~ScopedPixelBuffer();
+  virtual Ogre::HardwarePixelBufferSharedPtr getPixelBuffer();
+  virtual QImage getQImage(unsigned int width, unsigned int height);
+  virtual QImage getQImage(OverlayObject & overlay);
+  virtual QImage getQImage(unsigned int width, unsigned int height, QColor & bg_color);
+  virtual QImage getQImage(OverlayObject & overlay, QColor & bg_color);
+
+protected:
+  Ogre::HardwarePixelBufferSharedPtr pixel_buffer_;
+
+private:
+};
+
+// this is a class for put overlay object on rviz 3D panel.
+// This class suppose to be instantiated in onInitialize method
+// of rviz::Display class.
+class OverlayObject
+{
+public:
+  typedef std::shared_ptr<OverlayObject> Ptr;
+
+  OverlayObject(Ogre::SceneManager * manager, rclcpp::Logger logger, const std::string & name);
+  virtual ~OverlayObject();
+
+  virtual std::string getName();
+  virtual void hide();
+  virtual void show();
+  virtual bool isTextureReady();
+  virtual void updateTextureSize(unsigned int width, unsigned int height);
+  virtual ScopedPixelBuffer getBuffer();
+  virtual void setPosition(double left, double top);
+  virtual void setDimensions(double width, double height);
+  virtual bool isVisible();
+  virtual unsigned int getTextureWidth();
+  virtual unsigned int getTextureHeight();
+
+protected:
+  const std::string name_;
+  rclcpp::Logger logger_;
+  Ogre::Overlay * overlay_;
+  Ogre::PanelOverlayElement * panel_;
+  Ogre::MaterialPtr panel_material_;
+  Ogre::TexturePtr texture_;
+
+private:
+};
+
+// Ogre::Overlay* createOverlay(std::string name);
+// Ogre::PanelOverlayElement* createOverlayPanel(Ogre::Overlay* overlay);
+// Ogre::MaterialPtr createOverlayMaterial(Ogre::Overlay* overlay);
+}  // namespace jsk_rviz_plugins
+
+#endif  // TIER4_DEBUG_RVIZ_PLUGIN__JSK_OVERLAY_UTILS_HPP_
diff --git a/common/tier4_debug_rviz_plugin/package.xml b/common/tier4_debug_rviz_plugin/package.xml
new file mode 100644
index 000000000000..bb1189c02555
--- /dev/null
+++ b/common/tier4_debug_rviz_plugin/package.xml
@@ -0,0 +1,31 @@
+<?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_debug_rviz_plugin</name>
+  <version>0.1.0</version>
+  <description>The tier4_debug_rviz_plugin package</description>
+  <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_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_default_plugins</depend>
+  <depend>rviz_rendering</depend>
+  <depend>tier4_debug_msgs</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>
diff --git a/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml b/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml
new file mode 100644
index 000000000000..c1158465e1cf
--- /dev/null
+++ b/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml
@@ -0,0 +1,7 @@
+<library path="tier4_debug_rviz_plugin">
+  <class name="rviz_plugins/Float32MultiArrayStampedPieChart"
+    type="rviz_plugins::Float32MultiArrayStampedPieChartDisplay"
+    base_class_type="rviz_common::Display">
+    <description>Display drivable area of tier4_debug_msgs::msg::Float32MultiArrayStamped</description>
+  </class>
+</library>
diff --git a/common/tier4_debug_rviz_plugin/src/float32_multi_array_stamped_pie_chart.cpp b/common/tier4_debug_rviz_plugin/src/float32_multi_array_stamped_pie_chart.cpp
new file mode 100644
index 000000000000..1fa3aaaf2413
--- /dev/null
+++ b/common/tier4_debug_rviz_plugin/src/float32_multi_array_stamped_pie_chart.cpp
@@ -0,0 +1,476 @@
+// Copyright 2022 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.
+
+// Copyright (c) 2014, JSK Lab
+// All rights reserved.
+//
+// Software License Agreement (BSD License)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above
+//    copyright notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of {copyright_holder} nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE
+//  POSSIBILITY OF SUCH DAMAGE.
+
+#include <QPainter>
+#include <rviz_common/display_context.hpp>
+#include <rviz_common/uniform_string_stream.hpp>
+#include <tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp>
+
+namespace rviz_plugins
+{
+
+Float32MultiArrayStampedPieChartDisplay::Float32MultiArrayStampedPieChartDisplay()
+: rviz_common::Display(), update_required_(false), first_time_(true), data_(0.0)
+{
+  update_topic_property_ = new rviz_common::properties::StringProperty(
+    "Topic", "", "tier4_debug_msgs::msg::Float32MultiArrayStamped topic to subscribe to.", this,
+    SLOT(updateTopic()), this);
+  data_index_property_ = new rviz_common::properties::IntProperty(
+    "data index", 0, "data index in message to visualize", this, SLOT(updateDataIndex()));
+  size_property_ = new rviz_common::properties::IntProperty(
+    "size", 128, "size of the plotter window", this, SLOT(updateSize()));
+  left_property_ = new rviz_common::properties::IntProperty(
+    "left", 128, "left of the plotter window", this, SLOT(updateLeft()));
+  top_property_ = new rviz_common::properties::IntProperty(
+    "top", 128, "top of the plotter window", this, SLOT(updateTop()));
+  fg_color_property_ = new rviz_common::properties::ColorProperty(
+    "foreground color", QColor(25, 255, 240), "color to draw line", this, SLOT(updateFGColor()));
+  fg_alpha_property_ = new rviz_common::properties::FloatProperty(
+    "foreground alpha", 0.7, "alpha blending value for foreground", this, SLOT(updateFGAlpha()));
+  fg_alpha2_property_ = new rviz_common::properties::FloatProperty(
+    "foreground alpha 2", 0.4, "alpha blending value for foreground for indicator", this,
+    SLOT(updateFGAlpha2()));
+  bg_color_property_ = new rviz_common::properties::ColorProperty(
+    "background color", QColor(0, 0, 0), "background color", this, SLOT(updateBGColor()));
+  bg_alpha_property_ = new rviz_common::properties::FloatProperty(
+    "background alpha", 0.0, "alpha blending value for background", this, SLOT(updateBGAlpha()));
+  text_size_property_ = new rviz_common::properties::IntProperty(
+    "text size", 14, "text size", this, SLOT(updateTextSize()));
+  show_caption_property_ = new rviz_common::properties::BoolProperty(
+    "show caption", false, "show caption", this, SLOT(updateShowCaption()));
+  max_value_property_ = new rviz_common::properties::FloatProperty(
+    "max value", 1.0, "max value of pie chart", this, SLOT(updateMaxValue()));
+  min_value_property_ = new rviz_common::properties::FloatProperty(
+    "min value", 0.0, "min value of pie chart", this, SLOT(updateMinValue()));
+  auto_color_change_property_ = new rviz_common::properties::BoolProperty(
+    "auto color change", false, "change the color automatically", this,
+    SLOT(updateAutoColorChange()));
+  max_color_property_ = new rviz_common::properties::ColorProperty(
+    "max color", QColor(255, 0, 0), "only used if auto color change is set to True.", this,
+    SLOT(updateMaxColor()));
+
+  med_color_property_ = new rviz_common::properties::ColorProperty(
+    "med color", QColor(255, 0, 0), "only used if auto color change is set to True.", this,
+    SLOT(updateMedColor()));
+
+  max_color_threshold_property_ = new rviz_common::properties::FloatProperty(
+    "max color change threshold", 0, "change the max color at threshold", this,
+    SLOT(updateMaxColorThreshold()));
+
+  med_color_threshold_property_ = new rviz_common::properties::FloatProperty(
+    "med color change threshold", 0, "change the med color at threshold ", this,
+    SLOT(updateMedColorThreshold()));
+
+  clockwise_rotate_property_ = new rviz_common::properties::BoolProperty(
+    "clockwise rotate direction", false, "change the rotate direction", this,
+    SLOT(updateClockwiseRotate()));
+}
+
+Float32MultiArrayStampedPieChartDisplay::~Float32MultiArrayStampedPieChartDisplay()
+{
+  if (overlay_->isVisible()) {
+    overlay_->hide();
+  }
+  delete update_topic_property_;
+  delete fg_color_property_;
+  delete bg_color_property_;
+  delete fg_alpha_property_;
+  delete fg_alpha2_property_;
+  delete bg_alpha_property_;
+  delete top_property_;
+  delete left_property_;
+  delete size_property_;
+  delete min_value_property_;
+  delete max_value_property_;
+  delete max_color_property_;
+  delete med_color_property_;
+  delete text_size_property_;
+  delete show_caption_property_;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::onInitialize()
+{
+  static int count = 0;
+  rviz_common::UniformStringStream ss;
+  ss << "Float32MultiArrayStampedPieChart" << count++;
+  auto logger = context_->getRosNodeAbstraction().lock()->get_raw_node()->get_logger();
+  overlay_.reset(new jsk_rviz_plugins::OverlayObject(scene_manager_, logger, ss.str()));
+  onEnable();
+  updateSize();
+  updateLeft();
+  updateTop();
+  updateFGColor();
+  updateBGColor();
+  updateFGAlpha();
+  updateFGAlpha2();
+  updateBGAlpha();
+  updateMinValue();
+  updateMaxValue();
+  updateTextSize();
+  updateShowCaption();
+  updateAutoColorChange();
+  updateMaxColor();
+  updateMedColor();
+  updateMaxColorThreshold();
+  updateMedColorThreshold();
+  updateClockwiseRotate();
+  overlay_->updateTextureSize(texture_size_, texture_size_ + caption_offset_);
+  overlay_->hide();
+}
+
+void Float32MultiArrayStampedPieChartDisplay::update(
+  [[maybe_unused]] float wall_dt, [[maybe_unused]] float ros_dt)
+{
+  if (update_required_) {
+    update_required_ = false;
+    overlay_->updateTextureSize(texture_size_, texture_size_ + caption_offset_);
+    overlay_->setPosition(left_, top_);
+    overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight());
+    drawPlot(data_);
+  }
+}
+
+void Float32MultiArrayStampedPieChartDisplay::processMessage(
+  const tier4_debug_msgs::msg::Float32MultiArrayStamped::ConstSharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  if (!overlay_->isVisible()) {
+    return;
+  }
+  if (data_index_ < 0 || static_cast<int>(msg->data.size()) <= data_index_) {
+    return;
+  }
+
+  if (data_ != msg->data.at(data_index_) || first_time_) {
+    first_time_ = false;
+    data_ = msg->data.at(data_index_);
+    update_required_ = true;
+  }
+}
+
+void Float32MultiArrayStampedPieChartDisplay::drawPlot(double val)
+{
+  QColor fg_color(fg_color_);
+
+  if (auto_color_change_) {
+    double r = std::min(1.0, fabs((val - min_value_) / (max_value_ - min_value_)));
+    if (r > 0.6) {
+      double r2 = (r - 0.6) / 0.4;
+      fg_color.setRed((max_color_.red() - fg_color_.red()) * r2 + fg_color_.red());
+      fg_color.setGreen((max_color_.green() - fg_color_.green()) * r2 + fg_color_.green());
+      fg_color.setBlue((max_color_.blue() - fg_color_.blue()) * r2 + fg_color_.blue());
+    }
+    if (max_color_threshold_ != 0) {
+      if (r > max_color_threshold_) {
+        fg_color.setRed(max_color_.red());
+        fg_color.setGreen(max_color_.green());
+        fg_color.setBlue(max_color_.blue());
+      }
+    }
+    if (med_color_threshold_ != 0) {
+      if (max_color_threshold_ > r && r > med_color_threshold_) {
+        fg_color.setRed(med_color_.red());
+        fg_color.setGreen(med_color_.green());
+        fg_color.setBlue(med_color_.blue());
+      }
+    }
+  }
+
+  QColor fg_color2(fg_color);
+  QColor bg_color(bg_color_);
+  fg_color.setAlpha(fg_alpha_);
+  fg_color2.setAlpha(fg_alpha2_);
+  bg_color.setAlpha(bg_alpha_);
+  int width = overlay_->getTextureWidth();
+  int height = overlay_->getTextureHeight();
+  {
+    jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer();
+    QImage Hud = buffer.getQImage(*overlay_, bg_color);
+    QPainter painter(&Hud);
+    painter.setRenderHint(QPainter::Antialiasing, true);
+
+    const int outer_line_width = 5;
+    const int value_line_width = 10;
+    const int value_indicator_line_width = 2;
+    const int value_padding = 5;
+
+    const int value_offset = outer_line_width + value_padding + value_line_width / 2;
+
+    painter.setPen(QPen(fg_color, outer_line_width, Qt::SolidLine));
+
+    painter.drawEllipse(
+      outer_line_width / 2, outer_line_width / 2, width - outer_line_width,
+      height - outer_line_width - caption_offset_);
+
+    painter.setPen(QPen(fg_color2, value_indicator_line_width, Qt::SolidLine));
+    painter.drawEllipse(
+      value_offset, value_offset, width - value_offset * 2,
+      height - value_offset * 2 - caption_offset_);
+
+    const double ratio = (val - min_value_) / (max_value_ - min_value_);
+    const double rotate_direction = clockwise_rotate_ ? -1.0 : 1.0;
+    const double ratio_angle = ratio * 360.0 * rotate_direction;
+    const double start_angle_offset = -90;
+    painter.setPen(QPen(fg_color, value_line_width, Qt::SolidLine));
+    painter.drawArc(
+      QRectF(
+        value_offset, value_offset, width - value_offset * 2,
+        height - value_offset * 2 - caption_offset_),
+      start_angle_offset * 16, ratio_angle * 16);
+    QFont font = painter.font();
+    font.setPointSize(text_size_);
+    font.setBold(true);
+    painter.setFont(font);
+    painter.setPen(QPen(fg_color, value_line_width, Qt::SolidLine));
+    std::ostringstream s;
+    s << std::fixed << std::setprecision(2) << val;
+    painter.drawText(
+      0, 0, width, height - caption_offset_, Qt::AlignCenter | Qt::AlignVCenter, s.str().c_str());
+
+    // caption
+    if (show_caption_) {
+      painter.drawText(
+        0, height - caption_offset_, width, caption_offset_, Qt::AlignCenter | Qt::AlignVCenter,
+        getName());
+    }
+
+    // done
+    painter.end();
+    // Unlock the pixel buffer
+  }
+}
+
+void Float32MultiArrayStampedPieChartDisplay::subscribe()
+{
+  std::string topic_name = update_topic_property_->getStdString();
+
+  // NOTE: Remove all spaces since topic name filled with only spaces will crash
+  topic_name.erase(std::remove(topic_name.begin(), topic_name.end(), ' '), topic_name.end());
+
+  if (topic_name.length() > 0 && topic_name != "/") {
+    rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node();
+    sub_ = raw_node->create_subscription<tier4_debug_msgs::msg::Float32MultiArrayStamped>(
+      topic_name, 1,
+      std::bind(
+        &Float32MultiArrayStampedPieChartDisplay::processMessage, this, std::placeholders::_1));
+  }
+}
+
+void Float32MultiArrayStampedPieChartDisplay::unsubscribe() { sub_.reset(); }
+
+void Float32MultiArrayStampedPieChartDisplay::onEnable()
+{
+  subscribe();
+  overlay_->show();
+  first_time_ = true;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::onDisable()
+{
+  unsubscribe();
+  overlay_->hide();
+}
+
+void Float32MultiArrayStampedPieChartDisplay::updateSize()
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  texture_size_ = size_property_->getInt();
+  update_required_ = true;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::updateTop()
+{
+  top_ = top_property_->getInt();
+  update_required_ = true;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::updateLeft()
+{
+  left_ = left_property_->getInt();
+  update_required_ = true;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::updateBGColor()
+{
+  bg_color_ = bg_color_property_->getColor();
+  update_required_ = true;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::updateFGColor()
+{
+  fg_color_ = fg_color_property_->getColor();
+  update_required_ = true;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::updateFGAlpha()
+{
+  fg_alpha_ = fg_alpha_property_->getFloat() * 255.0;
+  update_required_ = true;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::updateFGAlpha2()
+{
+  fg_alpha2_ = fg_alpha2_property_->getFloat() * 255.0;
+  update_required_ = true;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::updateBGAlpha()
+{
+  bg_alpha_ = bg_alpha_property_->getFloat() * 255.0;
+  update_required_ = true;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::updateMinValue()
+{
+  min_value_ = min_value_property_->getFloat();
+  update_required_ = true;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::updateMaxValue()
+{
+  max_value_ = max_value_property_->getFloat();
+  update_required_ = true;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::updateTextSize()
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  text_size_ = text_size_property_->getInt();
+  QFont font;
+  font.setPointSize(text_size_);
+  caption_offset_ = QFontMetrics(font).height();
+  update_required_ = true;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::updateShowCaption()
+{
+  show_caption_ = show_caption_property_->getBool();
+  update_required_ = true;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::updateTopic()
+{
+  unsubscribe();
+  subscribe();
+}
+
+void Float32MultiArrayStampedPieChartDisplay::updateDataIndex()
+{
+  data_index_ = data_index_property_->getInt();
+  update_required_ = true;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::updateAutoColorChange()
+{
+  auto_color_change_ = auto_color_change_property_->getBool();
+  if (auto_color_change_) {
+    max_color_property_->show();
+    med_color_property_->show();
+    max_color_threshold_property_->show();
+    med_color_threshold_property_->show();
+  } else {
+    max_color_property_->hide();
+    med_color_property_->hide();
+    max_color_threshold_property_->hide();
+    med_color_threshold_property_->hide();
+  }
+  update_required_ = true;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::updateMaxColor()
+{
+  max_color_ = max_color_property_->getColor();
+  update_required_ = true;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::updateMedColor()
+{
+  med_color_ = med_color_property_->getColor();
+  update_required_ = true;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::updateMaxColorThreshold()
+{
+  max_color_threshold_ = max_color_threshold_property_->getFloat();
+  update_required_ = true;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::updateMedColorThreshold()
+{
+  med_color_threshold_ = med_color_threshold_property_->getFloat();
+  update_required_ = true;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::updateClockwiseRotate()
+{
+  clockwise_rotate_ = clockwise_rotate_property_->getBool();
+  update_required_ = true;
+}
+
+bool Float32MultiArrayStampedPieChartDisplay::isInRegion(int x, int y)
+{
+  return (top_ < y && top_ + texture_size_ > y && left_ < x && left_ + texture_size_ > x);
+}
+
+void Float32MultiArrayStampedPieChartDisplay::movePosition(int x, int y)
+{
+  top_ = y;
+  left_ = x;
+}
+
+void Float32MultiArrayStampedPieChartDisplay::setPosition(int x, int y)
+{
+  top_property_->setValue(y);
+  left_property_->setValue(x);
+}
+}  // namespace rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(rviz_plugins::Float32MultiArrayStampedPieChartDisplay, rviz_common::Display)
diff --git a/common/tier4_debug_rviz_plugin/src/jsk_overlay_utils.cpp b/common/tier4_debug_rviz_plugin/src/jsk_overlay_utils.cpp
new file mode 100644
index 000000000000..b3090346aafd
--- /dev/null
+++ b/common/tier4_debug_rviz_plugin/src/jsk_overlay_utils.cpp
@@ -0,0 +1,207 @@
+// Copyright 2022 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.
+
+// Copyright (c) 2014, JSK Lab
+// All rights reserved.
+//
+// Software License Agreement (BSD License)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above
+//    copyright notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of {copyright_holder} nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE
+//  POSSIBILITY OF SUCH DAMAGE.
+
+#include <tier4_debug_rviz_plugin/jsk_overlay_utils.hpp>
+
+#include <string>
+
+namespace jsk_rviz_plugins
+{
+ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer)
+: pixel_buffer_(pixel_buffer)
+{
+  pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL);
+}
+
+ScopedPixelBuffer::~ScopedPixelBuffer() { pixel_buffer_->unlock(); }
+
+Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() { return pixel_buffer_; }
+
+QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height)
+{
+  const Ogre::PixelBox & pixelBox = pixel_buffer_->getCurrentLock();
+  Ogre::uint8 * pDest = static_cast<Ogre::uint8 *>(pixelBox.data);
+  memset(pDest, 0, width * height);
+  return QImage(pDest, width, height, QImage::Format_ARGB32);
+}
+
+QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height, QColor & bg_color)
+{
+  QImage Hud = getQImage(width, height);
+  for (unsigned int i = 0; i < width; i++) {
+    for (unsigned int j = 0; j < height; j++) {
+      Hud.setPixel(i, j, bg_color.rgba());
+    }
+  }
+  return Hud;
+}
+
+QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay)
+{
+  return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight());
+}
+
+QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay, QColor & bg_color)
+{
+  return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight(), bg_color);
+}
+
+OverlayObject::OverlayObject(
+  Ogre::SceneManager * manager, rclcpp::Logger logger, const std::string & name)
+: name_(name), logger_(logger)
+{
+  rviz_rendering::RenderSystem::get()->prepareOverlays(manager);
+  std::string material_name = name_ + "Material";
+  Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr();
+  overlay_ = mOverlayMgr->create(name_);
+  panel_ = static_cast<Ogre::PanelOverlayElement *>(
+    mOverlayMgr->createOverlayElement("Panel", name_ + "Panel"));
+  panel_->setMetricsMode(Ogre::GMM_PIXELS);
+
+  panel_material_ = Ogre::MaterialManager::getSingleton().create(
+    material_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+  panel_->setMaterialName(panel_material_->getName());
+  overlay_->add2D(panel_);
+}
+
+OverlayObject::~OverlayObject()
+{
+  hide();
+  panel_material_->unload();
+  Ogre::MaterialManager::getSingleton().remove(panel_material_->getName());
+  // Ogre::OverlayManager* mOverlayMgr = Ogre::OverlayManager::getSingletonPtr();
+  // mOverlayMgr->destroyOverlayElement(panel_);
+  // delete panel_;
+  // delete overlay_;
+}
+
+std::string OverlayObject::getName() { return name_; }
+
+void OverlayObject::hide()
+{
+  if (overlay_->isVisible()) {
+    overlay_->hide();
+  }
+}
+
+void OverlayObject::show()
+{
+  if (!overlay_->isVisible()) {
+    overlay_->show();
+  }
+}
+
+bool OverlayObject::isTextureReady() { return static_cast<bool>(texture_); }
+
+void OverlayObject::updateTextureSize(unsigned int width, unsigned int height)
+{
+  const std::string texture_name = name_ + "Texture";
+  if (width == 0) {
+    RCLCPP_WARN(logger_, "width=0 is specified as texture size");
+    width = 1;
+  }
+  if (height == 0) {
+    RCLCPP_WARN(logger_, "height=0 is specified as texture size");
+    height = 1;
+  }
+  if (!isTextureReady() || ((width != texture_->getWidth()) || (height != texture_->getHeight()))) {
+    if (isTextureReady()) {
+      Ogre::TextureManager::getSingleton().remove(texture_name);
+      panel_material_->getTechnique(0)->getPass(0)->removeAllTextureUnitStates();
+    }
+    texture_ = Ogre::TextureManager::getSingleton().createManual(
+      texture_name,  // name
+      Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
+      Ogre::TEX_TYPE_2D,  // type
+      width, height,      // width & height of the render window
+      0,                  // number of mipmaps
+      Ogre::PF_A8R8G8B8,  // pixel format chosen to match a format Qt can use
+      Ogre::TU_DEFAULT    // usage
+    );
+    panel_material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name);
+
+    panel_material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
+  }
+}
+
+ScopedPixelBuffer OverlayObject::getBuffer()
+{
+  if (isTextureReady()) {
+    return ScopedPixelBuffer(texture_->getBuffer());
+  } else {
+    return ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr());
+  }
+}
+
+void OverlayObject::setPosition(double left, double top) { panel_->setPosition(left, top); }
+
+void OverlayObject::setDimensions(double width, double height)
+{
+  panel_->setDimensions(width, height);
+}
+
+bool OverlayObject::isVisible() { return overlay_->isVisible(); }
+
+unsigned int OverlayObject::getTextureWidth()
+{
+  if (isTextureReady()) {
+    return texture_->getWidth();
+  } else {
+    return 0;
+  }
+}
+
+unsigned int OverlayObject::getTextureHeight()
+{
+  if (isTextureReady()) {
+    return texture_->getHeight();
+  } else {
+    return 0;
+  }
+}
+
+}  // namespace jsk_rviz_plugins