Merge branch 'master' into retrieve/more/async/data

include/realtime_tools/async_function_handler.hpp

@@ -236,6 +236,12 @@ class AsyncFunctionHandler
    */
   std::thread & get_thread() { return thread_; }
 
+  /// Get the const version of async worker thread
+  /**
+   * @return The async callback thread
+   */
+  const std::thread & get_thread() const { return thread_; }
+
   /// Check if the async callback method is in progress
   /**
    * @return True if the async callback method is in progress, false otherwise

include/realtime_tools/realtime_helpers.hpp

@@ -30,6 +30,7 @@
 #define REALTIME_TOOLS__REALTIME_HELPERS_HPP_
 
 #include <string>
+#include <thread>
 #include <utility>
 
 namespace realtime_tools
@@ -60,16 +61,27 @@ bool lock_memory(std::string & message);
 
 /**
  * Configure the caller thread affinity - Tell the scheduler to prefer a certain
- * core for the current thread.
+ * core for the given thread handle.
+ * \note The threads created by the calling thread will inherit the affinity.
+ * \param[in] thread the thread handle of the thread
+ * \param[in] core the cpu number of the core. If a negative number is passed,
+ * the affinity is reset to the default.
+ * \returns a pair of a boolean indicating whether the operation succeeded or not
+ * and a message describing the result of the operation
+*/
+std::pair<bool, std::string> set_thread_affinity(pthread_t thread, int core);
+
+/**
+ * Configure the caller thread affinity - Tell the scheduler to prefer a certain
+ * core for the given thread.
  * \note The threads created by the calling thread will inherit the affinity.
- * \param[in] pid the process id of the thread to set the affinity for. If 0 is
- * passed, the affinity is set for the calling thread.
+ * \param[in] thread the reference of the thread
  * \param[in] core the cpu number of the core. If a negative number is passed,
  * the affinity is reset to the default.
  * \returns a pair of a boolean indicating whether the operation succeeded or not
  * and a message describing the result of the operation
 */
-std::pair<bool, std::string> set_thread_affinity(int pid, int core);
+std::pair<bool, std::string> set_thread_affinity(std::thread & thread, int core);
 
 /**
  * Configure the current thread affinity - Tell the scheduler to prefer a certain

src/realtime_helpers.cpp

@@ -115,7 +115,7 @@ bool lock_memory(std::string & message)
 #endif
 }
 
-std::pair<bool, std::string> set_thread_affinity(int pid, int core)
+std::pair<bool, std::string> set_thread_affinity(pthread_t thread, int core)
 {
   std::string message;
 #ifdef _WIN32
@@ -157,17 +157,17 @@ std::pair<bool, std::string> set_thread_affinity(int pid, int core)
       CPU_SET(i, &cpuset);
     }
     // And actually tell the schedular to set the affinity of the thread of respective pid
-    const auto result =
-      set_affinity_result_message(sched_setaffinity(pid, sizeof(cpu_set_t), &cpuset), message);
+    const auto result = set_affinity_result_message(
+      pthread_setaffinity_np(thread, sizeof(cpu_set_t), &cpuset), message);
     return std::make_pair(result, message);
   }
 
   if (core < number_of_cores) {
     // Set the passed core to the cpu set
     CPU_SET(core, &cpuset);
     // And actually tell the schedular to set the affinity of the thread of respective pid
-    const auto result =
-      set_affinity_result_message(sched_setaffinity(pid, sizeof(cpu_set_t), &cpuset), message);
+    const auto result = set_affinity_result_message(
+      pthread_setaffinity_np(thread, sizeof(cpu_set_t), &cpuset), message);
     return std::make_pair(result, message);
   }
   // Invalid core number passed
@@ -179,9 +179,18 @@ std::pair<bool, std::string> set_thread_affinity(int pid, int core)
 #endif
 }
 
+std::pair<bool, std::string> set_thread_affinity(std::thread & thread, int core)
+{
+  if (!thread.joinable()) {
+    return std::make_pair(
+      false, "Unable to set the thread affinity, as the thread is not joinable!");
+  }
+  return set_thread_affinity(thread.native_handle(), core);
+}
+
 std::pair<bool, std::string> set_current_thread_affinity(int core)
 {
-  return set_thread_affinity(0, core);
+  return set_thread_affinity(pthread_self(), core);
 }
 
 int64_t get_number_of_available_processors()

test/test_async_function_handler.cpp

@@ -131,6 +131,10 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering)
   ASSERT_THROW(async_class.trigger(), std::runtime_error);
   async_class.get_handler().start_thread();
 
+  ASSERT_TRUE(async_class.get_handler().get_thread().joinable());
+  ASSERT_TRUE(
+    realtime_tools::set_thread_affinity(async_class.get_handler().get_thread().native_handle(), 0)
+      .first);
   EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
   auto trigger_status = async_class.trigger();
   ASSERT_TRUE(trigger_status.first);
@@ -277,8 +281,13 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering_with_different_return_state_an
 {
   realtime_tools::TestAsyncFunctionHandler async_class;
   async_class.initialize();
+  ASSERT_FALSE(async_class.get_handler().get_thread().joinable());
+  ASSERT_FALSE(
+    realtime_tools::set_thread_affinity(async_class.get_handler().get_thread(), 0).first);
   async_class.get_handler().start_thread();
 
+  ASSERT_TRUE(async_class.get_handler().get_thread().joinable());
+  ASSERT_TRUE(realtime_tools::set_thread_affinity(async_class.get_handler().get_thread(), 0).first);
   EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
   auto trigger_status = async_class.trigger();
   ASSERT_TRUE(trigger_status.first);

test/thread_priority_tests.cpp

@@ -42,21 +42,43 @@ TEST(thread_priority, get_core_count)
 
 TEST(thread_priority, set_thread_affinity_valid)
 {
+  // create a basic thread for the test
+  std::thread t([]() { std::this_thread::sleep_for(std::chrono::milliseconds(100)); });
   // We should always have at least one core
-  EXPECT_TRUE(realtime_tools::set_thread_affinity(0, 0).first);
+  EXPECT_TRUE(realtime_tools::set_thread_affinity(t.native_handle(), 0).first);
+  t.join();
 }
 
 TEST(thread_priority, set_thread_affinity_invalid_too_many_cores)
 {
+  // create a basic thread for the test
+  std::thread t([]() { std::this_thread::sleep_for(std::chrono::milliseconds(100)); });
   const int count = static_cast<int>(realtime_tools::get_number_of_available_processors());
   // We should always have at least one core
-  EXPECT_FALSE(realtime_tools::set_thread_affinity(0, count + 10).first);
+  EXPECT_FALSE(realtime_tools::set_thread_affinity(t.native_handle(), count + 10).first);
+  t.join();
+}
+
+TEST(thread_priority, set_current_thread_affinity_invalid_too_many_cores)
+{
+  const int count = static_cast<int>(realtime_tools::get_number_of_available_processors());
+  // We should always have at least one core
+  EXPECT_FALSE(realtime_tools::set_current_thread_affinity(count + 10).first);
 }
 
 TEST(thread_priority, set_thread_affinity_valid_reset)
+{
+  // create a basic thread for the test
+  std::thread t([]() { std::this_thread::sleep_for(std::chrono::milliseconds(100)); });
+  // Reset core affinity
+  EXPECT_TRUE(realtime_tools::set_thread_affinity(t.native_handle(), -1).first);
+  t.join();
+}
+
+TEST(thread_priority, set_current_thread_affinity_valid_reset)
 {
   // Reset core affinity
-  EXPECT_TRUE(realtime_tools::set_thread_affinity(0, -1).first);
+  EXPECT_TRUE(realtime_tools::set_current_thread_affinity(-1).first);
 }
 
 TEST(thread_priority, set_current_thread_affinity_valid)