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 <chrono>
44 #include <condition_variable>
45 #include <memory>
46 #include <mutex>
47 #include <thread>
48 
49 namespace realtime_tools {
50 
51 template <class Msg>
53 {
54 
55 public:
57  Msg msg_;
58 
66  RealtimePublisher(const ros::NodeHandle &node, const std::string &topic, int queue_size, bool latched=false)
67  : topic_(topic), node_(node), is_running_(false), keep_running_(false), turn_(LOOP_NOT_STARTED)
68  {
69  construct(queue_size, latched);
70  }
71 
74  {
75  }
76 
79  {
80  stop();
81  while (is_running())
82  {
83  std::this_thread::sleep_for(std::chrono::microseconds(100));
84  }
85 
86  if (thread_.joinable())
87  {
88  thread_.join();
89  }
91  }
92 
93  void init(const ros::NodeHandle &node, const std::string &topic, int queue_size, bool latched=false)
94  {
95  topic_ = topic;
96  node_ = node;
97  construct(queue_size, latched);
98  }
99 
101  void stop()
102  {
103  keep_running_ = false;
104 #ifdef NON_POLLING
105  updated_cond_.notify_one(); // So the publishing loop can exit
106 #endif
107  }
108 
115  bool trylock()
116  {
117  if (msg_mutex_.try_lock())
118  {
119  if (turn_ == REALTIME)
120  {
121  return true;
122  }
123  else
124  {
125  msg_mutex_.unlock();
126  return false;
127  }
128  }
129  else
130  {
131  return false;
132  }
133  }
134 
142  {
144  msg_mutex_.unlock();
145 #ifdef NON_POLLING
146  updated_cond_.notify_one();
147 #endif
148  }
149 
156  void lock()
157  {
158 #ifdef NON_POLLING
159  msg_mutex_.lock();
160 #else
161  // never actually block on the lock
162  while (!msg_mutex_.try_lock())
163  {
164  std::this_thread::sleep_for(std::chrono::microseconds(200));
165  }
166 #endif
167  }
168 
172  void unlock()
173  {
174  msg_mutex_.unlock();
175  }
176 
177 private:
178  // non-copyable
179  RealtimePublisher(const RealtimePublisher &) = delete;
180  RealtimePublisher & operator=(const RealtimePublisher &) = delete;
181 
182  void construct(int queue_size, bool latched=false)
183  {
184  publisher_ = node_.advertise<Msg>(topic_, queue_size, latched);
185  keep_running_ = true;
186  thread_ = std::thread(&RealtimePublisher::publishingLoop, this);
187  }
188 
189 
190  bool is_running() const { return is_running_; }
191 
193  {
194  is_running_ = true;
195  turn_ = REALTIME;
196 
197  while (keep_running_)
198  {
199  Msg outgoing;
200 
201  // Locks msg_ and copies it
202  lock();
203  while (turn_ != NON_REALTIME && keep_running_)
204  {
205 #ifdef NON_POLLING
206  updated_cond_.wait(lock);
207 #else
208  unlock();
209  std::this_thread::sleep_for(std::chrono::microseconds(500));
210  lock();
211 #endif
212  }
213  outgoing = msg_;
214  turn_ = REALTIME;
215 
216  unlock();
217 
218  // Sends the outgoing message
219  if (keep_running_)
220  publisher_.publish(outgoing);
221  }
222  is_running_ = false;
223  }
224 
225  std::string topic_;
228  volatile bool is_running_;
229  volatile bool keep_running_;
230 
231  std::thread thread_;
232 
233  std::mutex msg_mutex_; // Protects msg_
234 
235 #ifdef NON_POLLING
236  std::condition_variable updated_cond_;
237 #endif
238 
240  int turn_; // Who's turn is it to use msg_?
241 };
242 
243 template <class Msg>
244 using RealtimePublisherSharedPtr = std::shared_ptr<RealtimePublisher<Msg> >;
245 
246 }
247 
248 #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 construct(int queue_size, bool latched=false)
void unlockAndPublish()
Unlock the msg_ variable.
bool trylock()
Try to get the data lock from realtime.
std::shared_ptr< RealtimePublisher< Msg > > RealtimePublisherSharedPtr
void publish(const boost::shared_ptr< M > &message) const
void lock()
Get the data lock form non-realtime.
RealtimePublisher & operator=(const RealtimePublisher &)=delete
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 Feb 28 2022 23:22:45