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

Pointer api updates #104

Merged
merged 2 commits into from
Nov 27, 2018
Merged
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
31 changes: 19 additions & 12 deletions image_transport/include/image_transport/camera_publisher.h
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,8 @@

#include "image_transport/single_subscriber_publisher.h"

namespace image_transport {
namespace image_transport
{

class ImageTransport;

Expand All @@ -68,9 +69,10 @@ class CameraPublisher
CameraPublisher() = default;


CameraPublisher(rclcpp::Node::SharedPtr node,
const std::string& base_topic,
rmw_qos_profile_t custom_qos = rmw_qos_profile_default);
CameraPublisher(
rclcpp::Node * node,
const std::string & base_topic,
rmw_qos_profile_t custom_qos = rmw_qos_profile_default);

//TODO(ros2) Restore support for SubscriberStatusCallbacks when available.

Expand All @@ -95,13 +97,16 @@ class CameraPublisher
/*!
* \brief Publish an (image, info) pair on the topics associated with this CameraPublisher.
*/
void publish(const sensor_msgs::msg::Image& image, const sensor_msgs::msg::CameraInfo& info) const;
void publish(
const sensor_msgs::msg::Image & image,
const sensor_msgs::msg::CameraInfo & info) const;

/*!
* \brief Publish an (image, info) pair on the topics associated with this CameraPublisher.
*/
void publish(const sensor_msgs::msg::Image::ConstSharedPtr& image,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info) const;
void publish(
const sensor_msgs::msg::Image::ConstSharedPtr & image,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info) const;

/*!
* \brief Publish an (image, info) pair with given timestamp on the topics associated with
Expand All @@ -110,17 +115,19 @@ class CameraPublisher
* Convenience version, which sets the timestamps of both image and info to stamp before
* publishing.
*/
void publish(sensor_msgs::msg::Image& image, sensor_msgs::msg::CameraInfo& info, rclcpp::Time stamp) const;
void publish(
sensor_msgs::msg::Image & image, sensor_msgs::msg::CameraInfo & info,
rclcpp::Time stamp) const;

/*!
* \brief Shutdown the advertisements associated with this Publisher.
*/
void shutdown();

operator void*() const;
bool operator< (const CameraPublisher& rhs) const { return impl_ < rhs.impl_; }
bool operator!=(const CameraPublisher& rhs) const { return impl_ != rhs.impl_; }
bool operator==(const CameraPublisher& rhs) const { return impl_ == rhs.impl_; }
operator void *() const;
bool operator<(const CameraPublisher & rhs) const {return impl_ < rhs.impl_;}
bool operator!=(const CameraPublisher & rhs) const {return impl_ != rhs.impl_;}
bool operator==(const CameraPublisher & rhs) const {return impl_ == rhs.impl_;}

private:
struct Impl;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ class CameraSubscriber

CameraSubscriber() = default;

CameraSubscriber(rclcpp::Node::SharedPtr node,
CameraSubscriber(rclcpp::Node * node,
const std::string& base_topic,
const Callback& callback,
const std::string& transport,
Expand Down
8 changes: 4 additions & 4 deletions image_transport/include/image_transport/image_transport.h
Original file line number Diff line number Diff line change
Expand Up @@ -55,15 +55,15 @@ namespace image_transport
* \brief Advertise an image topic, free function version.
*/
Publisher create_publisher(
rclcpp::Node::SharedPtr node,
rclcpp::Node* node,
const std::string & base_topic,
rmw_qos_profile_t custom_qos = rmw_qos_profile_default);

/**
* \brief Subscribe to an image topic, free function version.
*/
Subscriber create_subscription(
rclcpp::Node::SharedPtr node,
rclcpp::Node* node,
const std::string & base_topic,
const Subscriber::Callback & callback,
const std::string & transport,
Expand All @@ -73,15 +73,15 @@ Subscriber create_subscription(
* \brief Advertise a camera, free function version.
*/
CameraPublisher create_camera_publisher(
rclcpp::Node::SharedPtr node,
rclcpp::Node* node,
const std::string & base_topic,
rmw_qos_profile_t custom_qos = rmw_qos_profile_default);

/*!
* \brief Subscribe to a camera, free function version.
*/
CameraSubscriber create_camera_subscription(
rclcpp::Node::SharedPtr node,
rclcpp::Node* node,
const std::string & base_topic,
const CameraSubscriber::Callback & callback,
const std::string & transport,
Expand Down
2 changes: 1 addition & 1 deletion image_transport/include/image_transport/publisher.h
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,7 @@ class Publisher
Publisher() = default;

Publisher(
rclcpp::Node::SharedPtr nh,
rclcpp::Node * nh,
const std::string & base_topic,
PubLoaderPtr loader,
rmw_qos_profile_t custom_qos);
Expand Down
16 changes: 9 additions & 7 deletions image_transport/include/image_transport/publisher_plugin.h
Original file line number Diff line number Diff line change
Expand Up @@ -50,8 +50,8 @@ class PublisherPlugin
{
public:
PublisherPlugin() = default;
PublisherPlugin(const PublisherPlugin&) = delete;
PublisherPlugin& operator=( const PublisherPlugin& ) = delete;
PublisherPlugin(const PublisherPlugin &) = delete;
PublisherPlugin & operator=(const PublisherPlugin &) = delete;

virtual ~PublisherPlugin() {}

Expand All @@ -65,7 +65,7 @@ class PublisherPlugin
* \brief Advertise a topic, simple version.
*/
void advertise(
rclcpp::Node::SharedPtr & nh,
rclcpp::Node * nh,
const std::string & base_topic,
rmw_qos_profile_t custom_qos = rmw_qos_profile_default)
{
Expand All @@ -86,12 +86,12 @@ class PublisherPlugin
/**
* \brief Publish an image using the transport associated with this PublisherPlugin.
*/
virtual void publish(const sensor_msgs::msg::Image& message) const = 0;
virtual void publish(const sensor_msgs::msg::Image & message) const = 0;

/**
* \brief Publish an image using the transport associated with this PublisherPlugin.
*/
virtual void publish(const sensor_msgs::msg::Image::ConstSharedPtr& message) const
virtual void publish(const sensor_msgs::msg::Image::ConstSharedPtr & message) const
{
publish(*message);
}
Expand All @@ -103,7 +103,7 @@ class PublisherPlugin
* @param message an image message to use information from (but not data)
* @param data a pointer to the image data to use to fill the Image message
*/
virtual void publish(const sensor_msgs::msg::Image& message, const uint8_t* data) const
virtual void publish(const sensor_msgs::msg::Image & message, const uint8_t * data) const
{
sensor_msgs::msg::Image msg;
msg.header = message.header;
Expand Down Expand Up @@ -135,7 +135,9 @@ class PublisherPlugin
/**
* \brief Advertise a topic. Must be implemented by the subclass.
*/
virtual void advertiseImpl(rclcpp::Node::SharedPtr nh, const std::string& base_topic, rmw_qos_profile_t custom_qos) = 0;
virtual void advertiseImpl(
rclcpp::Node * nh, const std::string & base_topic,
rmw_qos_profile_t custom_qos) = 0;
};

} //namespace image_transport
Expand Down
50 changes: 28 additions & 22 deletions image_transport/include/image_transport/simple_publisher_plugin.h
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,8 @@

#include <memory>

namespace image_transport {
namespace image_transport
{

/**
* \brief Base class to simplify implementing most plugins to Publisher.
Expand All @@ -62,29 +63,30 @@ namespace image_transport {
* getTopicToAdvertise() controls the name of the internal communication topic.
* It defaults to \<base topic\>/\<transport name\>.
*/
template <class M>
template<class M>
class SimplePublisherPlugin : public PublisherPlugin
{
public:
virtual ~SimplePublisherPlugin() {}

virtual uint32_t getNumSubscribers() const
{
if (simple_impl_) return simple_impl_->node_->count_subscribers(getTopic());
// TODO(mjcarroll) replace with publisher-specific call.
if (simple_impl_) {return simple_impl_->node_->count_subscribers(getTopic());}
return 0;
}

virtual std::string getTopic() const
{
if (simple_impl_) return simple_impl_->pub_->get_topic_name();
if (simple_impl_) {return simple_impl_->pub_->get_topic_name();}
return std::string();
}

virtual void publish(const sensor_msgs::msg::Image& message) const
virtual void publish(const sensor_msgs::msg::Image & message) const
{
if (!simple_impl_ || !simple_impl_->pub_) {
rclcpp::Logger logger = simple_impl_->node_->get_logger();
RCLCPP_ERROR(logger, "Call to publish() on an invalid image_transport::SimplePublisherPlugin");
RCLCPP_ERROR(simple_impl_->logger_,
"Call to publish() on an invalid image_transport::SimplePublisherPlugin");
return;
}

Expand All @@ -97,18 +99,19 @@ class SimplePublisherPlugin : public PublisherPlugin
}

protected:
virtual void advertiseImpl(rclcpp::Node::SharedPtr node, const std::string& base_topic, rmw_qos_profile_t custom_qos)
virtual void advertiseImpl(
rclcpp::Node * node, const std::string & base_topic,
rmw_qos_profile_t custom_qos)
{
std::string transport_topic = getTopicToAdvertise(base_topic);
simple_impl_.reset(new SimplePublisherPluginImpl(node));
simple_impl_ = std::make_unique<SimplePublisherPluginImpl>(node);

rclcpp::Logger logger = simple_impl_->node_->get_logger();
RCLCPP_DEBUG(logger, "getTopicToAdvertise: %s", transport_topic);
RCLCPP_DEBUG(simple_impl_->logger_, "getTopicToAdvertise: %s", transport_topic);
simple_impl_->pub_ = node->create_publisher<M>(transport_topic, custom_qos);
}

//! Generic function for publishing the internal message type.
typedef std::function<void(const M&)> PublishFn;
typedef std::function<void (const M &)> PublishFn;

/**
* \brief Publish an image using the specified publish function. Must be implemented by
Expand All @@ -118,46 +121,49 @@ class SimplePublisherPlugin : public PublisherPlugin
* SimpleSubscriberPlugin to use this function for both normal broadcast publishing and
* single subscriber publishing (in subscription callbacks).
*/
virtual void publish(const sensor_msgs::msg::Image& message, const PublishFn& publish_fn) const = 0;
virtual void publish(
const sensor_msgs::msg::Image & message,
const PublishFn & publish_fn) const = 0;

/**
* \brief Return the communication topic name for a given base topic.
*
* Defaults to \<base topic\>/\<transport name\>.
*/
virtual std::string getTopicToAdvertise(const std::string& base_topic) const
virtual std::string getTopicToAdvertise(const std::string & base_topic) const
{
return base_topic + "/" + getTransportName();
}

private:
struct SimplePublisherPluginImpl
{
SimplePublisherPluginImpl(rclcpp::Node::SharedPtr node)
: node_(node)
SimplePublisherPluginImpl(rclcpp::Node* node)
: node_(node),
logger_(node->get_logger())
{

}

rclcpp::Node::SharedPtr node_;
rclcpp::Node* node_;
rclcpp::Logger logger_;
typename rclcpp::Publisher<M>::SharedPtr pub_;
};

std::unique_ptr<SimplePublisherPluginImpl> simple_impl_;

typedef std::function<void(const sensor_msgs::msg::Image&)> ImagePublishFn;
typedef std::function<void (const sensor_msgs::msg::Image &)> ImagePublishFn;

/**
* Returns a function object for publishing the transport-specific message type
* through some ROS publisher type.
*
* @param pub An object with method void publish(const M&)
*/
template <class PubT>
PublishFn bindInternalPublisher(PubT* pub) const
template<class PubT>
PublishFn bindInternalPublisher(PubT * pub) const
{
// Bind PubT::publish(const Message&) as PublishFn
typedef void (PubT::*InternalPublishMemFn)(const M&);
typedef void (PubT::* InternalPublishMemFn)(const M &);
InternalPublishMemFn internal_pub_mem_fn = &PubT::publish;
return std::bind(internal_pub_mem_fn, pub, std::placeholders::_1);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ class SimpleSubscriberPlugin : public SubscriberPlugin
}

virtual void subscribeImpl(
rclcpp::Node::SharedPtr node,
rclcpp::Node * node,
const std::string & base_topic,
const Callback & callback,
rmw_qos_profile_t custom_qos)
Expand Down
9 changes: 4 additions & 5 deletions image_transport/include/image_transport/subscriber.h
Original file line number Diff line number Diff line change
Expand Up @@ -62,16 +62,16 @@ namespace image_transport
class Subscriber
{
public:
typedef std::function<void(const sensor_msgs::msg::Image::ConstSharedPtr&)> Callback;
typedef std::function<void (const sensor_msgs::msg::Image::ConstSharedPtr &)> Callback;

Subscriber() = default;

Subscriber(
rclcpp::Node::SharedPtr node,
rclcpp::Node * node,
const std::string & base_topic,
const Callback& callback,
const Callback & callback,
SubLoaderPtr loader,
const std::string& transport,
const std::string & transport,
rmw_qos_profile_t custom_qos = rmw_qos_profile_default);

/**
Expand Down Expand Up @@ -103,7 +103,6 @@ class Subscriber
bool operator==(const Subscriber & rhs) const {return impl_ == rhs.impl_;}

private:

struct Impl;
std::shared_ptr<Impl> impl_;
};
Expand Down
Loading