diff --git a/image_transport/include/image_transport/camera_publisher.hpp b/image_transport/include/image_transport/camera_publisher.hpp index 6c6bcbe4..e5e43599 100644 --- a/image_transport/include/image_transport/camera_publisher.hpp +++ b/image_transport/include/image_transport/camera_publisher.hpp @@ -76,6 +76,26 @@ class CameraPublisher const std::string & base_topic, rmw_qos_profile_t custom_qos = rmw_qos_profile_default); + IMAGE_TRANSPORT_PUBLIC + CameraPublisher( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + const std::string & base_topic, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default); + + template + CameraPublisher( + NodeT && node, + const std::string & base_topic, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default) + : CameraPublisher( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_logging_interface(), + base_topic, custom_qos) + {} + //TODO(ros2) Restore support for SubscriberStatusCallbacks when available. /*! diff --git a/image_transport/include/image_transport/camera_subscriber.hpp b/image_transport/include/image_transport/camera_subscriber.hpp index 6e6a6c60..abae29aa 100644 --- a/image_transport/include/image_transport/camera_subscriber.hpp +++ b/image_transport/include/image_transport/camera_subscriber.hpp @@ -79,6 +79,25 @@ class CameraSubscriber const std::string& transport, rmw_qos_profile_t = rmw_qos_profile_default); + IMAGE_TRANSPORT_PUBLIC + CameraSubscriber( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + const std::string& base_topic, + const Callback& callback, + const std::string& transport, + rmw_qos_profile_t = rmw_qos_profile_default); + + template + CameraSubscriber(NodeT && node, + const std::string& base_topic, + const Callback& callback, + const std::string& transport, + rmw_qos_profile_t = rmw_qos_profile_default) + : CameraSubscriber(node->get_node_base_interface(), node->get_node_topics_interface(), node->get_node_logging_interface(), base_topic, callback, transport) + {} + /** * \brief Get the base topic (on which the raw image is published). */ diff --git a/image_transport/include/image_transport/image_transport.hpp b/image_transport/include/image_transport/image_transport.hpp index b9f9b7bc..405922d6 100644 --- a/image_transport/include/image_transport/image_transport.hpp +++ b/image_transport/include/image_transport/image_transport.hpp @@ -61,6 +61,14 @@ Publisher create_publisher( const std::string & base_topic, rmw_qos_profile_t custom_qos = rmw_qos_profile_default); +IMAGE_TRANSPORT_PUBLIC +Publisher create_publisher( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + const std::string & base_topic, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default); + /** * \brief Subscribe to an image topic, free function version. */ @@ -72,6 +80,16 @@ Subscriber create_subscription( const std::string & transport, rmw_qos_profile_t custom_qos = rmw_qos_profile_default); +IMAGE_TRANSPORT_PUBLIC +Subscriber create_subscription( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + const std::string & base_topic, + const Subscriber::Callback & callback, + const std::string & transport, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default); + /*! * \brief Advertise a camera, free function version. */ @@ -81,6 +99,14 @@ CameraPublisher create_camera_publisher( const std::string & base_topic, rmw_qos_profile_t custom_qos = rmw_qos_profile_default); +IMAGE_TRANSPORT_PUBLIC +CameraPublisher create_camera_publisher( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + const std::string & base_topic, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default); + /*! * \brief Subscribe to a camera, free function version. */ @@ -92,6 +118,16 @@ CameraSubscriber create_camera_subscription( const std::string & transport, rmw_qos_profile_t custom_qos = rmw_qos_profile_default); +IMAGE_TRANSPORT_PUBLIC +CameraSubscriber create_camera_subscription( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + const std::string & base_topic, + const CameraSubscriber::Callback & callback, + const std::string & transport, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default); + IMAGE_TRANSPORT_PUBLIC std::vector getDeclaredTransports(); @@ -115,6 +151,17 @@ class ImageTransport IMAGE_TRANSPORT_PUBLIC explicit ImageTransport(rclcpp::Node::SharedPtr node); + IMAGE_TRANSPORT_PUBLIC + ImageTransport( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface); + + template + explicit ImageTransport(NodeT && node) + : ImageTransport(node->get_node_base_interface(), node->get_node_topics_interface(), node->get_node_logging_interface()) + {} + IMAGE_TRANSPORT_PUBLIC ~ImageTransport(); diff --git a/image_transport/include/image_transport/publisher.hpp b/image_transport/include/image_transport/publisher.hpp index 32f9fc4d..938e104e 100644 --- a/image_transport/include/image_transport/publisher.hpp +++ b/image_transport/include/image_transport/publisher.hpp @@ -75,11 +75,32 @@ class Publisher IMAGE_TRANSPORT_PUBLIC Publisher( - rclcpp::Node * nh, + rclcpp::Node * node, const std::string & base_topic, PubLoaderPtr loader, rmw_qos_profile_t custom_qos); + template + Publisher( + NodeT && node, + const std::string & base_topic, + PubLoaderPtr loader, + rmw_qos_profile_t custom_qos) + : Publisher( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_logging_interface(), + base_topic, loader, custom_qos) + {} + + IMAGE_TRANSPORT_PUBLIC + Publisher( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + const std::string & base_topic, + PubLoaderPtr loader, rmw_qos_profile_t custom_qos); + /*! * \brief Returns the number of subscribers that are currently connected to * this Publisher. diff --git a/image_transport/include/image_transport/subscriber.hpp b/image_transport/include/image_transport/subscriber.hpp index 2c480188..7868257e 100644 --- a/image_transport/include/image_transport/subscriber.hpp +++ b/image_transport/include/image_transport/subscriber.hpp @@ -81,6 +81,26 @@ class Subscriber const std::string & transport, rmw_qos_profile_t custom_qos = rmw_qos_profile_default); + IMAGE_TRANSPORT_PUBLIC + Subscriber( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + const std::string & base_topic, + const Callback & callback, + SubLoaderPtr loader, + const std::string & transport, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default); + + template + Subscriber(NodeT && node, + const std::string& base_topic, + const Callback& callback, + const std::string& transport, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default) + : Subscriber(node->get_node_base_interface(), node->get_node_topics_interface(), node->get_node_logging_interface(), base_topic, callback, transport, custom_qos) + {} + /** * \brief Returns the base image topic. * diff --git a/image_transport/src/camera_publisher.cpp b/image_transport/src/camera_publisher.cpp index 8a6514a7..de70f619 100644 --- a/image_transport/src/camera_publisher.cpp +++ b/image_transport/src/camera_publisher.cpp @@ -46,8 +46,8 @@ namespace image_transport struct CameraPublisher::Impl { - Impl(rclcpp::Node * node) - : logger_(node->get_logger()), + Impl(rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) + : logger_(logger_interface->get_logger()), unadvertised_(false) { } @@ -83,17 +83,26 @@ CameraPublisher::CameraPublisher( rclcpp::Node * node, const std::string & base_topic, rmw_qos_profile_t custom_qos) -: impl_(std::make_shared(node)) +: CameraPublisher(node->get_node_base_interface(), node->get_node_topics_interface(), node->get_node_logging_interface(), base_topic, custom_qos) +{} + +CameraPublisher::CameraPublisher( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + const std::string & base_topic, + rmw_qos_profile_t custom_qos) +: impl_(std::make_shared(node_logging_interface)) { // Explicitly resolve name here so we compute the correct CameraInfo topic when the // image topic is remapped (#4539). std::string image_topic = rclcpp::expand_topic_or_service_name(base_topic, - node->get_name(), node->get_namespace()); + node_base_interface->get_name(), node_base_interface->get_namespace()); std::string info_topic = getCameraInfoTopic(image_topic); auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos); - impl_->image_pub_ = image_transport::create_publisher(node, image_topic, custom_qos); - impl_->info_pub_ = node->create_publisher(info_topic, qos); + impl_->image_pub_ = image_transport::create_publisher(node_base_interface, node_topics_interface, node_logging_interface, image_topic, custom_qos); + impl_->info_pub_ = rclcpp::create_publisher(node_topics_interface, info_topic, qos); } size_t CameraPublisher::getNumSubscribers() const diff --git a/image_transport/src/image_transport.cpp b/image_transport/src/image_transport.cpp index 188ffed7..565f0adc 100644 --- a/image_transport/src/image_transport.cpp +++ b/image_transport/src/image_transport.cpp @@ -60,6 +60,16 @@ struct Impl static Impl * kImpl = new Impl(); +Publisher create_publisher( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + const std::string & base_topic, + rmw_qos_profile_t custom_qos) +{ + return Publisher(node_base_interface, node_topics_interface, node_logging_interface, base_topic, kImpl->pub_loader_, custom_qos); +} + Publisher create_publisher( rclcpp::Node * node, const std::string & base_topic, @@ -68,6 +78,18 @@ Publisher create_publisher( return Publisher(node, base_topic, kImpl->pub_loader_, custom_qos); } +Subscriber create_subscription( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + const std::string & base_topic, + const Subscriber::Callback & callback, + const std::string & transport, + rmw_qos_profile_t custom_qos) +{ + return Subscriber(node_base_interface, node_topics_interface, node_logging_interface, base_topic, callback, kImpl->sub_loader_, transport, custom_qos); +} + Subscriber create_subscription( rclcpp::Node * node, const std::string & base_topic, @@ -78,6 +100,16 @@ Subscriber create_subscription( return Subscriber(node, base_topic, callback, kImpl->sub_loader_, transport, custom_qos); } +CameraPublisher create_camera_publisher( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + const std::string & base_topic, + rmw_qos_profile_t custom_qos) +{ + return CameraPublisher(node_base_interface, node_topics_interface, node_logging_interface, base_topic, custom_qos); +} + CameraPublisher create_camera_publisher( rclcpp::Node * node, const std::string & base_topic, @@ -86,6 +118,18 @@ CameraPublisher create_camera_publisher( return CameraPublisher(node, base_topic, custom_qos); } +CameraSubscriber create_camera_subscription( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + const std::string & base_topic, + const CameraSubscriber::Callback & callback, + const std::string & transport, + rmw_qos_profile_t custom_qos) +{ + return CameraSubscriber(node_base_interface, node_topics_interface, node_logging_interface, base_topic, callback, transport, custom_qos); +} + CameraSubscriber create_camera_subscription( rclcpp::Node * node, const std::string & base_topic, @@ -130,13 +174,25 @@ std::vector getLoadableTransports() struct ImageTransport::Impl { - rclcpp::Node::SharedPtr node_; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_; + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_; + //rclcpp::Node::SharedPtr node_; }; ImageTransport::ImageTransport(rclcpp::Node::SharedPtr node) +: ImageTransport(node->get_node_base_interface(), node->get_node_topics_interface(), node->get_node_logging_interface()) +{} + +ImageTransport::ImageTransport( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface) : impl_(std::make_unique()) { - impl_->node_ = node; + impl_->node_base_interface_ = node_base_interface; + impl_->node_topics_interface_ = node_topics_interface; + impl_->node_logging_interface_ = node_logging_interface; } ImageTransport::~ImageTransport() = default; @@ -147,7 +203,7 @@ Publisher ImageTransport::advertise(const std::string & base_topic, uint32_t que (void) latch; rmw_qos_profile_t custom_qos = rmw_qos_profile_default; custom_qos.depth = queue_size; - return create_publisher(impl_->node_.get(), base_topic, custom_qos); + return create_publisher(impl_->node_base_interface_, impl_->node_topics_interface_, impl_->node_logging_interface_, base_topic, custom_qos); } Subscriber ImageTransport::subscribe( @@ -159,7 +215,7 @@ Subscriber ImageTransport::subscribe( (void) tracked_object; rmw_qos_profile_t custom_qos = rmw_qos_profile_default; custom_qos.depth = queue_size; - return create_subscription(impl_->node_.get(), base_topic, callback, + return create_subscription(impl_->node_base_interface_, impl_->node_topics_interface_, impl_->node_logging_interface_, base_topic, callback, getTransportOrDefault(transport_hints), custom_qos); } @@ -171,7 +227,7 @@ CameraPublisher ImageTransport::advertiseCamera( (void) latch; rmw_qos_profile_t custom_qos = rmw_qos_profile_default; custom_qos.depth = queue_size; - return create_camera_publisher(impl_->node_.get(), base_topic, custom_qos); + return create_camera_publisher(impl_->node_base_interface_, impl_->node_topics_interface_, impl_->node_logging_interface_, base_topic, custom_qos); } CameraSubscriber ImageTransport::subscribeCamera( @@ -183,7 +239,7 @@ CameraSubscriber ImageTransport::subscribeCamera( (void) tracked_object; rmw_qos_profile_t custom_qos = rmw_qos_profile_default; custom_qos.depth = queue_size; - return create_camera_subscription(impl_->node_.get(), base_topic, callback, + return create_camera_subscription(impl_->node_base_interface_, impl_->node_topics_interface_, impl_->node_logging_interface_, base_topic, callback, getTransportOrDefault(transport_hints), custom_qos); } @@ -201,8 +257,10 @@ std::string ImageTransport::getTransportOrDefault(const TransportHints * transpo { std::string ret; if (nullptr == transport_hints) { - TransportHints th(impl_->node_.get()); - ret = th.getTransport(); + //TransportHints th(impl_->node_.get()); + //ret = th.getTransport(); + // TODO(karsten1987) fix + ret = transport_hints->getTransport(); } else { ret = transport_hints->getTransport(); } diff --git a/image_transport/src/publisher.cpp b/image_transport/src/publisher.cpp index 9d8d2831..697ba538 100644 --- a/image_transport/src/publisher.cpp +++ b/image_transport/src/publisher.cpp @@ -50,8 +50,8 @@ namespace image_transport struct Publisher::Impl { - Impl(rclcpp::Node * node) - : logger_(node->get_logger()), + Impl(rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface) + : logger_(logging_interface->get_logger()), unadvertised_(false) { } @@ -101,12 +101,25 @@ struct Publisher::Impl Publisher::Publisher( rclcpp::Node * node, const std::string & base_topic, PubLoaderPtr loader, rmw_qos_profile_t custom_qos) -: impl_(std::make_shared(node)) +: Publisher( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_logging_interface(), + base_topic, loader, custom_qos) +{} + +Publisher::Publisher( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + const std::string & base_topic, + PubLoaderPtr loader, rmw_qos_profile_t custom_qos) +: impl_(std::make_shared(node_logging_interface)) { // Resolve the name explicitly because otherwise the compressed topics don't remap // properly (#3652). std::string image_topic = rclcpp::expand_topic_or_service_name(base_topic, - node->get_name(), node->get_namespace()); + node_base_interface->get_name(), node_base_interface->get_namespace()); impl_->base_topic_ = image_topic; impl_->loader_ = loader; @@ -125,7 +138,10 @@ Publisher::Publisher( try { auto pub = loader->createUniqueInstance(lookup_name); - pub->advertise(node, image_topic, custom_qos); + // FIXME(karsten1987): Plugin also has to cope for lifecycle nodes + // pub->advertise(node, image_topic, custom_qos); + (void) node_topics_interface; + (void) custom_qos; impl_->publishers_.push_back(std::move(pub)); } catch (const std::runtime_error & e) { RCLCPP_ERROR(impl_->logger_, "Failed to load plugin %s, error string: %s\n",