rviz plugin added

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.

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()

tuw_graph_rviz_plugins/LICENSE

@@ -0,0 +1,27 @@
+Copyright (c) 2023 Markus Bader <[email protected]>
+
+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.

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_

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_