[RTPublisher] Use NON_POLLING as default for the realtime pubisher (#280

)

* Remove the non_polling tests as they are redundant now
* scope instead of calling unlock method
* Use memory orders with atomic State
* Change the turn_ to be aligned with the corresponding loop

CMakeLists.txt

@@ -90,12 +90,6 @@ if(BUILD_TESTING)
     target_link_libraries(lock_free_queue_tests realtime_tools atomic Boost::boost)
   endif()
 
-  ament_add_gmock(realtime_publisher_tests_non_polling
-                  test/realtime_publisher_non_polling.test
-                  test/realtime_publisher_tests_non_polling.cpp)
-  target_link_libraries(realtime_publisher_tests_non_polling realtime_tools)
-  ament_target_dependencies(realtime_publisher_tests_non_polling test_msgs)
-
   ament_add_gmock(realtime_publisher_tests
                   test/realtime_publisher.test
                   test/realtime_publisher_tests.cpp)

include/realtime_tools/realtime_publisher.hpp

@@ -99,9 +99,7 @@ class RealtimePublisher
   void stop()
   {
     keep_running_ = false;
-#ifdef NON_POLLING
     updated_cond_.notify_one();  // So the publishing loop can exit
-#endif
   }
 
   /**  \brief Try to get the data lock from realtime
@@ -112,13 +110,8 @@ class RealtimePublisher
    */
   bool trylock()
   {
-    if (msg_mutex_.try_lock()) {
-      if (turn_ == State::REALTIME) {
-        return true;
-      } else {
-        msg_mutex_.unlock();
-        return false;
-      }
+    if (turn_.load(std::memory_order_acquire) == State::NON_REALTIME && msg_mutex_.try_lock()) {
+      return true;
     } else {
       return false;
     }
@@ -149,7 +142,7 @@ class RealtimePublisher
    */
   void unlockAndPublish()
   {
-    turn_ = State::NON_REALTIME;
+    turn_.store(State::REALTIME, std::memory_order_release);
     unlock();
   }
 
@@ -159,27 +152,15 @@ class RealtimePublisher
    * attempt to get unique access to the msg_ variable. Trylock returns
    * true if the lock was acquired, and false if it failed to get the lock.
    */
-  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 lock() { msg_mutex_.lock(); }
 
   /**  \brief Unlocks the data without publishing anything
    *
    */
   void unlock()
   {
     msg_mutex_.unlock();
-#ifdef NON_POLLING
     updated_cond_.notify_one();
-#endif
   }
 
   std::thread & get_thread() { return thread_; }
@@ -196,37 +177,23 @@ class RealtimePublisher
   void publishingLoop()
   {
     is_running_ = true;
-    turn_ = State::REALTIME;
+    turn_.store(State::NON_REALTIME, std::memory_order_release);
 
     while (keep_running_) {
       MessageT outgoing;
 
-      // Locks msg_ and copies it
-
-#ifdef NON_POLLING
-      std::unique_lock<std::mutex> lock_(msg_mutex_);
-#else
-      lock();
-#endif
-
-      while (turn_ != State::NON_REALTIME && keep_running_) {
-#ifdef NON_POLLING
-        updated_cond_.wait(lock_);
-#else
-        unlock();
-        std::this_thread::sleep_for(std::chrono::microseconds(500));
-        lock();
-#endif
+      {
+        // Locks msg_ and copies it to outgoing
+        std::unique_lock<std::mutex> lock_(msg_mutex_);
+        updated_cond_.wait(lock_, [&] { return turn_ == State::REALTIME || !keep_running_; });
+        outgoing = msg_;
       }
-      outgoing = msg_;
-      turn_ = State::REALTIME;
-
-      unlock();
 
       // Sends the outgoing message
       if (keep_running_) {
         publisher_->publish(outgoing);
       }
+      turn_.store(State::NON_REALTIME, std::memory_order_release);
     }
     is_running_ = false;
   }
@@ -238,10 +205,7 @@ class RealtimePublisher
   std::thread thread_;
 
   std::mutex msg_mutex_;  // Protects msg_
-
-#ifdef NON_POLLING
   std::condition_variable updated_cond_;
-#endif
 
   enum class State : int { REALTIME, NON_REALTIME, LOOP_NOT_STARTED };
   std::atomic<State> turn_;  // Who's turn is it to use msg_?