18 #ifndef ROS_PUBQUEUE_H 19 #define ROS_PUBQUEUE_H 21 #include <boost/thread.hpp> 22 #include <boost/bind.hpp> 41 msg_(msg), pub_(pub) {}
66 boost::function<
void()> notify_func) :
67 queue_(queue), queue_lock_(queue_lock), notify_func_(notify_func) {}
76 boost::mutex::scoped_lock lock(*queue_lock_);
77 queue_->push_back(el);
85 boost::mutex::scoped_lock lock(*queue_lock_);
86 while(!queue_->empty())
88 els.push_back(queue_->front());
117 std::vector<boost::shared_ptr<PubMessagePair<T> > > els;
123 (*it)->pub_.publish((*it)->msg_);
131 if(service_thread_.joinable())
133 service_thread_running_ =
false;
134 notifyServiceThread();
135 service_thread_.join();
148 boost::function<void()>
f = boost::bind(&PubMultiQueue::serviceFunc<T>,
this, pq);
150 boost::mutex::scoped_lock lock(service_funcs_lock_);
151 service_funcs_.push_back(f);
159 boost::mutex::scoped_lock lock(service_funcs_lock_);
160 for(std::list<boost::function<
void()> >::iterator it = service_funcs_.begin();
161 it != service_funcs_.end();
172 while(
ros::ok() && service_thread_running_)
174 boost::unique_lock<boost::mutex> lock(service_cond_var_lock_);
175 service_cond_var_.wait(lock);
183 service_thread_running_ =
true;
191 service_cond_var_.notify_one();
PubMessagePair(T &msg, ros::Publisher &pub)
void push(T &msg, ros::Publisher &pub)
Push a new message onto the queue.
boost::shared_ptr< boost::mutex > queue_lock_
Mutex to control access to the queue.
Container for a (ROS publisher, outgoing message) pair. We'll have queues of these. Templated on a ROS message type.
void spin()
Service all queues indefinitely, waiting on a condition variable in between cycles.
void spinOnce()
Service each queue one time.
boost::shared_ptr< std::deque< boost::shared_ptr< PubMessagePair< T > > > > QueuePtr
void pop(std::vector< boost::shared_ptr< PubMessagePair< T > > > &els)
Pop all waiting messages off the queue.
QueuePtr queue_
Our queue of outgoing messages.
A collection of PubQueue objects, potentially of different types. This class is the programmer's inte...
boost::condition_variable service_cond_var_
Condition variable used to block and resume service_thread_.
boost::shared_ptr< PubQueue< T > > Ptr
boost::mutex service_cond_var_lock_
Mutex to accompany service_cond_var_.
void serviceFunc(boost::shared_ptr< PubQueue< T > > pq)
Service a given queue by popping outgoing message off it and publishing them.
A queue of outgoing messages. Instead of calling publish() directly, you can push() messages here to ...
ros::Publisher pub_
The publisher to use to publish the message.
void notifyServiceThread()
Wake up the queue serive thread (e.g., after having pushed a message onto one of the queues)...
boost::shared_ptr< PubQueue< T > > addPub()
Add a new queue. Call this once for each published topic (or at least each type of publish message)...
boost::thread service_thread_
If started, the thread that will call the service functions.
boost::function< void()> notify_func_
Function that will be called when a new message is pushed on.
boost::mutex service_funcs_lock_
Mutex to lock access to service_funcs_.
T msg_
The outgoing message.
void startServiceThread()
Start a thread to call spin().
std::list< boost::function< void()> > service_funcs_
List of functions to be called to service our queues.
bool service_thread_running_
Boolean flag to shutdown the service thread if PubMultiQueue is destructed.
PubQueue(QueuePtr queue, boost::shared_ptr< boost::mutex > queue_lock, boost::function< void()> notify_func)