realtime_publisher.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 /*
31  * Publishing ROS messages is difficult, as the publish function is
32  * not realtime safe. This class provides the proper locking so that
33  * you can call publish in realtime and a separate (non-realtime)
34  * thread will ensure that the message gets published over ROS.
35  *
36  * Author: Stuart Glaser
37  */
38 #ifndef REALTIME_TOOLS__REALTIME_PUBLISHER_H_
39 #define REALTIME_TOOLS__REALTIME_PUBLISHER_H_
40 
41 #include <string>
42 #include <ros/node_handle.h>
43 #include <boost/utility.hpp>
44 #include <boost/thread/mutex.hpp>
45 #include <boost/thread/thread.hpp>
46 #include <boost/thread/condition.hpp>
47 
48 namespace realtime_tools {
49 
50 template <class Msg>
51 class RealtimePublisher : boost::noncopyable
52 {
53 
54 public:
56  Msg msg_;
57 
65  RealtimePublisher(const ros::NodeHandle &node, const std::string &topic, int queue_size, bool latched=false)
66  : topic_(topic), node_(node), is_running_(false), keep_running_(false), turn_(REALTIME)
67  {
68  construct(queue_size, latched);
69  }
70 
72  : is_running_(false), keep_running_(false), turn_(REALTIME)
73  {
74  }
75 
78  {
79  stop();
80  while (is_running())
81  usleep(100);
82 
84  }
85 
86  void init(const ros::NodeHandle &node, const std::string &topic, int queue_size, bool latched=false)
87  {
88  topic_ = topic;
89  node_ = node;
90  construct(queue_size, latched);
91  }
92 
94  void stop()
95  {
96  keep_running_ = false;
97 #ifdef NON_POLLING
98  updated_cond_.notify_one(); // So the publishing loop can exit
99 #endif
100  }
101 
108  bool trylock()
109  {
110  if (msg_mutex_.try_lock())
111  {
112  if (turn_ == REALTIME)
113  {
114  return true;
115  }
116  else
117  {
118  msg_mutex_.unlock();
119  return false;
120  }
121  }
122  else
123  {
124  return false;
125  }
126  }
127 
135  {
137  msg_mutex_.unlock();
138 #ifdef NON_POLLING
139  updated_cond_.notify_one();
140 #endif
141  }
142 
149  void lock()
150  {
151 #ifdef NON_POLLING
152  msg_mutex_.lock();
153 #else
154  // never actually block on the lock
155  while (!msg_mutex_.try_lock())
156  usleep(200);
157 #endif
158  }
159 
163  void unlock()
164  {
165  msg_mutex_.unlock();
166  }
167 
168 private:
169  void construct(int queue_size, bool latched=false)
170  {
171  publisher_ = node_.advertise<Msg>(topic_, queue_size, latched);
172  keep_running_ = true;
173  thread_ = boost::thread(&RealtimePublisher::publishingLoop, this);
174  }
175 
176 
177  bool is_running() const { return is_running_; }
178 
180  {
181  is_running_ = true;
182  turn_ = REALTIME;
183 
184  while (keep_running_)
185  {
186  Msg outgoing;
187 
188  // Locks msg_ and copies it
189  lock();
190  while (turn_ != NON_REALTIME && keep_running_)
191  {
192 #ifdef NON_POLLING
193  updated_cond_.wait(lock);
194 #else
195  unlock();
196  usleep(500);
197  lock();
198 #endif
199  }
200  outgoing = msg_;
201  turn_ = REALTIME;
202 
203  unlock();
204 
205  // Sends the outgoing message
206  if (keep_running_)
207  publisher_.publish(outgoing);
208  }
209  is_running_ = false;
210  }
211 
212  std::string topic_;
215  volatile bool is_running_;
216  volatile bool keep_running_;
217 
218  boost::thread thread_;
219 
220  boost::mutex msg_mutex_; // Protects msg_
221 
222 #ifdef NON_POLLING
223  boost::condition_variable updated_cond_;
224 #endif
225 
227  int turn_; // Who's turn is it to use msg_?
228 };
229 
230 }
231 
232 #endif
void unlock()
Unlocks the data without publishing anything.
Msg msg_
The msg_ variable contains the data that will get published on the ROS topic.
void stop()
Stop the realtime publisher from sending out more ROS messages.
void publish(const boost::shared_ptr< M > &message) const
void construct(int queue_size, bool latched=false)
void unlockAndPublish()
Unlock the msg_ variable.
bool trylock()
Try to get the data lock from realtime.
void lock()
Get the data lock form non-realtime.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void init(const ros::NodeHandle &node, const std::string &topic, int queue_size, bool latched=false)
RealtimePublisher(const ros::NodeHandle &node, const std::string &topic, int queue_size, bool latched=false)
Constructor for the realtime publisher.


realtime_tools
Author(s): Stuart Glaser
autogenerated on Mon Mar 22 2021 02:20:09