diff --git a/cmake/cpplib.cmake b/cmake/cpplib.cmake index 0497114..c3e0646 100644 --- a/cmake/cpplib.cmake +++ b/cmake/cpplib.cmake @@ -10,6 +10,7 @@ target_sources(sas_core_pure PRIVATE ${CMAKE_CURRENT_LIST_DIR}/../src/sas_robot_driver.cpp ${CMAKE_CURRENT_LIST_DIR}/../src/examples/sas_robot_driver_example.cpp ${CMAKE_CURRENT_LIST_DIR}/../src/eigen3_std_conversions.cpp + ${CMAKE_CURRENT_LIST_DIR}/../src/sas_thread_manager.cpp ) set_target_properties(sas_core_pure PROPERTIES diff --git a/include/sas_core/sas_thread_manager.hpp b/include/sas_core/sas_thread_manager.hpp new file mode 100644 index 0000000..1549d98 --- /dev/null +++ b/include/sas_core/sas_thread_manager.hpp @@ -0,0 +1,110 @@ +/* +# Copyright (c) 2022-2026 Murilo Marques Marinho +# +# This file is part of sas_core. +# +# sas_core is free software: you can redistribute it and/or modify +# it under the terms of the GNU Lesser General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# sas_core is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU Lesser General Public License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with sas_core. If not, see . +# +# ################################################################ +# +# Author: Juan Jose Quiroz Omana, email: juanjose.quirozomana@manchester.ac.uk +# +# ################################################################*/ + +#pragma once +#include +#include +#include +#include +#include +#include + +namespace sas +{ + +class thread_manager +{ +public: + // PRIORITY levels + enum class PRIORITY { + LOWEST = 0, + BACKGROUND = 10, + NORMAL = 50, + HIGH = 80, + REALTIME = 90, + CRITICAL = 99 + }; + + + +private: + // Thread optimization parameters + PRIORITY priority_{PRIORITY::NORMAL}; + int cpu_core_{-1}; + void apply_priority(); + void apply_cpu_affinity(); + + +protected: + sas::Clock clock_; + std::function loop_callback_; + std::thread thread_; + std::string thread_name_; + std::atomic running_{false}; + std::atomic stop_requested_{false}; + +protected: + void run(); +public: + ~thread_manager(); + + thread_manager(const thread_manager&) = delete; // Copy constructor + thread_manager& operator=(const thread_manager&) = delete; // Copy assignment + thread_manager(thread_manager&&) = delete; // Move constructor + thread_manager& operator=(thread_manager&&) = delete; // Move assignment + + thread_manager(const std::string& thread_name, + const double& period, + std::function callback, + PRIORITY priority = PRIORITY::NORMAL, + int cpu_core = -1); + + + + // Performance monitoring using sas::Clock + double get_computation_time() const; + double get_sleep_time() const; + double get_effective_sampling_time() const; + double get_elapsed_time_sec() const; + long get_overrun_count() const; + + // Statistics from sas::Clock + double get_statistics(const Statistics& statistics, + const sas::Clock::TimeType& time_type) const; + + // Lifecycle + void start(); + void stop(); + bool is_running() const; + + + // Getters + std::string get_thread_name() const; + PRIORITY get_priority() const; + int get_cpu_core() const; + double get_period() const; +}; + +} + diff --git a/src/examples/thread_manager_example.cpp b/src/examples/thread_manager_example.cpp new file mode 100644 index 0000000..065b40b --- /dev/null +++ b/src/examples/thread_manager_example.cpp @@ -0,0 +1,45 @@ +/* +# Copyright (c) 2016-2026 Murilo Marques Marinho +# +# This file is part of sas_core. +# +# sas_core is free software: you can redistribute it and/or modify +# it under the terms of the GNU Lesser General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# sas_core is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU Lesser General Public License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with sas_core. If not, see . +# +# ################################################################ +# +# Author: Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) +# +# ################################################################*/ + +#include +#include +#include + +int main() { + int counter = 0; + + // Create and start thread + sas::thread_manager tm("test", 0.1, [&counter]() { + std::cout << ++counter << "\n"; + }); + + tm.start(); + + // Run for 1 second + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Clean up + tm.stop(); + return 0; +} diff --git a/src/sas_thread_manager.cpp b/src/sas_thread_manager.cpp new file mode 100644 index 0000000..2f95020 --- /dev/null +++ b/src/sas_thread_manager.cpp @@ -0,0 +1,437 @@ +/* +# Copyright (c) 2022-2026 Murilo Marques Marinho +# +# This file is part of sas_core. +# +# sas_core is free software: you can redistribute it and/or modify +# it under the terms of the GNU Lesser General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# sas_core is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU Lesser General Public License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with sas_core. If not, see . +# +# ################################################################ +# +# Author: Juan Jose Quiroz Omana, email: juanjose.quirozomana@manchester.ac.uk +# +# ################################################################*/ + +#include +#include +#include +#include + +#ifdef __linux__ +#include +#include +#include +#endif + +namespace sas +{ + +/** + * @brief Constructor for thread_manager + * + * Creates a thread manager that executes a callback function periodically + * with configurable priority and CPU affinity. + * + * @param thread_name Name of the thread for identification and debugging. + * On Linux, names are truncated to 15 characters. + * @param period Period in seconds between callback executions. + * Must be greater than 0.0. + * @param callback Function to call periodically. The callback should be + * thread-safe and should not block for extended periods + * to avoid missing deadlines. + * @param priority Thread priority level (default: PRIORITY::NORMAL). + * Higher priorities get more CPU time: + * - LOWEST (0): SCHED_OTHER, nice 19 + * - BACKGROUND (10): SCHED_OTHER, nice 10 + * - NORMAL (50): SCHED_OTHER, nice 0 + * - HIGH (80): SCHED_RR, priority 80 (requires root) + * - REALTIME (90): SCHED_FIFO, priority 90 (requires root) + * - CRITICAL (99): SCHED_FIFO, priority 99 (requires root) + * @param cpu_core CPU core to pin the thread to (-1 = no affinity, default). + * Pinning to a specific core improves cache locality and + * reduces context switching for real-time applications. + * + * @throws std::invalid_argument if period <= 0.0. + * + * @note Real-time priorities (HIGH, REALTIME, CRITICAL) require root + * privileges or CAP_SYS_NICE capability. + * + * @see PRIORITY + * @see start() + * @see apply_priority() + * @see apply_cpu_affinity() + * + * @code + * // Create a normal priority thread with no CPU affinity + * thread_manager worker("Worker", 0.01, []() { do_work(); }); + * + * // Create a real-time thread pinned to CPU core 2 + * thread_manager control( + * "Control", + * 0.001, + * []() { compute_control(); }, + * thread_manager::PRIORITY::CRITICAL, + * 2 + * ); + * @endcode + */ +thread_manager::thread_manager(const std::string& thread_name, + const double& period, + std::function callback, + PRIORITY priority, + int cpu_core) + :priority_{priority}, + cpu_core_{cpu_core}, + clock_{period}, + loop_callback_{std::move(callback)}, + thread_name_{thread_name} +{ + if (period <= 0.0) { + throw std::invalid_argument("thread_manager: period must be positive"); + } +} + +/** + * @brief Get the CPU core that this thread is pinned to. + * @return CPU core number, or -1 if no affinity is set. + */ +int thread_manager::get_cpu_core() const +{ + return cpu_core_; +} + +/** + * @brief Get the desired loop period (sampling time). + * @return Period in seconds. + * @details Returns the target period configured at construction time. + */ +double thread_manager::get_period() const +{ + return clock_.get_desired_thread_sampling_time_sec(); +} + +/** + * @brief Get the priority level of this thread. + * @return The PRIORITY enum value. + */ +thread_manager::PRIORITY thread_manager::get_priority() const +{ + return priority_; +} + +/** + * @brief thread_manager::~thread_manager destructor of the class + */ +thread_manager::~thread_manager() +{ + stop(); +} + + +/** + * @brief Start the thread execution + */ +void thread_manager::start() +{ + // Atomically set running_ to true and get the previous value. + if (running_.exchange(true)) { + return; // Already running - don't start another thread + } + + // If we get here, running_ was false before. We can safely start the thread. + + stop_requested_ = false; + clock_.init(); + thread_ = std::thread(&thread_manager::run, this); +} + +/** + * @brief Stop the thread execution and wait for completion + */ +void thread_manager::stop() +{ + if (!running_.exchange(false)) { + return; // Not running + } + + stop_requested_ = true; + if (thread_.joinable()) { + thread_.join(); + } +} + + +/** + * @brief Check if the thread is currently running + * @return true if running, false otherwise + */ +bool thread_manager::is_running() const +{ + // Returns the current value of running_ in thread-safe way. + return running_.load(); +} + + +/** + * @brief Get the thread name + * @return Thread name string + */ +std::string thread_manager::get_thread_name() const +{ + return thread_name_; +} + + +/** + * @brief Main thread entry function + */ +void thread_manager::run() +{ + // Apply thread optimizations + apply_priority(); + apply_cpu_affinity(); + + + // Set thread name (if supported by platform) +#ifdef __linux__ + // pthread_setname_np has a 16 character limit (including null terminator) + std::string truncated_name = thread_name_; + if (truncated_name.length() > 15) { + truncated_name.resize(15); + } + pthread_setname_np(pthread_self(), truncated_name.c_str()); +#endif + + // Main loop + while (!stop_requested_.load()) { + // Execute callback + if (loop_callback_) { + loop_callback_(); + } + clock_.update_and_sleep(); + } +} + +/** + * @brief Apply the configured priority and scheduling policy to the thread. + * + * This method sets the thread's scheduling priority and policy based on the + * PRIORITY enum value configured at construction time. It uses Linux's + * pthread_setschedparam API to apply the settings. + * + * @details The method maps the abstract PRIORITY levels to specific Linux + * scheduler policies and priority values: + * + * | PRIORITY Level | Linux Policy | Priority Value | Description | + * |------------------|--------------|----------------|-------------| + * | LOWEST | SCHED_OTHER | 0 (nice 19) | Low priority background tasks | + * | BACKGROUND | SCHED_OTHER | 0 (nice 10) | Background processing | + * | NORMAL | SCHED_OTHER | 0 (nice 0) | Standard scheduling | + * | HIGH | SCHED_RR | 80 | Round-robin real-time | + * | REALTIME | SCHED_FIFO | 90 | FIFO real-time | + * | CRITICAL | SCHED_FIFO | 99 | Highest priority (max) | + * + * @note This method is only available on Linux systems. + * @note Real-time policies (SCHED_RR and SCHED_FIFO) require root privileges + * or the CAP_SYS_NICE capability. + * @note This method should be called from within the thread context (after + * the thread has started). + * + * @warning Real-time priorities (HIGH, REALTIME, CRITICAL) can starve other + * threads if not used carefully. Only use them for time-critical tasks. + * @warning This method does NOT throw exceptions on failure; it only prints + * a warning to std::cerr. + * + * @see PRIORITY - Enum definition for priority levels + * @see apply_cpu_affinity() - Related thread optimization method + * @see start() - The method that triggers the priority application + * + * @code + * // Create a critical real-time thread + * thread_manager control( + * "Control", + * 0.001, + * []() { compute_control(); }, + * PRIORITY::CRITICAL, // Will use SCHED_FIFO with priority 99 + * 2 // Pin to core 2 + * ); + * control.start(); // Priority and affinity are applied in run() + * @endcode + */ +void thread_manager::apply_priority() { +#ifdef __linux__ + pthread_t thread_handle = thread_.native_handle(); + struct sched_param param = {0}; + int policy; + + // Map to Linux scheduling + switch (priority_) { + case PRIORITY::LOWEST: + policy = SCHED_OTHER; + param.sched_priority = 0; + setpriority(PRIO_PROCESS, pthread_self(), 19); + break; + case PRIORITY::BACKGROUND: + policy = SCHED_OTHER; + param.sched_priority = 0; + setpriority(PRIO_PROCESS, pthread_self(), 10); + break; + case PRIORITY::NORMAL: + policy = SCHED_OTHER; + param.sched_priority = 0; + setpriority(PRIO_PROCESS, pthread_self(), 0); + break; + case PRIORITY::HIGH: + policy = SCHED_RR; + param.sched_priority = 80; + break; + case PRIORITY::REALTIME: + policy = SCHED_FIFO; + param.sched_priority = 90; + break; + case PRIORITY::CRITICAL: + policy = SCHED_FIFO; + param.sched_priority = 99; + break; + } + + if (pthread_setschedparam(thread_handle, policy, ¶m) != 0) { + std::cerr << "Warning: Failed to set priority for thread '" + << thread_name_ << "'" << std::endl; + } +#endif +} + + +/** + * @brief Apply CPU core affinity to the thread. + * + * This method pins the thread to a specific CPU core to improve cache locality, + * reduce context switching, and ensure predictable performance for real-time + * applications. + * + * @details The method uses the Linux pthread_setaffinity_np API to bind the + * thread to the CPU core specified by cpu_core_. This is particularly + * important for: + * - Real-time control loops that need deterministic timing + * - High-frequency communication threads (UDP, CAN, etc.) + * - Threads that benefit from dedicated cache resources + * + * @note This method is only available on Linux systems. + * @note If cpu_core_ is negative (< 0), affinity is not applied and the + * thread can run on any available CPU core. + * @note This method should be called from within the thread context (after + * the thread has started). + * + * @warning Setting CPU affinity can degrade performance if the specified core + * becomes overloaded. Use with caution in multi-threaded systems. + * @warning Thread must be joinable and running when this method is called. + * + * @see set_cpu_affinity() - Setter for the cpu_core_ member + * @see apply_priority() - Related thread optimization method + * + * @code + * // Pin control thread to CPU core 2 + * thread_manager control("Control", 0.001, []() { + * compute_control(); + * }, PRIORITY::REALTIME, 2); + * + * // Thread will be pinned to core 2 when start() is called + * control.start(); + * @endcode + */ +void thread_manager::apply_cpu_affinity() { +#ifdef __linux__ + if (cpu_core_ < 0) return; + + cpu_set_t cpuset; + CPU_ZERO(&cpuset); + CPU_SET(cpu_core_, &cpuset); + + pthread_t thread_handle = thread_.native_handle(); + if (pthread_setaffinity_np(thread_handle, sizeof(cpu_set_t), &cpuset) != 0) { + std::cerr << "Warning: Failed to set CPU affinity for thread '" + << thread_name_ << "' to core " << cpu_core_ << std::endl; + } +#endif +} + + + +/** + * @brief Get the computation time of the most recent loop iteration. + * @return Computation time in seconds. + * @details Measures the time spent executing the callback function + * (between sleep end and next sleep start). + */ +double thread_manager::get_computation_time() const +{ + return clock_.get_time(sas::Clock::TimeType::Computational); +} + +/** + * @brief Get the sleep/idle time of the most recent loop iteration. + * @return Sleep time in seconds. + * @details Measures the time spent sleeping between callback executions. + */ +double thread_manager::get_sleep_time() const +{ + return clock_.get_time(sas::Clock::TimeType::Idle); +} + +/** + * @brief Get the effective sampling time of the most recent loop iteration. + * @return Effective sampling time in seconds. + * @details Total elapsed time from one loop iteration to the next + * (computation_time + sleep_time). + */ +double thread_manager::get_effective_sampling_time() const +{ + return clock_.get_time(sas::Clock::TimeType::EffectiveSampling); +} + +/** + * @brief Get the total elapsed time since clock initialization. + * @return Elapsed time in seconds. + */ +double thread_manager::get_elapsed_time_sec() const +{ + return clock_.get_elapsed_time_sec(); +} + +/** + * @brief Get the number of times the loop has overrun its target period. + * @return Overrun count. + * @details Incremented when the loop takes longer than the configured period. + */ +long thread_manager::get_overrun_count() const +{ + return clock_.get_overrun_count(); +} + +/** + * @brief Get statistical metrics for a specific time type. + * @param statistics The statistic to retrieve (e.g., Mean, Max, Min, StdDev). + * @param time_type The time type to analyze (Computational, Idle, EffectiveSampling). + * @return The requested statistic value in seconds. + * @throws std::runtime_error if statistics were not enabled at construction. + */ +double thread_manager::get_statistics(const Statistics& statistics, + const sas::Clock::TimeType& time_type) const +{ + return clock_.get_statistics(statistics, time_type); +} + + + +}