Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feat/Add LifecycleNode Support #109

Open
wants to merge 1 commit into
base: rolling
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions point_cloud_transport/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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 NodeType = rclcpp::Node>
class PointCloudTransportLoader
{
public:
Expand All @@ -80,21 +81,21 @@ class PointCloudTransportLoader

//! The loader that can load publisher plugins.
POINT_CLOUD_TRANSPORT_PUBLIC
PubLoaderPtr getPublisherLoader() const;
PubLoaderPtr<NodeType> 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<NodeType> 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<NodeType> pub_loader_;
point_cloud_transport::SubLoaderPtr<NodeType> sub_loader_;
};

/// \brief Advertise every available transport on pointcloud topics, free function version.
Expand All @@ -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<class NodeType = rclcpp::Node>
POINT_CLOUD_TRANSPORT_PUBLIC
Publisher create_publisher(
std::shared_ptr<rclcpp::Node> node,
Publisher<NodeType> create_publisher(
std::shared_ptr<NodeType> node,
const std::string & base_topic,
rmw_qos_profile_t custom_qos = rmw_qos_profile_default,
const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions());
Expand All @@ -127,14 +129,15 @@ Subscriber create_subscription(
rmw_qos_profile_t custom_qos = rmw_qos_profile_default,
rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions());

template<class NodeType = rclcpp::Node>
class PointCloudTransport : public PointCloudTransportLoader
{
using VoidPtr = std::shared_ptr<void>;

public:
//! Constructor
POINT_CLOUD_TRANSPORT_PUBLIC
explicit PointCloudTransport(rclcpp::Node::SharedPtr node);
explicit PointCloudTransport(NodeType::SharedPtr node);

POINT_CLOUD_TRANSPORT_PUBLIC
~PointCloudTransport() override = default;
Expand All @@ -154,7 +157,7 @@ class PointCloudTransport : public PointCloudTransportLoader

//! Advertise a PointCloud2 topic, simple version.
POINT_CLOUD_TRANSPORT_PUBLIC
Publisher advertise(
Publisher<NodeType> advertise(
const std::string & base_topic,
uint32_t queue_size)
{
Expand All @@ -166,7 +169,7 @@ class PointCloudTransport : public PointCloudTransportLoader

//! Advertise a PointCloud2 topic, simple version.
POINT_CLOUD_TRANSPORT_PUBLIC
Publisher advertise(
Publisher<NodeType> advertise(
const std::string & base_topic,
uint32_t queue_size,
const rclcpp::PublisherOptions & options)
Expand All @@ -177,7 +180,7 @@ class PointCloudTransport : public PointCloudTransportLoader
}

POINT_CLOUD_TRANSPORT_PUBLIC
Publisher advertise(
Publisher<NodeType> advertise(
const std::string & base_topic,
rmw_qos_profile_t custom_qos,
const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions())
Expand Down Expand Up @@ -318,7 +321,7 @@ class PointCloudTransport : public PointCloudTransportLoader
}

private:
rclcpp::Node::SharedPtr node_;
NodeType::SharedPtr node_;
};

} // namespace point_cloud_transport
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@

#include "rclcpp/macros.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"

#include <sensor_msgs/msg/point_cloud2.hpp>

Expand All @@ -48,6 +49,7 @@
namespace point_cloud_transport
{

template<class NodeType = rclcpp::Node>
class Publisher
{
public:
Expand All @@ -57,7 +59,7 @@ class Publisher

POINT_CLOUD_TRANSPORT_PUBLIC
Publisher(
std::shared_ptr<rclcpp::Node> node,
std::shared_ptr<NodeType> node,
const std::string & base_topic,
PubLoaderPtr loader,
rmw_qos_profile_t custom_qos,
Expand Down Expand Up @@ -107,7 +109,7 @@ class Publisher
private:
struct Impl;
std::shared_ptr<Impl> impl_;
friend class PointCloudTransport;
friend class PointCloudTransport<NodeType>;
};

} // namespace point_cloud_transport
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@
#include <vector>

#include "rclcpp/node.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <rcpputils/tl_expected/expected.hpp>

Expand All @@ -47,6 +48,7 @@ namespace point_cloud_transport
{

//! Base class for plugins to Publisher.
template<class NodeType = rclcpp::Node>
class PublisherPlugin
{
public:
Expand All @@ -67,7 +69,7 @@ class PublisherPlugin
//! \brief Advertise a topic, simple version.
POINT_CLOUD_TRANSPORT_PUBLIC
void advertise(
std::shared_ptr<rclcpp::Node> node,
std::shared_ptr<NodeType> node,
const std::string & base_topic,
rmw_qos_profile_t custom_qos = rmw_qos_profile_default,
const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions());
Expand Down Expand Up @@ -111,7 +113,7 @@ class PublisherPlugin
protected:
//! Advertise a topic. Must be implemented by the subclass.
virtual void advertiseImpl(
std::shared_ptr<rclcpp::Node> node, const std::string & base_topic,
std::shared_ptr<NodeType> node, const std::string & base_topic,
rmw_qos_profile_t custom_qos,
const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()) = 0;
};
Expand Down
1 change: 1 addition & 0 deletions point_cloud_transport/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
<depend>pluginlib</depend>
<depend>rclcpp_components</depend>
<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>
<depend>rcpputils</depend>
<depend>rmw</depend>
<depend>sensor_msgs</depend>
Expand Down
37 changes: 24 additions & 13 deletions point_cloud_transport/src/publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -51,9 +51,13 @@
namespace point_cloud_transport
{

struct Publisher::Impl
template class Publisher<rclcpp::Node>;
template class Publisher<rclcpp_lifecycle::LifecycleNode>;

template<class NodeType>
struct Publisher<NodeType>::Impl
{
explicit Impl(std::shared_ptr<rclcpp::Node> node)
explicit Impl(std::shared_ptr<NodeType> node)
: logger_(node->get_logger()),
unadvertised_(false)
{
Expand Down Expand Up @@ -96,14 +100,15 @@ struct Publisher::Impl

rclcpp::Logger logger_;
std::string base_topic_;
PubLoaderPtr loader_;
std::vector<std::shared_ptr<PublisherPlugin>> publishers_;
PubLoaderPtr<NodeType> loader_;
std::vector<std::shared_ptr<PublisherPlugin<NodeType>>> publishers_;
bool unadvertised_;
};

Publisher::Publisher(
std::shared_ptr<rclcpp::Node> node, const std::string & base_topic,
PubLoaderPtr loader, rmw_qos_profile_t custom_qos,
template<class NodeType>
Publisher<NodeType>::Publisher(
std::shared_ptr<NodeType> node, const std::string & base_topic,
PubLoaderPtr<NodeType> loader, rmw_qos_profile_t custom_qos,
const rclcpp::PublisherOptions & options)
: impl_(std::make_shared<Impl>(node))
{
Expand Down Expand Up @@ -166,23 +171,26 @@ Publisher::Publisher(
}
}

uint32_t Publisher::getNumSubscribers() const
template<class NodeType>
uint32_t Publisher<NodeType>::getNumSubscribers() const
{
if (impl_ && impl_->isValid()) {
return impl_->getNumSubscribers();
}
return 0;
}

std::string Publisher::getTopic() const
template<class NodeType>
std::string Publisher<NodeType>::getTopic() const
{
if (impl_) {
return impl_->getTopic();
}
return {};
}

void Publisher::publish(const sensor_msgs::msg::PointCloud2 & message) const
template<class NodeType>
void Publisher<NodeType>::publish(const sensor_msgs::msg::PointCloud2 & message) const
{
if (!impl_ || !impl_->isValid()) {
// TODO(ros2) Switch to RCUTILS_ASSERT when ros2/rcutils#112 is merged
Expand All @@ -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<class NodeType>
void Publisher<NodeType>::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
Expand All @@ -214,15 +223,17 @@ void Publisher::publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & me
}
}

void Publisher::shutdown()
template<class NodeType>
void Publisher<NodeType>::shutdown()
{
if (impl_) {
impl_->shutdown();
impl_.reset();
}
}

Publisher::operator void *() const
Publisher<NodeType>
Publisher<NodeType>::operator void *() const
{
return (impl_ && impl_->isValid()) ? reinterpret_cast<void *>(1) : reinterpret_cast<void *>(0);
}
Expand Down
14 changes: 10 additions & 4 deletions point_cloud_transport/src/publisher_plugin.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,22 +38,28 @@
namespace point_cloud_transport
{

void PublisherPlugin::advertise(
std::shared_ptr<rclcpp::Node> node,
template class PublisherPlugin<rclcpp::Node>;
template class PublisherPlugin<rclcpp_lifecycle::LifecycleNode>;

template<class NodeType = rclcpp::Node>
void PublisherPlugin<NodeType>::advertise(
std::shared_ptr<NodeType> node,
const std::string & base_topic,
rmw_qos_profile_t custom_qos,
const rclcpp::PublisherOptions & options)
{
advertiseImpl(node, base_topic, custom_qos, options);
}

void PublisherPlugin::publishPtr(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message)
template<class NodeType = rclcpp::Node>
void PublisherPlugin<NodeType>::publishPtr(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message)
const
{
publish(*message);
}

std::string PublisherPlugin::getLookupName(const std::string & transport_name)
template<class NodeType = rclcpp::Node>
std::string PublisherPlugin<NodeType>::getLookupName(const std::string & transport_name)
{
return "point_cloud_transport/" + transport_name + "_pub";
}
Expand Down