diff --git a/README.md b/README.md
index d43f240..81b7e3c 100644
--- a/README.md
+++ b/README.md
@@ -1,2 +1,5 @@
 # tuw_rviz
 package with rviz plugins to support tuw_msgs
+
+## tuw_pose_rviz_plugins
+This plugin is a copy of the common rviz pose plugin and can be used as a template for new plugins.
diff --git a/tuw_graph_rviz_plugins/CMakeLists.txt b/tuw_graph_rviz_plugins/CMakeLists.txt
new file mode 100644
index 0000000..9410dd5
--- /dev/null
+++ b/tuw_graph_rviz_plugins/CMakeLists.txt
@@ -0,0 +1,120 @@
+cmake_minimum_required(VERSION 3.8)
+project(tuw_graph_rviz_plugins)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(Qt5 REQUIRED COMPONENTS Widgets Core)
+find_package(yaml_cpp_vendor REQUIRED)
+find_package(ament_cmake REQUIRED)
+find_package(rcpputils)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rviz_common REQUIRED)
+find_package(rviz_default_plugins REQUIRED)
+find_package(rviz_rendering REQUIRED)
+
+## This setting causes Qt's "MOC" generation to happen automatically, which is required for
+## Qt's signal/slot connections to work.
+set(CMAKE_AUTOMOC ON)
+
+
+set(tuw_graph_rviz_plugins_headers_to_moc
+  include/tuw_graph_rviz_plugins/graph_display.hpp
+)
+
+foreach(header "${tuw_graph_rviz_plugins_headers_to_moc}")
+  qt5_wrap_cpp(tuw_graph_rviz_plugins_moc_files "${header}")
+endforeach()
+
+## Here we specify the list of source files.
+## The generated MOC files are included automatically as headers.
+set(tuw_graph_rviz_plugins_source_files
+src/graph_display.cpp
+src/graph_display_selection_handler.cpp
+)
+
+add_library(${PROJECT_NAME} SHARED
+  ${tuw_graph_rviz_plugins_moc_files}
+  ${tuw_graph_rviz_plugins_source_files}
+)
+
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+  ${Qt5Widgets_INCLUDE_DIRS}
+)
+
+target_link_libraries(${PROJECT_NAME} PUBLIC
+  rviz_ogre_vendor::OgreMain
+  rviz_ogre_vendor::OgreOverlay
+  rviz_common::rviz_common
+)
+
+# Causes the visibility macros to use dllexport rather than dllimport (for Windows, when your plugin should be used as library)
+target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY")
+
+
+ament_target_dependencies(${PROJECT_NAME}
+  PUBLIC
+  rcpputils
+  rclcpp
+  geometry_msgs
+  rviz_common
+  rviz_rendering
+  rviz_default_plugins
+  yaml_cpp_vendor
+)
+
+# Export the plugin to be imported by rviz2
+pluginlib_export_plugin_description_file(rviz_common plugins_description.xml)
+
+# Export old-style CMake variables
+ament_export_include_directories("include/${PROJECT_NAME}")
+
+# Export modern CMake targets
+ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
+
+ament_export_dependencies(
+  rcpputils
+  rclcpp
+  geometry_msgs
+  rviz_common
+  rviz_rendering
+)
+
+install(
+  TARGETS ${PROJECT_NAME}
+  EXPORT ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/icons"
+  DESTINATION "share/${PROJECT_NAME}"
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/tuw_graph_rviz_plugins/LICENSE b/tuw_graph_rviz_plugins/LICENSE
new file mode 100644
index 0000000..0dc4926
--- /dev/null
+++ b/tuw_graph_rviz_plugins/LICENSE
@@ -0,0 +1,27 @@
+Copyright (c) 2023 Markus Bader <markus.bader@tuwien.ac.at>
+
+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 the 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 HOLDER 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.
diff --git a/tuw_graph_rviz_plugins/icons/classes/Graph.png b/tuw_graph_rviz_plugins/icons/classes/Graph.png
new file mode 100644
index 0000000..f6e4c56
Binary files /dev/null and b/tuw_graph_rviz_plugins/icons/classes/Graph.png differ
diff --git a/tuw_graph_rviz_plugins/icons/classes/tuw.png b/tuw_graph_rviz_plugins/icons/classes/tuw.png
new file mode 100644
index 0000000..db823b2
Binary files /dev/null and b/tuw_graph_rviz_plugins/icons/classes/tuw.png differ
diff --git a/tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/graph_display.hpp b/tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/graph_display.hpp
new file mode 100644
index 0000000..259426c
--- /dev/null
+++ b/tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/graph_display.hpp
@@ -0,0 +1,128 @@
+/*
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * 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 the Willow Garage, Inc. 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.
+ */
+
+
+#ifndef TUW_GRAPH_RVIZ_PLUGINS__DISPLAYS__POSE__POSE_DISPLAY_HPP_
+#define TUW_GRAPH_RVIZ_PLUGINS__DISPLAYS__POSE__POSE_DISPLAY_HPP_
+
+#include <memory>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+#include "rviz_common/message_filter_display.hpp"
+#include "rviz_common/interaction/forwards.hpp"
+
+#include "tuw_graph_rviz_plugins/visibility_control.hpp"
+
+namespace rviz_rendering
+{
+class Arrow;
+class Axes;
+class Shape;
+}  // namespace rviz_rendering
+
+namespace rviz_common
+{
+namespace properties
+{
+class ColorProperty;
+class EnumProperty;
+class FloatProperty;
+}  // namespace properties
+}  // namespace rviz_common
+
+namespace tuw_graph_rviz_plugins
+{
+namespace displays
+{
+
+class PoseDisplaySelectionHandler;
+
+typedef std::shared_ptr<PoseDisplaySelectionHandler> PoseDisplaySelectionHandlerPtr;
+
+/** @brief Accumulates and displays the pose from a geometry_msgs::PoseStamped message. */
+class TUW_GRAPH_RVIZ_PLUGINS_PUBLIC PoseDisplay : public
+  rviz_common::MessageFilterDisplay<geometry_msgs::msg::PoseStamped>
+{
+  Q_OBJECT
+
+public:
+  enum Shape
+  {
+    Arrow,
+    Axes,
+  };
+
+  PoseDisplay();
+
+  ~PoseDisplay() override;
+  void onInitialize() override;
+  void reset() override;
+
+protected:
+  /** @brief Overridden from MessageFilterDisplay to get arrow/axes visibility correct. */
+  void onEnable() override;
+  void onDisable() override;
+  void processMessage(geometry_msgs::msg::PoseStamped::ConstSharedPtr message) override;
+
+private Q_SLOTS:
+  void updateShapeVisibility();
+  void updateColorAndAlpha();
+  void updateShapeChoice();
+  void updateAxisGeometry();
+  void updateArrowGeometry();
+
+private:
+  void setupSelectionHandler();
+
+  std::unique_ptr<rviz_rendering::Arrow> arrow_;
+  std::unique_ptr<rviz_rendering::Axes> axes_;
+  bool pose_valid_;
+  PoseDisplaySelectionHandlerPtr coll_handler_;
+
+  rviz_common::properties::EnumProperty * shape_property_;
+
+  rviz_common::properties::ColorProperty * color_property_;
+  rviz_common::properties::FloatProperty * alpha_property_;
+
+  rviz_common::properties::FloatProperty * head_radius_property_;
+  rviz_common::properties::FloatProperty * head_length_property_;
+  rviz_common::properties::FloatProperty * shaft_radius_property_;
+  rviz_common::properties::FloatProperty * shaft_length_property_;
+
+  rviz_common::properties::FloatProperty * axes_length_property_;
+  rviz_common::properties::FloatProperty * axes_radius_property_;
+
+  friend class PoseDisplaySelectionHandler;
+};
+
+}  // namespace displays
+}  // namespace tuw_graph_rviz_plugins
+
+#endif  // TUW_GRAPH_RVIZ_PLUGINS__DISPLAYS__POSE__POSE_DISPLAY_HPP_
\ No newline at end of file
diff --git a/tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/graph_display_selection_handler.hpp b/tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/graph_display_selection_handler.hpp
new file mode 100644
index 0000000..14db5f1
--- /dev/null
+++ b/tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/graph_display_selection_handler.hpp
@@ -0,0 +1,86 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2018, Bosch Software Innovations GmbH.
+ * All rights reserved.
+ *
+ * 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 the Willow Garage, Inc. 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.
+ */
+
+#ifndef TUW_GRAPH_RVIZ_PLUGINS__DISPLAYS__POSE__POSE_DISPLAY_SELECTION_HANDLER_HPP_
+#define TUW_GRAPH_RVIZ_PLUGINS__DISPLAYS__POSE__POSE_DISPLAY_SELECTION_HANDLER_HPP_
+
+#include <memory>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+#include "rviz_common/interaction/selection_handler.hpp"
+
+#include "tuw_graph_rviz_plugins/graph_display.hpp"
+#include "tuw_graph_rviz_plugins/visibility_control.hpp"
+
+namespace rviz_common
+{
+namespace properties
+{
+class StringProperty;
+class VectorProperty;
+class QuaternionProperty;
+}  // namespace properties
+}  // namespace rviz_common
+
+namespace tuw_graph_rviz_plugins
+{
+namespace displays
+{
+
+class TUW_GRAPH_RVIZ_PLUGINS_PUBLIC PoseDisplaySelectionHandler : public
+  rviz_common::interaction::SelectionHandler
+{
+public:
+  void createProperties(
+    const rviz_common::interaction::Picked & obj,
+    rviz_common::properties::Property * parent_property) override;
+
+  rviz_common::interaction::V_AABB getAABBs(const rviz_common::interaction::Picked & obj) override;
+
+  void setMessage(geometry_msgs::msg::PoseStamped::ConstSharedPtr message);
+
+private:
+  PoseDisplaySelectionHandler(PoseDisplay * display, rviz_common::DisplayContext * context);
+
+  PoseDisplay * display_;
+  rviz_common::properties::StringProperty * frame_property_;
+  rviz_common::properties::VectorProperty * position_property_;
+  rviz_common::properties::QuaternionProperty * orientation_property_;
+
+  template<typename T, typename ... Args>
+  friend typename std::shared_ptr<T>
+  rviz_common::interaction::createSelectionHandler(Args ... arguments);
+};
+
+}  // namespace displays
+}  // namespace tuw_graph_rviz_plugins
+
+#endif  // TUW_GRAPH_RVIZ_PLUGINS__DISPLAYS__POSE__POSE_DISPLAY_SELECTION_HANDLER_HPP_
\ No newline at end of file
diff --git a/tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/visibility_control.hpp b/tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/visibility_control.hpp
new file mode 100644
index 0000000..15882ba
--- /dev/null
+++ b/tuw_graph_rviz_plugins/include/tuw_graph_rviz_plugins/visibility_control.hpp
@@ -0,0 +1,64 @@
+/*
+ * Copyright (c) 2018, Open Source Robotics Foundation, Inc.
+ * All rights reserved.
+ *
+ * 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 the Willow Garage, Inc. 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.
+ */
+
+#ifndef TUW_GRAPH_RVIZ_PLUGINS__VISIBILITY_CONTROL_HPP_
+#define TUW_GRAPH_RVIZ_PLUGINS__VISIBILITY_CONTROL_HPP_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define TUW_GRAPH_RVIZ_PLUGINS_EXPORT __attribute__ ((dllexport))
+    #define TUW_GRAPH_RVIZ_PLUGINS_IMPORT __attribute__ ((dllimport))
+  #else
+    #define TUW_GRAPH_RVIZ_PLUGINS_EXPORT __declspec(dllexport)
+    #define TUW_GRAPH_RVIZ_PLUGINS_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef TUW_GRAPH_RVIZ_PLUGINS_BUILDING_LIBRARY
+    #define TUW_GRAPH_RVIZ_PLUGINS_PUBLIC TUW_GRAPH_RVIZ_PLUGINS_EXPORT
+  #else
+    #define TUW_GRAPH_RVIZ_PLUGINS_PUBLIC TUW_GRAPH_RVIZ_PLUGINS_IMPORT
+  #endif
+  #define TUW_GRAPH_RVIZ_PLUGINS_PUBLIC_TYPE TUW_GRAPH_RVIZ_PLUGINS_PUBLIC
+  #define TUW_GRAPH_RVIZ_PLUGINS_LOCAL
+#else
+  #define TUW_GRAPH_RVIZ_PLUGINS_EXPORT __attribute__ ((visibility("default")))
+  #define TUW_GRAPH_RVIZ_PLUGINS_IMPORT
+  #if __GNUC__ >= 4
+    #define TUW_GRAPH_RVIZ_PLUGINS_PUBLIC __attribute__ ((visibility("default")))
+    #define TUW_GRAPH_RVIZ_PLUGINS_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define TUW_GRAPH_RVIZ_PLUGINS_PUBLIC
+    #define TUW_GRAPH_RVIZ_PLUGINS_LOCAL
+  #endif
+  #define TUW_GRAPH_RVIZ_PLUGINS_PUBLIC_TYPE
+#endif
+
+#endif  // TUW_GRAPH_RVIZ_PLUGINS__VISIBILITY_CONTROL_HPP_
\ No newline at end of file
diff --git a/tuw_graph_rviz_plugins/package.xml b/tuw_graph_rviz_plugins/package.xml
new file mode 100644
index 0000000..e3e02c4
--- /dev/null
+++ b/tuw_graph_rviz_plugins/package.xml
@@ -0,0 +1,35 @@
+<?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>tuw_graph_rviz_plugins</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="markus.bader@tuwien.ac.at">markus</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+
+  <depend>yaml_cpp_vendor</depend>
+  <depend>rviz2</depend>
+
+
+  <depend>rclcpp</depend>
+  <depend>rclpy</depend>
+  <depend>pluginlib</depend>
+
+  <depend>rviz_common</depend>
+  <depend>rviz_default_plugins</depend>
+  <depend>rviz_rendering</depend>
+  <depend>geometry_msgs</depend>
+  
+  <!-- Python Samples -->
+  <exec_depend>python3-numpy</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/tuw_graph_rviz_plugins/plugins_description.xml b/tuw_graph_rviz_plugins/plugins_description.xml
new file mode 100644
index 0000000..1447ca1
--- /dev/null
+++ b/tuw_graph_rviz_plugins/plugins_description.xml
@@ -0,0 +1,14 @@
+<library path="tuw_graph_rviz_plugins">
+
+    <!-- Displays plugins -->
+  
+  <class
+    name="tuw_graph_rviz_plugins/Pose"
+    type="tuw_graph_rviz_plugins::displays::PoseDisplay"
+    base_class_type="rviz_common::Display">
+    <description>
+      Displays a geometry_msgs::PoseStamped message.  &lt;a href="http://www.ros.org/wiki/rviz/DisplayTypes/Pose"&gt;More Information&lt;/a&gt;.
+    </description>
+    <message_type>geometry_msgs/msg/PoseStamped</message_type>
+  </class>
+  </library>
\ No newline at end of file
diff --git a/tuw_graph_rviz_plugins/src/graph_display.cpp b/tuw_graph_rviz_plugins/src/graph_display.cpp
new file mode 100644
index 0000000..0114478
--- /dev/null
+++ b/tuw_graph_rviz_plugins/src/graph_display.cpp
@@ -0,0 +1,245 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * 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 the Willow Garage, Inc. 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.
+ */
+
+#include "tuw_graph_rviz_plugins/graph_display.hpp"
+
+#include <memory>
+
+#include <OgreSceneNode.h>
+
+#include "rviz_rendering/objects/arrow.hpp"
+#include "rviz_rendering/objects/axes.hpp"
+#include "rviz_rendering/objects/shape.hpp"
+#include "rviz_common/display_context.hpp"
+#include "rviz_common/frame_manager_iface.hpp"
+#include "rviz_common/logging.hpp"
+#include "rviz_common/properties/color_property.hpp"
+#include "rviz_common/properties/enum_property.hpp"
+#include "rviz_common/properties/float_property.hpp"
+#include "rviz_common/properties/quaternion_property.hpp"
+#include "rviz_common/properties/vector_property.hpp"
+#include "rviz_common/interaction/selection_manager.hpp"
+#include "rviz_common/validate_floats.hpp"
+#include "tuw_graph_rviz_plugins/graph_display_selection_handler.hpp"
+
+namespace tuw_graph_rviz_plugins
+{
+namespace displays
+{
+
+PoseDisplay::PoseDisplay()
+: arrow_(nullptr), axes_(nullptr), pose_valid_(false)
+{
+  shape_property_ = new rviz_common::properties::EnumProperty(
+    "Shape", "Arrow", "Shape to display the pose as.",
+    this, SLOT(updateShapeChoice()));
+  shape_property_->addOption("Arrow", Arrow);
+  shape_property_->addOption("Axes", Axes);
+
+  color_property_ = new rviz_common::properties::ColorProperty(
+    "Color", QColor(255, 25, 0), "Color to draw the arrow.",
+    this, SLOT(updateColorAndAlpha()));
+
+  alpha_property_ = new rviz_common::properties::FloatProperty(
+    "Alpha", 1, "Amount of transparency to apply to the arrow.",
+    this, SLOT(updateColorAndAlpha()));
+  alpha_property_->setMin(0);
+  alpha_property_->setMax(1);
+
+  shaft_length_property_ = new rviz_common::properties::FloatProperty(
+    "Shaft Length", 1, "Length of the arrow's shaft, in meters.",
+    this, SLOT(updateArrowGeometry()));
+
+  shaft_radius_property_ = new rviz_common::properties::FloatProperty(
+    "Shaft Radius", 0.05f, "Radius of the arrow's shaft, in meters.",
+    this, SLOT(updateArrowGeometry()));
+
+  head_length_property_ = new rviz_common::properties::FloatProperty(
+    "Head Length", 0.3f, "Length of the arrow's head, in meters.",
+    this, SLOT(updateArrowGeometry()));
+
+  head_radius_property_ = new rviz_common::properties::FloatProperty(
+    "Head Radius", 0.1f, "Radius of the arrow's head, in meters.",
+    this, SLOT(updateArrowGeometry()));
+
+  axes_length_property_ = new rviz_common::properties::FloatProperty(
+    "Axes Length", 1, "Length of each axis, in meters.",
+    this, SLOT(updateAxisGeometry()));
+
+  axes_radius_property_ = new rviz_common::properties::FloatProperty(
+    "Axes Radius", 0.1f, "Radius of each axis, in meters.",
+    this, SLOT(updateAxisGeometry()));
+}
+
+void PoseDisplay::onInitialize()
+{
+  MFDClass::onInitialize();
+
+  arrow_ = std::make_unique<rviz_rendering::Arrow>(
+    scene_manager_, scene_node_,
+    shaft_length_property_->getFloat(),
+    shaft_radius_property_->getFloat(),
+    head_length_property_->getFloat(),
+    head_radius_property_->getFloat());
+  arrow_->setDirection(Ogre::Vector3::UNIT_X);
+
+  axes_ = std::make_unique<rviz_rendering::Axes>(
+    scene_manager_, scene_node_,
+    axes_length_property_->getFloat(),
+    axes_radius_property_->getFloat());
+
+  updateShapeChoice();
+  updateColorAndAlpha();
+}
+
+PoseDisplay::~PoseDisplay() = default;
+
+void PoseDisplay::onEnable()
+{
+  MFDClass::onEnable();
+  updateShapeVisibility();
+  setupSelectionHandler();
+}
+
+void PoseDisplay::setupSelectionHandler()
+{
+  coll_handler_ = rviz_common::interaction::createSelectionHandler
+    <PoseDisplaySelectionHandler>(this, context_);
+  coll_handler_->addTrackedObjects(arrow_->getSceneNode());
+  coll_handler_->addTrackedObjects(axes_->getSceneNode());
+}
+
+void PoseDisplay::onDisable()
+{
+  MFDClass::onDisable();
+  coll_handler_.reset();
+}
+
+void PoseDisplay::updateColorAndAlpha()
+{
+  Ogre::ColourValue color = color_property_->getOgreColor();
+  color.a = alpha_property_->getFloat();
+
+  arrow_->setColor(color);
+
+  context_->queueRender();
+}
+
+void PoseDisplay::updateArrowGeometry()
+{
+  arrow_->set(
+    shaft_length_property_->getFloat(),
+    shaft_radius_property_->getFloat(),
+    head_length_property_->getFloat(),
+    head_radius_property_->getFloat());
+  context_->queueRender();
+}
+
+void PoseDisplay::updateAxisGeometry()
+{
+  axes_->set(
+    axes_length_property_->getFloat(),
+    axes_radius_property_->getFloat());
+  context_->queueRender();
+}
+
+void PoseDisplay::updateShapeChoice()
+{
+  bool use_arrow = (shape_property_->getOptionInt() == Arrow);
+
+  color_property_->setHidden(!use_arrow);
+  alpha_property_->setHidden(!use_arrow);
+  shaft_length_property_->setHidden(!use_arrow);
+  shaft_radius_property_->setHidden(!use_arrow);
+  head_length_property_->setHidden(!use_arrow);
+  head_radius_property_->setHidden(!use_arrow);
+
+  axes_length_property_->setHidden(use_arrow);
+  axes_radius_property_->setHidden(use_arrow);
+
+  updateShapeVisibility();
+
+  context_->queueRender();
+}
+
+void PoseDisplay::updateShapeVisibility()
+{
+  if (!pose_valid_) {
+    arrow_->getSceneNode()->setVisible(false);
+    axes_->getSceneNode()->setVisible(false);
+  } else {
+    bool use_arrow = (shape_property_->getOptionInt() == Arrow);
+    arrow_->getSceneNode()->setVisible(use_arrow);
+    axes_->getSceneNode()->setVisible(!use_arrow);
+  }
+}
+
+void PoseDisplay::processMessage(geometry_msgs::msg::PoseStamped::ConstSharedPtr message)
+{
+  if (!rviz_common::validateFloats(*message)) {
+    setStatus(
+      rviz_common::properties::StatusProperty::Error, "Topic",
+      "Message contained invalid floating point values (nans or infs)");
+    return;
+  }
+
+  Ogre::Vector3 position;
+  Ogre::Quaternion orientation;
+  if (
+    !context_->getFrameManager()->transform(
+      message->header, message->pose, position, orientation))
+  {
+    setMissingTransformToFixedFrame(message->header.frame_id);
+    return;
+  }
+  setTransformOk();
+
+  pose_valid_ = true;
+  updateShapeVisibility();
+
+  scene_node_->setPosition(position);
+  scene_node_->setOrientation(orientation);
+
+  coll_handler_->setMessage(message);
+
+  context_->queueRender();
+}
+
+void PoseDisplay::reset()
+{
+  MFDClass::reset();
+  pose_valid_ = false;
+  updateShapeVisibility();
+}
+
+}  // namespace displays
+}  // namespace tuw_graph_rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>  // NOLINT
+PLUGINLIB_EXPORT_CLASS(tuw_graph_rviz_plugins::displays::PoseDisplay, rviz_common::Display)
\ No newline at end of file
diff --git a/tuw_graph_rviz_plugins/src/graph_display_selection_handler.cpp b/tuw_graph_rviz_plugins/src/graph_display_selection_handler.cpp
new file mode 100644
index 0000000..e839a56
--- /dev/null
+++ b/tuw_graph_rviz_plugins/src/graph_display_selection_handler.cpp
@@ -0,0 +1,126 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2018, Bosch Software Innovations GmbH.
+ * All rights reserved.
+ *
+ * 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 the Willow Garage, Inc. 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.
+ */
+
+#include "tuw_graph_rviz_plugins/graph_display_selection_handler.hpp"
+
+#include <OgreEntity.h>
+
+#include "rviz_rendering/objects/axes.hpp"
+#include "rviz_rendering/objects/arrow.hpp"
+#include "rviz_rendering/objects/shape.hpp"
+#include "rviz_common/interaction/selection_handler.hpp"
+#include "rviz_common/msg_conversions.hpp"
+#include "rviz_common/properties/vector_property.hpp"
+#include "rviz_common/properties/string_property.hpp"
+#include "rviz_common/properties/quaternion_property.hpp"
+#include "rviz_common/properties/enum_property.hpp"
+#include "rviz_common/display_context.hpp"
+#include "tuw_graph_rviz_plugins/graph_display.hpp"
+
+namespace tuw_graph_rviz_plugins
+{
+namespace displays
+{
+
+PoseDisplaySelectionHandler::PoseDisplaySelectionHandler(
+  PoseDisplay * display, rviz_common::DisplayContext * context)
+: SelectionHandler(context),
+  display_(display),
+  frame_property_(nullptr),
+  position_property_(nullptr),
+  orientation_property_(nullptr)
+{}
+
+void PoseDisplaySelectionHandler::createProperties(
+  const rviz_common::interaction::Picked & obj,
+  rviz_common::properties::Property * parent_property)
+{
+  (void) obj;
+  rviz_common::properties::Property * cat = new rviz_common::properties::Property(
+    "Pose " + display_->getName(), QVariant(), "", parent_property);
+  properties_.push_back(cat);
+
+  frame_property_ = new rviz_common::properties::StringProperty("Frame", "", "", cat);
+  frame_property_->setReadOnly(true);
+
+  position_property_ = new rviz_common::properties::VectorProperty(
+    "Position", Ogre::Vector3::ZERO, "", cat);
+  position_property_->setReadOnly(true);
+
+  orientation_property_ = new rviz_common::properties::QuaternionProperty(
+    "Orientation", Ogre::Quaternion::IDENTITY, "", cat);
+  orientation_property_->setReadOnly(true);
+}
+
+rviz_common::interaction::V_AABB PoseDisplaySelectionHandler::getAABBs(
+  const rviz_common::interaction::Picked & obj)
+{
+  (void) obj;
+  rviz_common::interaction::V_AABB aabbs;
+  if (display_->pose_valid_) {
+    /** with 'derive_world_bounding_box' set to 'true', the WorldBoundingBox is derived each time.
+        setting it to 'false' results in the wire box not properly following the pose arrow, but it
+        would be less computationally expensive.
+     */
+    bool derive_world_bounding_box = true;
+    if (display_->shape_property_->getOptionInt() == PoseDisplay::Arrow) {
+      aabbs.push_back(
+        display_->arrow_->getHead()->getEntity()->getWorldBoundingBox(derive_world_bounding_box));
+      aabbs.push_back(
+        display_->arrow_->getShaft()->getEntity()->getWorldBoundingBox(derive_world_bounding_box));
+    } else {
+      aabbs.push_back(
+        display_->axes_->getXShape().getEntity()->getWorldBoundingBox(derive_world_bounding_box));
+      aabbs.push_back(
+        display_->axes_->getYShape().getEntity()->getWorldBoundingBox(derive_world_bounding_box));
+      aabbs.push_back(
+        display_->axes_->getZShape().getEntity()->getWorldBoundingBox(derive_world_bounding_box));
+    }
+  }
+  return aabbs;
+}
+
+void PoseDisplaySelectionHandler::setMessage(
+  geometry_msgs::msg::PoseStamped::ConstSharedPtr message)
+{
+  // properties_.size() should only be > 0 after createProperties()
+  // and before destroyProperties(), during which frame_property_,
+  // position_property_, and orientation_property_ should be valid
+  // pointers.
+  if (properties_.size() > 0) {
+    frame_property_->setStdString(message->header.frame_id);
+    position_property_->setVector(rviz_common::pointMsgToOgre(message->pose.position));
+    orientation_property_->setQuaternion(
+      rviz_common::quaternionMsgToOgre(message->pose.orientation));
+  }
+}
+
+}  // namespace displays
+}  // namespace tuw_graph_rviz_plugins
\ No newline at end of file
diff --git a/tuw_pose_rviz_plugins/LICENSE b/tuw_pose_rviz_plugins/LICENSE
index 574ef07..0dc4926 100644
--- a/tuw_pose_rviz_plugins/LICENSE
+++ b/tuw_pose_rviz_plugins/LICENSE
@@ -1,3 +1,5 @@
+Copyright (c) 2023 Markus Bader <markus.bader@tuwien.ac.at>
+
 Redistribution and use in source and binary forms, with or without
 modification, are permitted provided that the following conditions are met: