37 #ifndef REALTIME_TOOLS__REALTIME_PUBLISHER_H_
38 #define REALTIME_TOOLS__REALTIME_PUBLISHER_H_
42 #include <condition_variable>
48 #include "rclcpp/publisher.hpp"
56 using PublisherSharedPtr =
typename rclcpp::Publisher<Msg>::SharedPtr;
67 : publisher_(publisher), is_running_(false), keep_running_(true), turn_(LOOP_NOT_STARTED)
69 thread_ = std::thread(&RealtimePublisher::publishingLoop,
this);
72 RealtimePublisher() : is_running_(false), keep_running_(false), turn_(LOOP_NOT_STARTED) {}
78 while (is_running()) {
79 std::this_thread::sleep_for(std::chrono::microseconds(100));
81 if (thread_.joinable()) {
89 keep_running_ =
false;
91 updated_cond_.notify_one();
103 if (msg_mutex_.try_lock()) {
104 if (turn_ == REALTIME) {
123 turn_ = NON_REALTIME;
139 while (!msg_mutex_.try_lock()) {
140 std::this_thread::sleep_for(std::chrono::microseconds(200));
152 updated_cond_.notify_one();
161 bool is_running()
const {
return is_running_; }
163 void publishingLoop()
168 while (keep_running_) {
174 std::unique_lock<std::mutex> lock_(msg_mutex_);
179 while (turn_ != NON_REALTIME && keep_running_) {
181 updated_cond_.wait(lock_);
184 std::this_thread::sleep_for(std::chrono::microseconds(500));
195 publisher_->publish(outgoing);
201 PublisherSharedPtr publisher_;
202 std::atomic<bool> is_running_;
203 std::atomic<bool> keep_running_;
207 std::mutex msg_mutex_;
210 std::condition_variable updated_cond_;
213 enum { REALTIME, NON_REALTIME, LOOP_NOT_STARTED };
214 std::atomic<int> turn_;
218 using RealtimePublisherSharedPtr = std::shared_ptr<RealtimePublisher<Msg>>;