00001 #include "image_transport/image_transport.h"
00002 #include "image_transport/publisher_plugin.h"
00003 #include "image_transport/subscriber_plugin.h"
00004 #include <pluginlib/class_loader.h>
00005 #include <boost/make_shared.hpp>
00006 #include <boost/foreach.hpp>
00007
00008 namespace image_transport {
00009
00010 struct ImageTransport::Impl
00011 {
00012 ros::NodeHandle nh_;
00013 PubLoaderPtr pub_loader_;
00014 SubLoaderPtr sub_loader_;
00015
00016 Impl(const ros::NodeHandle& nh)
00017 : nh_(nh),
00018 pub_loader_( boost::make_shared<PubLoader>("image_transport", "image_transport::PublisherPlugin") ),
00019 sub_loader_( boost::make_shared<SubLoader>("image_transport", "image_transport::SubscriberPlugin") )
00020 {
00021 }
00022 };
00023
00024 ImageTransport::ImageTransport(const ros::NodeHandle& nh)
00025 : impl_(new Impl(nh))
00026 {
00027 }
00028
00029 ImageTransport::~ImageTransport()
00030 {
00031 }
00032
00033 Publisher ImageTransport::advertise(const std::string& base_topic, uint32_t queue_size, bool latch)
00034 {
00035 return advertise(base_topic, queue_size, SubscriberStatusCallback(),
00036 SubscriberStatusCallback(), ros::VoidPtr(), latch);
00037 }
00038
00039 Publisher ImageTransport::advertise(const std::string& base_topic, uint32_t queue_size,
00040 const SubscriberStatusCallback& connect_cb,
00041 const SubscriberStatusCallback& disconnect_cb,
00042 const ros::VoidPtr& tracked_object, bool latch)
00043 {
00044 return Publisher(impl_->nh_, base_topic, queue_size, connect_cb, disconnect_cb, tracked_object, latch, impl_->pub_loader_);
00045 }
00046
00047 Subscriber ImageTransport::subscribe(const std::string& base_topic, uint32_t queue_size,
00048 const boost::function<void(const sensor_msgs::ImageConstPtr&)>& callback,
00049 const ros::VoidPtr& tracked_object, const TransportHints& transport_hints)
00050 {
00051 return Subscriber(impl_->nh_, base_topic, queue_size, callback, tracked_object, transport_hints, impl_->sub_loader_);
00052 }
00053
00054 CameraPublisher ImageTransport::advertiseCamera(const std::string& base_topic, uint32_t queue_size, bool latch)
00055 {
00056 return advertiseCamera(base_topic, queue_size,
00057 SubscriberStatusCallback(), SubscriberStatusCallback(),
00058 ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(),
00059 ros::VoidPtr(), latch);
00060 }
00061
00062 CameraPublisher ImageTransport::advertiseCamera(const std::string& base_topic, uint32_t queue_size,
00063 const SubscriberStatusCallback& image_connect_cb,
00064 const SubscriberStatusCallback& image_disconnect_cb,
00065 const ros::SubscriberStatusCallback& info_connect_cb,
00066 const ros::SubscriberStatusCallback& info_disconnect_cb,
00067 const ros::VoidPtr& tracked_object, bool latch)
00068 {
00069 return CameraPublisher(*this, impl_->nh_, base_topic, queue_size, image_connect_cb, image_disconnect_cb,
00070 info_connect_cb, info_disconnect_cb, tracked_object, latch);
00071 }
00072
00073 CameraSubscriber ImageTransport::subscribeCamera(const std::string& base_topic, uint32_t queue_size,
00074 const CameraSubscriber::Callback& callback,
00075 const ros::VoidPtr& tracked_object,
00076 const TransportHints& transport_hints)
00077 {
00078 return CameraSubscriber(*this, impl_->nh_, base_topic, queue_size, callback, tracked_object, transport_hints);
00079 }
00080
00081 std::vector<std::string> ImageTransport::getDeclaredTransports() const
00082 {
00083 std::vector<std::string> transports = impl_->sub_loader_->getDeclaredClasses();
00084
00085 BOOST_FOREACH(std::string& transport, transports) {
00086 transport = transport.substr(0, transport.size() - 4);
00087 }
00088 return transports;
00089 }
00090
00091 }