From 16c80870a63c807d0b8b356adcfdac1eb465c23b Mon Sep 17 00:00:00 2001 From: elsayed Date: Mon, 27 Jan 2025 17:52:55 +0200 Subject: [PATCH] init --- point_cloud_transport/CMakeLists.txt | 2 + .../point_cloud_transport.hpp | 25 +++++++------ .../point_cloud_transport/publisher.hpp | 6 ++- .../publisher_plugin.hpp | 6 ++- point_cloud_transport/package.xml | 1 + point_cloud_transport/src/publisher.cpp | 37 ++++++++++++------- .../src/publisher_plugin.cpp | 14 +++++-- 7 files changed, 59 insertions(+), 32 deletions(-) diff --git a/point_cloud_transport/CMakeLists.txt b/point_cloud_transport/CMakeLists.txt index 415c236..2d1865b 100644 --- a/point_cloud_transport/CMakeLists.txt +++ b/point_cloud_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(rcpputils REQUIRED) find_package(rmw REQUIRED) @@ -43,6 +44,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC message_filters::message_filters pluginlib::pluginlib rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle rcpputils::rcpputils rmw::rmw ${sensor_msgs_TARGETS} diff --git a/point_cloud_transport/include/point_cloud_transport/point_cloud_transport.hpp b/point_cloud_transport/include/point_cloud_transport/point_cloud_transport.hpp index f5fa464..cea3567 100644 --- a/point_cloud_transport/include/point_cloud_transport/point_cloud_transport.hpp +++ b/point_cloud_transport/include/point_cloud_transport/point_cloud_transport.hpp @@ -57,6 +57,7 @@ namespace point_cloud_transport //! PointCloudTransport is analogous to rclcpp::Node in that it contains functions //! to create publishers and subscriptions of PointCloud2 topics. +template class PointCloudTransportLoader { public: @@ -80,21 +81,21 @@ class PointCloudTransportLoader //! The loader that can load publisher plugins. POINT_CLOUD_TRANSPORT_PUBLIC - PubLoaderPtr getPublisherLoader() const; + PubLoaderPtr getPublisherLoader() const; //! The loader that can load subscriber plugins. POINT_CLOUD_TRANSPORT_PUBLIC SubLoaderPtr getSubscriberLoader() const; POINT_CLOUD_TRANSPORT_PUBLIC - point_cloud_transport::PubLoaderPtr getPubLoader(); + point_cloud_transport::PubLoaderPtr getPubLoader(); POINT_CLOUD_TRANSPORT_PUBLIC point_cloud_transport::SubLoaderPtr getSubLoader(); protected: - point_cloud_transport::PubLoaderPtr pub_loader_; - point_cloud_transport::SubLoaderPtr sub_loader_; + point_cloud_transport::PubLoaderPtr pub_loader_; + point_cloud_transport::SubLoaderPtr sub_loader_; }; /// \brief Advertise every available transport on pointcloud topics, free function version. @@ -103,9 +104,10 @@ class PointCloudTransportLoader /// \param custom_qos The QoS profile to use for the underlying publisher(s) /// \param options The publisher options to use for the underlying publisher(s) /// \return The advertised publisher +template POINT_CLOUD_TRANSPORT_PUBLIC -Publisher create_publisher( - std::shared_ptr node, +Publisher create_publisher( + std::shared_ptr node, const std::string & base_topic, rmw_qos_profile_t custom_qos = rmw_qos_profile_default, const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()); @@ -127,6 +129,7 @@ Subscriber create_subscription( rmw_qos_profile_t custom_qos = rmw_qos_profile_default, rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); +template class PointCloudTransport : public PointCloudTransportLoader { using VoidPtr = std::shared_ptr; @@ -134,7 +137,7 @@ class PointCloudTransport : public PointCloudTransportLoader public: //! Constructor POINT_CLOUD_TRANSPORT_PUBLIC - explicit PointCloudTransport(rclcpp::Node::SharedPtr node); + explicit PointCloudTransport(NodeType::SharedPtr node); POINT_CLOUD_TRANSPORT_PUBLIC ~PointCloudTransport() override = default; @@ -154,7 +157,7 @@ class PointCloudTransport : public PointCloudTransportLoader //! Advertise a PointCloud2 topic, simple version. POINT_CLOUD_TRANSPORT_PUBLIC - Publisher advertise( + Publisher advertise( const std::string & base_topic, uint32_t queue_size) { @@ -166,7 +169,7 @@ class PointCloudTransport : public PointCloudTransportLoader //! Advertise a PointCloud2 topic, simple version. POINT_CLOUD_TRANSPORT_PUBLIC - Publisher advertise( + Publisher advertise( const std::string & base_topic, uint32_t queue_size, const rclcpp::PublisherOptions & options) @@ -177,7 +180,7 @@ class PointCloudTransport : public PointCloudTransportLoader } POINT_CLOUD_TRANSPORT_PUBLIC - Publisher advertise( + Publisher advertise( const std::string & base_topic, rmw_qos_profile_t custom_qos, const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()) @@ -318,7 +321,7 @@ class PointCloudTransport : public PointCloudTransportLoader } private: - rclcpp::Node::SharedPtr node_; + NodeType::SharedPtr node_; }; } // namespace point_cloud_transport diff --git a/point_cloud_transport/include/point_cloud_transport/publisher.hpp b/point_cloud_transport/include/point_cloud_transport/publisher.hpp index af04d13..8fee4c3 100644 --- a/point_cloud_transport/include/point_cloud_transport/publisher.hpp +++ b/point_cloud_transport/include/point_cloud_transport/publisher.hpp @@ -37,6 +37,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include @@ -48,6 +49,7 @@ namespace point_cloud_transport { +template class Publisher { public: @@ -57,7 +59,7 @@ class Publisher POINT_CLOUD_TRANSPORT_PUBLIC Publisher( - std::shared_ptr node, + std::shared_ptr node, const std::string & base_topic, PubLoaderPtr loader, rmw_qos_profile_t custom_qos, @@ -107,7 +109,7 @@ class Publisher private: struct Impl; std::shared_ptr impl_; - friend class PointCloudTransport; + friend class PointCloudTransport; }; } // namespace point_cloud_transport diff --git a/point_cloud_transport/include/point_cloud_transport/publisher_plugin.hpp b/point_cloud_transport/include/point_cloud_transport/publisher_plugin.hpp index 42dab30..6fad370 100644 --- a/point_cloud_transport/include/point_cloud_transport/publisher_plugin.hpp +++ b/point_cloud_transport/include/point_cloud_transport/publisher_plugin.hpp @@ -37,6 +37,7 @@ #include #include "rclcpp/node.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include #include @@ -47,6 +48,7 @@ namespace point_cloud_transport { //! Base class for plugins to Publisher. +template class PublisherPlugin { public: @@ -67,7 +69,7 @@ class PublisherPlugin //! \brief Advertise a topic, simple version. POINT_CLOUD_TRANSPORT_PUBLIC void advertise( - std::shared_ptr node, + std::shared_ptr node, const std::string & base_topic, rmw_qos_profile_t custom_qos = rmw_qos_profile_default, const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()); @@ -111,7 +113,7 @@ class PublisherPlugin protected: //! Advertise a topic. Must be implemented by the subclass. virtual void advertiseImpl( - std::shared_ptr node, const std::string & base_topic, + std::shared_ptr node, const std::string & base_topic, rmw_qos_profile_t custom_qos, const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()) = 0; }; diff --git a/point_cloud_transport/package.xml b/point_cloud_transport/package.xml index 491a9b9..46791ea 100644 --- a/point_cloud_transport/package.xml +++ b/point_cloud_transport/package.xml @@ -23,6 +23,7 @@ pluginlib rclcpp_components rclcpp + rclcpp_lifecycle rcpputils rmw sensor_msgs diff --git a/point_cloud_transport/src/publisher.cpp b/point_cloud_transport/src/publisher.cpp index deac37c..11086c4 100644 --- a/point_cloud_transport/src/publisher.cpp +++ b/point_cloud_transport/src/publisher.cpp @@ -51,9 +51,13 @@ namespace point_cloud_transport { -struct Publisher::Impl +template class Publisher; +template class Publisher; + +template +struct Publisher::Impl { - explicit Impl(std::shared_ptr node) + explicit Impl(std::shared_ptr node) : logger_(node->get_logger()), unadvertised_(false) { @@ -96,14 +100,15 @@ struct Publisher::Impl rclcpp::Logger logger_; std::string base_topic_; - PubLoaderPtr loader_; - std::vector> publishers_; + PubLoaderPtr loader_; + std::vector>> publishers_; bool unadvertised_; }; -Publisher::Publisher( - std::shared_ptr node, const std::string & base_topic, - PubLoaderPtr loader, rmw_qos_profile_t custom_qos, +template +Publisher::Publisher( + std::shared_ptr node, const std::string & base_topic, + PubLoaderPtr loader, rmw_qos_profile_t custom_qos, const rclcpp::PublisherOptions & options) : impl_(std::make_shared(node)) { @@ -166,7 +171,8 @@ Publisher::Publisher( } } -uint32_t Publisher::getNumSubscribers() const +template +uint32_t Publisher::getNumSubscribers() const { if (impl_ && impl_->isValid()) { return impl_->getNumSubscribers(); @@ -174,7 +180,8 @@ uint32_t Publisher::getNumSubscribers() const return 0; } -std::string Publisher::getTopic() const +template +std::string Publisher::getTopic() const { if (impl_) { return impl_->getTopic(); @@ -182,7 +189,8 @@ std::string Publisher::getTopic() const return {}; } -void Publisher::publish(const sensor_msgs::msg::PointCloud2 & message) const +template +void Publisher::publish(const sensor_msgs::msg::PointCloud2 & message) const { if (!impl_ || !impl_->isValid()) { // TODO(ros2) Switch to RCUTILS_ASSERT when ros2/rcutils#112 is merged @@ -198,7 +206,8 @@ void Publisher::publish(const sensor_msgs::msg::PointCloud2 & message) const } } -void Publisher::publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) const +template +void Publisher::publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) const { if (!impl_ || !impl_->isValid()) { // TODO(ros2) Switch to RCUTILS_ASSERT when ros2/rcutils#112 is merged @@ -214,7 +223,8 @@ void Publisher::publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & me } } -void Publisher::shutdown() +template +void Publisher::shutdown() { if (impl_) { impl_->shutdown(); @@ -222,7 +232,8 @@ void Publisher::shutdown() } } -Publisher::operator void *() const +Publisher +Publisher::operator void *() const { return (impl_ && impl_->isValid()) ? reinterpret_cast(1) : reinterpret_cast(0); } diff --git a/point_cloud_transport/src/publisher_plugin.cpp b/point_cloud_transport/src/publisher_plugin.cpp index 07b619b..77b404c 100644 --- a/point_cloud_transport/src/publisher_plugin.cpp +++ b/point_cloud_transport/src/publisher_plugin.cpp @@ -38,8 +38,12 @@ namespace point_cloud_transport { -void PublisherPlugin::advertise( - std::shared_ptr node, +template class PublisherPlugin; +template class PublisherPlugin; + +template +void PublisherPlugin::advertise( + std::shared_ptr node, const std::string & base_topic, rmw_qos_profile_t custom_qos, const rclcpp::PublisherOptions & options) @@ -47,13 +51,15 @@ void PublisherPlugin::advertise( advertiseImpl(node, base_topic, custom_qos, options); } -void PublisherPlugin::publishPtr(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) +template +void PublisherPlugin::publishPtr(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) const { publish(*message); } -std::string PublisherPlugin::getLookupName(const std::string & transport_name) +template +std::string PublisherPlugin::getLookupName(const std::string & transport_name) { return "point_cloud_transport/" + transport_name + "_pub"; }