diff --git a/image_transport/CMakeLists.txt b/image_transport/CMakeLists.txt index 089d5a12..12054820 100644 --- a/image_transport/CMakeLists.txt +++ b/image_transport/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(ament_cmake_ros REQUIRED) find_package(message_filters REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(rclcpp_components REQUIRED) find_package(sensor_msgs REQUIRED) @@ -37,6 +38,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC message_filters::message_filters rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ${sensor_msgs_TARGETS}) target_link_libraries(${PROJECT_NAME} PRIVATE pluginlib::pluginlib) @@ -107,7 +109,7 @@ install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) ament_export_targets(export_${PROJECT_NAME}) -ament_export_dependencies(message_filters rclcpp sensor_msgs pluginlib) +ament_export_dependencies(message_filters rclcpp rclcpp_lifecycle sensor_msgs pluginlib) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -131,30 +133,60 @@ if(BUILD_TESTING) target_link_libraries(${PROJECT_NAME}-publisher ${PROJECT_NAME}) endif() + ament_add_gtest(${PROJECT_NAME}-publisher_lifecycle test/test_publisher_lifecycle.cpp) + if(TARGET ${PROJECT_NAME}-publisher_lifecycle) + target_link_libraries(${PROJECT_NAME}-publisher_lifecycle ${PROJECT_NAME}) + endif() + ament_add_gtest(${PROJECT_NAME}-subscriber test/test_subscriber.cpp) if(TARGET ${PROJECT_NAME}-subscriber) target_link_libraries(${PROJECT_NAME}-subscriber ${PROJECT_NAME}) endif() + ament_add_gtest(${PROJECT_NAME}-subscriber_lifecycle test/test_subscriber_lifecycle.cpp) + if(TARGET ${PROJECT_NAME}-subscriber_lifecycle) + target_link_libraries(${PROJECT_NAME}-subscriber_lifecycle ${PROJECT_NAME}) + endif() + ament_add_gtest(${PROJECT_NAME}-message_passing test/test_message_passing.cpp) if(TARGET ${PROJECT_NAME}-message_passing) target_link_libraries(${PROJECT_NAME}-message_passing ${PROJECT_NAME}) endif() + ament_add_gtest(${PROJECT_NAME}-message_passing_lifecycle test/test_message_passing_lifecycle.cpp) + if(TARGET ${PROJECT_NAME}-message_passing_lifecycle) + target_link_libraries(${PROJECT_NAME}-message_passing_lifecycle ${PROJECT_NAME}) + endif() + ament_add_gtest(${PROJECT_NAME}-remapping test/test_remapping.cpp) if(TARGET ${PROJECT_NAME}-remapping) target_link_libraries(${PROJECT_NAME}-remapping ${PROJECT_NAME}) endif() + ament_add_gtest(${PROJECT_NAME}-remapping_lifecycle test/test_remapping_lifecycle.cpp) + if(TARGET ${PROJECT_NAME}-remapping_lifecycle) + target_link_libraries(${PROJECT_NAME}-remapping_lifecycle ${PROJECT_NAME}) + endif() + ament_add_gtest(${PROJECT_NAME}-qos_override test/test_qos_override.cpp) if(TARGET ${PROJECT_NAME}-qos_override) target_link_libraries(${PROJECT_NAME}-qos_override ${PROJECT_NAME}) endif() + ament_add_gtest(${PROJECT_NAME}-qos_override_lifecycle test/test_qos_override_lifecycle.cpp) + if(TARGET ${PROJECT_NAME}-qos_override_lifecycle) + target_link_libraries(${PROJECT_NAME}-qos_override_lifecycle ${PROJECT_NAME}) + endif() + ament_add_gtest(${PROJECT_NAME}-single_subscriber_publisher test/test_single_subscriber_publisher.cpp) if(TARGET ${PROJECT_NAME}-single_subscriber_publisher) target_link_libraries(${PROJECT_NAME}-single_subscriber_publisher ${PROJECT_NAME}) endif() + + ament_add_gtest(${PROJECT_NAME}-single_subscriber_publisher_lifecycle test/test_single_subscriber_publisher_lifecycle.cpp) + if(TARGET ${PROJECT_NAME}-single_subscriber_publisher_lifecycle) + target_link_libraries(${PROJECT_NAME}-single_subscriber_publisher_lifecycle ${PROJECT_NAME}) + endif() endif() ament_package() diff --git a/image_transport/default_plugins.xml b/image_transport/default_plugins.xml index 70c86767..fb3e1569 100644 --- a/image_transport/default_plugins.xml +++ b/image_transport/default_plugins.xml @@ -1,8 +1,17 @@ + type="image_transport::RawPublisher<rclcpp::Node>" + base_class_type="image_transport::PublisherPlugin<rclcpp::Node>"> + + This is the default publisher. It publishes the Image as-is on the base topic. + + + + This is the default publisher. It publishes the Image as-is on the base topic. @@ -10,8 +19,17 @@ + type="image_transport::RawSubscriber<rclcpp::Node>" + base_class_type="image_transport::SubscriberPlugin<rclcpp::Node>"> + + This is the default pass-through subscriber for topics of type sensor_msgs/Image. + + + + This is the default pass-through subscriber for topics of type sensor_msgs/Image. diff --git a/image_transport/include/image_transport/camera_publisher.hpp b/image_transport/include/image_transport/camera_publisher.hpp index ad80328c..5d489cbd 100644 --- a/image_transport/include/image_transport/camera_publisher.hpp +++ b/image_transport/include/image_transport/camera_publisher.hpp @@ -34,6 +34,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "sensor_msgs/msg/image.hpp" #include "sensor_msgs/msg/camera_info.hpp" @@ -43,9 +44,6 @@ namespace image_transport { - -class ImageTransport; - /** * \brief Manages advertisements for publishing camera images. * @@ -61,6 +59,7 @@ class ImageTransport; * associated with that handle will stop being called. Once all CameraPublisher for a * given base topic go out of scope the topic (and all subtopics) will be unadvertised. */ +template class CameraPublisher { public: @@ -69,7 +68,14 @@ class CameraPublisher IMAGE_TRANSPORT_PUBLIC CameraPublisher( - rclcpp::Node * node, + NodeType * node, + const std::string & base_topic, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default, + rclcpp::PublisherOptions = rclcpp::PublisherOptions()); + + IMAGE_TRANSPORT_PUBLIC + CameraPublisher( + std::shared_ptr node, const std::string & base_topic, rmw_qos_profile_t custom_qos = rmw_qos_profile_default, rclcpp::PublisherOptions = rclcpp::PublisherOptions()); @@ -163,6 +169,11 @@ class CameraPublisher bool operator==(const CameraPublisher & rhs) const {return impl_ == rhs.impl_;} private: + void initialise( + const std::string & base_topic, + rmw_qos_profile_t custom_qos, + rclcpp::PublisherOptions options); + struct Impl; std::shared_ptr impl_; }; diff --git a/image_transport/include/image_transport/camera_subscriber.hpp b/image_transport/include/image_transport/camera_subscriber.hpp index 5157111d..ec540753 100644 --- a/image_transport/include/image_transport/camera_subscriber.hpp +++ b/image_transport/include/image_transport/camera_subscriber.hpp @@ -34,6 +34,7 @@ #include #include "rclcpp/node.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "sensor_msgs/msg/camera_info.hpp" #include "sensor_msgs/msg/image.hpp" @@ -42,9 +43,6 @@ namespace image_transport { - -class ImageTransport; - /** * \brief Manages a subscription callback on synchronized Image and CameraInfo topics. * @@ -60,6 +58,7 @@ 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. */ +template class CameraSubscriber { public: @@ -71,7 +70,15 @@ class CameraSubscriber IMAGE_TRANSPORT_PUBLIC CameraSubscriber( - rclcpp::Node * node, + NodeType * node, + const std::string & base_topic, + const Callback & callback, + const std::string & transport, + rmw_qos_profile_t = rmw_qos_profile_default); + + IMAGE_TRANSPORT_PUBLIC + CameraSubscriber( + std::shared_ptr node, const std::string & base_topic, const Callback & callback, const std::string & transport, @@ -120,6 +127,12 @@ class CameraSubscriber bool operator==(const CameraSubscriber & rhs) const {return impl_ == rhs.impl_;} private: + void initialise( + const std::string & base_topic, + const Callback & callback, + const std::string & transport, + rmw_qos_profile_t custom_qos); + struct Impl; std::shared_ptr impl_; }; diff --git a/image_transport/include/image_transport/image_transport.hpp b/image_transport/include/image_transport/image_transport.hpp index e430cd92..5742bbe9 100644 --- a/image_transport/include/image_transport/image_transport.hpp +++ b/image_transport/include/image_transport/image_transport.hpp @@ -49,9 +49,18 @@ namespace image_transport /*! * \brief Advertise an image topic, free function version. */ +template IMAGE_TRANSPORT_PUBLIC -Publisher create_publisher( - rclcpp::Node * node, +Publisher create_publisher( + NodeType * node, + const std::string & base_topic, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default, + rclcpp::PublisherOptions options = rclcpp::PublisherOptions()); + +template +IMAGE_TRANSPORT_PUBLIC +Publisher create_publisher( + std::shared_ptr node, const std::string & base_topic, rmw_qos_profile_t custom_qos = rmw_qos_profile_default, rclcpp::PublisherOptions options = rclcpp::PublisherOptions()); @@ -59,11 +68,22 @@ Publisher create_publisher( /** * \brief Subscribe to an image topic, free function version. */ +template +IMAGE_TRANSPORT_PUBLIC +Subscriber create_subscription( + NodeType * node, + const std::string & base_topic, + const typename Subscriber::Callback & callback, + const std::string & transport, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default, + rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); + +template IMAGE_TRANSPORT_PUBLIC -Subscriber create_subscription( - rclcpp::Node * node, +Subscriber create_subscription( + std::shared_ptr node, const std::string & base_topic, - const Subscriber::Callback & callback, + const typename Subscriber::Callback & callback, const std::string & transport, rmw_qos_profile_t custom_qos = rmw_qos_profile_default, rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); @@ -71,9 +91,18 @@ Subscriber create_subscription( /*! * \brief Advertise a camera, free function version. */ +template IMAGE_TRANSPORT_PUBLIC -CameraPublisher create_camera_publisher( - rclcpp::Node * node, +CameraPublisher create_camera_publisher( + NodeType * node, + const std::string & base_topic, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default, + rclcpp::PublisherOptions pub_options = rclcpp::PublisherOptions()); + +template +IMAGE_TRANSPORT_PUBLIC +CameraPublisher create_camera_publisher( + std::shared_ptr node, const std::string & base_topic, rmw_qos_profile_t custom_qos = rmw_qos_profile_default, rclcpp::PublisherOptions pub_options = rclcpp::PublisherOptions()); @@ -81,17 +110,29 @@ CameraPublisher create_camera_publisher( /*! * \brief Subscribe to a camera, free function version. */ +template IMAGE_TRANSPORT_PUBLIC -CameraSubscriber create_camera_subscription( - rclcpp::Node * node, +CameraSubscriber create_camera_subscription( + NodeType * node, const std::string & base_topic, - const CameraSubscriber::Callback & callback, + const typename CameraSubscriber::Callback & callback, const std::string & transport, rmw_qos_profile_t custom_qos = rmw_qos_profile_default); +template +IMAGE_TRANSPORT_PUBLIC +CameraSubscriber create_camera_subscription( + std::shared_ptr node, + const std::string & base_topic, + const typename CameraSubscriber::Callback & callback, + const std::string & transport, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default); + +template IMAGE_TRANSPORT_PUBLIC std::vector getDeclaredTransports(); +template IMAGE_TRANSPORT_PUBLIC std::vector getLoadableTransports(); @@ -102,6 +143,7 @@ std::vector getLoadableTransports(); * subscribe() functions for creating advertisements and subscriptions of image topics. */ +template class ImageTransport { public: @@ -110,7 +152,10 @@ class ImageTransport using CameraInfoConstPtr = sensor_msgs::msg::CameraInfo::ConstSharedPtr; IMAGE_TRANSPORT_PUBLIC - explicit ImageTransport(rclcpp::Node::SharedPtr node); + explicit ImageTransport(NodeType * node); + + IMAGE_TRANSPORT_PUBLIC + explicit ImageTransport(std::shared_ptr node); IMAGE_TRANSPORT_PUBLIC ~ImageTransport(); @@ -119,13 +164,14 @@ class ImageTransport * \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); /*! * \brief Advertise an image topic, simple version. */ IMAGE_TRANSPORT_PUBLIC - Publisher advertise( + Publisher advertise( const std::string & base_topic, rmw_qos_profile_t custom_qos, bool latch = false); @@ -143,9 +189,9 @@ class ImageTransport * \brief Subscribe to an image topic, version for arbitrary std::function object. */ IMAGE_TRANSPORT_PUBLIC - Subscriber subscribe( + Subscriber subscribe( const std::string & base_topic, uint32_t queue_size, - const Subscriber::Callback & callback, + const typename Subscriber::Callback & callback, const VoidPtr & tracked_object = VoidPtr(), const TransportHints * transport_hints = nullptr, const rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); @@ -154,7 +200,7 @@ class ImageTransport * \brief Subscribe to an image topic, version for bare function. */ IMAGE_TRANSPORT_PUBLIC - Subscriber subscribe( + Subscriber subscribe( const std::string & base_topic, uint32_t queue_size, void (* fp)(const ImageConstPtr &), const TransportHints * transport_hints = nullptr, @@ -170,7 +216,7 @@ class ImageTransport * \brief Subscribe to an image topic, version for class member function with bare pointer. */ template - Subscriber subscribe( + Subscriber subscribe( const std::string & base_topic, uint32_t queue_size, void (T::* fp)(const ImageConstPtr &), T * obj, const TransportHints * transport_hints = nullptr, @@ -185,7 +231,7 @@ class ImageTransport * \brief Subscribe to an image topic, version for class member function with shared_ptr. */ template - Subscriber subscribe( + Subscriber subscribe( const std::string & base_topic, uint32_t queue_size, void (T::* fp)(const ImageConstPtr &), const std::shared_ptr & obj, @@ -201,9 +247,9 @@ class ImageTransport * \brief Subscribe to an image topic, version for arbitrary std::function object and QoS. */ IMAGE_TRANSPORT_PUBLIC - Subscriber subscribe( + Subscriber subscribe( const std::string & base_topic, rmw_qos_profile_t custom_qos, - const Subscriber::Callback & callback, + const typename Subscriber::Callback & callback, const VoidPtr & tracked_object, const TransportHints * transport_hints, const rclcpp::SubscriptionOptions options); @@ -212,7 +258,7 @@ class ImageTransport * \brief Subscribe to an image topic, version for bare function. */ IMAGE_TRANSPORT_PUBLIC - Subscriber subscribe( + Subscriber subscribe( const std::string & base_topic, rmw_qos_profile_t custom_qos, void (* fp)(const ImageConstPtr &), const TransportHints * transport_hints = nullptr, @@ -228,7 +274,7 @@ class ImageTransport * \brief Subscribe to an image topic, version for class member function with bare pointer. */ template - Subscriber subscribe( + Subscriber subscribe( const std::string & base_topic, rmw_qos_profile_t custom_qos, void (T::* fp)(const ImageConstPtr &), T * obj, const TransportHints * transport_hints = nullptr, @@ -243,7 +289,7 @@ class ImageTransport * \brief Subscribe to an image topic, version for class member function with shared_ptr. */ template - Subscriber subscribe( + Subscriber subscribe( const std::string & base_topic, rmw_qos_profile_t custom_qos, void (T::* fp)(const ImageConstPtr &), const std::shared_ptr & obj, @@ -259,7 +305,7 @@ class ImageTransport * \brief Advertise a synchronized camera raw image + info topic pair, simple version. */ IMAGE_TRANSPORT_PUBLIC - CameraPublisher advertiseCamera( + CameraPublisher advertiseCamera( const std::string & base_topic, uint32_t queue_size, bool latch = false); @@ -284,9 +330,9 @@ class ImageTransport * named "camera_info" in the same namespace as the base image topic. */ IMAGE_TRANSPORT_PUBLIC - CameraSubscriber subscribeCamera( + 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); @@ -294,7 +340,7 @@ class ImageTransport * \brief Subscribe to a synchronized image & camera info topic pair, version for bare function. */ IMAGE_TRANSPORT_PUBLIC - CameraSubscriber subscribeCamera( + CameraSubscriber subscribeCamera( const std::string & base_topic, uint32_t queue_size, void (* fp)( const ImageConstPtr &, @@ -302,7 +348,7 @@ class ImageTransport const TransportHints * transport_hints = nullptr) { return subscribeCamera( - base_topic, queue_size, CameraSubscriber::Callback(fp), VoidPtr(), + base_topic, queue_size, typename CameraSubscriber::Callback(fp), VoidPtr(), transport_hints); } @@ -311,7 +357,7 @@ class ImageTransport * function with bare pointer. */ template - CameraSubscriber subscribeCamera( + CameraSubscriber subscribeCamera( const std::string & base_topic, uint32_t queue_size, void (T::* fp)( const ImageConstPtr &, @@ -329,7 +375,7 @@ class ImageTransport * function with shared_ptr. */ template - CameraSubscriber subscribeCamera( + CameraSubscriber subscribeCamera( const std::string & base_topic, uint32_t queue_size, void (T::* fp)( const ImageConstPtr &, diff --git a/image_transport/include/image_transport/loader_fwds.hpp b/image_transport/include/image_transport/loader_fwds.hpp index 5f4cf991..de60b032 100644 --- a/image_transport/include/image_transport/loader_fwds.hpp +++ b/image_transport/include/image_transport/loader_fwds.hpp @@ -42,14 +42,20 @@ class ClassLoader; namespace image_transport { +template class PublisherPlugin; +template class SubscriberPlugin; -typedef pluginlib::ClassLoader PubLoader; -typedef std::shared_ptr PubLoaderPtr; +template +using PubLoader = pluginlib::ClassLoader>; +template +using PubLoaderPtr = std::shared_ptr>; -typedef pluginlib::ClassLoader SubLoader; -typedef std::shared_ptr SubLoaderPtr; +template +using SubLoader = pluginlib::ClassLoader>; +template +using SubLoaderPtr = std::shared_ptr>; } // namespace image_transport #endif // IMAGE_TRANSPORT__LOADER_FWDS_HPP_ diff --git a/image_transport/include/image_transport/publisher.hpp b/image_transport/include/image_transport/publisher.hpp index 4f529121..e95e76e0 100644 --- a/image_transport/include/image_transport/publisher.hpp +++ b/image_transport/include/image_transport/publisher.hpp @@ -34,6 +34,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "sensor_msgs/msg/image.hpp" @@ -62,6 +63,7 @@ namespace image_transport * associated with that handle will stop being called. Once all Publisher for a * given base topic go out of scope the topic (and all subtopics) will be unadvertised. */ +template class Publisher { public: @@ -70,12 +72,19 @@ class Publisher IMAGE_TRANSPORT_PUBLIC Publisher( - rclcpp::Node * nh, + NodeType * nh, const std::string & base_topic, - PubLoaderPtr loader, + PubLoaderPtr loader, rmw_qos_profile_t custom_qos, rclcpp::PublisherOptions options = rclcpp::PublisherOptions()); + IMAGE_TRANSPORT_PUBLIC + Publisher( + std::shared_ptr nh, + const std::string & base_topic, + PubLoaderPtr loader, + rmw_qos_profile_t custom_qos, + rclcpp::PublisherOptions options = rclcpp::PublisherOptions()); /*! * \brief Returns the number of subscribers that are currently connected to * this Publisher. @@ -128,6 +137,12 @@ class Publisher bool operator==(const Publisher & rhs) const {return impl_ == rhs.impl_;} private: + void initialise( + const std::string & base_topic, + PubLoaderPtr loader, + rmw_qos_profile_t custom_qos, + rclcpp::PublisherOptions options); + struct Impl; std::shared_ptr impl_; }; diff --git a/image_transport/include/image_transport/publisher_plugin.hpp b/image_transport/include/image_transport/publisher_plugin.hpp index c545b4ca..2a80b425 100644 --- a/image_transport/include/image_transport/publisher_plugin.hpp +++ b/image_transport/include/image_transport/publisher_plugin.hpp @@ -32,8 +32,10 @@ #include #include #include +#include #include "rclcpp/node.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "sensor_msgs/msg/image.hpp" #include "image_transport/single_subscriber_publisher.hpp" @@ -45,6 +47,7 @@ namespace image_transport /** * \brief Base class for plugins to Publisher. */ +template class PublisherPlugin { public: @@ -72,7 +75,16 @@ class PublisherPlugin * \brief Advertise a topic, simple version. */ void advertise( - rclcpp::Node * nh, + NodeType * nh, + const std::string & base_topic, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default, + rclcpp::PublisherOptions options = rclcpp::PublisherOptions()) + { + advertiseImpl(std::shared_ptr(nh), base_topic, custom_qos, options); + } + + void advertise( + std::shared_ptr nh, const std::string & base_topic, rmw_qos_profile_t custom_qos = rmw_qos_profile_default, rclcpp::PublisherOptions options = rclcpp::PublisherOptions()) @@ -156,7 +168,7 @@ class PublisherPlugin * \brief Advertise a topic. Must be implemented by the subclass. */ virtual void advertiseImpl( - rclcpp::Node * node, + std::shared_ptr nh, const std::string & base_topic, rmw_qos_profile_t custom_qos, rclcpp::PublisherOptions options) = 0; diff --git a/image_transport/include/image_transport/raw_publisher.hpp b/image_transport/include/image_transport/raw_publisher.hpp index 65fed4a7..461972fe 100644 --- a/image_transport/include/image_transport/raw_publisher.hpp +++ b/image_transport/include/image_transport/raw_publisher.hpp @@ -47,7 +47,8 @@ namespace image_transport * messages on the base topic. */ -class RawPublisher : public SimplePublisherPlugin +template +class RawPublisher : public SimplePublisherPlugin { public: virtual ~RawPublisher() {} @@ -64,19 +65,26 @@ class RawPublisher : public SimplePublisherPlugin protected: [[deprecated("Use publish(const sensor_msgs::msg::Image&, const PublisherT&) instead.")]] - virtual void publish(const sensor_msgs::msg::Image & message, const PublishFn & publish_fn) const + virtual void publish( + const sensor_msgs::msg::Image & message, + const typename SimplePublisherPlugin::PublishFn & publish_fn) const { publish_fn(message); } - virtual void publish(const sensor_msgs::msg::Image & message, const PublisherT & publisher) const + virtual void publish( + const sensor_msgs::msg::Image & message, + const typename SimplePublisherPlugin::PublisherT & publisher) const { publisher->publish(message); } virtual void publish( sensor_msgs::msg::Image::UniquePtr message, - const PublisherT & publisher) const + const typename SimplePublisherPlugin::PublisherT & publisher) const { publisher->publish(std::move(message)); } diff --git a/image_transport/include/image_transport/raw_subscriber.hpp b/image_transport/include/image_transport/raw_subscriber.hpp index efe66e5b..db677e4d 100644 --- a/image_transport/include/image_transport/raw_subscriber.hpp +++ b/image_transport/include/image_transport/raw_subscriber.hpp @@ -46,7 +46,8 @@ namespace image_transport * RawSubscriber is a simple wrapper for ros::Subscriber which listens for Image messages * and passes them through to the callback. */ -class RawSubscriber : public SimpleSubscriberPlugin +template +class RawSubscriber : public SimpleSubscriberPlugin { public: virtual ~RawSubscriber() {} @@ -59,7 +60,7 @@ class RawSubscriber : public SimpleSubscriberPlugin protected: void internalCallback( const std::shared_ptr & message, - const Callback & user_cb) override + const typename SubscriberPlugin::Callback & user_cb) override { user_cb(message); } diff --git a/image_transport/include/image_transport/republish.hpp b/image_transport/include/image_transport/republish.hpp index 11fb7f43..31577e5b 100644 --- a/image_transport/include/image_transport/republish.hpp +++ b/image_transport/include/image_transport/republish.hpp @@ -52,10 +52,10 @@ class Republisher : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; bool initialized_{false}; - image_transport::Subscriber sub; - image_transport::Publisher pub; - pluginlib::UniquePtr instance; - std::shared_ptr> loader; + image_transport::Subscriber sub; + image_transport::Publisher pub; + pluginlib::UniquePtr> instance; + std::shared_ptr>> loader; }; } // namespace image_transport diff --git a/image_transport/include/image_transport/simple_publisher_plugin.hpp b/image_transport/include/image_transport/simple_publisher_plugin.hpp index 4ca77524..6e8fc84e 100644 --- a/image_transport/include/image_transport/simple_publisher_plugin.hpp +++ b/image_transport/include/image_transport/simple_publisher_plugin.hpp @@ -61,8 +61,8 @@ namespace image_transport * getTopicToAdvertise() controls the name of the internal communication topic. * It defaults to \/\. */ -template -class SimplePublisherPlugin : public PublisherPlugin +template +class SimplePublisherPlugin : public PublisherPlugin { public: virtual ~SimplePublisherPlugin() {} @@ -114,17 +114,18 @@ class SimplePublisherPlugin : public PublisherPlugin protected: void advertiseImpl( - rclcpp::Node * node, + std::shared_ptr nh, 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); + auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos); + simple_impl_ = std::make_unique(nh); + simple_impl_->pub_ = simple_impl_->node_->template create_publisher( + transport_topic, qos, options); 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); } typedef typename rclcpp::Publisher::SharedPtr PublisherT; @@ -146,7 +147,7 @@ class SimplePublisherPlugin : public PublisherPlugin const PublishFn & /*publish_fn*/) const { throw std::logic_error( - "publish(const sensor_msgs::msg::Image&, const PublishFn&) is not implemented."); + "publish(const sensor_msgs::msg::Image&, const PublishFn&) is not implemented."); } /** @@ -173,7 +174,7 @@ class SimplePublisherPlugin : public PublisherPlugin const PublisherT & /*publisher*/) const { throw std::logic_error( - "publish(sensor_msgs::msg::Image::UniquePtr, const PublisherT&) is not implemented."); + "publish(sensor_msgs::msg::Image::UniquePtr, const PublisherT&) is not implemented."); } /** @@ -183,19 +184,25 @@ class SimplePublisherPlugin : public PublisherPlugin */ virtual std::string getTopicToAdvertise(const std::string & base_topic) const { - return base_topic + "/" + getTransportName(); + return base_topic + "/" + PublisherPlugin::getTransportName(); } private: struct SimplePublisherPluginImpl { - explicit SimplePublisherPluginImpl(rclcpp::Node * node) + explicit SimplePublisherPluginImpl(NodeType * node) + : node_(node), + logger_(node->get_logger()) + { + } + + explicit SimplePublisherPluginImpl(std::shared_ptr node) : node_(node), logger_(node->get_logger()) { } - rclcpp::Node * node_; + std::shared_ptr node_; rclcpp::Logger logger_; PublisherT 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..991e11e8 100644 --- a/image_transport/include/image_transport/simple_subscriber_plugin.hpp +++ b/image_transport/include/image_transport/simple_subscriber_plugin.hpp @@ -59,8 +59,8 @@ namespace image_transport * getTopicToSubscribe() controls the name of the internal communication topic. It * defaults to \/\. */ -template -class SimpleSubscriberPlugin : public SubscriberPlugin +template +class SimpleSubscriberPlugin : public SubscriberPlugin { public: virtual ~SimpleSubscriberPlugin() {} @@ -96,7 +96,7 @@ class SimpleSubscriberPlugin : public SubscriberPlugin virtual void internalCallback( const typename std::shared_ptr & message, - const Callback & user_cb) = 0; + const typename SubscriberPlugin::Callback & user_cb) = 0; /** * \brief Return the communication topic name for a given base topic. @@ -105,13 +105,13 @@ class SimpleSubscriberPlugin : public SubscriberPlugin */ virtual std::string getTopicToSubscribe(const std::string & base_topic) const { - return base_topic + "/" + getTransportName(); + return base_topic + "/" + SubscriberPlugin::getTransportName(); } void subscribeImpl( - rclcpp::Node * node, + std::shared_ptr node, const std::string & base_topic, - const Callback & callback, + const typename SubscriberPlugin::Callback & callback, rmw_qos_profile_t custom_qos, rclcpp::SubscriptionOptions options) override { @@ -120,7 +120,7 @@ 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_ = node->template create_subscription( getTopicToSubscribe(base_topic), qos, [this, callback](const typename std::shared_ptr msg) { internalCallback(msg, callback); diff --git a/image_transport/include/image_transport/single_subscriber_publisher.hpp b/image_transport/include/image_transport/single_subscriber_publisher.hpp index e6347db8..f553f9e2 100644 --- a/image_transport/include/image_transport/single_subscriber_publisher.hpp +++ b/image_transport/include/image_transport/single_subscriber_publisher.hpp @@ -80,8 +80,6 @@ class SingleSubscriberPublisher std::string topic_; GetNumSubscribersFn num_subscribers_fn_; PublishFn publish_fn_; - - friend class Publisher; // to get publish_fn_ directly }; typedef std::function SubscriberStatusCallback; diff --git a/image_transport/include/image_transport/subscriber.hpp b/image_transport/include/image_transport/subscriber.hpp index f0b87245..a32df9d0 100644 --- a/image_transport/include/image_transport/subscriber.hpp +++ b/image_transport/include/image_transport/subscriber.hpp @@ -34,6 +34,7 @@ #include #include "rclcpp/node.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "sensor_msgs/msg/image.hpp" #include "image_transport/exception.hpp" @@ -58,6 +59,7 @@ namespace image_transport * associated with that handle will stop being called. Once all Subscriber for a given * topic go out of scope the topic will be unsubscribed. */ +template class Subscriber { public: @@ -68,10 +70,20 @@ class Subscriber IMAGE_TRANSPORT_PUBLIC Subscriber( - rclcpp::Node * node, + NodeType * node, const std::string & base_topic, const Callback & callback, - SubLoaderPtr loader, + SubLoaderPtr loader, + const std::string & transport, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default, + rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); + + IMAGE_TRANSPORT_PUBLIC + Subscriber( + std::shared_ptr 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()); @@ -113,6 +125,13 @@ class Subscriber bool operator==(const Subscriber & rhs) const {return impl_ == rhs.impl_;} private: + void initialise( + const std::string & base_topic, + const Callback & callback, + const std::string & transport, + rmw_qos_profile_t custom_qos, + rclcpp::SubscriptionOptions options); + struct Impl; std::shared_ptr impl_; }; diff --git a/image_transport/include/image_transport/subscriber_filter.hpp b/image_transport/include/image_transport/subscriber_filter.hpp index 37b216ed..b7b731d6 100644 --- a/image_transport/include/image_transport/subscriber_filter.hpp +++ b/image_transport/include/image_transport/subscriber_filter.hpp @@ -62,6 +62,7 @@ void callback(const std::shared_ptr&); \endverbatim */ +template class SubscriberFilter : public message_filters::SimpleFilter { public: @@ -77,7 +78,15 @@ class SubscriberFilter : public message_filters::SimpleFilter node, const std::string & base_topic, const std::string & transport) { subscribe(node, base_topic, transport); @@ -107,7 +116,22 @@ class SubscriberFilter : public message_filters::SimpleFilter node, const std::string & base_topic, const std::string & transport, rmw_qos_profile_t custom_qos = rmw_qos_profile_default, @@ -157,7 +181,7 @@ class SubscriberFilter : public message_filters::SimpleFilter & getSubscriber() const { return sub_; } @@ -168,7 +192,7 @@ class SubscriberFilter : public message_filters::SimpleFilter sub_; }; } // namespace image_transport diff --git a/image_transport/include/image_transport/subscriber_plugin.hpp b/image_transport/include/image_transport/subscriber_plugin.hpp index bae15b9c..ee9ddf5e 100644 --- a/image_transport/include/image_transport/subscriber_plugin.hpp +++ b/image_transport/include/image_transport/subscriber_plugin.hpp @@ -34,6 +34,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "sensor_msgs/msg/image.hpp" #include "image_transport/visibility_control.hpp" @@ -44,6 +45,7 @@ namespace image_transport /** * \brief Base class for plugins to Subscriber. */ +template class IMAGE_TRANSPORT_PUBLIC SubscriberPlugin { public: @@ -65,7 +67,18 @@ 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, + NodeType * node, 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( + std::shared_ptr(node), base_topic, callback, custom_qos, + options); + } + + void subscribe( + std::shared_ptr node, const std::string & base_topic, const Callback & callback, rmw_qos_profile_t custom_qos = rmw_qos_profile_default, rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) @@ -77,7 +90,19 @@ class IMAGE_TRANSPORT_PUBLIC SubscriberPlugin * \brief Subscribe to an image topic, version for bare function. */ void subscribe( - rclcpp::Node * node, const std::string & base_topic, + NodeType * node, 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, + std::function(fp), + custom_qos, options); + } + + void subscribe( + std::shared_ptr node, 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()) @@ -93,7 +118,19 @@ class IMAGE_TRANSPORT_PUBLIC SubscriberPlugin */ template void subscribe( - rclcpp::Node * node, const std::string & base_topic, + NodeType * 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); + } + + template + void subscribe( + std::shared_ptr 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()) @@ -108,7 +145,19 @@ class IMAGE_TRANSPORT_PUBLIC SubscriberPlugin */ template void subscribe( - rclcpp::Node * node, const std::string & base_topic, + NodeType * 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); + } + + template + void subscribe( + std::shared_ptr 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) @@ -147,7 +196,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, + std::shared_ptr node, const std::string & base_topic, const Callback & callback, rmw_qos_profile_t custom_qos, diff --git a/image_transport/include/image_transport/transport_hints.hpp b/image_transport/include/image_transport/transport_hints.hpp index e014d04c..049485a4 100644 --- a/image_transport/include/image_transport/transport_hints.hpp +++ b/image_transport/include/image_transport/transport_hints.hpp @@ -33,6 +33,7 @@ #include #include "rclcpp/node.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "image_transport/visibility_control.hpp" @@ -66,6 +67,24 @@ class TransportHints node->get_parameter_or(parameter_name, transport_, default_transport); } + IMAGE_TRANSPORT_PUBLIC + TransportHints( + const rclcpp::Node::SharedPtr node, + const std::string & default_transport = "raw", + const std::string & parameter_name = "image_transport") + { + node->get_parameter_or(parameter_name, transport_, default_transport); + } + + IMAGE_TRANSPORT_PUBLIC + TransportHints( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::string & default_transport = "raw", + const std::string & parameter_name = "image_transport") + { + node->get_parameter_or(parameter_name, transport_, default_transport); + } + IMAGE_TRANSPORT_PUBLIC const std::string & getTransport() const { diff --git a/image_transport/include/image_transport/visibility_control.hpp b/image_transport/include/image_transport/visibility_control.hpp index 2f590311..62e23f9f 100644 --- a/image_transport/include/image_transport/visibility_control.hpp +++ b/image_transport/include/image_transport/visibility_control.hpp @@ -1,4 +1,4 @@ -// Copyright 2018 Open Source Robotics Foundation, Inc. +// Copyright 2024 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/image_transport/package.xml b/image_transport/package.xml index b177f902..be4cdbe6 100644 --- a/image_transport/package.xml +++ b/image_transport/package.xml @@ -26,6 +26,7 @@ message_filters pluginlib rclcpp + rclcpp_lifecycle rclcpp_components sensor_msgs diff --git a/image_transport/src/camera_publisher.cpp b/image_transport/src/camera_publisher.cpp index 66c7d7f1..9599060e 100644 --- a/image_transport/src/camera_publisher.cpp +++ b/image_transport/src/camera_publisher.cpp @@ -42,10 +42,19 @@ namespace image_transport { -struct CameraPublisher::Impl +template +struct CameraPublisher::Impl { - explicit Impl(rclcpp::Node * node) - : logger_(node->get_logger()), + explicit Impl(NodeType * node) + : node_(node), + logger_(node->get_logger()), + unadvertised_(false) + { + } + + explicit Impl(std::shared_ptr node) + : node_(node), + logger_(node->get_logger()), unadvertised_(false) { } @@ -69,32 +78,61 @@ struct CameraPublisher::Impl } } + std::shared_ptr node_; rclcpp::Logger logger_; - Publisher image_pub_; + Publisher image_pub_; rclcpp::Publisher::SharedPtr info_pub_; bool unadvertised_; }; -CameraPublisher::CameraPublisher( - rclcpp::Node * node, +template +CameraPublisher::CameraPublisher( + NodeType * node, const std::string & base_topic, rmw_qos_profile_t custom_qos, rclcpp::PublisherOptions pub_options) : impl_(std::make_shared(node)) { + initialise(base_topic, custom_qos, pub_options); +} + +template +CameraPublisher::CameraPublisher( + std::shared_ptr node, + const std::string & base_topic, + rmw_qos_profile_t custom_qos, + rclcpp::PublisherOptions pub_options) +: impl_(std::make_shared(node)) +{ + initialise(base_topic, custom_qos, pub_options); +} + +template +void CameraPublisher::initialise( + const std::string & base_topic, + rmw_qos_profile_t custom_qos, + rclcpp::PublisherOptions pub_options) +{ + if (!impl_) { + throw std::runtime_error("impl is not constructed!"); + } // 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( + std::string image_topic; + image_topic = rclcpp::expand_topic_or_service_name( base_topic, - node->get_name(), node->get_namespace()); + impl_->node_->get_name(), impl_->node_->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, pub_options); - impl_->info_pub_ = node->create_publisher(info_topic, qos); + impl_->image_pub_ = image_transport::create_publisher( + impl_->node_, image_topic, custom_qos, pub_options); + impl_->info_pub_ = + impl_->node_->template create_publisher(info_topic, qos); } -size_t CameraPublisher::getNumSubscribers() const +template +size_t CameraPublisher::getNumSubscribers() const { if (impl_ && impl_->isValid()) { return std::max( @@ -104,7 +142,8 @@ size_t CameraPublisher::getNumSubscribers() const return 0; } -std::string CameraPublisher::getTopic() const +template +std::string CameraPublisher::getTopic() const { if (impl_) { return impl_->image_pub_.getTopic(); @@ -112,7 +151,8 @@ std::string CameraPublisher::getTopic() const return std::string(); } -std::string CameraPublisher::getInfoTopic() const +template +std::string CameraPublisher::getInfoTopic() const { if (impl_) { return impl_->info_pub_->get_topic_name(); @@ -120,7 +160,8 @@ std::string CameraPublisher::getInfoTopic() const return std::string(); } -void CameraPublisher::publish( +template +void CameraPublisher::publish( const sensor_msgs::msg::Image & image, const sensor_msgs::msg::CameraInfo & info) const { @@ -136,7 +177,8 @@ void CameraPublisher::publish( impl_->info_pub_->publish(info); } -void CameraPublisher::publish( +template +void CameraPublisher::publish( const sensor_msgs::msg::Image::ConstSharedPtr & image, const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info) const { @@ -152,7 +194,8 @@ void CameraPublisher::publish( impl_->info_pub_->publish(*info); } -void CameraPublisher::publish( +template +void CameraPublisher::publish( sensor_msgs::msg::Image::UniquePtr image, sensor_msgs::msg::CameraInfo::UniquePtr info) const { @@ -168,7 +211,8 @@ void CameraPublisher::publish( impl_->info_pub_->publish(std::move(info)); } -void CameraPublisher::publish( +template +void CameraPublisher::publish( sensor_msgs::msg::Image & image, sensor_msgs::msg::CameraInfo & info, rclcpp::Time stamp) const { @@ -186,7 +230,8 @@ void CameraPublisher::publish( impl_->info_pub_->publish(info); } -void CameraPublisher::publish( +template +void CameraPublisher::publish( sensor_msgs::msg::Image::UniquePtr image, sensor_msgs::msg::CameraInfo::UniquePtr info, rclcpp::Time stamp) const @@ -205,7 +250,8 @@ void CameraPublisher::publish( impl_->info_pub_->publish(std::move(info)); } -void CameraPublisher::shutdown() +template +void CameraPublisher::shutdown() { if (impl_) { impl_->shutdown(); @@ -213,9 +259,13 @@ void CameraPublisher::shutdown() } } -CameraPublisher::operator void *() const +template +CameraPublisher::operator void *() const { return (impl_ && impl_->isValid()) ? reinterpret_cast(1) : reinterpret_cast(0); } } // namespace image_transport + +template class image_transport::CameraPublisher; +template class image_transport::CameraPublisher; diff --git a/image_transport/src/camera_subscriber.cpp b/image_transport/src/camera_subscriber.cpp index b90a6b4e..bc15cd71 100644 --- a/image_transport/src/camera_subscriber.cpp +++ b/image_transport/src/camera_subscriber.cpp @@ -45,14 +45,25 @@ inline void increment(int * value) namespace image_transport { -struct CameraSubscriber::Impl +template +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()), + explicit Impl(NodeType * node) + : node_(node), + logger_(node->get_logger()), + sync_(10), + unsubscribed_(false), + image_received_(0), info_received_(0), both_received_(0) + { + } + + explicit Impl(std::shared_ptr node) + : node_(node), + logger_(node->get_logger()), sync_(10), unsubscribed_(false), image_received_(0), info_received_(0), both_received_(0) @@ -82,6 +93,8 @@ struct CameraSubscriber::Impl { int threshold = 3 * both_received_; if (image_received_ > threshold || info_received_ > threshold) { + std::string info_topic; + info_topic = info_sub_.getTopic(); RCLCPP_WARN( logger_, "[image_transport] Topics '%s' and '%s' do not appear to be synchronized. " @@ -89,15 +102,16 @@ struct CameraSubscriber::Impl "\tImage messages received: %d\n" "\tCameraInfo messages received: %d\n" "\tSynchronized pairs: %d", - image_sub_.getTopic().c_str(), info_sub_.getTopic().c_str(), + image_sub_.getTopic().c_str(), info_topic.c_str(), image_received_, info_received_, both_received_); } image_received_ = info_received_ = both_received_ = 0; } + std::shared_ptr node_; rclcpp::Logger logger_; - SubscriberFilter image_sub_; - message_filters::Subscriber info_sub_; + SubscriberFilter image_sub_; + typename message_filters::Subscriber info_sub_; TimeSync sync_; bool unsubscribed_; @@ -106,74 +120,116 @@ struct CameraSubscriber::Impl int image_received_, info_received_, both_received_; }; -CameraSubscriber::CameraSubscriber( - rclcpp::Node * node, +template +CameraSubscriber::CameraSubscriber( + NodeType * node, const std::string & base_topic, const Callback & callback, const std::string & transport, rmw_qos_profile_t custom_qos) : impl_(std::make_shared(node)) { + initialise(base_topic, callback, transport, custom_qos); +} + +template +CameraSubscriber::CameraSubscriber( + std::shared_ptr node, + const std::string & base_topic, + const Callback & callback, + const std::string & transport, + rmw_qos_profile_t custom_qos) +: impl_(std::make_shared(node)) +{ + initialise(base_topic, callback, transport, custom_qos); +} + +template +void CameraSubscriber::initialise( + const std::string & base_topic, + const Callback & callback, + const std::string & transport, + rmw_qos_profile_t custom_qos) +{ + if (!impl_) { + throw std::runtime_error("impl is not constructed!"); + } // 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( + std::string image_topic; + image_topic = rclcpp::expand_topic_or_service_name( base_topic, - node->get_name(), node->get_namespace()); + impl_->node_->get_name(), impl_->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, + impl_->image_sub_.subscribe(impl_->node_, image_topic, transport, custom_qos); + impl_->info_sub_.subscribe(impl_->node_, info_topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos))); impl_->sync_.connectInput(impl_->image_sub_, impl_->info_sub_); + impl_->info_sub_.registerCallback(std::bind(increment, &impl_->info_received_)); + 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( + impl_->check_synced_timer_ = impl_->node_->create_wall_timer( std::chrono::seconds(1), std::bind(&Impl::checkImagesSynchronized, impl_.get())); } -std::string CameraSubscriber::getTopic() const +template +std::string CameraSubscriber::getTopic() const { if (impl_) {return impl_->image_sub_.getTopic();} return std::string(); } -std::string CameraSubscriber::getInfoTopic() const +template +std::string CameraSubscriber::getInfoTopic() const { - if (impl_) {return impl_->info_sub_.getSubscriber()->get_topic_name();} + if (impl_) { + return impl_->info_sub_.getSubscriber()->get_topic_name(); + } return std::string(); } -size_t CameraSubscriber::getNumPublishers() const +template +size_t CameraSubscriber::getNumPublishers() const { if (impl_) { + size_t info_pub_count; + info_pub_count = impl_->info_sub_.getSubscriber()->get_publisher_count(); + return std::max( impl_->image_sub_.getSubscriber().getNumPublishers(), - impl_->info_sub_.getSubscriber()->get_publisher_count()); + info_pub_count); } return 0; } -std::string CameraSubscriber::getTransport() const +template +std::string CameraSubscriber::getTransport() const { if (impl_) {return impl_->image_sub_.getTransport();} return std::string(); } -void CameraSubscriber::shutdown() +template +void CameraSubscriber::shutdown() { if (impl_) {impl_->shutdown();} } -CameraSubscriber::operator void *() const +template +CameraSubscriber::operator void *() const { return (impl_ && impl_->isValid()) ? reinterpret_cast(1) : reinterpret_cast(0); } } // namespace image_transport + +template class image_transport::CameraSubscriber; +template class image_transport::CameraSubscriber; diff --git a/image_transport/src/image_transport.cpp b/image_transport/src/image_transport.cpp index dfddcb91..e4014eab 100644 --- a/image_transport/src/image_transport.cpp +++ b/image_transport/src/image_transport.cpp @@ -44,40 +44,114 @@ namespace image_transport struct Impl { - PubLoaderPtr pub_loader_; - SubLoaderPtr sub_loader_; + PubLoaderPtr pub_loader_; + SubLoaderPtr sub_loader_; Impl() - : pub_loader_(std::make_shared("image_transport", "image_transport::PublisherPlugin")), - sub_loader_(std::make_shared("image_transport", "image_transport::SubscriberPlugin")) + : pub_loader_(std::make_shared>( + "image_transport", + "image_transport::PublisherPlugin")), + sub_loader_(std::make_shared>( + "image_transport", + "image_transport::SubscriberPlugin")) + { + } +}; + +struct ImplLifecycle +{ + PubLoaderPtr pub_loader_; + SubLoaderPtr sub_loader_; + + ImplLifecycle() + : pub_loader_(std::make_shared>( + "image_transport", + "image_transport::PublisherPlugin")), + sub_loader_(std::make_shared>( + "image_transport", + "image_transport::SubscriberPlugin")) { } }; static Impl * kImpl = new Impl(); +static ImplLifecycle * kImpl_lifecycle = new ImplLifecycle(); -Publisher create_publisher( - rclcpp::Node * node, +template +Publisher create_publisher( + NodeType * node, 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); + if constexpr (std::is_same_v) { + return Publisher(node, base_topic, kImpl->pub_loader_, custom_qos, options); + } + if constexpr (std::is_same_v) { + return Publisher(node, base_topic, kImpl_lifecycle->pub_loader_, custom_qos, options); + } } -Subscriber create_subscription( - rclcpp::Node * node, +template +Publisher create_publisher( + std::shared_ptr node, const std::string & base_topic, - const Subscriber::Callback & callback, + rmw_qos_profile_t custom_qos, + rclcpp::PublisherOptions options) +{ + if constexpr (std::is_same_v) { + return Publisher(node, base_topic, kImpl->pub_loader_, custom_qos, options); + } + if constexpr (std::is_same_v) { + return Publisher(node, base_topic, kImpl_lifecycle->pub_loader_, custom_qos, options); + } +} + +template +Subscriber create_subscription( + NodeType * node, + const std::string & base_topic, + const typename Subscriber::Callback & callback, + const std::string & transport, + rmw_qos_profile_t custom_qos, + rclcpp::SubscriptionOptions options) +{ + if constexpr (std::is_same_v) { + return Subscriber( + node, base_topic, callback, kImpl->sub_loader_, transport, custom_qos, + options); + } + if constexpr (std::is_same_v) { + return Subscriber( + node, base_topic, callback, kImpl_lifecycle->sub_loader_, + transport + "_lifecycle", custom_qos, options); + } +} + +template +Subscriber create_subscription( + std::shared_ptr node, + const std::string & base_topic, + const typename 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); + if constexpr (std::is_same_v) { + return Subscriber( + node, base_topic, callback, kImpl->sub_loader_, transport, custom_qos, + options); + } + if constexpr (std::is_same_v) { + return Subscriber( + node, base_topic, callback, kImpl_lifecycle->sub_loader_, + transport + "_lifecycle", custom_qos, options); + } } -CameraPublisher create_camera_publisher( - rclcpp::Node * node, +template +CameraPublisher create_camera_publisher( + NodeType * node, const std::string & base_topic, rmw_qos_profile_t custom_qos, rclcpp::PublisherOptions pub_options) @@ -85,19 +159,48 @@ CameraPublisher create_camera_publisher( return CameraPublisher(node, base_topic, custom_qos, pub_options); } -CameraSubscriber create_camera_subscription( - rclcpp::Node * node, +template +CameraPublisher create_camera_publisher( + std::shared_ptr node, const std::string & base_topic, - const CameraSubscriber::Callback & callback, + rmw_qos_profile_t custom_qos, + rclcpp::PublisherOptions pub_options) +{ + return CameraPublisher(node, base_topic, custom_qos, pub_options); +} + +template +CameraSubscriber create_camera_subscription( + NodeType * node, + const std::string & base_topic, + const typename CameraSubscriber::Callback & callback, + const std::string & transport, + rmw_qos_profile_t custom_qos) +{ + return CameraSubscriber(node, base_topic, callback, transport, custom_qos); +} + +template +CameraSubscriber create_camera_subscription( + std::shared_ptr node, + const std::string & base_topic, + const typename CameraSubscriber::Callback & callback, const std::string & transport, rmw_qos_profile_t custom_qos) { return CameraSubscriber(node, base_topic, callback, transport, custom_qos); } +template std::vector getDeclaredTransports() { - std::vector transports = kImpl->sub_loader_->getDeclaredClasses(); + std::vector transports; + if constexpr (std::is_same_v) { + transports = kImpl->sub_loader_->getDeclaredClasses(); + } + if constexpr (std::is_same_v) { + transports = kImpl_lifecycle->sub_loader_->getDeclaredClasses(); + } // Remove the "_sub" at the end of each class name. for (std::string & transport : transports) { transport = erase_last_copy(transport, "_sub"); @@ -105,17 +208,29 @@ std::vector getDeclaredTransports() return transports; } +template std::vector getLoadableTransports() { std::vector loadableTransports; - - for (const std::string & transportPlugin : kImpl->sub_loader_->getDeclaredClasses() ) { + std::vector transports; + if constexpr (std::is_same_v) { + transports = kImpl->sub_loader_->getDeclaredClasses(); + } + if constexpr (std::is_same_v) { + transports = kImpl_lifecycle->sub_loader_->getDeclaredClasses(); + } + for (const std::string & transportPlugin : transports) { // If the plugin loads without throwing an exception, add its // transport name to the list of valid plugins, otherwise ignore // it. try { - std::shared_ptr sub = - kImpl->sub_loader_->createUniqueInstance(transportPlugin); + std::shared_ptr> sub; + if constexpr (std::is_same_v) { + sub = kImpl->sub_loader_->createUniqueInstance(transportPlugin); + } + if constexpr (std::is_same_v) { + sub = kImpl_lifecycle->sub_loader_->createUniqueInstance(transportPlugin); + } // Remove the "_sub" at the end of each class name. loadableTransports.push_back(erase_last_copy(transportPlugin, "_sub")); } catch (const pluginlib::LibraryLoadException & e) { @@ -128,54 +243,78 @@ std::vector getLoadableTransports() return loadableTransports; } -struct ImageTransport::Impl +template +struct ImageTransport::Impl { - rclcpp::Node::SharedPtr node_; + explicit Impl(NodeType * node) + : node_(node) + { + } + + explicit Impl(std::shared_ptr node) + : node_(node) + { + } + + std::shared_ptr node_; }; -ImageTransport::ImageTransport(rclcpp::Node::SharedPtr node) -: impl_(std::make_unique()) +template +ImageTransport::ImageTransport(NodeType * node) +: impl_(std::make_unique::Impl>(node)) { - impl_->node_ = node; } -ImageTransport::~ImageTransport() = default; +template +ImageTransport::ImageTransport(std::shared_ptr node) +: impl_(std::make_unique::Impl>(node)) +{ +} -Publisher ImageTransport::advertise(const std::string & base_topic, uint32_t queue_size, bool latch) +template +ImageTransport::~ImageTransport() = default; + +template +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); + return create_publisher(impl_->node_, base_topic, custom_qos); } -Publisher ImageTransport::advertise( +template +Publisher ImageTransport::advertise( const std::string & base_topic, rmw_qos_profile_t custom_qos, bool latch) { // TODO(ros2) implement when resolved: https://github.com/ros2/ros2/issues/464 (void) latch; - return create_publisher(impl_->node_.get(), base_topic, custom_qos); + return create_publisher(impl_->node_, base_topic, custom_qos); } -Subscriber ImageTransport::subscribe( +template +Subscriber ImageTransport::subscribe( const std::string & base_topic, rmw_qos_profile_t custom_qos, - const Subscriber::Callback & callback, + const typename Subscriber::Callback & callback, const VoidPtr & tracked_object, const TransportHints * transport_hints, const rclcpp::SubscriptionOptions options) { (void) tracked_object; return create_subscription( - impl_->node_.get(), base_topic, callback, + impl_->node_, base_topic, callback, getTransportOrDefault(transport_hints), custom_qos, options); } -Subscriber ImageTransport::subscribe( +template +Subscriber ImageTransport::subscribe( const std::string & base_topic, uint32_t queue_size, - const Subscriber::Callback & callback, + const typename Subscriber::Callback & callback, const VoidPtr & tracked_object, const TransportHints * transport_hints, const rclcpp::SubscriptionOptions options) @@ -184,12 +323,13 @@ Subscriber ImageTransport::subscribe( rmw_qos_profile_t custom_qos = rmw_qos_profile_default; custom_qos.depth = queue_size; return create_subscription( - impl_->node_.get(), base_topic, callback, + impl_->node_, base_topic, callback, getTransportOrDefault(transport_hints), custom_qos, options); } -CameraPublisher ImageTransport::advertiseCamera( +template +CameraPublisher ImageTransport::advertiseCamera( const std::string & base_topic, uint32_t queue_size, bool latch) { @@ -197,12 +337,13 @@ 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_topic, custom_qos); } -CameraSubscriber ImageTransport::subscribeCamera( +template +CameraSubscriber ImageTransport::subscribeCamera( const std::string & base_topic, uint32_t queue_size, - const CameraSubscriber::Callback & callback, + const typename CameraSubscriber::Callback & callback, const VoidPtr & tracked_object, const TransportHints * transport_hints) { @@ -210,25 +351,28 @@ CameraSubscriber ImageTransport::subscribeCamera( 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, + impl_->node_, base_topic, callback, getTransportOrDefault(transport_hints), custom_qos); } -std::vector ImageTransport::getDeclaredTransports() const +template +std::vector ImageTransport::getDeclaredTransports() const { return image_transport::getDeclaredTransports(); } -std::vector ImageTransport::getLoadableTransports() const +template +std::vector ImageTransport::getLoadableTransports() const { return image_transport::getLoadableTransports(); } -std::string ImageTransport::getTransportOrDefault(const TransportHints * transport_hints) +template +std::string ImageTransport::getTransportOrDefault(const TransportHints * transport_hints) { std::string ret; if (nullptr == transport_hints) { - TransportHints th(impl_->node_.get()); + TransportHints th(impl_->node_); ret = th.getTransport(); } else { ret = transport_hints->getTransport(); @@ -237,3 +381,6 @@ std::string ImageTransport::getTransportOrDefault(const TransportHints * transpo } } // namespace image_transport + +template class image_transport::ImageTransport; +template class image_transport::ImageTransport; diff --git a/image_transport/src/list_transports.cpp b/image_transport/src/list_transports.cpp index ac9748ce..dbcc5580 100644 --- a/image_transport/src/list_transports.cpp +++ b/image_transport/src/list_transports.cpp @@ -54,10 +54,16 @@ struct TransportDesc int main(int /*argc*/, char ** /*argv*/) { - pluginlib::ClassLoader pub_loader( - "image_transport", "image_transport::PublisherPlugin"); - pluginlib::ClassLoader sub_loader( - "image_transport", "image_transport::SubscriberPlugin"); + pluginlib::ClassLoader> pub_loader( + "image_transport", "image_transport::PublisherPlugin"); + pluginlib::ClassLoader> + pub_loader_lifecycle( + "image_transport", "image_transport::PublisherPlugin"); + pluginlib::ClassLoader> sub_loader( + "image_transport", "image_transport::SubscriberPlugin"); + pluginlib::ClassLoader> + sub_loader_lifecycle( + "image_transport", "image_transport::SubscriberPlugin"); typedef std::map StatusMap; StatusMap transports; @@ -75,6 +81,22 @@ int main(int /*argc*/, char ** /*argv*/) } } + StatusMap transports_lifecycle; + for (const std::string & lookup_name : pub_loader_lifecycle.getDeclaredClasses()) { + std::string transport_name = image_transport::erase_last_copy(lookup_name, "_pub"); + transports_lifecycle[transport_name].pub_name = lookup_name; + transports_lifecycle[transport_name].package_name = pub_loader_lifecycle.getClassPackage( + lookup_name); + try { + auto pub = pub_loader_lifecycle.createUniqueInstance(lookup_name); + transports_lifecycle[transport_name].pub_status = SUCCESS; + } catch (const pluginlib::LibraryLoadException &) { + transports_lifecycle[transport_name].pub_status = LIB_LOAD_FAILURE; + } catch (const pluginlib::CreateClassException &) { + transports_lifecycle[transport_name].pub_status = CREATE_FAILURE; + } + } + for (const std::string & lookup_name : sub_loader.getDeclaredClasses()) { std::string transport_name = image_transport::erase_last_copy(lookup_name, "_sub"); transports[transport_name].sub_name = lookup_name; @@ -89,6 +111,22 @@ int main(int /*argc*/, char ** /*argv*/) } } + + for (const std::string & lookup_name : sub_loader_lifecycle.getDeclaredClasses()) { + std::string transport_name = image_transport::erase_last_copy(lookup_name, "_sub"); + transports_lifecycle[transport_name].sub_name = lookup_name; + transports_lifecycle[transport_name].package_name = sub_loader_lifecycle.getClassPackage( + lookup_name); + try { + auto sub = sub_loader_lifecycle.createUniqueInstance(lookup_name); + transports_lifecycle[transport_name].sub_status = SUCCESS; + } catch (const pluginlib::LibraryLoadException &) { + transports_lifecycle[transport_name].sub_status = LIB_LOAD_FAILURE; + } catch (const pluginlib::CreateClassException &) { + transports_lifecycle[transport_name].sub_status = CREATE_FAILURE; + } + } + printf("Declared transports:\n"); for (const StatusMap::value_type & value : transports) { const TransportDesc & td = value.second; @@ -126,5 +164,30 @@ int main(int /*argc*/, char ** /*argv*/) } } + printf("\nDetails Lifecycle:\n"); + for (const auto & value : transports_lifecycle) { + const TransportDesc & td = value.second; + printf("----------\n"); + printf("\"%s\"\n", value.first.c_str()); + if (td.pub_status == CREATE_FAILURE || td.sub_status == CREATE_FAILURE) { + printf( + "*** Plugins are built, but could not be loaded. The package may need to be rebuilt or may " + "not be compatible with this release of image_common. ***\n"); + } else if (td.pub_status == LIB_LOAD_FAILURE || td.sub_status == LIB_LOAD_FAILURE) { + printf("*** Plugins are not built. ***\n"); + } + printf(" - Provided by package: %s\n", td.package_name.c_str()); + if (td.pub_status == DOES_NOT_EXIST) { + printf(" - No publisher provided\n"); + } else { + printf(" - Publisher: %s\n", pub_loader_lifecycle.getClassDescription(td.pub_name).c_str()); + } + if (td.sub_status == DOES_NOT_EXIST) { + printf(" - No subscriber provided\n"); + } else { + printf(" - Subscriber: %s\n", sub_loader_lifecycle.getClassDescription(td.sub_name).c_str()); + } + } + return 0; } diff --git a/image_transport/src/manifest.cpp b/image_transport/src/manifest.cpp index 0d1d78d6..c588c3ec 100644 --- a/image_transport/src/manifest.cpp +++ b/image_transport/src/manifest.cpp @@ -31,5 +31,15 @@ #include "image_transport/raw_publisher.hpp" #include "image_transport/raw_subscriber.hpp" -PLUGINLIB_EXPORT_CLASS(image_transport::RawPublisher, image_transport::PublisherPlugin) -PLUGINLIB_EXPORT_CLASS(image_transport::RawSubscriber, image_transport::SubscriberPlugin) +PLUGINLIB_EXPORT_CLASS( + image_transport::RawPublisher, + image_transport::PublisherPlugin) +PLUGINLIB_EXPORT_CLASS( + image_transport::RawPublisher, + image_transport::PublisherPlugin) +PLUGINLIB_EXPORT_CLASS( + image_transport::RawSubscriber, + image_transport::SubscriberPlugin) +PLUGINLIB_EXPORT_CLASS( + image_transport::RawSubscriber, + image_transport::SubscriberPlugin) diff --git a/image_transport/src/publisher.cpp b/image_transport/src/publisher.cpp index 48f0257e..5422478c 100644 --- a/image_transport/src/publisher.cpp +++ b/image_transport/src/publisher.cpp @@ -47,10 +47,19 @@ namespace image_transport { -struct Publisher::Impl +template +struct Publisher::Impl { - explicit Impl(rclcpp::Node * node) - : logger_(node->get_logger()), + explicit Impl(NodeType * node) + : node_(node), + logger_(node->get_logger()), + unadvertised_(false) + { + } + + explicit Impl(std::shared_ptr node) + : node_(node), + logger_(node->get_logger()), unadvertised_(false) { } @@ -90,28 +99,53 @@ struct Publisher::Impl } } + std::shared_ptr node_; rclcpp::Logger logger_; std::string base_topic_; - PubLoaderPtr loader_; - std::vector> publishers_; + PubLoaderPtr loader_; + std::vector>> publishers_; bool unadvertised_; }; -Publisher::Publisher( - rclcpp::Node * node, const std::string & base_topic, - PubLoaderPtr loader, rmw_qos_profile_t custom_qos, +template +Publisher::Publisher( + NodeType * node, const std::string & base_topic, + PubLoaderPtr loader, rmw_qos_profile_t custom_qos, rclcpp::PublisherOptions options) : impl_(std::make_shared(node)) { + initialise(base_topic, loader, custom_qos, options); +} + +template +Publisher::Publisher( + std::shared_ptr node, const std::string & base_topic, + PubLoaderPtr loader, rmw_qos_profile_t custom_qos, + rclcpp::PublisherOptions options) +: impl_(std::make_shared(node)) +{ + initialise(base_topic, loader, custom_qos, options); +} + +template +void Publisher::initialise( + const std::string & base_topic, + PubLoaderPtr loader, rmw_qos_profile_t custom_qos, + rclcpp::PublisherOptions options) +{ + if (!impl_) { + throw std::runtime_error("impl is not constructed!"); + } // 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()); + std::string image_topic; + size_t ns_len; + image_topic = rclcpp::expand_topic_or_service_name( + base_topic, impl_->node_->get_name(), impl_->node_->get_namespace()); + ns_len = strlen(impl_->node_->get_namespace()); impl_->base_topic_ = image_topic; impl_->loader_ = loader; - auto ns_len = node->get_effective_namespace().length(); std::string param_base_name = image_topic.substr(ns_len); std::replace(param_base_name.begin(), param_base_name.end(), '/', '.'); if (param_base_name.front() == '.') { @@ -124,16 +158,16 @@ Publisher::Publisher( all_transport_names.emplace_back(erase_last_copy(lookup_name, "_pub")); } try { - allowlist_vec = node->declare_parameter>( + allowlist_vec = impl_->node_->template declare_parameter>( param_base_name + ".enable_pub_plugins", all_transport_names); } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { RCLCPP_DEBUG_STREAM( - node->get_logger(), param_base_name << ".enable_pub_plugins" << " was previously declared" + impl_->logger_, param_base_name << ".enable_pub_plugins" << " was previously declared" ); allowlist_vec = - node->get_parameter( + impl_->node_->get_parameter( param_base_name + - ".enable_pub_plugins").get_value>(); + ".enable_pub_plugins").template get_value>(); } for (size_t i = 0; i < allowlist_vec.size(); ++i) { allowlist.insert(allowlist_vec[i]); @@ -143,7 +177,7 @@ Publisher::Publisher( const auto & lookup_name = transport_name + "_pub"; try { auto pub = loader->createUniqueInstance(lookup_name); - pub->advertise(node, image_topic, custom_qos, options); + pub->advertise(impl_->node_, image_topic, custom_qos, options); impl_->publishers_.push_back(std::move(pub)); } catch (const std::runtime_error & e) { RCLCPP_ERROR( @@ -159,19 +193,22 @@ Publisher::Publisher( } } -size_t Publisher::getNumSubscribers() const +template +size_t Publisher::getNumSubscribers() const { if (impl_ && impl_->isValid()) {return impl_->getNumSubscribers();} return 0; } -std::string Publisher::getTopic() const +template +std::string Publisher::getTopic() const { if (impl_) {return impl_->getTopic();} return std::string(); } -void Publisher::publish(const sensor_msgs::msg::Image & message) const +template +void Publisher::publish(const sensor_msgs::msg::Image & message) const { if (!impl_ || !impl_->isValid()) { // TODO(ros2) Switch to RCUTILS_ASSERT when ros2/rcutils#112 is merged @@ -187,7 +224,8 @@ void Publisher::publish(const sensor_msgs::msg::Image & message) const } } -void Publisher::publish(const sensor_msgs::msg::Image::ConstSharedPtr & message) const +template +void Publisher::publish(const sensor_msgs::msg::Image::ConstSharedPtr & message) const { if (!impl_ || !impl_->isValid()) { // TODO(ros2) Switch to RCUTILS_ASSERT when ros2/rcutils#112 is merged @@ -203,7 +241,8 @@ void Publisher::publish(const sensor_msgs::msg::Image::ConstSharedPtr & message) } } -void Publisher::publish(sensor_msgs::msg::Image::UniquePtr message) const +template +void Publisher::publish(sensor_msgs::msg::Image::UniquePtr message) const { if (!impl_ || !impl_->isValid()) { auto logger = impl_ ? impl_->logger_ : rclcpp::get_logger("image_transport"); @@ -211,8 +250,8 @@ void Publisher::publish(sensor_msgs::msg::Image::UniquePtr message) const return; } - std::vector> pubs_take_reference; - std::optional> pub_takes_ownership{std::nullopt}; + std::vector>> pubs_take_reference; + std::optional>> pub_takes_ownership{std::nullopt}; for (const auto & pub : impl_->publishers_) { if (pub->getNumSubscribers() > 0) { @@ -233,7 +272,8 @@ void Publisher::publish(sensor_msgs::msg::Image::UniquePtr message) const } } -void Publisher::shutdown() +template +void Publisher::shutdown() { if (impl_) { impl_->shutdown(); @@ -241,9 +281,13 @@ void Publisher::shutdown() } } -Publisher::operator void *() const +template +Publisher::operator void *() const { return (impl_ && impl_->isValid()) ? reinterpret_cast(1) : reinterpret_cast(0); } } // namespace image_transport + +template class image_transport::Publisher; +template class image_transport::Publisher; diff --git a/image_transport/src/republish.cpp b/image_transport/src/republish.cpp index 412e3e55..6ea25a32 100644 --- a/image_transport/src/republish.cpp +++ b/image_transport/src/republish.cpp @@ -112,34 +112,34 @@ void Republisher::initialize() // Use all available transports for output this->pub = image_transport::create_publisher( - this, out_topic, + shared_from_this(), out_topic, rmw_qos_profile_default, pub_options); // Use Publisher::publish as the subscriber callback - typedef void (image_transport::Publisher::* PublishMemFn)( + typedef void (image_transport::Publisher::* PublishMemFn)( const sensor_msgs::msg::Image::ConstSharedPtr &) const; - PublishMemFn pub_mem_fn = &image_transport::Publisher::publish; + PublishMemFn pub_mem_fn = &image_transport::Publisher::publish; this->sub = image_transport::create_subscription( - this, in_topic, std::bind(pub_mem_fn, &pub, std::placeholders::_1), + shared_from_this(), in_topic, std::bind(pub_mem_fn, &pub, std::placeholders::_1), in_transport, rmw_qos_profile_default, sub_options); } else { // Use one specific transport for output // Load transport plugin - typedef image_transport::PublisherPlugin Plugin; + typedef image_transport::PublisherPlugin Plugin; loader = std::make_shared>( "image_transport", - "image_transport::PublisherPlugin"); + "image_transport::PublisherPlugin"); std::string lookup_name = Plugin::getLookupName(out_transport); instance = loader->createUniqueInstance(lookup_name); - instance->advertise(this, out_topic, rmw_qos_profile_default, pub_options); + instance->advertise(shared_from_this(), out_topic, rmw_qos_profile_default, pub_options); // Use PublisherPlugin::publish as the subscriber callback typedef void (Plugin::* PublishMemFn)(const sensor_msgs::msg::Image::ConstSharedPtr &) const; PublishMemFn pub_mem_fn = &Plugin::publishPtr; this->sub = image_transport::create_subscription( - this, in_topic, + shared_from_this(), in_topic, std::bind( pub_mem_fn, instance.get(), std::placeholders::_1), in_transport, rmw_qos_profile_default, sub_options); diff --git a/image_transport/src/subscriber.cpp b/image_transport/src/subscriber.cpp index 85c7927c..a8c2298d 100644 --- a/image_transport/src/subscriber.cpp +++ b/image_transport/src/subscriber.cpp @@ -44,10 +44,20 @@ namespace image_transport { -struct Subscriber::Impl +template +struct Subscriber::Impl { - Impl(rclcpp::Node * node, SubLoaderPtr loader) - : logger_(node->get_logger()), + Impl(NodeType * node, SubLoaderPtr loader) + : node_(node), + logger_(node->get_logger()), + loader_(loader), + unsubscribed_(false) + { + } + + explicit Impl(std::shared_ptr node, SubLoaderPtr loader) + : node_(node), + logger_(node->get_logger()), loader_(loader), unsubscribed_(false) { @@ -73,28 +83,58 @@ struct Subscriber::Impl } } + std::shared_ptr node_; rclcpp::Logger logger_; std::string lookup_name_; - SubLoaderPtr loader_; - std::shared_ptr subscriber_; + SubLoaderPtr loader_; + std::shared_ptr> subscriber_; bool unsubscribed_; // double constructed_; }; -Subscriber::Subscriber( - rclcpp::Node * node, +template +Subscriber::Subscriber( + NodeType * node, + 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)) +{ + initialise(base_topic, callback, transport, custom_qos, options); +} + +template +Subscriber::Subscriber( + std::shared_ptr node, const std::string & base_topic, const Callback & callback, - SubLoaderPtr loader, + SubLoaderPtr loader, const std::string & transport, rmw_qos_profile_t custom_qos, rclcpp::SubscriptionOptions options) : impl_(std::make_shared(node, loader)) { + initialise(base_topic, callback, transport, custom_qos, options); +} + +template +void Subscriber::initialise( + const std::string & base_topic, + const Callback & callback, + const std::string & transport, + rmw_qos_profile_t custom_qos, + rclcpp::SubscriptionOptions options) +{ + if (!impl_) { + throw std::runtime_error("impl is not constructed!"); + } // Load the plugin for the chosen transport. - impl_->lookup_name_ = SubscriberPlugin::getLookupName(transport); + impl_->lookup_name_ = SubscriberPlugin::getLookupName(transport); try { - impl_->subscriber_ = loader->createSharedInstance(impl_->lookup_name_); + impl_->subscriber_ = impl_->loader_->createSharedInstance(impl_->lookup_name_); } catch (pluginlib::PluginlibException & e) { throw TransportLoadException(impl_->lookup_name_, e.what()); } @@ -107,8 +147,8 @@ Subscriber::Subscriber( size_t found = clean_topic.rfind('/'); if (found != std::string::npos) { std::string transport = clean_topic.substr(found + 1); - std::string plugin_name = SubscriberPlugin::getLookupName(transport); - std::vector plugins = loader->getDeclaredClasses(); + std::string plugin_name = SubscriberPlugin::getLookupName(transport); + std::vector plugins = impl_->loader_->getDeclaredClasses(); if (std::find(plugins.begin(), plugins.end(), plugin_name) != plugins.end()) { std::string real_base_topic = clean_topic.substr(0, found); @@ -125,35 +165,43 @@ 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(impl_->node_, base_topic, callback, custom_qos, options); } -std::string Subscriber::getTopic() const +template +std::string Subscriber::getTopic() const { if (impl_) {return impl_->subscriber_->getTopic();} return std::string(); } -size_t Subscriber::getNumPublishers() const +template +size_t Subscriber::getNumPublishers() const { if (impl_) {return impl_->subscriber_->getNumPublishers();} return 0; } -std::string Subscriber::getTransport() const +template +std::string Subscriber::getTransport() const { if (impl_) {return impl_->subscriber_->getTransportName();} return std::string(); } -void Subscriber::shutdown() +template +void Subscriber::shutdown() { if (impl_) {impl_->shutdown();} } -Subscriber::operator void *() const +template +Subscriber::operator void *() const { return (impl_ && impl_->isValid()) ? reinterpret_cast(1) : reinterpret_cast(0); } } // namespace image_transport + +template class image_transport::Subscriber; +template class image_transport::Subscriber; diff --git a/image_transport/test/test_camera_common.cpp b/image_transport/test/test_camera_common.cpp index 89f61c2e..e769307e 100644 --- a/image_transport/test/test_camera_common.cpp +++ b/image_transport/test/test_camera_common.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2018 Open Source Robotics Foundation, Inc. +// Copyright (c) 2024 Open Source Robotics Foundation, Inc. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: diff --git a/image_transport/test/test_message_passing.cpp b/image_transport/test/test_message_passing.cpp index 0a4372c3..acb51edf 100644 --- a/image_transport/test/test_message_passing.cpp +++ b/image_transport/test/test_message_passing.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2018 Open Source Robotics Foundation, Inc. +// Copyright (c) 2024 Open Source Robotics Foundation, Inc. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -28,6 +28,8 @@ #include #include +#include +#include #include "gtest/gtest.h" @@ -77,11 +79,11 @@ TEST_F(MessagePassingTesting, one_message_passing) rclcpp::executors::SingleThreadedExecutor executor; - auto pub = image_transport::create_publisher(node_.get(), "camera/image"); + auto pub = image_transport::create_publisher(node_, "camera/image"); auto sub = - image_transport::create_subscription(node_.get(), "camera/image", imageCallback, "raw"); + image_transport::create_subscription(node_, "camera/image", imageCallback, "raw"); - test_rclcpp::wait_for_subscriber(node_, sub.getTopic()); + test_rclcpp::wait_for_subscriber(node_->get_node_graph_interface(), sub.getTopic()); ASSERT_EQ(0, total_images_received); ASSERT_EQ(1u, pub.getNumSubscribers()); @@ -114,9 +116,9 @@ TEST_F(MessagePassingTesting, one_camera_message_passing) rclcpp::executors::SingleThreadedExecutor executor; - auto pub = image_transport::create_camera_publisher(node_.get(), "camera/image"); + auto pub = image_transport::create_camera_publisher(node_, "camera/image"); auto sub = image_transport::create_camera_subscription( - node_.get(), "camera/image", + node_, "camera/image", [](const sensor_msgs::msg::Image::ConstSharedPtr & image, const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info) { (void) image; @@ -126,7 +128,7 @@ TEST_F(MessagePassingTesting, one_camera_message_passing) "raw" ); - test_rclcpp::wait_for_subscriber(node_, sub.getTopic()); + test_rclcpp::wait_for_subscriber(node_->get_node_graph_interface(), sub.getTopic()); ASSERT_EQ(0, total_images_received); executor.spin_node_some(node_); diff --git a/image_transport/test/test_message_passing_lifecycle.cpp b/image_transport/test/test_message_passing_lifecycle.cpp new file mode 100644 index 00000000..650c4420 --- /dev/null +++ b/image_transport/test/test_message_passing_lifecycle.cpp @@ -0,0 +1,185 @@ +// Copyright (c) 2024 Open Source Robotics Foundation, 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. + +#include +#include +#include +#include + +#include "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "image_transport/image_transport.hpp" +#include "sensor_msgs/msg/image.hpp" + +#include "utils.hpp" + +using namespace std::chrono_literals; + +int total_images_received = 0; + +class MessagePassingTestingLifecycle : public ::testing::Test +{ +public: + sensor_msgs::msg::Image::UniquePtr generate_random_image() + { + auto image = std::make_unique(); + return image; + } + +protected: + void SetUp() + { + node_ = rclcpp_lifecycle::LifecycleNode::make_shared("test_message_passing_lifecycle"); + total_images_received = 0; + } + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; +}; + + +void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg) +{ + (void) msg; + total_images_received++; +} + +TEST_F(MessagePassingTestingLifecycle, one_message_passing) +{ + const size_t max_retries = 3; + const size_t max_loops = 200; + const std::chrono::milliseconds sleep_per_loop = std::chrono::milliseconds(10); + + rclcpp::executors::SingleThreadedExecutor executor; + + auto pub = image_transport::create_publisher(node_, "camera/image"); + auto sub = + image_transport::create_subscription(node_, "camera/image", imageCallback, "raw"); + + test_rclcpp::wait_for_subscriber(node_->get_node_graph_interface(), sub.getTopic()); + + ASSERT_EQ(0, total_images_received); + ASSERT_EQ(1u, pub.getNumSubscribers()); + ASSERT_EQ(1u, sub.getNumPublishers()); + + executor.spin_node_some(node_->get_node_base_interface()); + ASSERT_EQ(0, total_images_received); + + size_t retry = 0; + while (retry < max_retries && total_images_received == 0) { + // generate random image and publish it + pub.publish(generate_random_image()); + + executor.spin_node_some(node_->get_node_base_interface()); + size_t loop = 0; + while ((total_images_received != 1) && (loop++ < max_loops)) { + std::this_thread::sleep_for(sleep_per_loop); + executor.spin_node_some(node_->get_node_base_interface()); + } + } + + ASSERT_EQ(1, total_images_received); +} + +TEST_F(MessagePassingTestingLifecycle, one_camera_message_passing) +{ + const size_t max_retries = 3; + const size_t max_loops = 200; + const std::chrono::milliseconds sleep_per_loop = std::chrono::milliseconds(10); + + rclcpp::executors::SingleThreadedExecutor executor; + + auto pub = image_transport::create_camera_publisher(node_, "camera/image"); + auto sub = image_transport::create_camera_subscription( + node_, "camera/image", + [](const sensor_msgs::msg::Image::ConstSharedPtr & image, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info) { + (void) image; + (void) info; + total_images_received++; + }, + "raw" + ); + + test_rclcpp::wait_for_subscriber(node_->get_node_graph_interface(), sub.getTopic()); + + ASSERT_EQ(0, total_images_received); + executor.spin_node_some(node_->get_node_base_interface()); + ASSERT_EQ(0, total_images_received); + + size_t retry = 0; + while (retry < max_retries && total_images_received == 0) { + // generate random image and publish it + pub.publish(*generate_random_image().get(), sensor_msgs::msg::CameraInfo()); + + executor.spin_node_some(node_->get_node_base_interface()); + size_t loop = 0; + while ((total_images_received != 1) && (loop++ < max_loops)) { + std::this_thread::sleep_for(sleep_per_loop); + executor.spin_node_some(node_->get_node_base_interface()); + } + } + + ASSERT_EQ(1, total_images_received); +} + +/* +TEST_F(MessagePassingTestingLifecycle, stress_message_passing) +{ + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node_); + + int images_to_stress = 1000; + + image_transport::Publisher pub = it().advertise("camera/image"); + image_transport::Subscriber sub = it().subscribe("camera/image", imageCallback); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // generate random image and publish it + int image_pubs = 0; + while (image_pubs < images_to_stress) { + pub.publish(generate_random_image()); + executor.spin_some(); + image_pubs++; + } + + ASSERT_EQ(images_to_stress, total_images_received); +} +*/ + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/image_transport/test/test_publisher.cpp b/image_transport/test/test_publisher.cpp index 7216b79a..375c597a 100644 --- a/image_transport/test/test_publisher.cpp +++ b/image_transport/test/test_publisher.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2018 Open Source Robotics Foundation, Inc. +// Copyright (c) 2024 Open Source Robotics Foundation, Inc. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -47,7 +47,7 @@ class TestPublisher : public ::testing::Test }; TEST_F(TestPublisher, publisher) { - auto pub = image_transport::create_publisher(node_.get(), "camera/image"); + auto pub = image_transport::create_publisher(node_, "camera/image"); EXPECT_EQ(node_->get_node_graph_interface()->count_publishers("camera/image"), 1u); pub.shutdown(); EXPECT_EQ(node_->get_node_graph_interface()->count_publishers("camera/image"), 0u); @@ -62,7 +62,7 @@ TEST_F(TestPublisher, image_transport_publisher) { } TEST_F(TestPublisher, camera_publisher) { - auto camera_pub = image_transport::create_camera_publisher(node_.get(), "camera/image"); + auto camera_pub = image_transport::create_camera_publisher(node_, "camera/image"); EXPECT_EQ(node_->get_node_graph_interface()->count_publishers("camera/image"), 1u); EXPECT_EQ(node_->get_node_graph_interface()->count_publishers("camera/camera_info"), 1u); camera_pub.shutdown(); diff --git a/image_transport/test/test_publisher_lifecycle.cpp b/image_transport/test/test_publisher_lifecycle.cpp new file mode 100644 index 00000000..68f840df --- /dev/null +++ b/image_transport/test/test_publisher_lifecycle.cpp @@ -0,0 +1,99 @@ +// Copyright (c) 2024 Open Source Robotics Foundation, 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. + +#include + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "image_transport/image_transport.hpp" + +class TestPublisherLifecycle : public ::testing::Test +{ +protected: + void SetUp() + { + node_ = rclcpp_lifecycle::LifecycleNode::make_shared("test_publisher_lifecycle"); + } + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; +}; + +TEST_F(TestPublisherLifecycle, publisher) { + auto pub = image_transport::create_publisher(node_, "camera/image"); + EXPECT_EQ(node_->get_node_graph_interface()->count_publishers("camera/image"), 1u); + pub.shutdown(); + EXPECT_EQ(node_->get_node_graph_interface()->count_publishers("camera/image"), 0u); + // coverage tests: invalid publisher should fail but not crash + pub.publish(sensor_msgs::msg::Image()); + pub.publish(sensor_msgs::msg::Image::ConstSharedPtr()); +} + +TEST_F(TestPublisherLifecycle, image_transport_publisher) { + image_transport::ImageTransport it(node_); + auto pub = it.advertise("camera/image", 1); +} + +TEST_F(TestPublisherLifecycle, camera_publisher) { + auto camera_pub = image_transport::create_camera_publisher(node_, "camera/image"); + EXPECT_EQ(node_->get_node_graph_interface()->count_publishers("camera/image"), 1u); + EXPECT_EQ(node_->get_node_graph_interface()->count_publishers("camera/camera_info"), 1u); + camera_pub.shutdown(); + EXPECT_EQ(node_->get_node_graph_interface()->count_publishers("camera/image"), 0u); + EXPECT_EQ(node_->get_node_graph_interface()->count_publishers("camera/camera_info"), 0u); + // coverage tests: invalid publisher should fail but not crash + camera_pub.publish(sensor_msgs::msg::Image(), sensor_msgs::msg::CameraInfo()); + camera_pub.publish( + sensor_msgs::msg::Image::ConstSharedPtr(), + sensor_msgs::msg::CameraInfo::ConstSharedPtr()); + sensor_msgs::msg::Image image; + sensor_msgs::msg::CameraInfo info; + camera_pub.publish(image, info, rclcpp::Time()); +} + +TEST_F(TestPublisherLifecycle, image_transport_camera_publisher) { + image_transport::ImageTransport it(node_); + auto pub = it.advertiseCamera("camera/image", 1); +} + +TEST_F(TestPublisherLifecycle, image_transport_camera_publisher_qos) { + image_transport::ImageTransport it(node_); + auto pub = it.advertise("camera/image", rmw_qos_profile_sensor_data); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/image_transport/test/test_qos_override.cpp b/image_transport/test/test_qos_override.cpp index 3b2b7ea3..574f57da 100644 --- a/image_transport/test/test_qos_override.cpp +++ b/image_transport/test/test_qos_override.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Open Source Robotics Foundation, Inc. +// Copyright (c) 2024 Open Source Robotics Foundation, Inc. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -64,13 +64,13 @@ class TestQosOverride : public ::testing::Test TEST_F(TestQosOverride, qos_override_publisher_without_options) { auto pub = - image_transport::create_publisher(pub_node_.get(), "camera/image", rmw_qos_profile_default); + image_transport::create_publisher(pub_node_, "camera/image", rmw_qos_profile_default); auto endpoint_info_vec = pub_node_->get_publishers_info_by_topic("camera/image"); EXPECT_EQ(endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable); pub.shutdown(); pub = image_transport::create_publisher( - qos_override_pub_node_.get(), "camera/image", rmw_qos_profile_default); + qos_override_pub_node_, "camera/image", rmw_qos_profile_default); endpoint_info_vec = qos_override_pub_node_->get_publishers_info_by_topic("camera/image"); EXPECT_EQ( @@ -90,13 +90,13 @@ TEST_F(TestQosOverride, qos_override_publisher_with_options) { }); auto pub = image_transport::create_publisher( - pub_node_.get(), "camera/image", rmw_qos_profile_default, options); + pub_node_, "camera/image", rmw_qos_profile_default, options); auto endpoint_info_vec = pub_node_->get_publishers_info_by_topic("camera/image"); EXPECT_EQ(endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable); pub.shutdown(); pub = image_transport::create_publisher( - qos_override_pub_node_.get(), "camera/image", rmw_qos_profile_default, options); + qos_override_pub_node_, "camera/image", rmw_qos_profile_default, options); endpoint_info_vec = qos_override_pub_node_->get_publishers_info_by_topic("camera/image"); EXPECT_EQ( @@ -110,13 +110,13 @@ TEST_F(TestQosOverride, qos_override_subscriber_without_options) { [](const auto & msg) {(void)msg;}; auto sub = image_transport::create_subscription( - sub_node_.get(), "camera/image", fcn, "raw", rmw_qos_profile_default); + sub_node_, "camera/image", fcn, "raw", rmw_qos_profile_default); auto endpoint_info_vec = sub_node_->get_subscriptions_info_by_topic("camera/image"); EXPECT_EQ(endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable); sub.shutdown(); sub = image_transport::create_subscription( - qos_override_sub_node_.get(), "camera/image", fcn, "raw", rmw_qos_profile_default); + qos_override_sub_node_, "camera/image", fcn, "raw", rmw_qos_profile_default); endpoint_info_vec = qos_override_sub_node_->get_subscriptions_info_by_topic("camera/image"); EXPECT_EQ( @@ -138,13 +138,13 @@ TEST_F(TestQosOverride, qos_override_subscriber_with_options) { }); auto sub = image_transport::create_subscription( - sub_node_.get(), "camera/image", fcn, "raw", rmw_qos_profile_default, options); + sub_node_, "camera/image", fcn, "raw", rmw_qos_profile_default, options); auto endpoint_info_vec = sub_node_->get_subscriptions_info_by_topic("camera/image"); EXPECT_EQ(endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable); sub.shutdown(); sub = image_transport::create_subscription( - qos_override_sub_node_.get(), "camera/image", fcn, "raw", rmw_qos_profile_default, options); + qos_override_sub_node_, "camera/image", fcn, "raw", rmw_qos_profile_default, options); endpoint_info_vec = qos_override_sub_node_->get_subscriptions_info_by_topic("camera/image"); EXPECT_EQ( diff --git a/image_transport/test/test_qos_override_lifecycle.cpp b/image_transport/test/test_qos_override_lifecycle.cpp new file mode 100644 index 00000000..6413f966 --- /dev/null +++ b/image_transport/test/test_qos_override_lifecycle.cpp @@ -0,0 +1,163 @@ +// Copyright (c) 2024 Open Source Robotics Foundation, 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. + +#include + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "image_transport/image_transport.hpp" + +class TestQosOverrideLifecycle : public ::testing::Test +{ +protected: + void SetUp() + { + pub_node_ = rclcpp_lifecycle::LifecycleNode::make_shared("test_publisher_lifecycle"); + qos_override_pub_node_ = rclcpp_lifecycle::LifecycleNode::make_shared( + "test_qos_override_publisher_lifecycle", rclcpp::NodeOptions().parameter_overrides( + { + rclcpp::Parameter( + "qos_overrides./camera/image.publisher.reliability", "best_effort"), + })); + sub_node_ = rclcpp_lifecycle::LifecycleNode::make_shared("test_subscriber_lifecycle"); + qos_override_sub_node_ = rclcpp_lifecycle::LifecycleNode::make_shared( + "test_qos_override_subscriber_lifecycle", rclcpp::NodeOptions().parameter_overrides( + { + rclcpp::Parameter( + "qos_overrides./camera/image.subscription.reliability", "best_effort"), + })); + } + + rclcpp_lifecycle::LifecycleNode::SharedPtr pub_node_; + rclcpp_lifecycle::LifecycleNode::SharedPtr qos_override_pub_node_; + rclcpp_lifecycle::LifecycleNode::SharedPtr sub_node_; + rclcpp_lifecycle::LifecycleNode::SharedPtr qos_override_sub_node_; +}; + +TEST_F(TestQosOverrideLifecycle, qos_override_publisher_without_options) { + auto pub = + image_transport::create_publisher(pub_node_, "camera/image", rmw_qos_profile_default); + auto endpoint_info_vec = pub_node_->get_publishers_info_by_topic("camera/image"); + EXPECT_EQ(endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable); + pub.shutdown(); + + pub = image_transport::create_publisher( + qos_override_pub_node_, "camera/image", rmw_qos_profile_default); + + endpoint_info_vec = qos_override_pub_node_->get_publishers_info_by_topic("camera/image"); + EXPECT_EQ( + endpoint_info_vec[0].qos_profile().reliability(), + rclcpp::ReliabilityPolicy::Reliable); + pub.shutdown(); +} + +TEST_F(TestQosOverrideLifecycle, qos_override_publisher_with_options) { + rclcpp::PublisherOptions options; + options.qos_overriding_options = rclcpp::QosOverridingOptions( + { + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Reliability, + }); + + auto pub = image_transport::create_publisher( + pub_node_, "camera/image", rmw_qos_profile_default, options); + auto endpoint_info_vec = pub_node_->get_publishers_info_by_topic("camera/image"); + EXPECT_EQ(endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable); + pub.shutdown(); + + pub = image_transport::create_publisher( + qos_override_pub_node_, "camera/image", rmw_qos_profile_default, options); + + endpoint_info_vec = qos_override_pub_node_->get_publishers_info_by_topic("camera/image"); + EXPECT_EQ( + endpoint_info_vec[0].qos_profile().reliability(), + rclcpp::ReliabilityPolicy::BestEffort); + pub.shutdown(); +} + +TEST_F(TestQosOverrideLifecycle, qos_override_subscriber_without_options) { + std::function fcn = + [](const auto & msg) {(void)msg;}; + + auto sub = image_transport::create_subscription( + sub_node_, "camera/image", fcn, "raw", rmw_qos_profile_default); + auto endpoint_info_vec = sub_node_->get_subscriptions_info_by_topic("camera/image"); + EXPECT_EQ(endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable); + sub.shutdown(); + + sub = image_transport::create_subscription( + qos_override_sub_node_, "camera/image", fcn, "raw", rmw_qos_profile_default); + + endpoint_info_vec = qos_override_sub_node_->get_subscriptions_info_by_topic("camera/image"); + EXPECT_EQ( + endpoint_info_vec[0].qos_profile().reliability(), + rclcpp::ReliabilityPolicy::Reliable); +} + +TEST_F(TestQosOverrideLifecycle, qos_override_subscriber_with_options) { + std::function fcn = + [](const auto & msg) {(void)msg;}; + + rclcpp::SubscriptionOptions options; + options.qos_overriding_options = rclcpp::QosOverridingOptions( + { + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Reliability, + }); + + auto sub = image_transport::create_subscription( + sub_node_, "camera/image", fcn, "raw", rmw_qos_profile_default, options); + auto endpoint_info_vec = sub_node_->get_subscriptions_info_by_topic("camera/image"); + EXPECT_EQ(endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable); + sub.shutdown(); + + sub = image_transport::create_subscription( + qos_override_sub_node_, "camera/image", fcn, "raw", rmw_qos_profile_default, options); + + endpoint_info_vec = qos_override_sub_node_->get_subscriptions_info_by_topic("camera/image"); + EXPECT_EQ( + endpoint_info_vec[0].qos_profile().reliability(), + rclcpp::ReliabilityPolicy::BestEffort); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/image_transport/test/test_remapping.cpp b/image_transport/test/test_remapping.cpp index 9106a710..4d9f2bfe 100644 --- a/image_transport/test/test_remapping.cpp +++ b/image_transport/test/test_remapping.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2018 Open Source Robotics Foundation, Inc. +// Copyright (c) 2024 Open Source Robotics Foundation, Inc. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -32,6 +32,8 @@ #include #include #include +#include +#include #include "rclcpp/rclcpp.hpp" #include "rclcpp/node.hpp" @@ -73,19 +75,19 @@ TEST_F(TestPublisher, RemappedPublisher) { // Subscribe bool received{false}; auto sub = image_transport::create_subscription( - node_remap_.get(), "old_topic", + node_remap_, "old_topic", [&received](const sensor_msgs::msg::Image::ConstSharedPtr & msg) { (void)msg; received = true; }, "raw"); // Publish - auto pub = image_transport::create_publisher(node_.get(), "new_topic"); + auto pub = image_transport::create_publisher(node_, "new_topic"); ASSERT_EQ("/namespace/new_topic", sub.getTopic()); ASSERT_EQ("/namespace/new_topic", pub.getTopic()); - test_rclcpp::wait_for_subscriber(node_, sub.getTopic()); + test_rclcpp::wait_for_subscriber(node_->get_node_graph_interface(), sub.getTopic()); ASSERT_FALSE(received); diff --git a/image_transport/test/test_remapping_lifecycle.cpp b/image_transport/test/test_remapping_lifecycle.cpp new file mode 100644 index 00000000..33474f39 --- /dev/null +++ b/image_transport/test/test_remapping_lifecycle.cpp @@ -0,0 +1,132 @@ +// Copyright (c) 2024 Open Source Robotics Foundation, 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. + +#include + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "utils.hpp" + +#include "image_transport/image_transport.hpp" + +class TestPublisherLifecycle : public ::testing::Test +{ +protected: + void SetUp() + { + node_ = rclcpp_lifecycle::LifecycleNode::make_shared("node", "namespace"); + std::vector arguments{ + "--ros-args", "-r", "old_topic:=new_topic" + }; + rclcpp::NodeOptions node_options; + node_options.arguments(arguments); + + node_remap_ = rclcpp_lifecycle::LifecycleNode::make_shared( + "node_remap", + "namespace", + node_options + ); + } + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + rclcpp_lifecycle::LifecycleNode::SharedPtr node_remap_; +}; + +TEST_F(TestPublisherLifecycle, RemappedPublisher) { + const size_t max_retries = 3; + const size_t max_loops = 200; + const std::chrono::milliseconds sleep_per_loop = std::chrono::milliseconds(10); + + rclcpp::executors::SingleThreadedExecutor executor; + auto image = std::make_shared(); + + // Subscribe + bool received{false}; + auto sub = image_transport::create_subscription( + node_remap_, "old_topic", + [&received](const sensor_msgs::msg::Image::ConstSharedPtr & msg) { + (void)msg; + received = true; + }, "raw"); + + // Publish + auto pub = image_transport::create_publisher(node_, "new_topic"); + + ASSERT_EQ("/namespace/new_topic", sub.getTopic()); + ASSERT_EQ("/namespace/new_topic", pub.getTopic()); + + test_rclcpp::wait_for_subscriber(node_->get_node_graph_interface(), sub.getTopic()); + + ASSERT_FALSE(received); + + size_t retry = 0; + size_t nSub = 0; + size_t nPub = 0; + while (retry < max_retries && nPub == 0 && nSub == 0) { + nSub = pub.getNumSubscribers(); + nPub = sub.getNumPublishers(); + std::this_thread::sleep_for(sleep_per_loop); + } + + executor.spin_node_some(node_->get_node_base_interface()); + executor.spin_node_some(node_remap_->get_node_base_interface()); + + retry = 0; + while (retry < max_retries && !received) { + // generate random image and publish it + pub.publish(image); + + executor.spin_node_some(node_->get_node_base_interface()); + executor.spin_node_some(node_remap_->get_node_base_interface()); + + size_t loop = 0; + while ((!received) && (loop++ < max_loops)) { + std::this_thread::sleep_for(sleep_per_loop); + executor.spin_node_some(node_->get_node_base_interface()); + executor.spin_node_some(node_remap_->get_node_base_interface()); + } + } + + EXPECT_TRUE(received); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/image_transport/test/test_single_subscriber_publisher.cpp b/image_transport/test/test_single_subscriber_publisher.cpp index f2137d3a..72b48fa0 100644 --- a/image_transport/test/test_single_subscriber_publisher.cpp +++ b/image_transport/test/test_single_subscriber_publisher.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2018 Open Source Robotics Foundation, Inc. +// Copyright (c) 2024 Open Source Robotics Foundation, Inc. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -29,6 +29,7 @@ #include #include #include +#include #include "gtest/gtest.h" diff --git a/image_transport/test/test_single_subscriber_publisher_lifecycle.cpp b/image_transport/test/test_single_subscriber_publisher_lifecycle.cpp new file mode 100644 index 00000000..1f969177 --- /dev/null +++ b/image_transport/test/test_single_subscriber_publisher_lifecycle.cpp @@ -0,0 +1,89 @@ +// Copyright (c) 2024 Open Source Robotics Foundation, 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. + +#include +#include +#include +#include + +#include "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "sensor_msgs/msg/image.hpp" + +#include "image_transport/single_subscriber_publisher.hpp" + +class TestPublisherLifecycle : public ::testing::Test +{ +protected: + static constexpr const char * caller_id = "node"; + static constexpr const char * topic = "/topic"; + + + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + node = std::make_shared("image_transport", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + + rclcpp_lifecycle::LifecycleNode::SharedPtr node; +}; + +TEST_F(TestPublisherLifecycle, construction_and_destruction) { + auto get_num_subscribers = []() -> size_t {return 0;}; + auto publish_fn = [](const sensor_msgs::msg::Image & /*image*/) {}; + + image_transport::SingleSubscriberPublisher ssp(caller_id, topic, + get_num_subscribers, publish_fn); +} + +TEST_F(TestPublisherLifecycle, getNumSubscribers) { + size_t nSub = 0; + + auto get_num_subscribers = [&nSub]() -> size_t {return nSub;}; + auto publish_fn = [](const sensor_msgs::msg::Image & /*image*/) {}; + + image_transport::SingleSubscriberPublisher ssp(caller_id, topic, + get_num_subscribers, publish_fn); + + nSub = 0; + ASSERT_EQ(ssp.getNumSubscribers(), 0u); + nSub = 1; + ASSERT_EQ(ssp.getNumSubscribers(), 1u); +} diff --git a/image_transport/test/test_subscriber.cpp b/image_transport/test/test_subscriber.cpp index 75ecc827..c092b12f 100644 --- a/image_transport/test/test_subscriber.cpp +++ b/image_transport/test/test_subscriber.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2018 Open Source Robotics Foundation, Inc. +// Copyright (c) 2024 Open Source Robotics Foundation, Inc. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -30,6 +30,10 @@ #include #include +#include +#include +#include +#include #include "rclcpp/rclcpp.hpp" @@ -50,7 +54,7 @@ TEST_F(TestSubscriber, construction_and_destruction) { std::function fcn = [](const auto & msg) {(void)msg;}; - auto sub = image_transport::create_subscription(node_.get(), "camera/image", fcn, "raw"); + auto sub = image_transport::create_subscription(node_, "camera/image", fcn, "raw"); rclcpp::executors::SingleThreadedExecutor executor; executor.spin_node_some(node_); @@ -60,7 +64,7 @@ TEST_F(TestSubscriber, shutdown) { std::function fcn = [](const auto & msg) {(void)msg;}; - auto sub = image_transport::create_subscription(node_.get(), "camera/image", fcn, "raw"); + auto sub = image_transport::create_subscription(node_, "camera/image", fcn, "raw"); EXPECT_EQ(node_->get_node_graph_interface()->count_subscribers("camera/image"), 1u); sub.shutdown(); EXPECT_EQ(node_->get_node_graph_interface()->count_subscribers("camera/image"), 0u); @@ -72,7 +76,7 @@ TEST_F(TestSubscriber, camera_sub_shutdown) { const sensor_msgs::msg::CameraInfo::ConstSharedPtr &)> fcn = [](const auto & msg, const auto &) {(void)msg;}; - auto sub = image_transport::create_camera_subscription(node_.get(), "camera/image", fcn, "raw"); + auto sub = image_transport::create_camera_subscription(node_, "camera/image", fcn, "raw"); EXPECT_EQ(node_->get_node_graph_interface()->count_subscribers("camera/image"), 1u); EXPECT_EQ(node_->get_node_graph_interface()->count_subscribers("camera/camera_info"), 1u); sub.shutdown(); diff --git a/image_transport/test/test_subscriber_lifecycle.cpp b/image_transport/test/test_subscriber_lifecycle.cpp new file mode 100644 index 00000000..38667f7b --- /dev/null +++ b/image_transport/test/test_subscriber_lifecycle.cpp @@ -0,0 +1,223 @@ +// Copyright (c) 2024 Open Source Robotics Foundation, 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. + +#include + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "image_transport/image_transport.hpp" + +class TestSubscriberLifecycle : public ::testing::Test +{ +protected: + void SetUp() + { + node_ = rclcpp_lifecycle::LifecycleNode::make_shared("test_subscriber_lifecycle"); + } + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; +}; + +TEST_F(TestSubscriberLifecycle, construction_and_destruction) { + std::function fcn = + [](const auto & msg) {(void)msg;}; + + auto sub = image_transport::create_subscription(node_, "camera/image", fcn, "raw"); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.spin_node_some(node_->get_node_base_interface()); +} + +TEST_F(TestSubscriberLifecycle, shutdown) { + std::function fcn = + [](const auto & msg) {(void)msg;}; + + auto sub = image_transport::create_subscription(node_, "camera/image", fcn, "raw"); + EXPECT_EQ(node_->get_node_graph_interface()->count_subscribers("camera/image"), 1u); + sub.shutdown(); + EXPECT_EQ(node_->get_node_graph_interface()->count_subscribers("camera/image"), 0u); +} + +TEST_F(TestSubscriberLifecycle, camera_sub_shutdown) { + std::function fcn = + [](const auto & msg, const auto &) {(void)msg;}; + + auto sub = image_transport::create_camera_subscription(node_, "camera/image", fcn, "raw"); + EXPECT_EQ(node_->get_node_graph_interface()->count_subscribers("camera/image"), 1u); + EXPECT_EQ(node_->get_node_graph_interface()->count_subscribers("camera/camera_info"), 1u); + sub.shutdown(); + EXPECT_EQ(node_->get_node_graph_interface()->count_subscribers("camera/image"), 0u); + EXPECT_EQ(node_->get_node_graph_interface()->count_subscribers("camera/camera_info"), 0u); +} + +TEST_F(TestSubscriberLifecycle, callback_groups) { + using namespace std::chrono_literals; + + // Create a publisher node. + auto node_publisher = rclcpp_lifecycle::LifecycleNode::make_shared( + "image_publisher", + rclcpp::NodeOptions()); + image_transport::ImageTransport it_publisher(node_publisher); + image_transport::Publisher pub = it_publisher.advertise("camera/image", 1); + + auto msg = sensor_msgs::msg::Image(); + auto timer = node_publisher->create_wall_timer(100ms, [&]() {pub.publish(msg);}); + + // Create a subscriber to read the images. + std::atomic flag_1 = false; + std::atomic flag_2 = false; + std::function fcn1 = + [&](const auto & msg) { + (void)msg; + flag_1 = true; + std::this_thread::sleep_for(1s); + }; + std::function fcn2 = + [&](const auto & msg) { + (void)msg; + flag_2 = true; + std::this_thread::sleep_for(1s); + }; + + auto cb_group = node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group; + + image_transport::ImageTransport it(node_); + + auto subscriber_1 = it.subscribe("camera/image", 1, fcn1, nullptr, nullptr, sub_options); + auto subscriber_2 = it.subscribe("camera/image", 1, fcn2, nullptr, nullptr, sub_options); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node_->get_node_base_interface()); + executor.add_node(node_publisher->get_node_base_interface()); + // Both callbacks should be executed and the flags should be set. + std::thread executor_thread([&]() {executor.spin();}); + + // The callbacks sleep for 5 seconds and mutually exclusive callbacks should be blocked. + // However, because of the the multithreaded executor and renentrant callback group, + // the flags should be set, as the callbacks should be in different threads. + auto timeout_elapsed = 0.0s; + auto sleep_duration = 0.1s; + auto timeout = 0.5s; + + while (!(flag_1 && flag_2)) { + std::this_thread::sleep_for(sleep_duration); + timeout_elapsed += sleep_duration; + } + executor.cancel(); + executor_thread.join(); + + EXPECT_LT(timeout_elapsed, timeout); +} + +TEST_F(TestSubscriberLifecycle, callback_groups_custom_qos) { + using namespace std::chrono_literals; + + // Create a publisher node. + auto node_publisher = rclcpp_lifecycle::LifecycleNode::make_shared( + "image_publisher", + rclcpp::NodeOptions()); + image_transport::ImageTransport it_publisher(node_publisher); + image_transport::Publisher pub = it_publisher.advertise( + "camera/image", + rmw_qos_profile_sensor_data); + + auto msg = sensor_msgs::msg::Image(); + auto timer = node_publisher->create_wall_timer(100ms, [&]() {pub.publish(msg);}); + + // Create a subscriber to read the images. + std::atomic flag_1 = false; + std::atomic flag_2 = false; + std::function fcn1 = + [&](const auto & msg) { + (void)msg; + flag_1 = true; + std::this_thread::sleep_for(1s); + }; + std::function fcn2 = + [&](const auto & msg) { + (void)msg; + flag_2 = true; + std::this_thread::sleep_for(1s); + }; + + auto cb_group = node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group; + + image_transport::ImageTransport it(node_); + + auto subscriber_1 = it.subscribe( + "camera/image", rmw_qos_profile_sensor_data, fcn1, nullptr, + nullptr, sub_options); + auto subscriber_2 = it.subscribe( + "camera/image", rmw_qos_profile_sensor_data, fcn2, nullptr, + nullptr, sub_options); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node_->get_node_base_interface()); + executor.add_node(node_publisher->get_node_base_interface()); + // Both callbacks should be executed and the flags should be set. + std::thread executor_thread([&]() {executor.spin();}); + + // The callbacks sleep for 5 seconds and mutually exclusive callbacks should be blocked. + // However, because of the the multithreaded executor and renentrant callback group, + // the flags should be set, as the callbacks should be in different threads. + auto timeout_elapsed = 0.0s; + auto sleep_duration = 0.1s; + auto timeout = 0.5s; + + while (!(flag_1 && flag_2)) { + std::this_thread::sleep_for(sleep_duration); + timeout_elapsed += sleep_duration; + } + executor.cancel(); + executor_thread.join(); + + EXPECT_LT(timeout_elapsed, timeout); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/image_transport/test/utils.hpp b/image_transport/test/utils.hpp index aee9d9cf..2148298f 100644 --- a/image_transport/test/utils.hpp +++ b/image_transport/test/utils.hpp @@ -32,7 +32,7 @@ namespace test_rclcpp // subscribers to be > 0, if false it will wait for the number of // subscribers to be 0 void wait_for_subscriber( - std::shared_ptr node, + std::shared_ptr node, const std::string & topic_name, bool to_be_available = true, std::chrono::milliseconds timeout = std::chrono::milliseconds(1),