feat(tier4_debug_rviz_plugin): pie chart with float32 multi array sta…

…mped (autowarefoundation#2055) * feat(tier4_debug_rviz_plugin): add ros2 pie chart with Float32MultiArrayStamped Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update README.md Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix typo Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fixed license Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * removed unnecessary include Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: scepter914 <scepter914@gmail.com>
scepter914 · Nov 1, 2022 · 05fc5b4 · 05fc5b4
1 parent 859e148
commit 05fc5b4
Show file tree

Hide file tree

Showing 10 changed files with 1,078 additions and 0 deletions.
diff --git a/common/tier4_debug_rviz_plugin/CMakeLists.txt 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
@@ -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
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
diff --git a/...bug_rviz_plugin/include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp b/...bug_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
@@ -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
@@ -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>