ros2_control - iron
Loading...
Searching...
No Matches
realtime_publisher.hpp
1// Copyright (c) 2008, Willow Garage, Inc.
2//
3// Redistribution and use in source and binary forms, with or without
4// modification, are permitted provided that the following conditions are met:
5//
6// * Redistributions of source code must retain the above copyright
7// notice, this list of conditions and the following disclaimer.
8//
9// * Redistributions in binary form must reproduce the above copyright
10// notice, this list of conditions and the following disclaimer in the
11// documentation and/or other materials provided with the distribution.
12//
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 HOLDER 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 * Publishing ROS messages is difficult, as the publish function is
31 * not realtime safe. This class provides the proper locking so that
32 * you can call publish in realtime and a separate (non-realtime)
33 * thread will ensure that the message gets published over ROS.
34 *
35 * Author: Stuart Glaser
36 */
37#ifndef REALTIME_TOOLS__REALTIME_PUBLISHER_HPP_
38#define REALTIME_TOOLS__REALTIME_PUBLISHER_HPP_
39
40#include <atomic>
41#include <chrono>
42#include <condition_variable>
43#include <memory>
44#include <mutex>
45#include <string>
46#include <thread>
47
48#include "rclcpp/publisher.hpp"
49
50namespace realtime_tools
51{
52template <class MessageT>
54{
55public:
57 using PublisherType = rclcpp::Publisher<MessageT>;
58 using PublisherSharedPtr = typename rclcpp::Publisher<MessageT>::SharedPtr;
59
60 using PublishedType = typename rclcpp::TypeAdapter<MessageT>::custom_type;
61 using ROSMessageType = typename rclcpp::TypeAdapter<MessageT>::ros_message_type;
62
63 RCLCPP_SMART_PTR_DEFINITIONS(RealtimePublisher<MessageT>)
64
65
66 MessageT msg_;
67
77 explicit RealtimePublisher(PublisherSharedPtr publisher)
78 : publisher_(publisher), is_running_(false), keep_running_(true), turn_(State::LOOP_NOT_STARTED)
79 {
80 thread_ = std::thread(&RealtimePublisher::publishingLoop, this);
81 }
82
83 [[deprecated(
84 "Use constructor with rclcpp::Publisher<T>::SharedPtr instead - this class does not make sense "
85 "without a real publisher")]]
87 : is_running_(false), keep_running_(false), turn_(State::LOOP_NOT_STARTED)
88 {
89 }
90
93 {
94 stop();
95 while (is_running()) {
96 std::this_thread::sleep_for(std::chrono::microseconds(100));
97 }
98 if (thread_.joinable()) {
99 thread_.join();
100 }
101 }
102
110 void stop()
111 {
112 keep_running_ = false;
113 updated_cond_.notify_one(); // So the publishing loop can exit
114 }
115
124 bool trylock()
125 {
126 if (turn_.load(std::memory_order_acquire) == State::NON_REALTIME && msg_mutex_.try_lock()) {
127 return true;
128 } else {
129 return false;
130 }
131 }
132
142 bool tryPublish(const MessageT & msg)
143 {
144 if (!trylock()) {
145 return false;
146 }
147
148 msg_ = msg;
150 return true;
151 }
152
161 {
162 turn_.store(State::REALTIME, std::memory_order_release);
163 unlock();
164 }
165
172 void lock() { msg_mutex_.lock(); }
173
178 void unlock()
179 {
180 msg_mutex_.unlock();
181 updated_cond_.notify_one();
182 }
183
184 std::thread & get_thread() { return thread_; }
185
186 const std::thread & get_thread() const { return thread_; }
187
188private:
189 // non-copyable
190 RealtimePublisher(const RealtimePublisher &) = delete;
191 RealtimePublisher & operator=(const RealtimePublisher &) = delete;
192
193 bool is_running() const { return is_running_; }
194
206 void publishingLoop()
207 {
208 is_running_ = true;
209 turn_.store(State::NON_REALTIME, std::memory_order_release);
210
211 while (keep_running_) {
212 MessageT outgoing;
213
214 {
215 // Locks msg_ and copies it to outgoing
216 std::unique_lock<std::mutex> lock_(msg_mutex_);
217 updated_cond_.wait(lock_, [&] { return turn_ == State::REALTIME || !keep_running_; });
218 outgoing = msg_;
219 }
220
221 // Sends the outgoing message
222 if (keep_running_) {
223 publisher_->publish(outgoing);
224 }
225 turn_.store(State::NON_REALTIME, std::memory_order_release);
226 }
227 is_running_ = false;
228 }
229
230 PublisherSharedPtr publisher_;
231 std::atomic<bool> is_running_;
232 std::atomic<bool> keep_running_;
233
234 std::thread thread_;
235
236 std::mutex msg_mutex_; // Protects msg_
237 std::condition_variable updated_cond_;
238
239 enum class State : int { REALTIME, NON_REALTIME, LOOP_NOT_STARTED };
240 std::atomic<State> turn_; // Who's turn is it to use msg_?
241};
242
243template <class MessageT>
244using RealtimePublisherSharedPtr = std::shared_ptr<RealtimePublisher<MessageT>>;
245
246} // namespace realtime_tools
247#endif // REALTIME_TOOLS__REALTIME_PUBLISHER_HPP_
Definition realtime_publisher.hpp:54
void stop()
Stop the realtime publisher.
Definition realtime_publisher.hpp:110
RealtimePublisher(PublisherSharedPtr publisher)
Constructor for the realtime publisher.
Definition realtime_publisher.hpp:77
bool tryPublish(const MessageT &msg)
Try to get the data lock from realtime and publish the given message.
Definition realtime_publisher.hpp:142
bool trylock()
Try to acquire the data lock for non-realtime message publishing.
Definition realtime_publisher.hpp:124
~RealtimePublisher()
Destructor.
Definition realtime_publisher.hpp:92
void unlock()
Unlocks the data without publishing anything.
Definition realtime_publisher.hpp:178
void unlockAndPublish()
Unlock the msg_ variable for the non-realtime thread to start publishing.
Definition realtime_publisher.hpp:160
rclcpp::Publisher< MessageT > PublisherType
Provide various typedefs to resemble the rclcpp::Publisher type.
Definition realtime_publisher.hpp:57
void lock()
Acquire the data lock.
Definition realtime_publisher.hpp:172
MessageT msg_
The msg_ variable contains the data that will get published on the ROS topic.
Definition realtime_publisher.hpp:66
A pthread mutex wrapper that provides a mutex with the priority inheritance protocol and a priority c...
Definition async_function_handler.hpp:38