Feat/Add LifecycleNode Support

point_cloud_transport/CMakeLists.txt

@@ -163,4 +163,4 @@ if(BUILD_TESTING)
   endif()
 endif()
 
-ament_package()
+ament_package()

point_cloud_transport/include/point_cloud_transport/point_cloud_transport.hpp

@@ -103,13 +103,32 @@ class PointCloudTransportLoader
 /// \param custom_qos The QoS profile to use for the underlying publisher(s)
 /// \param options The publisher options to use for the underlying publisher(s)
 /// \return The advertised publisher
+[[deprecated("Use create_publisher(rclcpp::node_interfaces...) instead")]]
 POINT_CLOUD_TRANSPORT_PUBLIC
 Publisher create_publisher(
   std::shared_ptr<rclcpp::Node> node,
   const std::string & base_topic,
   rmw_qos_profile_t custom_qos = rmw_qos_profile_default,
   const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions());
 
+/// \brief Advertise every available transport on pointcloud topics, free function version.
+/// \param node_interfaces the ROS node interfaces required for core node functionality, including
+///    NodeBaseInterface, NodeParametersInterface, NodeTopicsInterface, and NodeLoggingInterface.
+/// \param base_topic The base topic for the publisher
+/// \param custom_qos The QoS profile to use for the underlying publisher(s)
+/// \param options The publisher options to use for the underlying publisher(s)
+/// \return The advertised publisher
+POINT_CLOUD_TRANSPORT_PUBLIC
+Publisher create_publisher(
+  std::shared_ptr<rclcpp::node_interfaces::NodeInterfaces<
+    rclcpp::node_interfaces::NodeBaseInterface,
+    rclcpp::node_interfaces::NodeParametersInterface,
+    rclcpp::node_interfaces::NodeTopicsInterface,
+    rclcpp::node_interfaces::NodeLoggingInterface>> & node_interfaces,
+  const std::string & base_topic,
+  rmw_qos_profile_t custom_qos = rmw_qos_profile_default,
+  const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions());
+
 /// \brief Subscribe to a pointcloud transport topic, free function version.
 /// \param node The ROS node to use for any ROS operations
 /// \param base_topic The base topic for the sbuscription
@@ -118,6 +137,7 @@ Publisher create_publisher(
 /// \param custom_qos The QoS profile to use for the underlying publisher
 /// \param options The publisher options to use for the underlying publisher
 /// \return The subscriber
+[[deprecated("Use create_subscription(rclcpp::node_interfaces...) instead")]]
 POINT_CLOUD_TRANSPORT_PUBLIC
 Subscriber create_subscription(
   std::shared_ptr<rclcpp::Node> node,
@@ -127,15 +147,46 @@ Subscriber create_subscription(
   rmw_qos_profile_t custom_qos = rmw_qos_profile_default,
   rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions());
 
+/// \brief Subscribe to a pointcloud transport topic, free function version.
+/// \param node_interfaces the ROS node interfaces required for core node functionality, including
+///    NodeBaseInterface, NodeParametersInterface, NodeTopicsInterface, and NodeLoggingInterface.
+/// \param base_topic The base topic for the sbuscription
+/// \param callback The callback to invoke on receipt of a message
+/// \param transport The transport to use for the subscription
+/// \param custom_qos The QoS profile to use for the underlying publisher
+/// \param options The publisher options to use for the underlying publisher
+/// \return The subscriber
+POINT_CLOUD_TRANSPORT_PUBLIC
+Subscriber create_subscription(
+  std::shared_ptr<rclcpp::node_interfaces::NodeInterfaces<
+    rclcpp::node_interfaces::NodeBaseInterface,
+    rclcpp::node_interfaces::NodeParametersInterface,
+    rclcpp::node_interfaces::NodeTopicsInterface,
+    rclcpp::node_interfaces::NodeLoggingInterface>> & node_interfaces,
+  const std::string & base_topic,
+  const Subscriber::Callback & callback,
+  const std::string & transport,
+  rmw_qos_profile_t custom_qos = rmw_qos_profile_default,
+  rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions());
+
 class PointCloudTransport : public PointCloudTransportLoader
 {
   using VoidPtr = std::shared_ptr<void>;
 
 public:
   //! Constructor
+  [[deprecated("Use PointCloudTransport(rclcpp::node_interfaces...) instead")]]
   POINT_CLOUD_TRANSPORT_PUBLIC
   explicit PointCloudTransport(rclcpp::Node::SharedPtr node);
 
+  POINT_CLOUD_TRANSPORT_PUBLIC
+  explicit PointCloudTransport(
+    std::shared_ptr<rclcpp::node_interfaces::NodeInterfaces<
+      rclcpp::node_interfaces::NodeBaseInterface,
+      rclcpp::node_interfaces::NodeParametersInterface,
+      rclcpp::node_interfaces::NodeTopicsInterface,
+      rclcpp::node_interfaces::NodeLoggingInterface>> node_interfaces);
+
   POINT_CLOUD_TRANSPORT_PUBLIC
   ~PointCloudTransport() override = default;
 
@@ -144,7 +195,9 @@ class PointCloudTransport : public PointCloudTransportLoader
   {
     std::string ret;
     if (nullptr == transport_hints) {
-      TransportHints th(node_);
+      auto ni_param = node_interfaces_->get_node_parameters_interface();
+      TransportHints th(std::make_shared<rclcpp::node_interfaces::NodeInterfaces<
+          rclcpp::node_interfaces::NodeParametersInterface>>(ni_param));
       ret = th.getTransport();
     } else {
       ret = transport_hints->getTransport();
@@ -161,7 +214,7 @@ class PointCloudTransport : public PointCloudTransportLoader
     rclcpp::PublisherOptions options = rclcpp::PublisherOptions();
     rmw_qos_profile_t custom_qos = rmw_qos_profile_sensor_data;
     custom_qos.depth = queue_size;
-    return Publisher(node_, base_topic, pub_loader_, custom_qos, options);
+    return Publisher(node_interfaces_, base_topic, pub_loader_, custom_qos, options);
   }
 
   //! Advertise a PointCloud2 topic, simple version.
@@ -173,7 +226,7 @@ class PointCloudTransport : public PointCloudTransportLoader
   {
     rmw_qos_profile_t custom_qos = rmw_qos_profile_sensor_data;
     custom_qos.depth = queue_size;
-    return Publisher(node_, base_topic, pub_loader_, custom_qos, options);
+    return Publisher(node_interfaces_, base_topic, pub_loader_, custom_qos, options);
   }
 
   POINT_CLOUD_TRANSPORT_PUBLIC
@@ -182,7 +235,7 @@ class PointCloudTransport : public PointCloudTransportLoader
     rmw_qos_profile_t custom_qos,
     const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions())
   {
-    return Publisher(node_, base_topic, pub_loader_, custom_qos, options);
+    return Publisher(node_interfaces_, base_topic, pub_loader_, custom_qos, options);
   }
 
   // //! Subscribe to a point cloud topic, version for arbitrary std::function object.
@@ -216,7 +269,7 @@ class PointCloudTransport : public PointCloudTransportLoader
   {
     (void)tracked_object;
     return Subscriber(
-      node_, base_topic, callback, sub_loader_,
+      node_interfaces_, base_topic, callback, sub_loader_,
       getTransportOrDefault(transport_hints), custom_qos, options);
   }
 
@@ -318,7 +371,11 @@ class PointCloudTransport : public PointCloudTransportLoader
   }
 
 private:
-  rclcpp::Node::SharedPtr node_;
+  std::shared_ptr<rclcpp::node_interfaces::NodeInterfaces<
+      rclcpp::node_interfaces::NodeBaseInterface,
+      rclcpp::node_interfaces::NodeParametersInterface,
+      rclcpp::node_interfaces::NodeTopicsInterface,
+      rclcpp::node_interfaces::NodeLoggingInterface>> node_interfaces_;
 };
 
 }  // namespace point_cloud_transport

point_cloud_transport/include/point_cloud_transport/publisher.hpp

@@ -37,6 +37,7 @@
 
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node.hpp"
+#include "rclcpp/node_interfaces/node_interfaces.hpp"
 
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
@@ -55,12 +56,33 @@ class Publisher
   POINT_CLOUD_TRANSPORT_PUBLIC
   Publisher() = default;
 
+  [[deprecated("Use Publisher(rclcpp::node_interfaces...) instead")]]
   POINT_CLOUD_TRANSPORT_PUBLIC
   Publisher(
     std::shared_ptr<rclcpp::Node> node,
     const std::string & base_topic,
     PubLoaderPtr loader,
     rmw_qos_profile_t custom_qos,
+    const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions())
+  : Publisher(
+      std::make_shared<rclcpp::node_interfaces::NodeInterfaces<
+        rclcpp::node_interfaces::NodeBaseInterface,
+        rclcpp::node_interfaces::NodeParametersInterface,
+        rclcpp::node_interfaces::NodeTopicsInterface,
+        rclcpp::node_interfaces::NodeLoggingInterface>>(*node),
+      base_topic, loader, custom_qos, options)
+  {}
+
+  POINT_CLOUD_TRANSPORT_PUBLIC
+  Publisher(
+    std::shared_ptr<rclcpp::node_interfaces::NodeInterfaces<
+      rclcpp::node_interfaces::NodeBaseInterface,
+      rclcpp::node_interfaces::NodeParametersInterface,
+      rclcpp::node_interfaces::NodeTopicsInterface,
+      rclcpp::node_interfaces::NodeLoggingInterface>> node_interfaces,
+    const std::string & base_topic,
+    PubLoaderPtr loader,
+    rmw_qos_profile_t custom_qos,
     const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions());
 
   //! get total number of subscribers to all advertised topics.

point_cloud_transport/include/point_cloud_transport/publisher_plugin.hpp

@@ -37,6 +37,7 @@
 #include <vector>
 
 #include "rclcpp/node.hpp"
+#include "rclcpp/node_interfaces/node_interfaces.hpp"
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <rcpputils/tl_expected/expected.hpp>
 
@@ -65,13 +66,25 @@ class PublisherPlugin
   virtual std::string getTransportName() const = 0;
 
   //! \brief Advertise a topic, simple version.
+  [[deprecated("Use advertise(rclcpp::node_interfaces...) instead")]]
   POINT_CLOUD_TRANSPORT_PUBLIC
   void advertise(
     std::shared_ptr<rclcpp::Node> node,
     const std::string & base_topic,
     rmw_qos_profile_t custom_qos = rmw_qos_profile_default,
     const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions());
 
+  POINT_CLOUD_TRANSPORT_PUBLIC
+  void advertise(
+    std::shared_ptr<rclcpp::node_interfaces::NodeInterfaces<
+      rclcpp::node_interfaces::NodeBaseInterface,
+      rclcpp::node_interfaces::NodeParametersInterface,
+      rclcpp::node_interfaces::NodeTopicsInterface,
+      rclcpp::node_interfaces::NodeLoggingInterface>> node_interfaces,
+    const std::string & base_topic,
+    rmw_qos_profile_t custom_qos = rmw_qos_profile_default,
+    const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions());
+
   //! Returns the number of subscribers that are currently connected to this PublisherPlugin
   virtual uint32_t getNumSubscribers() const = 0;
 
@@ -110,9 +123,29 @@ class PublisherPlugin
 
 protected:
   //! Advertise a topic. Must be implemented by the subclass.
+  [[deprecated("Use advertiseImpl(rclcpp::node_interfaces...) instead")]]
   virtual void advertiseImpl(
     std::shared_ptr<rclcpp::Node> node, const std::string & base_topic,
     rmw_qos_profile_t custom_qos,
+    const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions())
+  {
+    advertiseImpl(
+      std::make_shared<rclcpp::node_interfaces::NodeInterfaces<
+        rclcpp::node_interfaces::NodeBaseInterface,
+        rclcpp::node_interfaces::NodeParametersInterface,
+        rclcpp::node_interfaces::NodeTopicsInterface,
+        rclcpp::node_interfaces::NodeLoggingInterface>>(*node),
+      base_topic, custom_qos, options);
+  }
+
+  virtual void advertiseImpl(
+    std::shared_ptr<rclcpp::node_interfaces::NodeInterfaces<
+      rclcpp::node_interfaces::NodeBaseInterface,
+      rclcpp::node_interfaces::NodeParametersInterface,
+      rclcpp::node_interfaces::NodeTopicsInterface,
+      rclcpp::node_interfaces::NodeLoggingInterface>> node_interfaces,
+    const std::string & base_topic,
+    rmw_qos_profile_t custom_qos = rmw_qos_profile_default,
     const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()) = 0;
 };
 

point_cloud_transport/include/point_cloud_transport/simple_publisher_plugin.hpp

@@ -36,6 +36,7 @@
 #include <functional>
 #include <memory>
 #include <string>
+#include <utility>
 #include <type_traits>
 #include <optional>
 
@@ -94,13 +95,19 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin
   bool getParam(const std::string & parameter_name, T & value) const
   {
     if (simple_impl_) {
-      uint ns_len = simple_impl_->node_->get_effective_namespace().length();
+      uint ns_len = strlen(simple_impl_->node_interfaces_
+          ->get_node_base_interface()->get_namespace());
       std::string param_base_name = getTopic().substr(ns_len);
       std::replace(param_base_name.begin(), param_base_name.end(), '/', '.');
 
       std::string param_name = param_base_name + "." + parameter_name;
-
-      return simple_impl_->node_->get_parameter(param_name, value);
+      rclcpp::Parameter param;
+      if (simple_impl_->node_interfaces_->get_node_parameters_interface()
+        ->get_parameter(param_name, param))
+      {
+        value = param.get_value<T>();
+        return true;
+      }
     }
     return false;
   }
@@ -113,7 +120,8 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin
   {
     if (simple_impl_) {
       // Declare Parameters
-      uint ns_len = simple_impl_->node_->get_effective_namespace().length();
+      uint ns_len = strlen(simple_impl_->node_interfaces_
+          ->get_node_base_interface()->get_namespace());
       std::string param_base_name = getTopic().substr(ns_len);
       std::replace(param_base_name.begin(), param_base_name.end(), '/', '.');
 
@@ -122,8 +130,14 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin
       rcl_interfaces::msg::ParameterDescriptor param_descriptor = parameter_descriptor;
       param_descriptor.name = param_name;
 
-      simple_impl_->node_->template declare_parameter<T>(
-        param_name, value, param_descriptor);
+      try {
+        simple_impl_->node_interfaces_->get_node_parameters_interface()
+        ->declare_parameter(param_name, rclcpp::ParameterValue(value));
+      } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) {
+        RCLCPP_DEBUG(
+          simple_impl_->logger_, "%s was previously declared",
+          param_descriptor.name.c_str());
+      }
       return true;
     }
     return false;
@@ -135,7 +149,8 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin
   {
     if (simple_impl_) {
       simple_impl_->on_set_parameters_callback_handle_ =
-        simple_impl_->node_->add_on_set_parameters_callback(param_change_callback);
+        simple_impl_->node_interfaces_->get_node_parameters_interface()
+        ->add_on_set_parameters_callback(param_change_callback);
     }
   }
 
@@ -203,16 +218,25 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin
   std::string base_topic_;
 
   void advertiseImpl(
-    std::shared_ptr<rclcpp::Node> node, const std::string & base_topic,
-    rmw_qos_profile_t custom_qos,
-    const rclcpp::PublisherOptions & options) override
+    std::shared_ptr<rclcpp::node_interfaces::NodeInterfaces<
+      rclcpp::node_interfaces::NodeBaseInterface,
+      rclcpp::node_interfaces::NodeParametersInterface,
+      rclcpp::node_interfaces::NodeTopicsInterface,
+      rclcpp::node_interfaces::NodeLoggingInterface>> node_interfaces,
+    const std::string & base_topic,
+    rmw_qos_profile_t custom_qos = rmw_qos_profile_default,
+    const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()) override
   {
     std::string transport_topic = getTopicToAdvertise(base_topic);
-    simple_impl_ = std::make_unique<SimplePublisherPluginImpl>(node);
+    simple_impl_ = std::make_unique<SimplePublisherPluginImpl>(node_interfaces);
 
-    RCLCPP_DEBUG(node->get_logger(), "getTopicToAdvertise: %s", transport_topic.c_str());
+    RCLCPP_DEBUG(simple_impl_->logger_, "getTopicToAdvertise: %s", transport_topic.c_str());
     auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos);
-    simple_impl_->pub_ = node->create_publisher<M>(transport_topic, qos, options);
+    auto node_parameters = node_interfaces->get_node_parameters_interface();
+    auto node_topics = node_interfaces->get_node_topics_interface();
+    // simple_impl_->pub_ = node->create_publisher<M>(transport_topic, qos, options);
+    simple_impl_->pub_ = rclcpp::create_publisher<M>(
+      node_parameters, node_topics, transport_topic, qos, options);
 
     base_topic_ = simple_impl_->pub_->get_topic_name();
 
@@ -277,13 +301,22 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin
 private:
   struct SimplePublisherPluginImpl
   {
-    explicit SimplePublisherPluginImpl(std::shared_ptr<rclcpp::Node> node)
-    : node_(node),
-      logger_(node->get_logger())
+    explicit SimplePublisherPluginImpl(
+      std::shared_ptr<rclcpp::node_interfaces::NodeInterfaces<
+        rclcpp::node_interfaces::NodeBaseInterface,
+        rclcpp::node_interfaces::NodeParametersInterface,
+        rclcpp::node_interfaces::NodeTopicsInterface,
+        rclcpp::node_interfaces::NodeLoggingInterface>> node_interfaces)
+    :node_interfaces_(std::move(node_interfaces)),
+      logger_(node_interfaces_->get_node_logging_interface()->get_logger())
     {
     }
 
-    std::shared_ptr<rclcpp::Node> node_;
+    std::shared_ptr<rclcpp::node_interfaces::NodeInterfaces<
+        rclcpp::node_interfaces::NodeBaseInterface,
+        rclcpp::node_interfaces::NodeParametersInterface,
+        rclcpp::node_interfaces::NodeTopicsInterface,
+        rclcpp::node_interfaces::NodeLoggingInterface>> node_interfaces_;
     rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_;
     rclcpp::Logger logger_;
     typename rclcpp::Publisher<M>::SharedPtr pub_;