00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "image_transport/image_transport.h"
00036 #include "image_transport/publisher_plugin.h"
00037 #include "image_transport/subscriber_plugin.h"
00038 #include <pluginlib/class_loader.h>
00039 #include <boost/make_shared.hpp>
00040 #include <boost/foreach.hpp>
00041 #include <boost/algorithm/string/erase.hpp>
00042
00043 namespace image_transport {
00044
00045 struct ImageTransport::Impl
00046 {
00047 ros::NodeHandle nh_;
00048 PubLoaderPtr pub_loader_;
00049 SubLoaderPtr sub_loader_;
00050
00051 Impl(const ros::NodeHandle& nh)
00052 : nh_(nh),
00053 pub_loader_( boost::make_shared<PubLoader>("image_transport", "image_transport::PublisherPlugin") ),
00054 sub_loader_( boost::make_shared<SubLoader>("image_transport", "image_transport::SubscriberPlugin") )
00055 {
00056 }
00057 };
00058
00059 ImageTransport::ImageTransport(const ros::NodeHandle& nh)
00060 : impl_(new Impl(nh))
00061 {
00062 }
00063
00064 ImageTransport::~ImageTransport()
00065 {
00066 }
00067
00068 Publisher ImageTransport::advertise(const std::string& base_topic, uint32_t queue_size, bool latch)
00069 {
00070 return advertise(base_topic, queue_size, SubscriberStatusCallback(),
00071 SubscriberStatusCallback(), ros::VoidPtr(), latch);
00072 }
00073
00074 Publisher ImageTransport::advertise(const std::string& base_topic, uint32_t queue_size,
00075 const SubscriberStatusCallback& connect_cb,
00076 const SubscriberStatusCallback& disconnect_cb,
00077 const ros::VoidPtr& tracked_object, bool latch)
00078 {
00079 return Publisher(impl_->nh_, base_topic, queue_size, connect_cb, disconnect_cb, tracked_object, latch, impl_->pub_loader_);
00080 }
00081
00082 Subscriber ImageTransport::subscribe(const std::string& base_topic, uint32_t queue_size,
00083 const boost::function<void(const sensor_msgs::ImageConstPtr&)>& callback,
00084 const ros::VoidPtr& tracked_object, const TransportHints& transport_hints)
00085 {
00086 return Subscriber(impl_->nh_, base_topic, queue_size, callback, tracked_object, transport_hints, impl_->sub_loader_);
00087 }
00088
00089 CameraPublisher ImageTransport::advertiseCamera(const std::string& base_topic, uint32_t queue_size, bool latch)
00090 {
00091 return advertiseCamera(base_topic, queue_size,
00092 SubscriberStatusCallback(), SubscriberStatusCallback(),
00093 ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(),
00094 ros::VoidPtr(), latch);
00095 }
00096
00097 CameraPublisher ImageTransport::advertiseCamera(const std::string& base_topic, uint32_t queue_size,
00098 const SubscriberStatusCallback& image_connect_cb,
00099 const SubscriberStatusCallback& image_disconnect_cb,
00100 const ros::SubscriberStatusCallback& info_connect_cb,
00101 const ros::SubscriberStatusCallback& info_disconnect_cb,
00102 const ros::VoidPtr& tracked_object, bool latch)
00103 {
00104 return CameraPublisher(*this, impl_->nh_, base_topic, queue_size, image_connect_cb, image_disconnect_cb,
00105 info_connect_cb, info_disconnect_cb, tracked_object, latch);
00106 }
00107
00108 CameraSubscriber ImageTransport::subscribeCamera(const std::string& base_topic, uint32_t queue_size,
00109 const CameraSubscriber::Callback& callback,
00110 const ros::VoidPtr& tracked_object,
00111 const TransportHints& transport_hints)
00112 {
00113 return CameraSubscriber(*this, impl_->nh_, base_topic, queue_size, callback, tracked_object, transport_hints);
00114 }
00115
00116 std::vector<std::string> ImageTransport::getDeclaredTransports() const
00117 {
00118 std::vector<std::string> transports = impl_->sub_loader_->getDeclaredClasses();
00119
00120 BOOST_FOREACH(std::string& transport, transports) {
00121 transport = boost::erase_last_copy(transport, "_sub");
00122 }
00123 return transports;
00124 }
00125
00126 std::vector<std::string> ImageTransport::getLoadableTransports() const
00127 {
00128 std::vector<std::string> loadableTransports;
00129
00130 BOOST_FOREACH( const std::string& transportPlugin, impl_->sub_loader_->getDeclaredClasses() )
00131 {
00132
00133
00134
00135 try
00136 {
00137 boost::shared_ptr<image_transport::SubscriberPlugin> sub = impl_->sub_loader_->createInstance(transportPlugin);
00138 loadableTransports.push_back(boost::erase_last_copy(transportPlugin, "_sub"));
00139 }
00140 catch (const pluginlib::LibraryLoadException& e) {}
00141 catch (const pluginlib::CreateClassException& e) {}
00142 }
00143
00144 return loadableTransports;
00145
00146 }
00147
00148 }