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 d3150ab..ee121aa 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 @@ -263,8 +263,7 @@ class PointCloudTransport : public PointCloudTransportLoader point_cloud_transport::Subscriber subscribe( const std::string & base_topic, rmw_qos_profile_t custom_qos, void (T::* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &) const, T * obj, - const point_cloud_transport::TransportHints * transport_hints = nullptr, - bool allow_concurrent_callbacks = false) + const point_cloud_transport::TransportHints * transport_hints = nullptr) { return subscribe( base_topic, custom_qos, std::bind( @@ -276,8 +275,7 @@ class PointCloudTransport : public PointCloudTransportLoader point_cloud_transport::Subscriber subscribe( const std::string & base_topic, uint32_t queue_size, void (T::* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &) const, T * obj, - const point_cloud_transport::TransportHints * transport_hints = nullptr, - bool allow_concurrent_callbacks = false) + const point_cloud_transport::TransportHints * transport_hints = nullptr) { return subscribe( base_topic, queue_size, std::bind( 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 f845599..42dab30 100644 --- a/point_cloud_transport/include/point_cloud_transport/publisher_plugin.hpp +++ b/point_cloud_transport/include/point_cloud_transport/publisher_plugin.hpp @@ -59,6 +59,8 @@ class PublisherPlugin PublisherPlugin(const PublisherPlugin &) = delete; PublisherPlugin & operator=(const PublisherPlugin &) = delete; + virtual ~PublisherPlugin() {} + //! Get a string identifier for the transport provided by this plugin virtual std::string getTransportName() const = 0; @@ -85,14 +87,14 @@ class PublisherPlugin /// or an error message. /// POINT_CLOUD_TRANSPORT_PUBLIC - virtual EncodeResult encode(const sensor_msgs::msg::PointCloud2 & raw) const; + virtual EncodeResult encode(const sensor_msgs::msg::PointCloud2 & raw) const = 0; //! Publish a point cloud using the transport associated with this PublisherPlugin. virtual void publish(const sensor_msgs::msg::PointCloud2 & message) const = 0; //! Publish a point cloud using the transport associated with this PublisherPlugin. POINT_CLOUD_TRANSPORT_PUBLIC - virtual void publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) const; + virtual void publishPtr(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) const; //! Shutdown any advertisements associated with this PublisherPlugin. virtual void shutdown() = 0; diff --git a/point_cloud_transport/include/point_cloud_transport/raw_publisher.hpp b/point_cloud_transport/include/point_cloud_transport/raw_publisher.hpp index db2c4dc..162d6f5 100644 --- a/point_cloud_transport/include/point_cloud_transport/raw_publisher.hpp +++ b/point_cloud_transport/include/point_cloud_transport/raw_publisher.hpp @@ -51,7 +51,7 @@ class RawPublisher public: virtual ~RawPublisher() {} - virtual std::string getTransportName() const + std::string getTransportName() const override { return "raw"; } @@ -66,26 +66,27 @@ class RawPublisher } RawPublisher::TypedEncodeResult encodeTyped(const sensor_msgs::msg::PointCloud2 & raw) const + override { return raw; } protected: - virtual void publish( + void publish( const sensor_msgs::msg::PointCloud2 & message, - const PublishFn & publish_fn) const + const PublishFn & publish_fn) const override { publish_fn(message); } - virtual void publish( + void publish( const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message_ptr, - const PublishFn & publish_fn) const + const PublishFn & publish_fn) const override { publish_fn(*message_ptr); } - virtual std::string getTopicToAdvertise(const std::string & base_topic) const + std::string getTopicToAdvertise(const std::string & base_topic) const override { return base_topic; } diff --git a/point_cloud_transport/include/point_cloud_transport/raw_subscriber.hpp b/point_cloud_transport/include/point_cloud_transport/raw_subscriber.hpp index 89b8c59..3f4aac2 100644 --- a/point_cloud_transport/include/point_cloud_transport/raw_subscriber.hpp +++ b/point_cloud_transport/include/point_cloud_transport/raw_subscriber.hpp @@ -62,7 +62,7 @@ class RawSubscriber POINT_CLOUD_TRANSPORT_PUBLIC SubscriberPlugin::DecodeResult decodeTyped( - const sensor_msgs::msg::PointCloud2 & compressed) const; + const sensor_msgs::msg::PointCloud2 & compressed) const override; POINT_CLOUD_TRANSPORT_PUBLIC std::string getDataType() const override; diff --git a/point_cloud_transport/include/point_cloud_transport/simple_publisher_plugin.hpp b/point_cloud_transport/include/point_cloud_transport/simple_publisher_plugin.hpp index a5f850f..3d3531f 100644 --- a/point_cloud_transport/include/point_cloud_transport/simple_publisher_plugin.hpp +++ b/point_cloud_transport/include/point_cloud_transport/simple_publisher_plugin.hpp @@ -196,10 +196,10 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin protected: std::string base_topic_; - virtual void advertiseImpl( + void advertiseImpl( std::shared_ptr node, const std::string & base_topic, rmw_qos_profile_t custom_qos, - const rclcpp::PublisherOptions & options) + const rclcpp::PublisherOptions & options) override { std::string transport_topic = getTopicToAdvertise(base_topic); simple_impl_ = std::make_unique(node); @@ -237,6 +237,27 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin } } + /// + /// \brief Publish a point cloud using the specified publish function. + /// + /// The PublishFn publishes the transport-specific message type. This indirection allows + /// SimplePublisherPlugin to use this function for both normal broadcast publishing and + /// single subscriber publishing (in subscription callbacks). + /// + virtual void publish( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message, + const PublishFn & publish_fn) const + { + const auto res = this->encodeTyped(*message.get()); + if (!res) { + RCLCPP_ERROR( + this->getLogger(), "Error encoding message by transport %s: %s.", + this->getTransportName().c_str(), res.error().c_str()); + } else if (res.value()) { + publish_fn(res.value().value()); + } + } + /// /// \brief Return the communication topic name for a given base topic. /// diff --git a/point_cloud_transport/src/publisher.cpp b/point_cloud_transport/src/publisher.cpp index c6ba16e..deac37c 100644 --- a/point_cloud_transport/src/publisher.cpp +++ b/point_cloud_transport/src/publisher.cpp @@ -209,7 +209,7 @@ void Publisher::publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & me for (const auto & pub : impl_->publishers_) { if (pub->getNumSubscribers() > 0) { - pub->publish(message); + pub->publishPtr(message); } } } diff --git a/point_cloud_transport/src/publisher_plugin.cpp b/point_cloud_transport/src/publisher_plugin.cpp index 62d8514..07b619b 100644 --- a/point_cloud_transport/src/publisher_plugin.cpp +++ b/point_cloud_transport/src/publisher_plugin.cpp @@ -38,12 +38,6 @@ namespace point_cloud_transport { -PublisherPlugin::EncodeResult PublisherPlugin::encode(const sensor_msgs::msg::PointCloud2 & raw) -const -{ - return this->encode(raw); -} - void PublisherPlugin::advertise( std::shared_ptr node, const std::string & base_topic, @@ -53,7 +47,8 @@ void PublisherPlugin::advertise( advertiseImpl(node, base_topic, custom_qos, options); } -void PublisherPlugin::publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) const +void PublisherPlugin::publishPtr(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) +const { publish(*message); } diff --git a/point_cloud_transport/src/republish.cpp b/point_cloud_transport/src/republish.cpp index b7e8666..a452d04 100644 --- a/point_cloud_transport/src/republish.cpp +++ b/point_cloud_transport/src/republish.cpp @@ -147,7 +147,7 @@ void Republisher::initialize() typedef void (point_cloud_transport::PublisherPlugin::* PublishMemFn)( const sensor_msgs::msg:: PointCloud2::ConstSharedPtr &) const; - PublishMemFn pub_mem_fn = &point_cloud_transport::PublisherPlugin::publish; + PublishMemFn pub_mem_fn = &point_cloud_transport::PublisherPlugin::publishPtr; RCLCPP_INFO(this->get_logger(), "Loading %s subscriber", in_topic.c_str());