Go to the documentation of this file.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/subscriber.h"
00036 #include "image_transport/subscriber_plugin.h"
00037 #include <ros/names.h>
00038 #include <pluginlib/class_loader.h>
00039 #include <boost/scoped_ptr.hpp>
00040
00041 namespace image_transport {
00042
00043 struct Subscriber::Impl
00044 {
00045 Impl()
00046 : unsubscribed_(false)
00047 {
00048 }
00049
00050 ~Impl()
00051 {
00052 shutdown();
00053 }
00054
00055 bool isValid() const
00056 {
00057 return !unsubscribed_;
00058 }
00059
00060 void shutdown()
00061 {
00062 if (!unsubscribed_) {
00063 unsubscribed_ = true;
00064 if (subscriber_)
00065 subscriber_->shutdown();
00066 }
00067 }
00068
00069 SubLoaderPtr loader_;
00070 boost::shared_ptr<SubscriberPlugin> subscriber_;
00071 bool unsubscribed_;
00072
00073 };
00074
00075 Subscriber::Subscriber(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size,
00076 const boost::function<void(const sensor_msgs::ImageConstPtr&)>& callback,
00077 const ros::VoidPtr& tracked_object, const TransportHints& transport_hints,
00078 const SubLoaderPtr& loader)
00079 : impl_(new Impl)
00080 {
00081
00082 std::string lookup_name = SubscriberPlugin::getLookupName(transport_hints.getTransport());
00083 try {
00084 impl_->subscriber_ = loader->createInstance(lookup_name);
00085 }
00086 catch (pluginlib::PluginlibException& e) {
00087 throw TransportLoadException(transport_hints.getTransport(), e.what());
00088 }
00089
00090 impl_->loader_ = loader;
00091
00092
00093 std::string clean_topic = ros::names::clean(base_topic);
00094 size_t found = clean_topic.rfind('/');
00095 if (found != std::string::npos) {
00096 std::string transport = clean_topic.substr(found+1);
00097 std::string plugin_name = SubscriberPlugin::getLookupName(transport);
00098 std::vector<std::string> plugins = loader->getDeclaredClasses();
00099 if (std::find(plugins.begin(), plugins.end(), plugin_name) != plugins.end()) {
00100 std::string real_base_topic = clean_topic.substr(0, found);
00101 ROS_WARN("[image_transport] It looks like you are trying to subscribe directly to a "
00102 "transport-specific image topic '%s', in which case you will likely get a connection "
00103 "error. Try subscribing to the base topic '%s' instead with parameter ~image_transport "
00104 "set to '%s' (on the command line, _image_transport:=%s). "
00105 "See http://ros.org/wiki/image_transport for details.",
00106 clean_topic.c_str(), real_base_topic.c_str(), transport.c_str(), transport.c_str());
00107 }
00108 }
00109
00110
00111 impl_->subscriber_->subscribe(nh, base_topic, queue_size, callback, tracked_object, transport_hints);
00112 }
00113
00114 std::string Subscriber::getTopic() const
00115 {
00116 if (impl_) return impl_->subscriber_->getTopic();
00117 return std::string();
00118 }
00119
00120 uint32_t Subscriber::getNumPublishers() const
00121 {
00122 if (impl_) return impl_->subscriber_->getNumPublishers();
00123 return 0;
00124 }
00125
00126 std::string Subscriber::getTransport() const
00127 {
00128 if (impl_) return impl_->subscriber_->getTransportName();
00129 return std::string();
00130 }
00131
00132 void Subscriber::shutdown()
00133 {
00134 if (impl_) impl_->shutdown();
00135 }
00136
00137 Subscriber::operator void*() const
00138 {
00139 return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0;
00140 }
00141
00142 }