.. _program_listing_file__tmp_ws_src_realtime_tools_include_realtime_tools_realtime_publisher.h:

Program Listing for File realtime_publisher.h
=============================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_realtime_tools_include_realtime_tools_realtime_publisher.h>` (``/tmp/ws/src/realtime_tools/include/realtime_tools/realtime_publisher.h``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   // Copyright (c) 2008, Willow Garage, Inc.
   //
   // Redistribution and use in source and binary forms, with or without
   // modification, are permitted provided that the following conditions are met:
   //
   //    * Redistributions of source code must retain the above copyright
   //      notice, this list of conditions and the following disclaimer.
   //
   //    * Redistributions in binary form must reproduce the above copyright
   //      notice, this list of conditions and the following disclaimer in the
   //      documentation and/or other materials provided with the distribution.
   //
   //    * Neither the name of the Willow Garage, Inc. nor the names of its
   //      contributors may be used to endorse or promote products derived from
   //      this software without specific prior written permission.
   //
   // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
   // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
   // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
   // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
   // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
   // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
   // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
   // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
   // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
   // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
   // POSSIBILITY OF SUCH DAMAGE.
   
   /*
    * Publishing ROS messages is difficult, as the publish function is
    * not realtime safe.  This class provides the proper locking so that
    * you can call publish in realtime and a separate (non-realtime)
    * thread will ensure that the message gets published over ROS.
    *
    * Author: Stuart Glaser
    */
   #ifndef REALTIME_TOOLS__REALTIME_PUBLISHER_H_
   #define REALTIME_TOOLS__REALTIME_PUBLISHER_H_
   
   #include <atomic>
   #include <chrono>
   #include <condition_variable>
   #include <memory>
   #include <mutex>
   #include <string>
   #include <thread>
   
   #include "rclcpp/publisher.hpp"
   
   namespace realtime_tools
   {
   template <class Msg>
   class RealtimePublisher
   {
   private:
     using PublisherSharedPtr = typename rclcpp::Publisher<Msg>::SharedPtr;
   
   public:
     Msg msg_;
   
     explicit RealtimePublisher(PublisherSharedPtr publisher)
     : publisher_(publisher), is_running_(false), keep_running_(true), turn_(LOOP_NOT_STARTED)
     {
       thread_ = std::thread(&RealtimePublisher::publishingLoop, this);
     }
   
     RealtimePublisher() : is_running_(false), keep_running_(false), turn_(LOOP_NOT_STARTED) {}
   
     ~RealtimePublisher()
     {
       stop();
       while (is_running()) {
         std::this_thread::sleep_for(std::chrono::microseconds(100));
       }
       if (thread_.joinable()) {
         thread_.join();
       }
     }
   
     void stop()
     {
       keep_running_ = false;
   #ifdef NON_POLLING
       updated_cond_.notify_one();  // So the publishing loop can exit
   #endif
     }
   
     bool trylock()
     {
       if (msg_mutex_.try_lock()) {
         if (turn_ == REALTIME) {
           return true;
         } else {
           msg_mutex_.unlock();
           return false;
         }
       } else {
         return false;
       }
     }
   
     void unlockAndPublish()
     {
       turn_ = NON_REALTIME;
       unlock();
     }
   
     void lock()
     {
   #ifdef NON_POLLING
       msg_mutex_.lock();
   #else
       // never actually block on the lock
       while (!msg_mutex_.try_lock()) {
         std::this_thread::sleep_for(std::chrono::microseconds(200));
       }
   #endif
     }
   
     void unlock()
     {
       msg_mutex_.unlock();
   #ifdef NON_POLLING
       updated_cond_.notify_one();
   #endif
     }
   
   private:
     // non-copyable
     RealtimePublisher(const RealtimePublisher &) = delete;
     RealtimePublisher & operator=(const RealtimePublisher &) = delete;
   
     bool is_running() const { return is_running_; }
   
     void publishingLoop()
     {
       is_running_ = true;
       turn_ = REALTIME;
   
       while (keep_running_) {
         Msg outgoing;
   
         // Locks msg_ and copies it
   
   #ifdef NON_POLLING
         std::unique_lock<std::mutex> lock_(msg_mutex_);
   #else
         lock();
   #endif
   
         while (turn_ != NON_REALTIME && keep_running_) {
   #ifdef NON_POLLING
           updated_cond_.wait(lock_);
   #else
           unlock();
           std::this_thread::sleep_for(std::chrono::microseconds(500));
           lock();
   #endif
         }
         outgoing = msg_;
         turn_ = REALTIME;
   
         unlock();
   
         // Sends the outgoing message
         if (keep_running_) {
           publisher_->publish(outgoing);
         }
       }
       is_running_ = false;
     }
   
     PublisherSharedPtr publisher_;
     std::atomic<bool> is_running_;
     std::atomic<bool> keep_running_;
   
     std::thread thread_;
   
     std::mutex msg_mutex_;  // Protects msg_
   
   #ifdef NON_POLLING
     std::condition_variable updated_cond_;
   #endif
   
     enum { REALTIME, NON_REALTIME, LOOP_NOT_STARTED };
     std::atomic<int> turn_;  // Who's turn is it to use msg_?
   };
   
   template <class Msg>
   using RealtimePublisherSharedPtr = std::shared_ptr<RealtimePublisher<Msg>>;
   
   }  // namespace realtime_tools
   #endif  // REALTIME_TOOLS__REALTIME_PUBLISHER_H_