support new image transport node interface

Author: berndpfrommer

CMakeLists.txt

@@ -37,12 +37,27 @@ find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 
 
+if(${image_transport_VERSION} VERSION_GREATER_EQUAL "3.1.0")
+  # Humble and later (formerly API_V2)
+  # add_definitions(-DIMAGE_TRANSPORT_API_V2)
+endif()
+
+if(${image_transport_VERSION} VERSION_GREATER_EQUAL "5.1.7")
+  # Jazzy and later (formerly API_V3)
+  add_definitions(-DIMAGE_TRANSPORT_USE_PUBLISHER_T)
+  add_definitions(-DIMAGE_TRANSPORT_NEEDS_PUBLISHEROPTIONS)
+  # add_definitions(-DIMAGE_TRANSPORT_API_V3)
+endif()
+
 if(${image_transport_VERSION} VERSION_GREATER_EQUAL "6.0.0")
   add_definitions(-DIMAGE_TRANSPORT_RESOLVES_BASE_TOPIC)
 endif()
 if(${image_transport_VERSION} VERSION_GREATER_EQUAL "6.3.0")
   add_definitions(-DIMAGE_TRANSPORT_USE_QOS)
 endif()
+if(${image_transport_VERSION} VERSION_GREATER_EQUAL "6.4.0")
+  add_definitions(-DIMAGE_TRANSPORT_USE_NODEINTERFACE)
+endif()
 
 set(LIBRARY_NAME ${PROJECT_NAME}_component)
 

include/ffmpeg_image_transport/ffmpeg_publisher.hpp

@@ -32,28 +32,33 @@ using FFMPEGPacketConstPtr = FFMPEGPacket::ConstSharedPtr;
 class FFMPEGPublisher : public FFMPEGPublisherPlugin
 {
 public:
-#if defined(IMAGE_TRANSPORT_API_V1) || defined(IMAGE_TRANSPORT_API_V2)
-  using PublisherTFn = PublishFn;
-#else
+#ifdef IMAGE_TRANSPORT_USE_PUBLISHER_T
   using PublisherTFn = PublisherT;
+#else
+  using PublisherTFn = PublishFn;
 #endif
 #ifdef IMAGE_TRANSPORT_USE_QOS
   using QoSType = rclcpp::QoS;
 #else
   using QoSType = rmw_qos_profile_t;
 #endif
+#ifdef IMAGE_TRANSPORT_USE_NODEINTERFACE
+  using NodeType = image_transport::RequiredInterfaces;
+#else
+  using NodeType = rclcpp::Node *;
+#endif
+
   FFMPEGPublisher();
   ~FFMPEGPublisher() override;
   std::string getTransportName() const override { return "ffmpeg"; }
 
 protected:
-#if defined(IMAGE_TRANSPORT_API_V1) || defined(IMAGE_TRANSPORT_API_V2)
+#ifdef IMAGE_TRANSPORT_NEEDS_PUBLISHEROPTIONS
   void advertiseImpl(
-    rclcpp::Node * node, const std::string & base_topic, QoSType custom_qos) override;
-#else
-  void advertiseImpl(
-    rclcpp::Node * node, const std::string & base_topic, QoSType custom_qos,
+    NodeType node, const std::string & base_topic, QoSType custom_qos,
     rclcpp::PublisherOptions opt) override;
+#else
+  void advertiseImpl(NodeType node, const std::string & base_topic, QoSType custom_qos) override;
 #endif
   void publish(const Image & message, const PublisherTFn & publisher) const override;
   void shutdown() override;
@@ -63,8 +68,8 @@ class FFMPEGPublisher : public FFMPEGPublisherPlugin
     const std::string & frame_id, const rclcpp::Time & stamp, const std::string & codec,
     uint32_t width, uint32_t height, uint64_t pts, uint8_t flags, uint8_t * data, size_t sz);
 
-  QoSType initialize(rclcpp::Node * node, const std::string & base_name, QoSType custom_qos);
-  void declareParameter(rclcpp::Node * node, const ParameterDefinition & definition);
+  QoSType initialize(NodeType node, const std::string & base_name, QoSType custom_qos);
+  void declareParameter(NodeType node, const ParameterDefinition & definition);
   void handleAVOptions(const std::string & opt);
   // variables ---------
   rclcpp::Logger logger_;

include/ffmpeg_image_transport/ffmpeg_subscriber.hpp

@@ -24,20 +24,25 @@
 
 namespace ffmpeg_image_transport
 {
-using Image = sensor_msgs::msg::Image;
-using ImageConstPtr = Image::ConstSharedPtr;
-using ffmpeg_image_transport_msgs::msg::FFMPEGPacket;
-using FFMPEGPacketConstPtr = FFMPEGPacket::ConstSharedPtr;
 using FFMPEGSubscriberPlugin = image_transport::SimpleSubscriberPlugin<FFMPEGPacket>;
+class FFMPEGSubscriber : public FFMPEGSubscriberPlugin
+{
+public:
+  using Image = sensor_msgs::msg::Image;
+  using ImageConstPtr = Image::ConstSharedPtr;
+  using FFMPEGPacket = ffmpeg_image_transport_msgs::msg::FFMPEGPacket;
+  using FFMPEGPacketConstPtr = FFMPEGPacket::ConstSharedPtr;
 #ifdef IMAGE_TRANSPORT_USE_QOS
-using QoSType = rclcpp::QoS;
+  using QoSType = rclcpp::QoS;
 #else
-using QoSType = rmw_qos_profile_t;
+  using QoSType = rmw_qos_profile_t;
+#endif
+#ifdef IMAGE_TRANSPORT_USE_NODEINTERFACE
+  using NodeType = image_transport::RequiredInterfaces;
+#else
+  using NodeType = rclcpp::Node *;
 #endif
 
-class FFMPEGSubscriber : public FFMPEGSubscriberPlugin
-{
-public:
   FFMPEGSubscriber();
   ~FFMPEGSubscriber() override;
 
@@ -46,26 +51,22 @@ class FFMPEGSubscriber : public FFMPEGSubscriberPlugin
 protected:
   void internalCallback(const FFMPEGPacketConstPtr & msg, const Callback & user_cb) override;
 
-#ifdef IMAGE_TRANSPORT_API_V1
-  void subscribeImpl(
-    rclcpp::Node * node, const std::string & base_topic, const Callback & callback,
-    QoSType custom_qos) override;
-#else
   void subscribeImpl(
-    rclcpp::Node * node, const std::string & base_topic, const Callback & callback,
-    QoSType custom_qos, rclcpp::SubscriptionOptions) override;
-#endif
+    NodeType node, const std::string & base_topic, const Callback & callback, QoSType custom_qos,
+    rclcpp::SubscriptionOptions) override;
+
   void shutdown() override;
+  rclcpp::Logger logger_;
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_param_interface_;
 
 private:
   void frameReady(const ImageConstPtr & img, bool /*isKeyFrame*/) const;
-  void initialize(rclcpp::Node * node, const std::string & base_topic);
+  void initialize(NodeType node, const std::string & base_topic);
   std::string getDecodersFromMap(const std::string & encoding);
-  void declareParameter(rclcpp::Node * node, const ParameterDefinition & definition);
+  void declareParameter(NodeType node, const ParameterDefinition & definition);
   void handleAVOptions(const std::string & opt);
   // -------------- variables
-  rclcpp::Logger logger_;
-  rclcpp::Node * node_{nullptr};
+  NodeType node_;
   ffmpeg_encoder_decoder::Decoder decoder_;
   std::string decoderType_;
   const Callback * userCallback_{nullptr};

include/ffmpeg_image_transport/parameter_definition.hpp

@@ -16,6 +16,7 @@
 #ifndef FFMPEG_IMAGE_TRANSPORT__PARAMETER_DEFINITION_HPP_
 #define FFMPEG_IMAGE_TRANSPORT__PARAMETER_DEFINITION_HPP_
 
+#include <image_transport/image_transport.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 namespace ffmpeg_image_transport
@@ -24,8 +25,13 @@ struct ParameterDefinition
 {
   using ParameterDescriptor = rcl_interfaces::msg::ParameterDescriptor;
   using ParameterValue = rclcpp::ParameterValue;
+#ifdef IMAGE_TRANSPORT_USE_NODEINTERFACE
+  using NodeType = image_transport::RequiredInterfaces;
+#else
+  using NodeType = rclcpp::Node *;
+#endif
 
-  rclcpp::ParameterValue declare(rclcpp::Node * node, const std::string & paramBase) const;
+  rclcpp::ParameterValue declare(NodeType node, const std::string & paramBase) const;
   // ---- variables
   ParameterValue defaultValue;
   ParameterDescriptor descriptor;

src/ffmpeg_publisher.cpp

@@ -101,7 +101,7 @@ void FFMPEGPublisher::shutdown()
 
 // This code was lifted from compressed_image_transport
 
-void FFMPEGPublisher::declareParameter(rclcpp::Node * node, const ParameterDefinition & definition)
+void FFMPEGPublisher::declareParameter(NodeType node, const ParameterDefinition & definition)
 {
   // transport scoped parameter (e.g. image_raw.compressed.format)
   const auto v = definition.declare(node, paramNamespace_);
@@ -163,31 +163,34 @@ void FFMPEGPublisher::packetReady(
 #endif
 }
 
-#if defined(IMAGE_TRANSPORT_API_V1) || defined(IMAGE_TRANSPORT_API_V2)
+#ifdef IMAGE_TRANSPORT_NEEDS_PUBLISHEROPTIONS
 void FFMPEGPublisher::advertiseImpl(
-  rclcpp::Node * node, const std::string & base_topic, rmw_qos_profile_t custom_qos)
+  NodeType node, const std::string & base_topic, QoSType custom_qos, rclcpp::PublisherOptions opt)
 {
   auto qos = initialize(node, base_topic, custom_qos);
-  FFMPEGPublisherPlugin::advertiseImpl(node, base_topic, qos);
+  FFMPEGPublisherPlugin::advertiseImpl(node, base_topic, qos, opt);
 }
 #else
 void FFMPEGPublisher::advertiseImpl(
-  rclcpp::Node * node, const std::string & base_topic, QoSType custom_qos,
-  rclcpp::PublisherOptions opt)
+  NodeType node, const std::string & base_topic, rmw_qos_profile_t custom_qos)
 {
   auto qos = initialize(node, base_topic, custom_qos);
-  FFMPEGPublisherPlugin::advertiseImpl(node, base_topic, qos, opt);
+  FFMPEGPublisherPlugin::advertiseImpl(node, base_topic, qos);
 }
 #endif
 
 FFMPEGPublisher::QoSType FFMPEGPublisher::initialize(
-  rclcpp::Node * node, const std::string & base_topic, QoSType custom_qos)
+  NodeType node, const std::string & base_topic, QoSType custom_qos)
 {
   // namespace handling code lifted from compressed_image_transport
+#ifdef IMAGE_TRANSPORT_USE_NODEINTERFACE
+  uint ns_prefix_len = std::string(node.get_node_base_interface()->get_namespace()).length();
+#else
   uint ns_len = node->get_effective_namespace().length();
   // if a namespace is given (ns_len > 1), then strip one more
   // character to avoid a leading "/" that will then become a "."
   uint ns_prefix_len = ns_len > 1 ? ns_len + 1 : ns_len;
+#endif
   std::string param_base_name = base_topic.substr(ns_prefix_len);
   std::replace(param_base_name.begin(), param_base_name.end(), '/', '.');
   paramNamespace_ = param_base_name + "." + getTransportName() + ".";

src/ffmpeg_subscriber.cpp

@@ -40,7 +40,12 @@ static const ParameterDefinition params[] = {
                     .set__description("enable performance timing")
                     .set__read_only(false)}};
 
-FFMPEGSubscriber::FFMPEGSubscriber() : logger_(rclcpp::get_logger("FFMPEGSubscriber")) {}
+FFMPEGSubscriber::FFMPEGSubscriber() : logger_(rclcpp::get_logger("FFMPEGSubscriber"))
+{
+#ifndef IMAGE_TRANSPORT_USE_NODEINTERFACE
+  node_ = nullptr;
+#endif
+}
 
 FFMPEGSubscriber::~FFMPEGSubscriber() { decoder_.reset(); }
 
@@ -56,29 +61,20 @@ void FFMPEGSubscriber::shutdown()
 
 void FFMPEGSubscriber::frameReady(const ImageConstPtr & img, bool) const { (*userCallback_)(img); }
 
-#ifdef IMAGE_TRANSPORT_API_V1
 void FFMPEGSubscriber::subscribeImpl(
-  rclcpp::Node * node, const std::string & base_topic, const Callback & callback,
-  rmw_qos_profile_t custom_qos)
+  NodeType node, const std::string & base_topic, const Callback & callback, QoSType custom_qos,
+  rclcpp::SubscriptionOptions opt)
 {
   initialize(node, base_topic);
-  FFMPEGSubscriberPlugin::subscribeImpl(node, base_topic, callback, custom_qos);
-}
+#ifdef IMAGE_TRANSPORT_NEEDS_PUBLISHEROPTIONS
+  FFMPEGSubscriberPlugin::subscribeImpl(node, base_topic, callback, custom_qos, opt);
 #else
-void FFMPEGSubscriber::subscribeImpl(
-  rclcpp::Node * node, const std::string & base_topic, const Callback & callback,
-  QoSType custom_qos, rclcpp::SubscriptionOptions opt)
-{
-  initialize(node, base_topic);
-#ifdef IMAGE_TRANSPORT_API_V2
   (void)opt;  // to suppress compiler warning
   FFMPEGSubscriberPlugin::subscribeImpl(node, base_topic, callback, custom_qos);
-#else
-  FFMPEGSubscriberPlugin::subscribeImpl(node, base_topic, callback, custom_qos, opt);
 #endif
 }
-#endif
-void FFMPEGSubscriber::initialize(rclcpp::Node * node, const std::string & base_topic_o)
+
+void FFMPEGSubscriber::initialize(NodeType node, const std::string & base_topic_o)
 {
   node_ = node;
 #ifdef IMAGE_TRANSPORT_RESOLVES_BASE_TOPIC
@@ -87,10 +83,15 @@ void FFMPEGSubscriber::initialize(rclcpp::Node * node, const std::string & base_
   const std::string base_topic =
     node_->get_node_topics_interface()->resolve_topic_name(base_topic_o);
 #endif
-  uint ns_len = node_->get_effective_namespace().length();
+
+#ifdef IMAGE_TRANSPORT_USE_NODEINTERFACE
+  uint ns_prefix_len = std::string(node.get_node_base_interface()->get_namespace()).length();
+#else
+  uint ns_len = node->get_effective_namespace().length();
   // if a namespace is given (ns_len > 1), then strip one more
   // character to avoid a leading "/" that will then become a "."
   uint ns_prefix_len = ns_len > 1 ? ns_len + 1 : ns_len;
+#endif
   std::string param_base_name = base_topic.substr(ns_prefix_len);
   std::replace(param_base_name.begin(), param_base_name.end(), '/', '.');
   paramNamespace_ = param_base_name + "." + getTransportName() + ".";
@@ -100,7 +101,7 @@ void FFMPEGSubscriber::initialize(rclcpp::Node * node, const std::string & base_
   }
 }
 
-void FFMPEGSubscriber::declareParameter(rclcpp::Node * node, const ParameterDefinition & definition)
+void FFMPEGSubscriber::declareParameter(NodeType node, const ParameterDefinition & definition)
 {
   const auto v = definition.declare(node, paramNamespace_);
   const auto & n = definition.descriptor.name;

src/parameter_definition.cpp

@@ -18,17 +18,29 @@
 namespace ffmpeg_image_transport
 {
 rclcpp::ParameterValue ParameterDefinition::declare(
-  rclcpp::Node * node, const std::string & paramBase) const
+  NodeType node, const std::string & paramBase) const
 {
   // transport scoped parameter (e.g. image_raw.compressed.format)
   const std::string paramName = paramBase + descriptor.name;
   rclcpp::ParameterValue v;
+#ifdef IMAGE_TRANSPORT_USE_NODEINTERFACE
+  try {
+    v =
+      node.get_node_parameters_interface()->declare_parameter(paramName, defaultValue, descriptor);
+  } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) {
+    RCLCPP_DEBUG(
+      node.get_node_logging_interface()->get_logger(), "%s was previously declared",
+      descriptor.name.c_str());
+    v = node.get_node_parameters_interface()->get_parameter(paramName).get_parameter_value();
+  }
+#else
   try {
     v = node->declare_parameter(paramName, defaultValue, descriptor);
   } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) {
     RCLCPP_DEBUG(node->get_logger(), "%s was previously declared", descriptor.name.c_str());
     v = node->get_parameter(paramName).get_parameter_value();
   }
+#endif
   return (v);
 }
 }  // namespace ffmpeg_image_transport

test/test_1.cpp

@@ -128,9 +128,14 @@ class TestSubscriber : public rclcpp::Node
   }
   void initialize()
   {
-    image_transport::TransportHints hints(this);
+    // image_transport::TransportHints hints(this->get_node_interface());
     sub_ = std::make_shared<image_transport::Subscriber>(image_transport::create_subscription(
-      this, "camera/image", std::bind(&TestSubscriber::imageCallback, this, std::placeholders::_1),
+#ifdef IMAGE_TRANSPORT_USE_NODEINTERFACE
+      *this,
+#else
+      this,
+#endif
+      "camera/image", std::bind(&TestSubscriber::imageCallback, this, std::placeholders::_1),
       "ffmpeg", convert_profile(rmw_qos_profile_default)));
   }
   void shutDown()