Add new API for the RealtimePublisher (backport #323) (#352)

---------

Co-authored-by: Sai Kishor Kothakota <[email protected]>
Co-authored-by: Christoph Froehlich <[email protected]>

Author: 3 people

doc/migration.rst

@@ -11,3 +11,4 @@ This list summarizes important changes between Humble (previous) and Jazzy (curr
 RealtimeBox
 *******************************
 * ``RealtimeBox`` is deprecated. Update your code to use ``realtime_thread_safe_box.hpp`` header, and class name ``RealtimeThreadSafeBox`` instead. (`#318 <https://github.com/ros-controls/realtime_tools/pull/318>`__, `#342 <https://github.com/ros-controls/realtime_tools/pull/342>`__).
+* ``RealtimePublisher`` is updated with a new ``try_publish`` API. Update your code with a local message variable and call ``try_publish`` with that variable. The old API is deprecated and will be removed in a future release. (`#323 <https://github.com/ros-controls/realtime_tools/pull/323>`__).

doc/release_notes.rst

@@ -11,3 +11,4 @@ This list summarizes the changes between Humble (previous) and Jazzy (current) r
 RealtimeBox
 *******************************
 * ``RealtimeBox`` got renamed to ``RealtimeThreadSafeBox`` and uses real-time mutexes now (`#318 <https://github.com/ros-controls/realtime_tools/pull/318>`__, `#342 <https://github.com/ros-controls/realtime_tools/pull/342>`__).
+* ``RealtimePublisher`` is updated with a new ``try_publish`` API and few methods are deprecated (`#323 <https://github.com/ros-controls/realtime_tools/pull/323>`__).

realtime_tools/doc/realtime_publisher.rst

@@ -12,6 +12,7 @@ The ``realtime_tools::RealtimePublisher`` allows users that write C++ ros2_contr
   private:
     std::shared_ptr<realtime_tools::RealtimePublisher<my_msgs::msg::MyMsg>> state_publisher_;
     std::shared_ptr<rclcpp::Publisher<my_msgs::msg::MyMsg>> s_publisher_;
+    my_msgs::msg::MyMsg some_msg_;
   }
 
   controller_interface::CallbackReturn MyController::on_configure(
@@ -21,7 +22,8 @@ The ``realtime_tools::RealtimePublisher`` allows users that write C++ ros2_contr
     s_publisher_ = get_node()->create_publisher<my_msgs::msg::MyMsg>(
       "~/status", rclcpp::SystemDefaultsQoS());
     state_publisher_ =
-      std::make_unique<realtime_tools::RealtimePublisher<ControllerStateMsg>>(s_publisher_);
+      std::make_unique<realtime_tools::RealtimePublisher<my_msgs::msg::MyMsg>>(s_publisher_);
+    some_msg_.header.frame_id = params_.frame_id;
     ...
   }
 
@@ -30,7 +32,8 @@ The ``realtime_tools::RealtimePublisher`` allows users that write C++ ros2_contr
   {
     ...
     // Publish controller state
-    state_publisher_->lock();
-    state_publisher_->msg_ = some_msg;
-    state_publisher_->unlockAndPublish();
+    some_msg_.header.stamp = get_node()->now();
+    // Fill in the rest of the message
+    ....
+    state_publisher_->try_publish(some_msg_);
   }

realtime_tools/include/realtime_tools/realtime_publisher.hpp

@@ -44,6 +44,7 @@
 #include <mutex>
 #include <string>
 #include <thread>
+#include <utility>
 
 #include "rclcpp/publisher.hpp"
 
@@ -62,7 +63,6 @@ class RealtimePublisher
 
   RCLCPP_SMART_PTR_DEFINITIONS(RealtimePublisher<MessageT>)
 
-  /// The msg_ variable contains the data that will get published on the ROS topic.
   MessageT msg_;
 
   /**
@@ -137,31 +137,61 @@ class RealtimePublisher
   */
   bool trylock()
   {
-    if (turn_.load(std::memory_order_acquire) == State::REALTIME && msg_mutex_.try_lock()) {
+    return turn_.load(std::memory_order_acquire) == State::REALTIME && msg_mutex_.try_lock();
+  }
+
+  /**
+   * \brief Check if the realtime publisher is in a state to publish messages
+   * \return true if the publisher is in a state to publish messages
+   * \note The msg_ variable can be safely accessed if this function returns true
+  */
+  bool can_publish() const
+  {
+    std::unique_lock<std::mutex> lock(msg_mutex_, std::try_to_lock);
+    return can_publish(lock);
+  }
+
+  /**
+   * \brief Try to publish the given message
+   *
+   * This method attempts to publish the given message if the publisher is in a state to do so.
+   * It uses a try_lock to avoid blocking if the mutex is already held by another thread.
+   *
+   * \param [in] msg The message to publish
+   * \return true if the message was successfully published, false otherwise
+   */
+  bool try_publish(const MessageT & msg)
+  {
+    std::unique_lock<std::mutex> lock(msg_mutex_, std::try_to_lock);
+    if (can_publish(lock)) {
+      {
+        std::unique_lock<std::mutex> scoped_lock(std::move(lock));
+        msg_ = msg;
+        turn_.store(State::NON_REALTIME, std::memory_order_release);
+      }
+      updated_cond_.notify_one();  // Notify the publishing thread
       return true;
-    } else {
-      return false;
     }
+    return false;
   }
 
   /**
-   * \brief Try to get the data lock from realtime and publish the given message
+   * \brief Try to publish the given message (deprecated)
+   * \deprecated This method is deprecated and should be replaced with try_publish()
    *
-   * Tries to gain unique access to msg_ variable. If this succeeds
-   * update the msg_ variable and call unlockAndPublish
+   * This method is deprecated and should be replaced with try_publish().
+   * It attempts to publish the given message if the publisher is in a state to do so.
+   * It uses a try_lock to avoid blocking if the mutex is already held by another thread.
    *
    * \param [in] msg The message to publish
-   * \return false in case no lock for the realtime variable is acquired. This implies the message will not be published.
+   * \return true if the message was successfully published, false otherwise
    */
+  [[deprecated(
+    "Use try_publish() method instead of this method. This method may be removed in future "
+    "versions.")]]
   bool tryPublish(const MessageT & msg)
   {
-    if (!trylock()) {
-      return false;
-    }
-
-    msg_ = msg;
-    unlockAndPublish();
-    return true;
+    return try_publish(msg);
   }
 
   /**
@@ -199,7 +229,24 @@ class RealtimePublisher
 
   const std::thread & get_thread() const { return thread_; }
 
+  const MessageT & get_msg() const { return msg_; }
+
+  std::mutex & get_mutex() { return msg_mutex_; }
+
+  const std::mutex & get_mutex() const { return msg_mutex_; }
+
 private:
+  /**
+   * \brief Check if the realtime publisher is in a state to publish messages
+   * \param lock A unique_lock that is already acquired on the msg_mutex_
+   * \return true if the publisher is in a state to publish messages
+   * \note The msg_ variable can be safely accessed if this function returns true
+  */
+  bool can_publish(std::unique_lock<std::mutex> & lock) const
+  {
+    return turn_.load(std::memory_order_acquire) == State::REALTIME && lock.owns_lock();
+  }
+
   // non-copyable
   RealtimePublisher(const RealtimePublisher &) = delete;
   RealtimePublisher & operator=(const RealtimePublisher &) = delete;
@@ -246,7 +293,7 @@ class RealtimePublisher
 
   std::thread thread_;
 
-  std::mutex msg_mutex_;  // Protects msg_
+  mutable std::mutex msg_mutex_;  // Protects msg_
   std::condition_variable updated_cond_;
 
   enum class State : int { REALTIME, NON_REALTIME, LOOP_NOT_STARTED };

realtime_tools/test/realtime_publisher_tests.cpp

@@ -132,3 +132,45 @@ TEST(RealtimePublisher, rt_try_publish)
   EXPECT_STREQ(expected_msg, str_callback.msg_.string_value.c_str());
   rclcpp::shutdown();
 }
+
+TEST(RealtimePublisher, rt_can_try_publish)
+{
+  rclcpp::init(0, nullptr);
+  const size_t ATTEMPTS = 10;
+  const std::chrono::milliseconds DELAY(250);
+
+  const char * expected_msg = "Hello World";
+  auto node = std::make_shared<rclcpp::Node>("construct_move_destruct");
+  rclcpp::QoS qos(10);
+  qos.reliable().transient_local();
+  auto pub = node->create_publisher<StringMsg>("~/rt_publish", qos);
+  RealtimePublisher<StringMsg> rt_pub(pub);
+  ASSERT_TRUE(rt_pub.can_publish());
+
+  // try publish a latched message
+  bool publish_success = false;
+  for (std::size_t i = 0; i < ATTEMPTS; ++i) {
+    StringMsg msg;
+    msg.string_value = expected_msg;
+
+    if (rt_pub.can_publish()) {
+      ASSERT_TRUE(rt_pub.try_publish(msg));
+      publish_success = true;
+    }
+    std::this_thread::sleep_for(DELAY);
+  }
+  ASSERT_TRUE(publish_success);
+
+  // make sure subscriber gets it
+  StringCallback str_callback;
+
+  auto sub = node->create_subscription<StringMsg>(
+    "~/rt_publish", qos,
+    std::bind(&StringCallback::callback, &str_callback, std::placeholders::_1));
+  for (size_t i = 0; i < ATTEMPTS && str_callback.msg_.string_value.empty(); ++i) {
+    rclcpp::spin_some(node);
+    std::this_thread::sleep_for(DELAY);
+  }
+  EXPECT_STREQ(expected_msg, str_callback.msg_.string_value.c_str());
+  rclcpp::shutdown();
+}