Merge branch 'master' into add/realtime/mutexes

Author: saikishor

.github/workflows/downstream-build.yml

@@ -8,7 +8,7 @@ Contains a set of tools that can be used from a hard realtime thread, without br
 ## Build status
 ROS2 Distro | Branch | Build status | Documentation | Released packages
 :---------: | :----: | :----------: | :-----------: | :---------------:
-**Rolling** | [`master`](https://github.com/ros-controls/realtime_tools/tree/master) | [![Binary Build](https://github.com/ros-controls/realtime_tools/actions/workflows/binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/realtime_tools/actions/workflows/binary-build.yml?branch=master) <br /> [![Source Build](https://github.com/ros-controls/realtime_tools/actions/workflows/source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/realtime_tools/actions/workflows/source-build.yml?branch=master) | [API](http://docs.ros.org/en/rolling/p/realtime_tools/)  | [realtime_tools](https://index.ros.org/p/realtime_tools/#rolling)
+**Rolling** | [`master`](https://github.com/ros-controls/realtime_tools/tree/master) | [![Binary Build](https://github.com/ros-controls/realtime_tools/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/realtime_tools/actions/workflows/rolling-binary-build.yml?branch=master) <br /> [![Source Build](https://github.com/ros-controls/realtime_tools/actions/workflows/rolling-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/realtime_tools/actions/workflows/rolling-source-build.yml?branch=master) | [API](http://docs.ros.org/en/rolling/p/realtime_tools/)  | [realtime_tools](https://index.ros.org/p/realtime_tools/#rolling)
 **Jazzy** | [`master`](https://github.com/ros-controls/realtime_tools/tree/master) | see above | [API](http://docs.ros.org/en/jazzy/p/realtime_tools/) | [realtime_tools](https://index.ros.org/p/realtime_tools/#jazzy)
 **Humble** | [`humble`](https://github.com/ros-controls/realtime_tools/tree/humble) | [![Binary Build](https://github.com/ros-controls/realtime_tools/actions/workflows/humble-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/realtime_tools/actions/workflows/humble-binary-build.yml?branch=master) <br /> [![Source Build](https://github.com/ros-controls/realtime_tools/actions/workflows/humble-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/realtime_tools/actions/workflows/humble-source-build.yml?branch=master) | [API](http://docs.ros.org/en/humble/p/realtime_tools/) | [realtime_tools](https://index.ros.org/p/realtime_tools/#humble)
 

README.md

@@ -49,27 +49,39 @@
 
 namespace realtime_tools
 {
-template <class Msg>
+template <class MessageT>
 class RealtimePublisher
 {
-private:
-  using PublisherSharedPtr = typename rclcpp::Publisher<Msg>::SharedPtr;
-
 public:
+  /// Provide various typedefs to resemble the rclcpp::Publisher type
+  using PublisherType = rclcpp::Publisher<MessageT>;
+  using PublisherSharedPtr = typename rclcpp::Publisher<MessageT>::SharedPtr;
+
+  using PublishedType = typename rclcpp::TypeAdapter<MessageT>::custom_type;
+  using ROSMessageType = typename rclcpp::TypeAdapter<MessageT>::ros_message_type;
+
+  RCLCPP_SMART_PTR_DEFINITIONS(RealtimePublisher<MessageT>)
+
   /// The msg_ variable contains the data that will get published on the ROS topic.
-  Msg msg_;
+  MessageT msg_;
 
   /**  \brief Constructor for the realtime publisher
    *
    * \param publisher the publisher to wrap
    */
   explicit RealtimePublisher(PublisherSharedPtr publisher)
-  : publisher_(publisher), is_running_(false), keep_running_(true), turn_(LOOP_NOT_STARTED)
+  : publisher_(publisher), is_running_(false), keep_running_(true), turn_(State::LOOP_NOT_STARTED)
   {
     thread_ = std::thread(&RealtimePublisher::publishingLoop, this);
   }
 
-  RealtimePublisher() : is_running_(false), keep_running_(false), turn_(LOOP_NOT_STARTED) {}
+  [[deprecated(
+    "Use constructor with rclcpp::Publisher<T>::SharedPtr instead - this class does not make sense "
+    "without a real publisher")]]
+  RealtimePublisher()
+  : is_running_(false), keep_running_(false), turn_(State::LOOP_NOT_STARTED)
+  {
+  }
 
   /// Destructor
   ~RealtimePublisher()
@@ -101,7 +113,7 @@ class RealtimePublisher
   bool trylock()
   {
     if (msg_mutex_.try_lock()) {
-      if (turn_ == REALTIME) {
+      if (turn_ == State::REALTIME) {
         return true;
       } else {
         msg_mutex_.unlock();
@@ -112,6 +124,23 @@ class RealtimePublisher
     }
   }
 
+  /**  \brief Try to get the data lock from realtime and publish the given message
+   *
+   * Tries to gain unique access to msg_ variable. If this succeeds
+   * update the msg_ variable and call unlockAndPublish
+   * @return false in case no lock for the realtime variable could be acquired
+   */
+  bool tryPublish(const MessageT & msg)
+  {
+    if (!trylock()) {
+      return false;
+    }
+
+    msg_ = msg;
+    unlockAndPublish();
+    return true;
+  }
+
   /**  \brief Unlock the msg_ variable
    *
    * After a successful trylock and after the data is written to the mgs_
@@ -120,7 +149,7 @@ class RealtimePublisher
    */
   void unlockAndPublish()
   {
-    turn_ = NON_REALTIME;
+    turn_ = State::NON_REALTIME;
     unlock();
   }
 
@@ -163,10 +192,10 @@ class RealtimePublisher
   void publishingLoop()
   {
     is_running_ = true;
-    turn_ = REALTIME;
+    turn_ = State::REALTIME;
 
     while (keep_running_) {
-      Msg outgoing;
+      MessageT outgoing;
 
       // Locks msg_ and copies it
 
@@ -176,7 +205,7 @@ class RealtimePublisher
       lock();
 #endif
 
-      while (turn_ != NON_REALTIME && keep_running_) {
+      while (turn_ != State::NON_REALTIME && keep_running_) {
 #ifdef NON_POLLING
         updated_cond_.wait(lock_);
 #else
@@ -186,7 +215,7 @@ class RealtimePublisher
 #endif
       }
       outgoing = msg_;
-      turn_ = REALTIME;
+      turn_ = State::REALTIME;
 
       unlock();
 
@@ -210,12 +239,12 @@ class RealtimePublisher
   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_?
+  enum class State : int { REALTIME, NON_REALTIME, LOOP_NOT_STARTED };
+  std::atomic<State> turn_;  // Who's turn is it to use msg_?
 };
 
-template <class Msg>
-using RealtimePublisherSharedPtr = std::shared_ptr<RealtimePublisher<Msg>>;
+template <class MessageT>
+using RealtimePublisherSharedPtr = std::shared_ptr<RealtimePublisher<MessageT>>;
 
 }  // namespace realtime_tools
 #endif  // REALTIME_TOOLS__REALTIME_PUBLISHER_HPP_

include/realtime_tools/realtime_publisher.hpp

@@ -35,7 +35,7 @@
  * Author: Wim Meeussen
  */
 
-#include "realtime_tools/realtime_clock.h"
+#include "realtime_tools/realtime_clock.hpp"
 
 #include <chrono>
 

src/realtime_clock.cpp

@@ -91,3 +91,44 @@ TEST(RealtimePublisher, rt_publish)
   EXPECT_STREQ(expected_msg, str_callback.msg_.string_value.c_str());
   rclcpp::shutdown();
 }
+
+TEST(RealtimePublisher, rt_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);
+
+  // 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.tryPublish(msg)) {
+      publish_success = true;
+      break;
+    }
+    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();
+}

test/realtime_publisher_tests.cpp

@@ -93,3 +93,44 @@ TEST(RealtimePublisherNonPolling, rt_publish)
   EXPECT_STREQ(expected_msg, str_callback.msg_.string_value.c_str());
   rclcpp::shutdown();
 }
+
+TEST(RealtimePublisher, rt_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);
+
+  // 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.tryPublish(msg)) {
+      publish_success = true;
+      break;
+    }
+    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();
+}

test/realtime_publisher_tests_non_polling.cpp

@@ -104,6 +104,7 @@ TEST_F(AsyncFunctionHandlerTest, check_initialization)
 
   // Once initialized, it should not be possible to initialize again
   async_class.get_handler().start_thread();
+  std::this_thread::sleep_for(std::chrono::milliseconds(10));
   auto trigger_status = async_class.trigger();
   ASSERT_TRUE(trigger_status.first);
   ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second);
@@ -130,6 +131,7 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering)
   // It shouldn't be possible to trigger without starting the thread
   ASSERT_THROW(async_class.trigger(), std::runtime_error);
   async_class.get_handler().start_thread();
+  std::this_thread::sleep_for(std::chrono::milliseconds(10));
 
   ASSERT_TRUE(async_class.get_handler().get_thread().joinable());
   ASSERT_TRUE(
@@ -174,6 +176,7 @@ TEST_F(AsyncFunctionHandlerTest, trigger_for_several_cycles)
   ASSERT_FALSE(async_class.get_handler().is_running());
   ASSERT_FALSE(async_class.get_handler().is_stopped());
   async_class.get_handler().start_thread();
+  std::this_thread::sleep_for(std::chrono::milliseconds(10));
 
   EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
 
@@ -193,8 +196,8 @@ TEST_F(AsyncFunctionHandlerTest, trigger_for_several_cycles)
       missed_triggers++;
     }
   }
-  // Make sure that the failed triggers are less than 0.1%
-  ASSERT_LT(missed_triggers, static_cast<int>(0.001 * total_cycles))
+  // Make sure that the failed triggers are less than 0.5%
+  ASSERT_LT(missed_triggers, static_cast<int>(0.005 * total_cycles))
     << "The missed triggers cannot be more than 0.1%!";
   async_class.get_handler().stop_thread();
 
@@ -216,6 +219,7 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles)
   ASSERT_FALSE(async_class.get_handler().is_running());
   ASSERT_FALSE(async_class.get_handler().is_stopped());
   async_class.get_handler().start_thread();
+  std::this_thread::sleep_for(std::chrono::milliseconds(10));
   ASSERT_TRUE(async_class.get_handler().is_running());
   ASSERT_FALSE(async_class.get_handler().is_stopped());
   EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
@@ -285,6 +289,7 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering_with_different_return_state_an
   ASSERT_FALSE(
     realtime_tools::set_thread_affinity(async_class.get_handler().get_thread(), 0).first);
   async_class.get_handler().start_thread();
+  std::this_thread::sleep_for(std::chrono::milliseconds(10));
 
   ASSERT_TRUE(async_class.get_handler().get_thread().joinable());
   ASSERT_TRUE(realtime_tools::set_thread_affinity(async_class.get_handler().get_thread(), 0).first);
@@ -356,6 +361,7 @@ TEST_F(AsyncFunctionHandlerTest, check_exception_handling)
   realtime_tools::TestAsyncFunctionHandler async_class;
   async_class.initialize();
   async_class.get_handler().start_thread();
+  std::this_thread::sleep_for(std::chrono::milliseconds(10));
 
   EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
   auto trigger_status = async_class.trigger();
@@ -401,6 +407,7 @@ TEST_F(AsyncFunctionHandlerTest, check_exception_handling)
 
   async_class.reset_counter(0);
   async_class.get_handler().start_thread();
+  std::this_thread::sleep_for(std::chrono::milliseconds(10));
   trigger_status = async_class.trigger();
   ASSERT_TRUE(trigger_status.first);
   ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second);