Skip to content

Commit

Permalink
Pointer api updates (#104)
Browse files Browse the repository at this point in the history
* Fix rcutils API change by just removing it.

* Switch from SharedPtr to raw.
  • Loading branch information
mjcarroll authored Nov 27, 2018
1 parent fbfc49c commit 650a93b
Show file tree
Hide file tree
Showing 21 changed files with 168 additions and 134 deletions.
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

0 comments on commit 650a93b

Please sign in to comment.