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 <ros/node_handle.h>
42 
43 #include <atomic>
44 #include <chrono>
45 #include <condition_variable>
46 #include <memory>
47 #include <mutex>
48 #include <string>
49 #include <thread>
50 
51 namespace realtime_tools {
52 
53 template <class Msg>
55 {
56 
57 public:
59  Msg msg_;
60 
68  RealtimePublisher(const ros::NodeHandle &node, const std::string &topic, int queue_size, bool latched=false)
69  : topic_(topic), node_(node), is_running_(false), keep_running_(false), turn_(LOOP_NOT_STARTED)
70  {
71  construct(queue_size, latched);
72  }
73 
76  {
77  }
78 
81  {
82  stop();
83  while (is_running_)
84  {
85  std::this_thread::sleep_for(std::chrono::microseconds(100));
86  }
87 
88  if (thread_.joinable())
89  {
90  thread_.join();
91  }
93  }
94 
95  void init(const ros::NodeHandle &node, const std::string &topic, int queue_size, bool latched=false)
96  {
97  topic_ = topic;
98  node_ = node;
99  construct(queue_size, latched);
100  }
101 
103  void stop()
104  {
105  keep_running_ = false;
106 #ifdef NON_POLLING
107  updated_cond_.notify_one(); // So the publishing loop can exit
108 #endif
109  }
110 
117  bool trylock()
118  {
119  if (msg_mutex_.try_lock())
120  {
121  if (turn_ == REALTIME)
122  {
123  return true;
124  }
125  else
126  {
127  unlock();
128  return false;
129  }
130  }
131  else
132  {
133  return false;
134  }
135  }
136 
144  {
146  unlock();
147  }
148 
155  void lock()
156  {
157 #ifdef NON_POLLING
158  msg_mutex_.lock();
159 #else
160  // never actually block on the lock
161  while (!msg_mutex_.try_lock())
162  {
163  std::this_thread::sleep_for(std::chrono::microseconds(200));
164  }
165 #endif
166  }
167 
171  void unlock()
172  {
173  msg_mutex_.unlock();
174 #ifdef NON_POLLING
175  updated_cond_.notify_one();
176 #endif
177  }
178 
179 private:
180  // non-copyable
181  RealtimePublisher(const RealtimePublisher &) = delete;
182  RealtimePublisher & operator=(const RealtimePublisher &) = delete;
183 
184  void construct(int queue_size, bool latched=false)
185  {
186  publisher_ = node_.advertise<Msg>(topic_, queue_size, latched);
187  keep_running_ = true;
188  turn_ = REALTIME;
189  thread_ = std::thread(&RealtimePublisher::publishingLoop, this);
190  }
191 
193  {
194  is_running_ = true;
195 #ifdef NON_POLLING
196  std::unique_lock<std::mutex> cond_lock(msg_mutex_, std::defer_lock_t());
197 #endif
198 
199  while (keep_running_)
200  {
201  Msg outgoing;
202 
203  // Locks msg_ and copies it
204  lock();
205  while (turn_ != NON_REALTIME && keep_running_)
206  {
207 #ifdef NON_POLLING
208  updated_cond_.wait(cond_lock);
209 #else
210  unlock();
211  std::this_thread::sleep_for(std::chrono::microseconds(500));
212  lock();
213 #endif
214  }
215  outgoing = msg_;
216  turn_ = REALTIME;
217 
218  unlock();
219 
220  // Sends the outgoing message
221  if (keep_running_)
222  publisher_.publish(outgoing);
223  }
224  is_running_ = false;
225  }
226 
227  std::string topic_;
230  std::atomic<bool> is_running_;
231  std::atomic<bool> keep_running_;
232 
233  std::thread thread_;
234 
235  std::mutex msg_mutex_; // Protects msg_ and turn_
236 
237 #ifdef NON_POLLING
238  std::condition_variable updated_cond_;
239 #endif
240 
242  std::atomic<int> turn_; // Who's turn is it to use msg_?
243 };
244 
245 template <class Msg>
246 using RealtimePublisherSharedPtr = std::shared_ptr<RealtimePublisher<Msg> >;
247 
248 }
249 
250 #endif
node_handle.h
ros::Publisher
realtime_tools::RealtimePublisher::NON_REALTIME
@ NON_REALTIME
Definition: realtime_publisher.h:241
realtime_tools::RealtimePublisher::node_
ros::NodeHandle node_
Definition: realtime_publisher.h:228
realtime_tools::RealtimePublisher::lock
void lock()
Get the data lock form non-realtime.
Definition: realtime_publisher.h:155
realtime_tools::RealtimePublisher::thread_
std::thread thread_
Definition: realtime_publisher.h:233
realtime_tools::RealtimePublisher::init
void init(const ros::NodeHandle &node, const std::string &topic, int queue_size, bool latched=false)
Definition: realtime_publisher.h:95
realtime_tools::RealtimePublisher::operator=
RealtimePublisher & operator=(const RealtimePublisher &)=delete
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
realtime_tools::RealtimePublisher
Definition: realtime_publisher.h:54
realtime_tools::RealtimePublisher::msg_mutex_
std::mutex msg_mutex_
Definition: realtime_publisher.h:235
ros::Publisher::shutdown
void shutdown()
realtime_tools::RealtimePublisher::unlockAndPublish
void unlockAndPublish()
Unlock the msg_ variable.
Definition: realtime_publisher.h:143
realtime_tools::RealtimePublisher::REALTIME
@ REALTIME
Definition: realtime_publisher.h:241
realtime_tools::RealtimePublisher::topic_
std::string topic_
Definition: realtime_publisher.h:227
realtime_tools::RealtimePublisherSharedPtr
std::shared_ptr< RealtimePublisher< Msg > > RealtimePublisherSharedPtr
Definition: realtime_publisher.h:246
realtime_tools::RealtimePublisher::RealtimePublisher
RealtimePublisher(const ros::NodeHandle &node, const std::string &topic, int queue_size, bool latched=false)
Constructor for the realtime publisher.
Definition: realtime_publisher.h:68
realtime_tools::RealtimePublisher::msg_
Msg msg_
The msg_ variable contains the data that will get published on the ROS topic.
Definition: realtime_publisher.h:59
realtime_tools::RealtimePublisher::stop
void stop()
Stop the realtime publisher from sending out more ROS messages.
Definition: realtime_publisher.h:103
realtime_tools::RealtimePublisher::trylock
bool trylock()
Try to get the data lock from realtime.
Definition: realtime_publisher.h:117
realtime_tools::RealtimePublisher::unlock
void unlock()
Unlocks the data without publishing anything.
Definition: realtime_publisher.h:171
realtime_tools::RealtimePublisher::turn_
std::atomic< int > turn_
Definition: realtime_publisher.h:242
realtime_tools::RealtimePublisher::is_running_
std::atomic< bool > is_running_
Definition: realtime_publisher.h:230
realtime_tools::RealtimePublisher::publishingLoop
void publishingLoop()
Definition: realtime_publisher.h:192
realtime_tools::RealtimePublisher::construct
void construct(int queue_size, bool latched=false)
Definition: realtime_publisher.h:184
realtime_tools::RealtimePublisher::~RealtimePublisher
~RealtimePublisher()
Destructor.
Definition: realtime_publisher.h:80
realtime_tools::RealtimePublisher::publisher_
ros::Publisher publisher_
Definition: realtime_publisher.h:229
realtime_tools
Definition: realtime_box.h:38
realtime_tools::RealtimePublisher::LOOP_NOT_STARTED
@ LOOP_NOT_STARTED
Definition: realtime_publisher.h:241
realtime_tools::RealtimePublisher::keep_running_
std::atomic< bool > keep_running_
Definition: realtime_publisher.h:231
realtime_tools::RealtimePublisher::RealtimePublisher
RealtimePublisher()
Definition: realtime_publisher.h:74
ros::NodeHandle


realtime_tools
Author(s): Stuart Glaser
autogenerated on Mon Jan 8 2024 03:20:12