combine image_transport_display and image_display

ros2 · Nov 20, 2024 · 4f5f147 · 4f5f147
1 parent 2dc6b05
commit 4f5f147
Show file tree

Hide file tree

Showing 5 changed files with 139 additions and 277 deletions.
diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/camera/camera_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/camera/camera_display.hpp
@@ -52,7 +52,7 @@
 # include "sensor_msgs/msg/camera_info.hpp"
 # include "tf2_ros/message_filter.h"
 
-# include "rviz_default_plugins/displays/image/image_transport_display.hpp"
+# include "rviz_default_plugins/displays/image/image_display.hpp"
 # include "rviz_default_plugins/displays/image/ros_image_texture_iface.hpp"
 # include "rviz_default_plugins/visibility_control.hpp"
 # include "rviz_rendering/render_window.hpp"
@@ -101,7 +101,7 @@ struct ImageDimensions
  *
  */
 class RVIZ_DEFAULT_PLUGINS_PUBLIC CameraDisplay
-  : public rviz_default_plugins::displays::ImageTransportDisplay<sensor_msgs::msg::Image>,
+  : public ImageDisplay,
   public Ogre::RenderTargetListener
 {
   Q_OBJECT

diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_display.hpp
@@ -38,9 +38,11 @@
 #include <OgreSharedPtr.h>
 
 #include <QObject>  // NOLINT cpplint cannot handle include order here
+#include <QString>  // NOLINT cpplint cannot handle include order here
 
 #include <memory>
 #include <string>
+#include <unordered_map>
 
 #include <sensor_msgs/msg/image.hpp>
 
@@ -50,9 +52,12 @@
 #include "rviz_common/properties/float_property.hpp"
 #include "rviz_common/properties/int_property.hpp"
 #include "rviz_common/render_panel.hpp"
-#include "rviz_default_plugins/displays/image/image_transport_display.hpp"
 #include "rviz_default_plugins/displays/image/ros_image_texture_iface.hpp"
 #include "rviz_default_plugins/visibility_control.hpp"
+#include "get_transport_from_topic.hpp"
+#include "image_transport/image_transport.hpp"
+#include "image_transport/subscriber_filter.hpp"
+#include "rviz_common/ros_topic_display.hpp"
 #endif
 
 namespace Ogre
@@ -70,8 +75,7 @@ namespace displays
  * \class ImageDisplay
  *
  */
-class RVIZ_DEFAULT_PLUGINS_PUBLIC ImageDisplay
-  : public rviz_default_plugins::displays::ImageTransportDisplay<sensor_msgs::msg::Image>
+class RVIZ_DEFAULT_PLUGINS_PUBLIC ImageDisplay : public rviz_common::_RosTopicDisplay
 {
   Q_OBJECT
 
@@ -81,26 +85,41 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC ImageDisplay
   ~ImageDisplay() override;
 
   void onInitialize() override;
-  void update(float wall_dt, float ros_dt) override;
+  void update(float wall_dt, float ros_dt);
   void reset() override;
 
 public Q_SLOTS:
   virtual void updateNormalizeOptions();
 
 protected Q_SLOTS:
-  void subscribe() override;
+  virtual void subscribe();
 
 protected:
   void onEnable() override;
   void onDisable() override;
-  void unsubscribe() override;
-
+  virtual void unsubscribe();
+  void updateTopic() override;
+  void transformerChangedCallback() override;
+  void resetSubscription();
   void incomingMessage(const sensor_msgs::msg::Image::ConstSharedPtr & img_msg);
+  void setTopic(const QString & topic, const QString & datatype) override;
 
   /* This is called by incomingMessage(). */
-  void processMessage(sensor_msgs::msg::Image::ConstSharedPtr msg) override;
-
-  image_transport::Subscriber subscription_;
+  virtual void processMessage(sensor_msgs::msg::Image::ConstSharedPtr msg);
+
+  std::shared_ptr<image_transport::SubscriberFilter> subscription_;
+  uint32_t messages_received_;
+  rclcpp::Time subscription_start_time_;
+  message_filters::Connection subscription_callback_;
+  const std::unordered_map<std::string, std::string> transport_message_types_ = {
+    /* *INDENT-OFF* */
+    {"raw",             "sensor_msgs/msg/Image"},
+    {"compressed",      "sensor_msgs/msg/CompressedImage"},
+    {"compressedDepth", "sensor_msgs/msg/CompressedImage"},
+    {"theora",          "theora_image_transport/msg/Packet"},
+    {"zstd",            "sensor_msgs/msg/CompressedImage"},
+    /* *INDENT-ON* */
+  };
 
 private:
   void setupScreenRectangle();

diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_transport_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_transport_display.hpp
diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp
@@ -165,7 +165,7 @@ CameraDisplay::~CameraDisplay()
 
 void CameraDisplay::onInitialize()
 {
-  ITDClass::onInitialize();
+  ImageDisplay::onInitialize();
 
   setupSceneNodes();
   setupRenderPanel();
@@ -309,7 +309,7 @@ void CameraDisplay::fixedFrameChanged()
 
 void CameraDisplay::subscribe()
 {
-  ITDClass::subscribe();
+  ImageDisplay::subscribe();
 
   if (!subscription_) {
     return;
@@ -378,7 +378,7 @@ void CameraDisplay::createCameraInfoSubscription()
 
 void CameraDisplay::unsubscribe()
 {
-  ITDClass::unsubscribe();
+  ImageDisplay::unsubscribe();
   caminfo_sub_.reset();
   tf_filter_.reset();
 }
@@ -669,7 +669,7 @@ void CameraDisplay::processMessage(sensor_msgs::msg::Image::ConstSharedPtr msg)
 
 void CameraDisplay::reset()
 {
-  ITDClass::reset();
+  ImageDisplay::reset();
   clear();
 }