From 679f4a8767aa8f5cadf642ed247b0fcf5b997450 Mon Sep 17 00:00:00 2001 From: Denys Kotelovych Date: Tue, 1 Nov 2022 23:10:07 +0200 Subject: [PATCH 1/4] Subscriber & publisher with base interfaces; --- .../image_transport/image_transport.hpp | 31 +++++++- .../image_transport/node_interfaces.hpp | 74 +++++++++++++++++++ .../include/image_transport/publisher.hpp | 13 +++- .../image_transport/publisher_plugin.hpp | 20 ++++- .../simple_publisher_plugin.hpp | 32 ++++---- .../simple_subscriber_plugin.hpp | 5 +- .../include/image_transport/subscriber.hpp | 17 ++++- .../image_transport/subscriber_plugin.hpp | 33 +++++---- image_transport/src/image_transport.cpp | 9 ++- image_transport/src/publisher.cpp | 18 ++--- image_transport/src/subscriber.cpp | 11 +-- 11 files changed, 209 insertions(+), 54 deletions(-) create mode 100644 image_transport/include/image_transport/node_interfaces.hpp diff --git a/image_transport/include/image_transport/image_transport.hpp b/image_transport/include/image_transport/image_transport.hpp index 53f38bad..0f050ffc 100644 --- a/image_transport/include/image_transport/image_transport.hpp +++ b/image_transport/include/image_transport/image_transport.hpp @@ -33,6 +33,7 @@ #include #include #include +#include #include "rclcpp/node.hpp" @@ -41,6 +42,7 @@ #include "image_transport/publisher.hpp" #include "image_transport/subscriber.hpp" #include "image_transport/transport_hints.hpp" +#include "image_transport/node_interfaces.hpp" #include "image_transport/visibility_control.hpp" namespace image_transport @@ -51,23 +53,48 @@ namespace image_transport */ IMAGE_TRANSPORT_PUBLIC Publisher create_publisher( - rclcpp::Node * node, + NodeInterfaces::SharedPtr node_interfaces, const std::string & base_topic, rmw_qos_profile_t custom_qos = rmw_qos_profile_default, rclcpp::PublisherOptions options = rclcpp::PublisherOptions()); +template +Publisher create_publisher( + NodeT && node, + const std::string & base_topic, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default, + rclcpp::PublisherOptions options = rclcpp::PublisherOptions()) +{ + return create_publisher( + create_node_interfaces(std::forward(node)), base_topic, custom_qos, options); +} + /** * \brief Subscribe to an image topic, free function version. */ IMAGE_TRANSPORT_PUBLIC Subscriber create_subscription( - rclcpp::Node * node, + NodeInterfaces::SharedPtr 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()); +template +Subscriber create_subscription( + NodeT && node, + 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()) +{ + return create_subscription( + create_node_interfaces(std::forward(node)), + base_topic, callback, transport, custom_qos, options); +} + /*! * \brief Advertise a camera, free function version. */ diff --git a/image_transport/include/image_transport/node_interfaces.hpp b/image_transport/include/image_transport/node_interfaces.hpp new file mode 100644 index 00000000..b9ef0eb8 --- /dev/null +++ b/image_transport/include/image_transport/node_interfaces.hpp @@ -0,0 +1,74 @@ +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 the Willow Garage 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 HOLDER 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. + +#ifndef IMAGE_TRANSPORT__NODE_INTERFACES_HPP_ +#define IMAGE_TRANSPORT__NODE_INTERFACES_HPP_ + +#include +#include + +#include "rclcpp/node.hpp" + +namespace image_transport +{ + +struct NodeInterfaces +{ + RCLCPP_SMART_PTR_DEFINITIONS(NodeInterfaces) + + NodeInterfaces( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface) + : base(std::move(base_interface)), + topics(std::move(topics_interface)), + logging(std::move(logging_interface)), + parameters(std::move(parameters_interface)) + {} + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr base; + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters; +}; + +template +NodeInterfaces::SharedPtr create_node_interfaces(NodeT && node) +{ + return std::make_shared( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_logging_interface(), + node->get_node_parameters_interface() + ); +} + +} // namespace image_transport + +#endif // IMAGE_TRANSPORT__NODE_INTERFACES_HPP_ diff --git a/image_transport/include/image_transport/publisher.hpp b/image_transport/include/image_transport/publisher.hpp index dc28d017..6b18f775 100644 --- a/image_transport/include/image_transport/publisher.hpp +++ b/image_transport/include/image_transport/publisher.hpp @@ -31,6 +31,7 @@ #include #include +#include #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" @@ -41,6 +42,7 @@ #include "image_transport/loader_fwds.hpp" #include "image_transport/single_subscriber_publisher.hpp" #include "image_transport/visibility_control.hpp" +#include "image_transport/node_interfaces.hpp" namespace image_transport { @@ -70,12 +72,21 @@ class Publisher IMAGE_TRANSPORT_PUBLIC Publisher( - rclcpp::Node * nh, + NodeInterfaces::SharedPtr node_interfaces, const std::string & base_topic, PubLoaderPtr loader, rmw_qos_profile_t custom_qos, rclcpp::PublisherOptions options = rclcpp::PublisherOptions()); + template + Publisher( + NodeT && node, + const std::string & base_topic, + PubLoaderPtr loader, + rmw_qos_profile_t custom_qos) + : Publisher(create_node_interfaces(std::forward(node)), base_topic, loader, custom_qos) + {} + /*! * \brief Returns the number of subscribers that are currently connected to * this Publisher. diff --git a/image_transport/include/image_transport/publisher_plugin.hpp b/image_transport/include/image_transport/publisher_plugin.hpp index d899349e..099e345c 100644 --- a/image_transport/include/image_transport/publisher_plugin.hpp +++ b/image_transport/include/image_transport/publisher_plugin.hpp @@ -31,10 +31,12 @@ #include #include +#include #include "rclcpp/node.hpp" #include "sensor_msgs/msg/image.hpp" +#include "image_transport/node_interfaces.hpp" #include "image_transport/single_subscriber_publisher.hpp" #include "image_transport/visibility_control.hpp" @@ -63,12 +65,24 @@ class PublisherPlugin * \brief Advertise a topic, simple version. */ void advertise( - rclcpp::Node * nh, + NodeInterfaces::SharedPtr node_interfaces, const std::string & base_topic, rmw_qos_profile_t custom_qos = rmw_qos_profile_default, rclcpp::PublisherOptions options = rclcpp::PublisherOptions()) { - advertiseImpl(nh, base_topic, custom_qos, options); + advertiseImpl(node_interfaces, base_topic, custom_qos, options); + } + + template + void advertise( + NodeT && node, + const std::string & base_topic, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default, + rclcpp::PublisherOptions options = rclcpp::PublisherOptions()) + { + advertiseImpl( + create_node_interfaces(std::forward(node)), base_topic, custom_qos, + options); } /** @@ -135,7 +149,7 @@ class PublisherPlugin * \brief Advertise a topic. Must be implemented by the subclass. */ virtual void advertiseImpl( - rclcpp::Node * node, + NodeInterfaces::SharedPtr node_interfaces, const std::string & base_topic, rmw_qos_profile_t custom_qos, rclcpp::PublisherOptions options) = 0; diff --git a/image_transport/include/image_transport/simple_publisher_plugin.hpp b/image_transport/include/image_transport/simple_publisher_plugin.hpp index ca50aeec..533b1770 100644 --- a/image_transport/include/image_transport/simple_publisher_plugin.hpp +++ b/image_transport/include/image_transport/simple_publisher_plugin.hpp @@ -31,10 +31,12 @@ #include #include +#include #include "rclcpp/node.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" +#include "rclcpp/create_publisher.hpp" #include "image_transport/publisher_plugin.hpp" #include "image_transport/visibility_control.hpp" @@ -63,9 +65,10 @@ template class SimplePublisherPlugin : public PublisherPlugin { public: - virtual ~SimplePublisherPlugin() {} + ~SimplePublisherPlugin() = default; - virtual size_t getNumSubscribers() const +public: + size_t getNumSubscribers() const override { if (simple_impl_) { return simple_impl_->pub_->get_subscription_count(); @@ -73,13 +76,13 @@ class SimplePublisherPlugin : public PublisherPlugin return 0; } - virtual std::string getTopic() const + std::string getTopic() const override { if (simple_impl_) {return simple_impl_->pub_->get_topic_name();} return std::string(); } - virtual void publish(const sensor_msgs::msg::Image & message) const + void publish(const sensor_msgs::msg::Image & message) const override { if (!simple_impl_ || !simple_impl_->pub_) { auto logger = simple_impl_ ? simple_impl_->logger_ : rclcpp::get_logger("image_transport"); @@ -92,24 +95,26 @@ class SimplePublisherPlugin : public PublisherPlugin publish(message, bindInternalPublisher(simple_impl_->pub_.get())); } - virtual void shutdown() + void shutdown() override { simple_impl_.reset(); } protected: void advertiseImpl( - rclcpp::Node * node, + NodeInterfaces::SharedPtr node_interfaces, const std::string & base_topic, rmw_qos_profile_t custom_qos, rclcpp::PublisherOptions options) override { std::string transport_topic = getTopicToAdvertise(base_topic); - simple_impl_ = std::make_unique(node); + simple_impl_ = std::make_unique(node_interfaces); 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(transport_topic, qos, options); + + simple_impl_->pub_ = rclcpp::create_publisher( + node_interfaces->parameters, node_interfaces->topics, transport_topic, qos, options); } //! Generic function for publishing the internal message type. @@ -140,13 +145,12 @@ class SimplePublisherPlugin : public PublisherPlugin private: struct SimplePublisherPluginImpl { - explicit SimplePublisherPluginImpl(rclcpp::Node * node) - : node_(node), - logger_(node->get_logger()) - { - } + explicit SimplePublisherPluginImpl(NodeInterfaces::SharedPtr node_interfaces) + : node_interfaces_(std::move(node_interfaces)), + logger_(node_interfaces_->logging->get_logger()) + {} - rclcpp::Node * node_; + NodeInterfaces::SharedPtr node_interfaces_; rclcpp::Logger logger_; typename rclcpp::Publisher::SharedPtr pub_; }; diff --git a/image_transport/include/image_transport/simple_subscriber_plugin.hpp b/image_transport/include/image_transport/simple_subscriber_plugin.hpp index 189e07ab..fbfac49a 100644 --- a/image_transport/include/image_transport/simple_subscriber_plugin.hpp +++ b/image_transport/include/image_transport/simple_subscriber_plugin.hpp @@ -109,7 +109,7 @@ class SimpleSubscriberPlugin : public SubscriberPlugin } void subscribeImpl( - rclcpp::Node * node, + NodeInterfaces::SharedPtr node_interfaces, const std::string & base_topic, const Callback & callback, rmw_qos_profile_t custom_qos, @@ -120,7 +120,8 @@ class SimpleSubscriberPlugin : public SubscriberPlugin // ros::NodeHandle param_nh(transport_hints.getParameterNH(), getTransportName()); // auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos); - impl_->sub_ = node->create_subscription( + impl_->sub_ = rclcpp::create_subscription( + node_interfaces->parameters, node_interfaces->topics, getTopicToSubscribe(base_topic), qos, [this, callback](const typename std::shared_ptr msg) { internalCallback(msg, callback); diff --git a/image_transport/include/image_transport/subscriber.hpp b/image_transport/include/image_transport/subscriber.hpp index f0b87245..2513576a 100644 --- a/image_transport/include/image_transport/subscriber.hpp +++ b/image_transport/include/image_transport/subscriber.hpp @@ -32,12 +32,14 @@ #include #include #include +#include #include "rclcpp/node.hpp" #include "sensor_msgs/msg/image.hpp" #include "image_transport/exception.hpp" #include "image_transport/loader_fwds.hpp" +#include "image_transport/node_interfaces.hpp" #include "image_transport/visibility_control.hpp" namespace image_transport @@ -68,7 +70,7 @@ class Subscriber IMAGE_TRANSPORT_PUBLIC Subscriber( - rclcpp::Node * node, + NodeInterfaces::SharedPtr node_interfaces, const std::string & base_topic, const Callback & callback, SubLoaderPtr loader, @@ -76,6 +78,19 @@ class Subscriber rmw_qos_profile_t custom_qos = rmw_qos_profile_default, rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); + template + Subscriber( + NodeT && node, + const std::string & base_topic, + const Callback & callback, + SubLoaderPtr loader, + const std::string & transport, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default, + rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) + : Subscriber(create_node_interfaces( + std::forward(node)), base_topic, callback, loader, transport, custom_qos, options) + {} + /** * \brief Returns the base image topic. * diff --git a/image_transport/include/image_transport/subscriber_plugin.hpp b/image_transport/include/image_transport/subscriber_plugin.hpp index bae15b9c..43a0d098 100644 --- a/image_transport/include/image_transport/subscriber_plugin.hpp +++ b/image_transport/include/image_transport/subscriber_plugin.hpp @@ -31,7 +31,9 @@ #include #include +#include +#include "image_transport/node_interfaces.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" #include "sensor_msgs/msg/image.hpp" @@ -65,25 +67,25 @@ class IMAGE_TRANSPORT_PUBLIC SubscriberPlugin * \brief Subscribe to an image topic, version for arbitrary std::function object. */ void subscribe( - rclcpp::Node * node, const std::string & base_topic, + NodeInterfaces::SharedPtr node_interfaces, const std::string & base_topic, const Callback & callback, rmw_qos_profile_t custom_qos = rmw_qos_profile_default, rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) { - return subscribeImpl(node, base_topic, callback, custom_qos, options); + return subscribeImpl(node_interfaces, base_topic, callback, custom_qos, options); } /** * \brief Subscribe to an image topic, version for bare function. */ void subscribe( - rclcpp::Node * node, const std::string & base_topic, + NodeInterfaces::SharedPtr node_interfaces, const std::string & base_topic, void (* fp)(const sensor_msgs::msg::Image::ConstSharedPtr &), rmw_qos_profile_t custom_qos = rmw_qos_profile_default, rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) { return subscribe( - node, base_topic, + node_interfaces, base_topic, std::function(fp), custom_qos, options); } @@ -91,31 +93,36 @@ class IMAGE_TRANSPORT_PUBLIC SubscriberPlugin /** * \brief Subscribe to an image topic, version for class member function with bare pointer. */ - template + template void subscribe( - rclcpp::Node * node, const std::string & base_topic, + NodeT && node, const std::string & base_topic, void (T::* fp)(const sensor_msgs::msg::Image::ConstSharedPtr &), T * obj, rmw_qos_profile_t custom_qos = rmw_qos_profile_default, rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) { return subscribe( - node, base_topic, - std::bind(fp, obj, std::placeholders::_1), custom_qos, options); + create_node_interfaces(std::forward(node)), + base_topic, + std::bind(fp, obj, std::placeholders::_1), + custom_qos, + options); } /** * \brief Subscribe to an image topic, version for class member function with shared_ptr. */ - template + template void subscribe( - rclcpp::Node * node, const std::string & base_topic, + NodeT && node, const std::string & base_topic, void (T::* fp)(const sensor_msgs::msg::Image::ConstSharedPtr &), std::shared_ptr & obj, rmw_qos_profile_t custom_qos = rmw_qos_profile_default) { return subscribe( - node, base_topic, - std::bind(fp, obj, std::placeholders::_1), custom_qos); + create_node_interfaces(std::forward(node)), + base_topic, + std::bind(fp, obj, std::placeholders::_1), + custom_qos); } /** @@ -147,7 +154,7 @@ class IMAGE_TRANSPORT_PUBLIC SubscriberPlugin * \brief Subscribe to an image transport topic. Must be implemented by the subclass. */ virtual void subscribeImpl( - rclcpp::Node * node, + NodeInterfaces::SharedPtr node_interfaces, const std::string & base_topic, const Callback & callback, rmw_qos_profile_t custom_qos, diff --git a/image_transport/src/image_transport.cpp b/image_transport/src/image_transport.cpp index 71c91d18..27d98446 100644 --- a/image_transport/src/image_transport.cpp +++ b/image_transport/src/image_transport.cpp @@ -57,23 +57,24 @@ struct Impl static Impl * kImpl = new Impl(); Publisher create_publisher( - rclcpp::Node * node, + NodeInterfaces::SharedPtr node_interfaces, const std::string & base_topic, rmw_qos_profile_t custom_qos, rclcpp::PublisherOptions options) { - return Publisher(node, base_topic, kImpl->pub_loader_, custom_qos, options); + return Publisher(node_interfaces, base_topic, kImpl->pub_loader_, custom_qos, options); } Subscriber create_subscription( - rclcpp::Node * node, + NodeInterfaces::SharedPtr node_interfaces, const std::string & base_topic, const Subscriber::Callback & callback, const std::string & transport, rmw_qos_profile_t custom_qos, rclcpp::SubscriptionOptions options) { - return Subscriber(node, base_topic, callback, kImpl->sub_loader_, transport, custom_qos, options); + return Subscriber( + node_interfaces, base_topic, callback, kImpl->sub_loader_, transport, custom_qos, options); } CameraPublisher create_camera_publisher( diff --git a/image_transport/src/publisher.cpp b/image_transport/src/publisher.cpp index 61701dee..66fcd39c 100644 --- a/image_transport/src/publisher.cpp +++ b/image_transport/src/publisher.cpp @@ -48,11 +48,9 @@ namespace image_transport struct Publisher::Impl { - explicit Impl(rclcpp::Node * node) - : logger_(node->get_logger()), - unadvertised_(false) - { - } + explicit Impl(const NodeInterfaces::SharedPtr & node_interfaces) + : logger_(node_interfaces->logging->get_logger()), + unadvertised_(false) {} ~Impl() { @@ -89,6 +87,7 @@ struct Publisher::Impl } } + rclcpp::Logger logger_; std::string base_topic_; PubLoaderPtr loader_; @@ -97,16 +96,17 @@ struct Publisher::Impl }; Publisher::Publisher( - rclcpp::Node * node, const std::string & base_topic, + NodeInterfaces::SharedPtr node_interfaces, + const std::string & base_topic, PubLoaderPtr loader, rmw_qos_profile_t custom_qos, rclcpp::PublisherOptions options) -: impl_(std::make_shared(node)) +: impl_(std::make_shared(node_interfaces)) { // 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_interfaces->base->get_name(), node_interfaces->base->get_namespace()); impl_->base_topic_ = image_topic; impl_->loader_ = loader; @@ -125,7 +125,7 @@ Publisher::Publisher( try { auto pub = loader->createUniqueInstance(lookup_name); - pub->advertise(node, image_topic, custom_qos, options); + pub->advertise(node_interfaces, image_topic, custom_qos, options); impl_->publishers_.push_back(std::move(pub)); } catch (const std::runtime_error & e) { RCLCPP_ERROR( diff --git a/image_transport/src/subscriber.cpp b/image_transport/src/subscriber.cpp index 85c7927c..6ebfd01e 100644 --- a/image_transport/src/subscriber.cpp +++ b/image_transport/src/subscriber.cpp @@ -32,6 +32,7 @@ #include #include +#include "image_transport/node_interfaces.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/logging.hpp" @@ -46,8 +47,8 @@ namespace image_transport struct Subscriber::Impl { - Impl(rclcpp::Node * node, SubLoaderPtr loader) - : logger_(node->get_logger()), + Impl(const NodeInterfaces::SharedPtr & node_interfaces, SubLoaderPtr loader) + : logger_(node_interfaces->logging->get_logger()), loader_(loader), unsubscribed_(false) { @@ -82,14 +83,14 @@ struct Subscriber::Impl }; Subscriber::Subscriber( - rclcpp::Node * node, + NodeInterfaces::SharedPtr node_interfaces, const std::string & base_topic, const Callback & callback, SubLoaderPtr loader, const std::string & transport, rmw_qos_profile_t custom_qos, rclcpp::SubscriptionOptions options) -: impl_(std::make_shared(node, loader)) +: impl_(std::make_shared(node_interfaces, loader)) { // Load the plugin for the chosen transport. impl_->lookup_name_ = SubscriberPlugin::getLookupName(transport); @@ -125,7 +126,7 @@ Subscriber::Subscriber( // Tell plugin to subscribe. RCLCPP_DEBUG(impl_->logger_, "Subscribing to: %s\n", base_topic.c_str()); - impl_->subscriber_->subscribe(node, base_topic, callback, custom_qos, options); + impl_->subscriber_->subscribe(node_interfaces, base_topic, callback, custom_qos, options); } std::string Subscriber::getTopic() const From 7a64f0097b3e011c6cbf205962312bb704c2f035 Mon Sep 17 00:00:00 2001 From: Denys Kotelovych Date: Thu, 3 Nov 2022 10:10:08 +0000 Subject: [PATCH 2/4] Updated CameraPublisher --- .../include/image_transport/camera_publisher.hpp | 11 ++++++++++- image_transport/src/camera_publisher.cpp | 15 ++++++++------- 2 files changed, 18 insertions(+), 8 deletions(-) diff --git a/image_transport/include/image_transport/camera_publisher.hpp b/image_transport/include/image_transport/camera_publisher.hpp index 9945ea54..2ccabd0e 100644 --- a/image_transport/include/image_transport/camera_publisher.hpp +++ b/image_transport/include/image_transport/camera_publisher.hpp @@ -31,6 +31,7 @@ #include #include +#include #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" @@ -39,6 +40,7 @@ #include "sensor_msgs/msg/camera_info.hpp" #include "image_transport/single_subscriber_publisher.hpp" +#include "image_transport/node_interfaces.hpp" #include "image_transport/visibility_control.hpp" namespace image_transport @@ -69,10 +71,17 @@ class CameraPublisher IMAGE_TRANSPORT_PUBLIC CameraPublisher( - rclcpp::Node * node, + NodeInterfaces::SharedPtr node_interfaces, 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(create_node_interfaces(std::forward(node)), base_topic, custom_qos) {} + // TODO(ros2) Restore support for SubscriberStatusCallbacks when available. /*! diff --git a/image_transport/src/camera_publisher.cpp b/image_transport/src/camera_publisher.cpp index 0b6542a7..aceb708e 100644 --- a/image_transport/src/camera_publisher.cpp +++ b/image_transport/src/camera_publisher.cpp @@ -43,8 +43,8 @@ namespace image_transport struct CameraPublisher::Impl { - explicit Impl(rclcpp::Node * node) - : logger_(node->get_logger()), + explicit Impl(const NodeInterfaces::SharedPtr node_interfaces) + : logger_(node_interfaces->logging->get_logger()), unadvertised_(false) { } @@ -76,21 +76,22 @@ struct CameraPublisher::Impl // TODO(ros2) Add support for SubscriberStatusCallbacks when rcl/rmw support it. CameraPublisher::CameraPublisher( - rclcpp::Node * node, + NodeInterfaces::SharedPtr node_interfaces, const std::string & base_topic, rmw_qos_profile_t custom_qos) -: impl_(std::make_shared(node)) +: impl_(std::make_shared(node_interfaces)) { // 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_interfaces->base->get_name(), node_interfaces->base->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_interfaces, image_topic, custom_qos); + impl_->info_pub_ = rclcpp::create_publisher( + node_interfaces->topics, info_topic, qos); } size_t CameraPublisher::getNumSubscribers() const From 751cffa0e4475ddc50901661e9843914c7528aec Mon Sep 17 00:00:00 2001 From: Denys Kotelovych Date: Thu, 3 Nov 2022 15:23:03 +0000 Subject: [PATCH 3/4] Updated image_transport --- .../image_transport/camera_publisher.hpp | 2 - .../image_transport/camera_subscriber.hpp | 2 - .../image_transport/image_transport.hpp | 71 +++++++++++++--- image_transport/src/image_transport.cpp | 83 ------------------- 4 files changed, 59 insertions(+), 99 deletions(-) diff --git a/image_transport/include/image_transport/camera_publisher.hpp b/image_transport/include/image_transport/camera_publisher.hpp index 2ccabd0e..a1fd92aa 100644 --- a/image_transport/include/image_transport/camera_publisher.hpp +++ b/image_transport/include/image_transport/camera_publisher.hpp @@ -46,8 +46,6 @@ namespace image_transport { -class ImageTransport; - /** * \brief Manages advertisements for publishing camera images. * diff --git a/image_transport/include/image_transport/camera_subscriber.hpp b/image_transport/include/image_transport/camera_subscriber.hpp index 5157111d..a46306df 100644 --- a/image_transport/include/image_transport/camera_subscriber.hpp +++ b/image_transport/include/image_transport/camera_subscriber.hpp @@ -43,8 +43,6 @@ namespace image_transport { -class ImageTransport; - /** * \brief Manages a subscription callback on synchronized Image and CameraInfo topics. * diff --git a/image_transport/include/image_transport/image_transport.hpp b/image_transport/include/image_transport/image_transport.hpp index 0f050ffc..363de187 100644 --- a/image_transport/include/image_transport/image_transport.hpp +++ b/image_transport/include/image_transport/image_transport.hpp @@ -128,6 +128,7 @@ std::vector getLoadableTransports(); * subscribe() functions for creating advertisements and subscriptions of image topics. */ +template class ImageTransport { public: @@ -135,17 +136,24 @@ class ImageTransport using ImageConstPtr = sensor_msgs::msg::Image::ConstSharedPtr; using CameraInfoConstPtr = sensor_msgs::msg::CameraInfo::ConstSharedPtr; - IMAGE_TRANSPORT_PUBLIC - explicit ImageTransport(rclcpp::Node::SharedPtr node); + explicit ImageTransport(std::shared_ptr node) + : node_(std::move(node)) {} IMAGE_TRANSPORT_PUBLIC - ~ImageTransport(); + ~ImageTransport() = default; /*! * \brief Advertise an image topic, simple version. */ IMAGE_TRANSPORT_PUBLIC - Publisher advertise(const std::string & base_topic, uint32_t queue_size, bool latch = false); + Publisher advertise(const std::string & base_topic, uint32_t queue_size, bool latch = false) + { + // TODO(ros2) implement when resolved: https://github.com/ros2/ros2/issues/464 + (void) latch; + rmw_qos_profile_t custom_qos = rmw_qos_profile_default; + custom_qos.depth = queue_size; + return create_publisher(node_.get(), base_topic, custom_qos); + } /*! * \brief Advertise an image topic with subcriber status callbacks. @@ -165,7 +173,15 @@ class ImageTransport const std::string & base_topic, uint32_t queue_size, const Subscriber::Callback & callback, const VoidPtr & tracked_object = VoidPtr(), - const TransportHints * transport_hints = nullptr); + const TransportHints * transport_hints = nullptr) + { + (void) tracked_object; + rmw_qos_profile_t custom_qos = rmw_qos_profile_default; + custom_qos.depth = queue_size; + return create_subscription( + node_.get(), base_topic, callback, + getTransportOrDefault(transport_hints), custom_qos); + } /** * \brief Subscribe to an image topic, version for bare function. @@ -217,7 +233,14 @@ class ImageTransport IMAGE_TRANSPORT_PUBLIC CameraPublisher advertiseCamera( const std::string & base_topic, uint32_t queue_size, - bool latch = false); + bool latch = false) + { + // TODO(ros2) implement when resolved: https://github.com/ros2/ros2/issues/464 + (void) latch; + rmw_qos_profile_t custom_qos = rmw_qos_profile_default; + custom_qos.depth = queue_size; + return create_camera_publisher(node_.get(), base_topic, custom_qos); + } /*! * \brief Advertise a synchronized camera raw image + info topic pair with subscriber status @@ -244,7 +267,15 @@ class ImageTransport const std::string & base_topic, uint32_t queue_size, const CameraSubscriber::Callback & callback, const VoidPtr & tracked_object = VoidPtr(), - const TransportHints * transport_hints = nullptr); + const TransportHints * transport_hints = nullptr) + { + (void) tracked_object; + rmw_qos_profile_t custom_qos = rmw_qos_profile_default; + custom_qos.depth = queue_size; + return create_camera_subscription( + node_.get(), base_topic, callback, + getTransportOrDefault(transport_hints), custom_qos); + } /** * \brief Subscribe to a synchronized image & camera info topic pair, version for bare function. @@ -305,19 +336,35 @@ class ImageTransport * transports are not necessarily built or loadable. */ IMAGE_TRANSPORT_PUBLIC - std::vector getDeclaredTransports() const; + std::vector getDeclaredTransports() const + { + return image_transport::getDeclaredTransports(); + } /** * \brief Returns the names of all transports that are loadable in the system. */ IMAGE_TRANSPORT_PUBLIC - std::vector getLoadableTransports() const; + std::vector getLoadableTransports() const + { + return image_transport::getLoadableTransports(); + } private: - std::string getTransportOrDefault(const TransportHints * transport_hints); + std::string getTransportOrDefault(const TransportHints * transport_hints) + { + std::string ret; + if (nullptr == transport_hints) { + TransportHints th(node_.get()); + ret = th.getTransport(); + } else { + ret = transport_hints->getTransport(); + } + return ret; + } - struct Impl; - std::unique_ptr impl_; +private: + std::shared_ptr node_; }; } // namespace image_transport diff --git a/image_transport/src/image_transport.cpp b/image_transport/src/image_transport.cpp index 27d98446..70ce5e03 100644 --- a/image_transport/src/image_transport.cpp +++ b/image_transport/src/image_transport.cpp @@ -128,87 +128,4 @@ std::vector getLoadableTransports() return loadableTransports; } -struct ImageTransport::Impl -{ - rclcpp::Node::SharedPtr node_; -}; - -ImageTransport::ImageTransport(rclcpp::Node::SharedPtr node) -: impl_(std::make_unique()) -{ - impl_->node_ = node; -} - -ImageTransport::~ImageTransport() = default; - -Publisher ImageTransport::advertise(const std::string & base_topic, uint32_t queue_size, bool latch) -{ - // TODO(ros2) implement when resolved: https://github.com/ros2/ros2/issues/464 - (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); -} - -Subscriber ImageTransport::subscribe( - const std::string & base_topic, uint32_t queue_size, - const Subscriber::Callback & callback, - const VoidPtr & tracked_object, - const TransportHints * transport_hints) -{ - (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, - getTransportOrDefault(transport_hints), custom_qos); -} - -CameraPublisher ImageTransport::advertiseCamera( - const std::string & base_topic, uint32_t queue_size, - bool latch) -{ - // TODO(ros2) implement when resolved: https://github.com/ros2/ros2/issues/464 - (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); -} - -CameraSubscriber ImageTransport::subscribeCamera( - const std::string & base_topic, uint32_t queue_size, - const CameraSubscriber::Callback & callback, - const VoidPtr & tracked_object, - const TransportHints * transport_hints) -{ - (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, - getTransportOrDefault(transport_hints), custom_qos); -} - -std::vector ImageTransport::getDeclaredTransports() const -{ - return image_transport::getDeclaredTransports(); -} - -std::vector ImageTransport::getLoadableTransports() const -{ - return image_transport::getLoadableTransports(); -} - -std::string ImageTransport::getTransportOrDefault(const TransportHints * transport_hints) -{ - std::string ret; - if (nullptr == transport_hints) { - TransportHints th(impl_->node_.get()); - ret = th.getTransport(); - } else { - ret = transport_hints->getTransport(); - } - return ret; -} - } // namespace image_transport From ffc63c2951fdc4a2e9fc2983fbb5bd70ca3828d8 Mon Sep 17 00:00:00 2001 From: Denys Kotelovych Date: Fri, 4 Nov 2022 08:45:04 +0200 Subject: [PATCH 4/4] Updated CameraSubscriber; --- image_transport/CMakeLists.txt | 1 - .../image_transport/camera_subscriber.hpp | 159 +++++++++++++--- .../create_camera_publisher.hpp | 54 ++++++ .../create_camera_subscription.hpp | 56 ++++++ .../image_transport/create_publisher.hpp | 70 +++++++ .../image_transport/create_subscription.hpp | 74 ++++++++ .../image_transport/image_transport.hpp | 79 +------- .../image_transport/subscriber_filter.hpp | 8 +- image_transport/src/camera_publisher.cpp | 1 + image_transport/src/camera_subscriber.cpp | 179 ------------------ image_transport/src/image_transport.cpp | 8 +- image_transport/src/republish.cpp | 2 +- image_transport/test/test_message_passing.cpp | 3 + image_transport/test/test_publisher.cpp | 2 + image_transport/test/test_qos_override.cpp | 1 + image_transport/test/test_remapping.cpp | 1 + image_transport/test/test_subscriber.cpp | 1 + 17 files changed, 413 insertions(+), 286 deletions(-) create mode 100644 image_transport/include/image_transport/create_camera_publisher.hpp create mode 100644 image_transport/include/image_transport/create_camera_subscription.hpp create mode 100644 image_transport/include/image_transport/create_publisher.hpp create mode 100644 image_transport/include/image_transport/create_subscription.hpp delete mode 100644 image_transport/src/camera_subscriber.cpp diff --git a/image_transport/CMakeLists.txt b/image_transport/CMakeLists.txt index e6994733..af6e0a2a 100644 --- a/image_transport/CMakeLists.txt +++ b/image_transport/CMakeLists.txt @@ -25,7 +25,6 @@ add_library(${PROJECT_NAME} src/subscriber.cpp src/single_subscriber_publisher.cpp src/camera_publisher.cpp - src/camera_subscriber.cpp src/image_transport.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC diff --git a/image_transport/include/image_transport/camera_subscriber.hpp b/image_transport/include/image_transport/camera_subscriber.hpp index a46306df..d52baab2 100644 --- a/image_transport/include/image_transport/camera_subscriber.hpp +++ b/image_transport/include/image_transport/camera_subscriber.hpp @@ -37,7 +37,12 @@ #include "sensor_msgs/msg/camera_info.hpp" #include "sensor_msgs/msg/image.hpp" +#include "message_filters/subscriber.h" +#include "message_filters/time_synchronizer.h" +#include "image_transport/camera_common.hpp" +#include "image_transport/subscriber_filter.hpp" +#include "image_transport/node_interfaces.hpp" #include "image_transport/visibility_control.hpp" namespace image_transport @@ -58,67 +63,177 @@ void callback(const sensor_msgs::msg::Image::ConstSharedPtr&, const sensor_msgs: * associated with that handle will stop being called. Once all CameraSubscriber for a given * topic go out of scope the topic will be unsubscribed. */ + +inline void increment(int * value) +{ + ++(*value); +} + +template class CameraSubscriber { public: - typedef std::function Callback; + using Callback = std::function; - IMAGE_TRANSPORT_PUBLIC CameraSubscriber() = default; - IMAGE_TRANSPORT_PUBLIC CameraSubscriber( - rclcpp::Node * node, + NodeT * node, const std::string & base_topic, const Callback & callback, const std::string & transport, - rmw_qos_profile_t = rmw_qos_profile_default); + rmw_qos_profile_t custom_qos = rmw_qos_profile_default) + : impl_(std::make_shared(node)) + { + // Must explicitly remap the image topic since we then do some string manipulation on it + // to figure out the sibling camera_info topic. + std::string image_topic = rclcpp::expand_topic_or_service_name( + base_topic, + node->get_node_base_interface()->get_name(), + node->get_node_base_interface()->get_namespace()); + std::string info_topic = getCameraInfoTopic(image_topic); + + impl_->image_sub_.subscribe(node, image_topic, transport, custom_qos); + impl_->info_sub_.subscribe(node, info_topic, custom_qos); + + impl_->sync_.connectInput(impl_->image_sub_, impl_->info_sub_); + impl_->sync_.registerCallback( + std::bind( + callback, std::placeholders::_1, + std::placeholders::_2)); + + // Complain every 10s if it appears that the image and info topics are not synchronized + impl_->image_sub_.registerCallback(std::bind(increment, &impl_->image_received_)); + impl_->info_sub_.registerCallback(std::bind(increment, &impl_->info_received_)); + impl_->sync_.registerCallback(std::bind(increment, &impl_->both_received_)); + + impl_->check_synced_timer_ = node->create_wall_timer( + std::chrono::seconds(1), + std::bind(&Impl::checkImagesSynchronized, impl_.get())); + } /** * \brief Get the base topic (on which the raw image is published). */ - IMAGE_TRANSPORT_PUBLIC - std::string getTopic() const; + std::string getTopic() const + { + if (impl_) {return impl_->image_sub_.getTopic();} + return std::string(); + } /** * \brief Get the camera info topic. */ - IMAGE_TRANSPORT_PUBLIC - std::string getInfoTopic() const; + std::string getInfoTopic() const + { + if (impl_) {return impl_->info_sub_.getSubscriber()->get_topic_name();} + return std::string(); + } /** * \brief Returns the number of publishers this subscriber is connected to. */ - IMAGE_TRANSPORT_PUBLIC - size_t getNumPublishers() const; + size_t getNumPublishers() const + { + // TODO(ros2) Fix this when ros2 has better subscriber counting. + // @todo Fix this when message_filters::Subscriber has getNumPublishers() + // if (impl_) { + // return std::max(impl_->image_sub_.getNumPublishers(), impl_->info_sub_.getNumPublishers()); + // } + // if (impl_) return impl_->image_sub_.getNumPublishers(); + return 0; + } /** * \brief Returns the name of the transport being used. */ - IMAGE_TRANSPORT_PUBLIC - std::string getTransport() const; + std::string getTransport() const + { + if (impl_) {return impl_->image_sub_.getTransport();} + return std::string(); + } /** * \brief Unsubscribe the callback associated with this CameraSubscriber. */ - IMAGE_TRANSPORT_PUBLIC - void shutdown(); + void shutdown() + { + if (impl_) {impl_->shutdown();} + } - IMAGE_TRANSPORT_PUBLIC - operator void *() const; + operator void *() const + { + return (impl_ && impl_->isValid()) ? reinterpret_cast(1) : reinterpret_cast(0); + } - IMAGE_TRANSPORT_PUBLIC bool operator<(const CameraSubscriber & rhs) const {return impl_ < rhs.impl_;} - IMAGE_TRANSPORT_PUBLIC bool operator!=(const CameraSubscriber & rhs) const {return impl_ != rhs.impl_;} - IMAGE_TRANSPORT_PUBLIC bool operator==(const CameraSubscriber & rhs) const {return impl_ == rhs.impl_;} private: - struct Impl; + struct Impl + { + using Image = sensor_msgs::msg::Image; + using CameraInfo = sensor_msgs::msg::CameraInfo; + using TimeSync = message_filters::TimeSynchronizer; + + explicit Impl(NodeT * node) + : logger_(node->get_node_logging_interface()->get_logger()), + sync_(10), + unsubscribed_(false), + image_received_(0), info_received_(0), both_received_(0) + { + } + + ~Impl() + { + shutdown(); + } + + bool isValid() const + { + return !unsubscribed_; + } + + void shutdown() + { + if (!unsubscribed_) { + unsubscribed_ = true; + image_sub_.unsubscribe(); + info_sub_.unsubscribe(); + } + } + + void checkImagesSynchronized() + { + int threshold = 3 * both_received_; + if (image_received_ > threshold || info_received_ > threshold) { + RCLCPP_WARN( + logger_, + "[image_transport] Topics '%s' and '%s' do not appear to be synchronized. " + "In the last 10s:\n" + "\tImage messages received: %d\n" + "\tCameraInfo messages received: %d\n" + "\tSynchronized pairs: %d", + image_sub_.getTopic().c_str(), info_sub_.getTopic().c_str(), + image_received_, info_received_, both_received_); + } + image_received_ = info_received_ = both_received_ = 0; + } + + rclcpp::Logger logger_; + SubscriberFilter image_sub_; + message_filters::Subscriber info_sub_; + TimeSync sync_; + + bool unsubscribed_; + // For detecting when the topics aren't synchronized + std::shared_ptr check_synced_timer_; + int image_received_, info_received_, both_received_; + }; std::shared_ptr impl_; }; diff --git a/image_transport/include/image_transport/create_camera_publisher.hpp b/image_transport/include/image_transport/create_camera_publisher.hpp new file mode 100644 index 00000000..f40ae3eb --- /dev/null +++ b/image_transport/include/image_transport/create_camera_publisher.hpp @@ -0,0 +1,54 @@ +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 the Willow Garage 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 HOLDER 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. + +#ifndef IMAGE_TRANSPORT__CREATE_CAMERA_PUBLISHER_HPP_ +#define IMAGE_TRANSPORT__CREATE_CAMERA_PUBLISHER_HPP_ + +#include +#include +#include +#include + +#include "image_transport/camera_publisher.hpp" +#include "image_transport/visibility_control.hpp" + +namespace image_transport +{ + +/*! + * \brief Advertise a camera, free function version. + */ +IMAGE_TRANSPORT_PUBLIC +CameraPublisher create_camera_publisher( + rclcpp::Node * node, + const std::string & base_topic, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default); + +} // namespace image_transport + +#endif // IMAGE_TRANSPORT__CREATE_CAMERA_PUBLISHER_HPP_ diff --git a/image_transport/include/image_transport/create_camera_subscription.hpp b/image_transport/include/image_transport/create_camera_subscription.hpp new file mode 100644 index 00000000..cd8f3a64 --- /dev/null +++ b/image_transport/include/image_transport/create_camera_subscription.hpp @@ -0,0 +1,56 @@ +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 the Willow Garage 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 HOLDER 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. + +#ifndef IMAGE_TRANSPORT__CREATE_CAMERA_SUBSCRIPTION_HPP_ +#define IMAGE_TRANSPORT__CREATE_CAMERA_SUBSCRIPTION_HPP_ + +#include +#include +#include +#include + +#include "image_transport/camera_subscriber.hpp" +#include "image_transport/visibility_control.hpp" + +namespace image_transport +{ + +/*! + * \brief Subscribe to a camera, free function version. + */ +IMAGE_TRANSPORT_PUBLIC +CameraSubscriber create_camera_subscription( + rclcpp::Node * node, + const std::string & base_topic, + const CameraSubscriber::Callback & callback, + const std::string & transport, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default); + +} // namespace image_transport + +#endif // IMAGE_TRANSPORT__CREATE_CAMERA_SUBSCRIPTION_HPP_ diff --git a/image_transport/include/image_transport/create_publisher.hpp b/image_transport/include/image_transport/create_publisher.hpp new file mode 100644 index 00000000..dfcc6ac5 --- /dev/null +++ b/image_transport/include/image_transport/create_publisher.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 the Willow Garage 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 HOLDER 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. + +#ifndef IMAGE_TRANSPORT__CREATE_PUBLISHER_HPP_ +#define IMAGE_TRANSPORT__CREATE_PUBLISHER_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/publisher_options.hpp" + +#include "image_transport/camera_publisher.hpp" +#include "image_transport/publisher.hpp" +#include "image_transport/node_interfaces.hpp" +#include "image_transport/visibility_control.hpp" + +namespace image_transport +{ + +/*! + * \brief Advertise an image topic, free function version. + */ +IMAGE_TRANSPORT_PUBLIC +Publisher create_publisher( + NodeInterfaces::SharedPtr node_interfaces, + const std::string & base_topic, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default, + rclcpp::PublisherOptions options = rclcpp::PublisherOptions()); + +template +Publisher create_publisher( + NodeT && node, + const std::string & base_topic, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default, + rclcpp::PublisherOptions options = rclcpp::PublisherOptions()) +{ + return create_publisher( + create_node_interfaces(std::forward(node)), base_topic, custom_qos, options); +} + +} // namespace image_transport + +#endif // IMAGE_TRANSPORT__CREATE_PUBLISHER_HPP_ diff --git a/image_transport/include/image_transport/create_subscription.hpp b/image_transport/include/image_transport/create_subscription.hpp new file mode 100644 index 00000000..01e31910 --- /dev/null +++ b/image_transport/include/image_transport/create_subscription.hpp @@ -0,0 +1,74 @@ +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 the Willow Garage 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 HOLDER 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. + +#ifndef IMAGE_TRANSPORT__CREATE_SUBSCRIPTION_HPP_ +#define IMAGE_TRANSPORT__CREATE_SUBSCRIPTION_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/subscription_options.hpp" + +#include "image_transport/subscriber.hpp" +#include "image_transport/node_interfaces.hpp" +#include "image_transport/visibility_control.hpp" + +namespace image_transport +{ + +/** + * \brief Subscribe to an image topic, free function version. + */ +IMAGE_TRANSPORT_PUBLIC +Subscriber create_subscription( + NodeInterfaces::SharedPtr 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()); + +template +Subscriber create_subscription( + NodeT && node, + 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()) +{ + return create_subscription( + create_node_interfaces(std::forward(node)), + base_topic, callback, transport, custom_qos, options); +} + +} // namespace image_transport + +#endif // IMAGE_TRANSPORT__CREATE_SUBSCRIPTION_HPP_ diff --git a/image_transport/include/image_transport/image_transport.hpp b/image_transport/include/image_transport/image_transport.hpp index 363de187..76146759 100644 --- a/image_transport/include/image_transport/image_transport.hpp +++ b/image_transport/include/image_transport/image_transport.hpp @@ -48,73 +48,6 @@ namespace image_transport { -/*! - * \brief Advertise an image topic, free function version. - */ -IMAGE_TRANSPORT_PUBLIC -Publisher create_publisher( - NodeInterfaces::SharedPtr node_interfaces, - const std::string & base_topic, - rmw_qos_profile_t custom_qos = rmw_qos_profile_default, - rclcpp::PublisherOptions options = rclcpp::PublisherOptions()); - -template -Publisher create_publisher( - NodeT && node, - const std::string & base_topic, - rmw_qos_profile_t custom_qos = rmw_qos_profile_default, - rclcpp::PublisherOptions options = rclcpp::PublisherOptions()) -{ - return create_publisher( - create_node_interfaces(std::forward(node)), base_topic, custom_qos, options); -} - -/** - * \brief Subscribe to an image topic, free function version. - */ -IMAGE_TRANSPORT_PUBLIC -Subscriber create_subscription( - NodeInterfaces::SharedPtr 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()); - -template -Subscriber create_subscription( - NodeT && node, - 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()) -{ - return create_subscription( - create_node_interfaces(std::forward(node)), - base_topic, callback, transport, custom_qos, options); -} - -/*! - * \brief Advertise a camera, free function version. - */ -IMAGE_TRANSPORT_PUBLIC -CameraPublisher create_camera_publisher( - rclcpp::Node * node, - const std::string & base_topic, - rmw_qos_profile_t custom_qos = rmw_qos_profile_default); - -/*! - * \brief Subscribe to a camera, free function version. - */ -IMAGE_TRANSPORT_PUBLIC -CameraSubscriber create_camera_subscription( - rclcpp::Node * node, - 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(); @@ -135,17 +68,16 @@ class ImageTransport using VoidPtr = std::shared_ptr; using ImageConstPtr = sensor_msgs::msg::Image::ConstSharedPtr; using CameraInfoConstPtr = sensor_msgs::msg::CameraInfo::ConstSharedPtr; + using CameraSubscriber = CameraSubscriber; explicit ImageTransport(std::shared_ptr node) : node_(std::move(node)) {} - IMAGE_TRANSPORT_PUBLIC ~ImageTransport() = default; /*! * \brief Advertise an image topic, simple version. */ - IMAGE_TRANSPORT_PUBLIC Publisher advertise(const std::string & base_topic, uint32_t queue_size, bool latch = false) { // TODO(ros2) implement when resolved: https://github.com/ros2/ros2/issues/464 @@ -168,7 +100,6 @@ class ImageTransport /** * \brief Subscribe to an image topic, version for arbitrary std::function object. */ - IMAGE_TRANSPORT_PUBLIC Subscriber subscribe( const std::string & base_topic, uint32_t queue_size, const Subscriber::Callback & callback, @@ -186,7 +117,6 @@ class ImageTransport /** * \brief Subscribe to an image topic, version for bare function. */ - IMAGE_TRANSPORT_PUBLIC Subscriber subscribe( const std::string & base_topic, uint32_t queue_size, void (* fp)(const ImageConstPtr &), @@ -230,7 +160,6 @@ class ImageTransport /*! * \brief Advertise a synchronized camera raw image + info topic pair, simple version. */ - IMAGE_TRANSPORT_PUBLIC CameraPublisher advertiseCamera( const std::string & base_topic, uint32_t queue_size, bool latch = false) @@ -262,10 +191,9 @@ class ImageTransport * This version assumes the standard topic naming scheme, where the info topic is * named "camera_info" in the same namespace as the base image topic. */ - IMAGE_TRANSPORT_PUBLIC CameraSubscriber subscribeCamera( const std::string & base_topic, uint32_t queue_size, - const CameraSubscriber::Callback & callback, + const typename CameraSubscriber::Callback & callback, const VoidPtr & tracked_object = VoidPtr(), const TransportHints * transport_hints = nullptr) { @@ -280,7 +208,6 @@ class ImageTransport /** * \brief Subscribe to a synchronized image & camera info topic pair, version for bare function. */ - IMAGE_TRANSPORT_PUBLIC CameraSubscriber subscribeCamera( const std::string & base_topic, uint32_t queue_size, void (* fp)( @@ -335,7 +262,6 @@ class ImageTransport * \brief Returns the names of all transports declared in the system. Declared * transports are not necessarily built or loadable. */ - IMAGE_TRANSPORT_PUBLIC std::vector getDeclaredTransports() const { return image_transport::getDeclaredTransports(); @@ -344,7 +270,6 @@ class ImageTransport /** * \brief Returns the names of all transports that are loadable in the system. */ - IMAGE_TRANSPORT_PUBLIC std::vector getLoadableTransports() const { return image_transport::getLoadableTransports(); diff --git a/image_transport/include/image_transport/subscriber_filter.hpp b/image_transport/include/image_transport/subscriber_filter.hpp index 37b216ed..109174f8 100644 --- a/image_transport/include/image_transport/subscriber_filter.hpp +++ b/image_transport/include/image_transport/subscriber_filter.hpp @@ -36,7 +36,7 @@ #include "sensor_msgs/msg/image.hpp" #include "message_filters/simple_filter.h" -#include "image_transport/image_transport.hpp" +#include "image_transport/create_subscription.hpp" #include "image_transport/visibility_control.hpp" namespace image_transport @@ -62,6 +62,7 @@ void callback(const std::shared_ptr&); \endverbatim */ +template class SubscriberFilter : public message_filters::SimpleFilter { public: @@ -75,9 +76,8 @@ class SubscriberFilter : public message_filters::SimpleFilter -#include - -#include "message_filters/subscriber.h" -#include "message_filters/time_synchronizer.h" - -#include "image_transport/camera_common.hpp" -#include "image_transport/subscriber_filter.hpp" - -inline void increment(int * value) -{ - ++(*value); -} - -namespace image_transport -{ - -struct CameraSubscriber::Impl -{ - using Image = sensor_msgs::msg::Image; - using CameraInfo = sensor_msgs::msg::CameraInfo; - using TimeSync = message_filters::TimeSynchronizer; - - explicit Impl(rclcpp::Node * node) - : logger_(node->get_logger()), - sync_(10), - unsubscribed_(false), - image_received_(0), info_received_(0), both_received_(0) - { - } - - ~Impl() - { - shutdown(); - } - - bool isValid() const - { - return !unsubscribed_; - } - - void shutdown() - { - if (!unsubscribed_) { - unsubscribed_ = true; - image_sub_.unsubscribe(); - info_sub_.unsubscribe(); - } - } - - void checkImagesSynchronized() - { - int threshold = 3 * both_received_; - if (image_received_ > threshold || info_received_ > threshold) { - RCLCPP_WARN( - logger_, - "[image_transport] Topics '%s' and '%s' do not appear to be synchronized. " - "In the last 10s:\n" - "\tImage messages received: %d\n" - "\tCameraInfo messages received: %d\n" - "\tSynchronized pairs: %d", - image_sub_.getTopic().c_str(), info_sub_.getTopic().c_str(), - image_received_, info_received_, both_received_); - } - image_received_ = info_received_ = both_received_ = 0; - } - - rclcpp::Logger logger_; - SubscriberFilter image_sub_; - message_filters::Subscriber info_sub_; - TimeSync sync_; - - bool unsubscribed_; - // For detecting when the topics aren't synchronized - std::shared_ptr check_synced_timer_; - int image_received_, info_received_, both_received_; -}; - -CameraSubscriber::CameraSubscriber( - rclcpp::Node * node, - const std::string & base_topic, - const Callback & callback, - const std::string & transport, - rmw_qos_profile_t custom_qos) -: impl_(std::make_shared(node)) -{ - // Must explicitly remap the image topic since we then do some string manipulation on it - // to figure out the sibling camera_info topic. - std::string image_topic = rclcpp::expand_topic_or_service_name( - base_topic, - node->get_name(), node->get_namespace()); - std::string info_topic = getCameraInfoTopic(image_topic); - - impl_->image_sub_.subscribe(node, image_topic, transport, custom_qos); - impl_->info_sub_.subscribe(node, info_topic, custom_qos); - - impl_->sync_.connectInput(impl_->image_sub_, impl_->info_sub_); - impl_->sync_.registerCallback(std::bind(callback, std::placeholders::_1, std::placeholders::_2)); - - // Complain every 10s if it appears that the image and info topics are not synchronized - impl_->image_sub_.registerCallback(std::bind(increment, &impl_->image_received_)); - impl_->info_sub_.registerCallback(std::bind(increment, &impl_->info_received_)); - impl_->sync_.registerCallback(std::bind(increment, &impl_->both_received_)); - - impl_->check_synced_timer_ = node->create_wall_timer( - std::chrono::seconds(1), - std::bind(&Impl::checkImagesSynchronized, impl_.get())); -} - -std::string CameraSubscriber::getTopic() const -{ - if (impl_) {return impl_->image_sub_.getTopic();} - return std::string(); -} - -std::string CameraSubscriber::getInfoTopic() const -{ - if (impl_) {return impl_->info_sub_.getSubscriber()->get_topic_name();} - return std::string(); -} - -size_t CameraSubscriber::getNumPublishers() const -{ - // TODO(ros2) Fix this when ros2 has better subscriber counting. - // @todo Fix this when message_filters::Subscriber has getNumPublishers() - // if (impl_) { - // return std::max(impl_->image_sub_.getNumPublishers(), impl_->info_sub_.getNumPublishers()); - // } - // if (impl_) return impl_->image_sub_.getNumPublishers(); - return 0; -} - -std::string CameraSubscriber::getTransport() const -{ - if (impl_) {return impl_->image_sub_.getTransport();} - return std::string(); -} - -void CameraSubscriber::shutdown() -{ - if (impl_) {impl_->shutdown();} -} - -CameraSubscriber::operator void *() const -{ - return (impl_ && impl_->isValid()) ? reinterpret_cast(1) : reinterpret_cast(0); -} - -} // namespace image_transport diff --git a/image_transport/src/image_transport.cpp b/image_transport/src/image_transport.cpp index 70ce5e03..0a6e87a8 100644 --- a/image_transport/src/image_transport.cpp +++ b/image_transport/src/image_transport.cpp @@ -34,6 +34,10 @@ #include "pluginlib/class_loader.hpp" +#include "image_transport/create_subscription.hpp" +#include "image_transport/create_publisher.hpp" +#include "image_transport/create_camera_publisher.hpp" +#include "image_transport/create_camera_subscription.hpp" #include "image_transport/camera_common.hpp" #include "image_transport/loader_fwds.hpp" #include "image_transport/publisher_plugin.hpp" @@ -85,10 +89,10 @@ CameraPublisher create_camera_publisher( return CameraPublisher(node, base_topic, custom_qos); } -CameraSubscriber create_camera_subscription( +CameraSubscriber create_camera_subscription( rclcpp::Node * node, const std::string & base_topic, - const CameraSubscriber::Callback & callback, + const CameraSubscriber::Callback & callback, const std::string & transport, rmw_qos_profile_t custom_qos) { diff --git a/image_transport/src/republish.cpp b/image_transport/src/republish.cpp index a92df611..014d3fd3 100644 --- a/image_transport/src/republish.cpp +++ b/image_transport/src/republish.cpp @@ -31,9 +31,9 @@ #include #include "pluginlib/class_loader.hpp" - #include "rclcpp/rclcpp.hpp" +#include "image_transport/create_publisher.hpp" #include "image_transport/image_transport.hpp" #include "image_transport/publisher_plugin.hpp" diff --git a/image_transport/test/test_message_passing.cpp b/image_transport/test/test_message_passing.cpp index 0a4372c3..e2b7c158 100644 --- a/image_transport/test/test_message_passing.cpp +++ b/image_transport/test/test_message_passing.cpp @@ -34,6 +34,9 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp/node.hpp" +#include "image_transport/create_publisher.hpp" +#include "image_transport/create_camera_subscription.hpp" +#include "image_transport/create_camera_publisher.hpp" #include "image_transport/image_transport.hpp" #include "sensor_msgs/msg/image.hpp" diff --git a/image_transport/test/test_publisher.cpp b/image_transport/test/test_publisher.cpp index 03399c0a..a2a79b67 100644 --- a/image_transport/test/test_publisher.cpp +++ b/image_transport/test/test_publisher.cpp @@ -33,6 +33,8 @@ #include "rclcpp/rclcpp.hpp" +#include "image_transport/create_publisher.hpp" +#include "image_transport/create_camera_publisher.hpp" #include "image_transport/image_transport.hpp" class TestPublisher : public ::testing::Test diff --git a/image_transport/test/test_qos_override.cpp b/image_transport/test/test_qos_override.cpp index 3b2b7ea3..d126cdd4 100644 --- a/image_transport/test/test_qos_override.cpp +++ b/image_transport/test/test_qos_override.cpp @@ -33,6 +33,7 @@ #include "rclcpp/rclcpp.hpp" +#include "image_transport/create_publisher.hpp" #include "image_transport/image_transport.hpp" class TestQosOverride : public ::testing::Test diff --git a/image_transport/test/test_remapping.cpp b/image_transport/test/test_remapping.cpp index 9106a710..d491bcd3 100644 --- a/image_transport/test/test_remapping.cpp +++ b/image_transport/test/test_remapping.cpp @@ -37,6 +37,7 @@ #include "rclcpp/node.hpp" #include "utils.hpp" +#include "image_transport/create_publisher.hpp" #include "image_transport/image_transport.hpp" class TestPublisher : public ::testing::Test diff --git a/image_transport/test/test_subscriber.cpp b/image_transport/test/test_subscriber.cpp index 787ae93b..794d01e8 100644 --- a/image_transport/test/test_subscriber.cpp +++ b/image_transport/test/test_subscriber.cpp @@ -33,6 +33,7 @@ #include "rclcpp/rclcpp.hpp" +#include "image_transport/create_camera_subscription.hpp" #include "image_transport/image_transport.hpp" class TestSubscriber : public ::testing::Test