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 #ifndef MESSAGE_FILTERS_SUBSCRIBER_H
00036 #define MESSAGE_FILTERS_SUBSCRIBER_H
00037
00038 #include <ros/ros.h>
00039
00040 #include <boost/signals.hpp>
00041 #include <boost/thread/mutex.hpp>
00042
00043 #include "connection.h"
00044 #include "simple_filter.h"
00045
00046 namespace message_filters
00047 {
00048
00049 class SubscriberBase
00050 {
00051 public:
00052 virtual ~SubscriberBase() {}
00064 virtual void subscribe(ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size, const ros::TransportHints& transport_hints = ros::TransportHints(), ros::CallbackQueueInterface* callback_queue = 0) = 0;
00068 virtual void subscribe() = 0;
00072 virtual void unsubscribe() = 0;
00073 };
00074 typedef boost::shared_ptr<SubscriberBase> SubscriberBasePtr;
00075
00095 template<class M>
00096 class Subscriber : public SubscriberBase, public SimpleFilter<M>
00097 {
00098 public:
00099 typedef boost::shared_ptr<M const> MConstPtr;
00100 typedef ros::MessageEvent<M const> EventType;
00101
00113 Subscriber(ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size, const ros::TransportHints& transport_hints = ros::TransportHints(), ros::CallbackQueueInterface* callback_queue = 0)
00114 {
00115 subscribe(nh, topic, queue_size, transport_hints, callback_queue);
00116 }
00117
00121 Subscriber()
00122 {
00123 }
00124
00125 ~Subscriber()
00126 {
00127 unsubscribe();
00128 }
00129
00141 void subscribe(ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size, const ros::TransportHints& transport_hints = ros::TransportHints(), ros::CallbackQueueInterface* callback_queue = 0)
00142 {
00143 unsubscribe();
00144
00145 if (!topic.empty())
00146 {
00147 ops_.template initByFullCallbackType<const EventType&>(topic, queue_size, boost::bind(&Subscriber<M>::cb, this, _1));
00148 ops_.callback_queue = callback_queue;
00149 ops_.transport_hints = transport_hints;
00150 sub_ = nh.subscribe(ops_);
00151 nh_ = nh;
00152 }
00153 }
00154
00158 void subscribe()
00159 {
00160 unsubscribe();
00161
00162 if (!ops_.topic.empty())
00163 {
00164 sub_ = nh_.subscribe(ops_);
00165 }
00166 }
00167
00171 void unsubscribe()
00172 {
00173 sub_.shutdown();
00174 }
00175
00176 std::string getTopic() const
00177 {
00178 return ops_.topic;
00179 }
00180
00184 const ros::Subscriber& getSubscriber() const { return sub_; }
00185
00189 template<typename F>
00190 void connectInput(F& f)
00191 {
00192 }
00193
00197 void add(const EventType& e)
00198 {
00199 }
00200
00201 private:
00202
00203 void cb(const EventType& e)
00204 {
00205 this->signalMessage(e);
00206 }
00207
00208 ros::Subscriber sub_;
00209 ros::SubscribeOptions ops_;
00210 ros::NodeHandle nh_;
00211 };
00212
00213 }
00214
00215 #endif