37#ifndef REALTIME_TOOLS__REALTIME_PUBLISHER_HPP_
38#define REALTIME_TOOLS__REALTIME_PUBLISHER_HPP_
42#include <condition_variable>
48#include "rclcpp/publisher.hpp"
52template <
class MessageT>
58 using PublisherSharedPtr =
typename rclcpp::Publisher<MessageT>::SharedPtr;
60 using PublishedType =
typename rclcpp::TypeAdapter<MessageT>::custom_type;
61 using ROSMessageType =
typename rclcpp::TypeAdapter<MessageT>::ros_message_type;
78 : publisher_(publisher), is_running_(false), keep_running_(true), turn_(State::LOOP_NOT_STARTED)
80 thread_ = std::thread(&RealtimePublisher::publishingLoop,
this);
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)
95 while (is_running()) {
96 std::this_thread::sleep_for(std::chrono::microseconds(100));
98 if (thread_.joinable()) {
112 keep_running_ =
false;
113 updated_cond_.notify_one();
126 if (turn_.load(std::memory_order_acquire) == State::NON_REALTIME && msg_mutex_.try_lock()) {
162 turn_.store(State::REALTIME, std::memory_order_release);
172 void lock() { msg_mutex_.lock(); }
181 updated_cond_.notify_one();
184 std::thread & get_thread() {
return thread_; }
186 const std::thread & get_thread()
const {
return thread_; }
193 bool is_running()
const {
return is_running_; }
206 void publishingLoop()
209 turn_.store(State::NON_REALTIME, std::memory_order_release);
211 while (keep_running_) {
216 std::unique_lock<std::mutex> lock_(msg_mutex_);
217 updated_cond_.wait(lock_, [&] {
return turn_ == State::REALTIME || !keep_running_; });
223 publisher_->publish(outgoing);
225 turn_.store(State::NON_REALTIME, std::memory_order_release);
230 PublisherSharedPtr publisher_;
231 std::atomic<bool> is_running_;
232 std::atomic<bool> keep_running_;
236 std::mutex msg_mutex_;
237 std::condition_variable updated_cond_;
239 enum class State :
int { REALTIME, NON_REALTIME, LOOP_NOT_STARTED };
240 std::atomic<State> turn_;
243template <
class MessageT>
244using RealtimePublisherSharedPtr = std::shared_ptr<RealtimePublisher<MessageT>>;