PubQueue.h
Go to the documentation of this file.
1 /*
2  * Copyright 2012 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16  */
17 
18 #ifndef ROS_PUBQUEUE_H
19 #define ROS_PUBQUEUE_H
20 
21 #include <boost/thread.hpp>
22 #include <boost/bind.hpp>
23 #include <deque>
24 #include <list>
25 #include <vector>
26 
27 #include <ros/ros.h>
28 
29 
32 template<class T>
34 {
35  public:
37  T msg_;
41  msg_(msg), pub_(pub) {}
42 };
43 
47 template<class T>
48 class PubQueue
49 {
50  public:
51  typedef boost::shared_ptr<std::deque<boost::shared_ptr<
54 
55  private:
57  QueuePtr queue_;
61  boost::function<void()> notify_func_;
62 
63  public:
64  PubQueue(QueuePtr queue,
66  boost::function<void()> notify_func) :
67  queue_(queue), queue_lock_(queue_lock), notify_func_(notify_func) {}
68  ~PubQueue() {}
69 
73  void push(T& msg, ros::Publisher& pub)
74  {
76  boost::mutex::scoped_lock lock(*queue_lock_);
77  queue_->push_back(el);
78  notify_func_();
79  }
80 
83  void pop(std::vector<boost::shared_ptr<PubMessagePair<T> > >& els)
84  {
85  boost::mutex::scoped_lock lock(*queue_lock_);
86  while(!queue_->empty())
87  {
88  els.push_back(queue_->front());
89  queue_->pop_front();
90  }
91  }
92 };
93 
97 {
98  private:
100  std::list<boost::function<void()> > service_funcs_;
102  boost::mutex service_funcs_lock_;
104  boost::thread service_thread_;
108  boost::condition_variable service_cond_var_;
111 
114  template <class T>
116  {
117  std::vector<boost::shared_ptr<PubMessagePair<T> > > els;
118  pq->pop(els);
119  for(typename std::vector<boost::shared_ptr<PubMessagePair<T> > >::iterator it = els.begin();
120  it != els.end();
121  ++it)
122  {
123  (*it)->pub_.publish((*it)->msg_);
124  }
125  }
126 
127  public:
130  {
131  if(service_thread_.joinable())
132  {
133  service_thread_running_ = false;
134  notifyServiceThread();
135  service_thread_.join();
136  }
137  }
138 
142  template <class T>
144  {
145  typename PubQueue<T>::QueuePtr queue(new std::deque<boost::shared_ptr<PubMessagePair<T> > >);
146  boost::shared_ptr<boost::mutex> queue_lock(new boost::mutex);
147  boost::shared_ptr<PubQueue<T> > pq(new PubQueue<T>(queue, queue_lock, boost::bind(&PubMultiQueue::notifyServiceThread, this)));
148  boost::function<void()> f = boost::bind(&PubMultiQueue::serviceFunc<T>, this, pq);
149  {
150  boost::mutex::scoped_lock lock(service_funcs_lock_);
151  service_funcs_.push_back(f);
152  }
153  return pq;
154  }
155 
157  void spinOnce()
158  {
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();
162  ++it)
163  {
164  (*it)();
165  }
166  }
167 
170  void spin()
171  {
172  while(ros::ok() && service_thread_running_)
173  {
174  boost::unique_lock<boost::mutex> lock(service_cond_var_lock_);
175  service_cond_var_.wait(lock);
176  spinOnce();
177  }
178  }
179 
182  {
183  service_thread_running_ = true;
184  service_thread_ = boost::thread(boost::bind(&PubMultiQueue::spin, this));
185  }
186 
190  {
191  service_cond_var_.notify_one();
192  }
193 };
194 
195 #endif
PubMessagePair(T &msg, ros::Publisher &pub)
Definition: PubQueue.h:40
void push(T &msg, ros::Publisher &pub)
Push a new message onto the queue.
Definition: PubQueue.h:73
boost::shared_ptr< boost::mutex > queue_lock_
Mutex to control access to the queue.
Definition: PubQueue.h:59
Container for a (ROS publisher, outgoing message) pair. We&#39;ll have queues of these. Templated on a ROS message type.
Definition: PubQueue.h:33
void spin()
Service all queues indefinitely, waiting on a condition variable in between cycles.
Definition: PubQueue.h:170
f
void spinOnce()
Service each queue one time.
Definition: PubQueue.h:157
boost::shared_ptr< std::deque< boost::shared_ptr< PubMessagePair< T > > > > QueuePtr
Definition: PubQueue.h:52
void pop(std::vector< boost::shared_ptr< PubMessagePair< T > > > &els)
Pop all waiting messages off the queue.
Definition: PubQueue.h:83
QueuePtr queue_
Our queue of outgoing messages.
Definition: PubQueue.h:57
A collection of PubQueue objects, potentially of different types. This class is the programmer&#39;s inte...
Definition: PubQueue.h:96
boost::condition_variable service_cond_var_
Condition variable used to block and resume service_thread_.
Definition: PubQueue.h:108
~PubQueue()
Definition: PubQueue.h:68
boost::shared_ptr< PubQueue< T > > Ptr
Definition: PubQueue.h:53
boost::mutex service_cond_var_lock_
Mutex to accompany service_cond_var_.
Definition: PubQueue.h:110
void serviceFunc(boost::shared_ptr< PubQueue< T > > pq)
Service a given queue by popping outgoing message off it and publishing them.
Definition: PubQueue.h:115
A queue of outgoing messages. Instead of calling publish() directly, you can push() messages here to ...
Definition: PubQueue.h:48
ros::Publisher pub_
The publisher to use to publish the message.
Definition: PubQueue.h:39
ROSCPP_DECL bool ok()
void notifyServiceThread()
Wake up the queue serive thread (e.g., after having pushed a message onto one of the queues)...
Definition: PubQueue.h:189
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)...
Definition: PubQueue.h:143
boost::thread service_thread_
If started, the thread that will call the service functions.
Definition: PubQueue.h:104
boost::function< void()> notify_func_
Function that will be called when a new message is pushed on.
Definition: PubQueue.h:61
boost::mutex service_funcs_lock_
Mutex to lock access to service_funcs_.
Definition: PubQueue.h:102
T msg_
The outgoing message.
Definition: PubQueue.h:37
void startServiceThread()
Start a thread to call spin().
Definition: PubQueue.h:181
ROSCPP_DECL void spinOnce()
std::list< boost::function< void()> > service_funcs_
List of functions to be called to service our queues.
Definition: PubQueue.h:100
bool service_thread_running_
Boolean flag to shutdown the service thread if PubMultiQueue is destructed.
Definition: PubQueue.h:106
PubQueue(QueuePtr queue, boost::shared_ptr< boost::mutex > queue_lock, boost::function< void()> notify_func)
Definition: PubQueue.h:64


gazebo_plugins
Author(s): John Hsu
autogenerated on Wed Aug 24 2022 02:47:52