diff --git a/include/realtime_tools/async_function_handler.hpp b/include/realtime_tools/async_function_handler.hpp index 6c8db652..ae81f2e2 100644 --- a/include/realtime_tools/async_function_handler.hpp +++ b/include/realtime_tools/async_function_handler.hpp @@ -222,6 +222,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 diff --git a/include/realtime_tools/realtime_helpers.hpp b/include/realtime_tools/realtime_helpers.hpp index eef95c61..b0790924 100644 --- a/include/realtime_tools/realtime_helpers.hpp +++ b/include/realtime_tools/realtime_helpers.hpp @@ -30,6 +30,7 @@ #define REALTIME_TOOLS__REALTIME_HELPERS_HPP_ #include +#include #include 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 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 set_thread_affinity(int pid, int core); +std::pair set_thread_affinity(std::thread & thread, int core); /** * Configure the current thread affinity - Tell the scheduler to prefer a certain diff --git a/src/realtime_helpers.cpp b/src/realtime_helpers.cpp index a955c938..866acc34 100644 --- a/src/realtime_helpers.cpp +++ b/src/realtime_helpers.cpp @@ -115,7 +115,7 @@ bool lock_memory(std::string & message) #endif } -std::pair set_thread_affinity(int pid, int core) +std::pair set_thread_affinity(pthread_t thread, int core) { std::string message; #ifdef _WIN32 @@ -157,8 +157,8 @@ std::pair 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); } @@ -166,8 +166,8 @@ std::pair set_thread_affinity(int pid, int core) // 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 set_thread_affinity(int pid, int core) #endif } +std::pair 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 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() diff --git a/test/test_async_function_handler.cpp b/test/test_async_function_handler.cpp index 77646ad7..dff2691f 100644 --- a/test/test_async_function_handler.cpp +++ b/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); diff --git a/test/thread_priority_tests.cpp b/test/thread_priority_tests.cpp index 1dc7c106..fc552cdd 100644 --- a/test/thread_priority_tests.cpp +++ b/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(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(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)