From 0d429af699500d9036ba235767eed025734edfcb Mon Sep 17 00:00:00 2001 From: srvald Date: Wed, 1 Jul 2026 10:14:16 +0200 Subject: [PATCH 01/25] feat: add real-time helper functions for windows --- include/ur_client_library/helpers.h | 89 +++++++- src/helpers.cpp | 312 +++++++++++++++++++++++++++- 2 files changed, 398 insertions(+), 3 deletions(-) diff --git a/include/ur_client_library/helpers.h b/include/ur_client_library/helpers.h index f0af4e248..64f0b1531 100644 --- a/include/ur_client_library/helpers.h +++ b/include/ur_client_library/helpers.h @@ -48,12 +48,18 @@ # define SCHED_FIFO (1) typedef HANDLE pthread_t; +typedef HANDLE pprocess_t; static inline pthread_t pthread_self() { return ::GetCurrentThread(); } +static inline pprocess_t pprocess_self() +{ + return ::GetCurrentProcess(); +} + static inline int sched_get_priority_max(int policy) { (void)policy; @@ -96,7 +102,88 @@ static inline int sched_get_priority_max(int policy) namespace urcl { -bool setFiFoScheduling(pthread_t& thread, const int priority); + +#ifdef _WIN32 + +/*! + * \brief Set the priority class of a process. + * + * Wraps the corresponding operating system API and verifies that the requested + * priority class has been applied successfully. + * + * \param process Process handle. + * \param priority Windows process priority class. + * + * \returns True if the priority class was applied successfully. + */ +bool setProcessPriority(pprocess_t& process, DWORD priority); + +/*! + * \brief Restrict a process to a set of CPUs. + * + * Wraps the corresponding operating system API and verifies that the requested + * CPU affinity mask has been applied successfully. + * + * \param process Process handle. + * \param cpu_mask Bit mask describing the CPUs available to the process. + * + * \returns True if the affinity mask was applied successfully. + */ +bool setProcessAffinity(pprocess_t& process, DWORD_PTR cpu_mask); + +/*! + * \brief Restrict a thread to a set of CPUs. + * + * \param thread Thread handle. + * \param cpu_mask Bit mask describing the CPUs available to the thread. + * + * \returns True if the affinity mask was applied successfully. + */ +bool setThreadAffinity(pthread_t& thread, DWORD_PTR cpu_mask); + +/*! + * \brief Set the scheduling priority of a thread. + * + * Wraps the corresponding operating system API and verifies that the requested + * priority has been applied successfully. + * + * \param thread Thread handle. + * \param priority Windows thread priority value. + * + * \returns True if the priority was applied successfully. + */ +bool setThreadPriority(pthread_t& thread, const int priority); + +#else + +/*! + * \brief Restrict a thread to a set of CPUs. + * + * \param thread Thread handle. + * \param cpuset Set of CPUs available to the thread. + * + * \returns True if the CPU affinity was applied successfully. + */ +bool setThreadAffinity(pthread_t& thread, const cpu_set_t& cpuset); + +#endif + +/*! + * \brief Configure high-priority scheduling for a thread. + * + * On Linux, this configures ``SCHED_FIFO`` scheduling using the supplied + * priority. + * + * On Windows, this configures the process to use + * ``REALTIME_PRIORITY_CLASS`` and sets the thread to the supplied Windows + * thread priority. + * + * \param thread Thread handle. + * \param priority Scheduling priority to apply. + * + * \returns True if the scheduling configuration was applied successfully. + */ +bool setFiFoScheduling(pthread_t& thread, int priority); /*! * \brief Wait for a condition to be true. diff --git a/src/helpers.cpp b/src/helpers.cpp index 4535c6d3b..ad9163058 100644 --- a/src/helpers.cpp +++ b/src/helpers.cpp @@ -25,6 +25,7 @@ * */ //---------------------------------------------------------------------- +#define _GNU_SOURCE #include #include @@ -38,6 +39,8 @@ #include #include #include +#include + // clang-format off // We want to keep the URL in one line to avoid formatting issues. This will make it easier to @@ -47,10 +50,315 @@ const std::string RT_DOC_URL = "https://docs.universal-robots.com/Universal_Robo namespace urcl { -bool setFiFoScheduling(pthread_t& thread, const int priority) + +#ifdef _WIN32 + +const char* processPriorityToString(DWORD priority) +{ + switch (priority) + { + case IDLE_PRIORITY_CLASS: return "IDLE"; + case BELOW_NORMAL_PRIORITY_CLASS: return "BELOW_NORMAL"; + case NORMAL_PRIORITY_CLASS: return "NORMAL"; + case ABOVE_NORMAL_PRIORITY_CLASS: return "ABOVE_NORMAL"; + case HIGH_PRIORITY_CLASS: return "HIGH"; + case REALTIME_PRIORITY_CLASS: return "REALTIME"; + default: return "UNKNOWN"; + } +} + +const char* threadPriorityToString(int priority) +{ + switch (priority) + { + case THREAD_PRIORITY_LOWEST: return "LOWEST"; + case THREAD_PRIORITY_BELOW_NORMAL: return "BELOW_NORMAL"; + case THREAD_PRIORITY_NORMAL: return "NORMAL"; + case THREAD_PRIORITY_ABOVE_NORMAL: return "ABOVE_NORMAL"; + case THREAD_PRIORITY_HIGHEST: return "HIGHEST"; + case THREAD_PRIORITY_TIME_CRITICAL: return "TIME_CRITICAL"; + default: return "UNKNOWN"; + } +} + +bool isProcessElevated() { + bool isElevated = false; + HANDLE hToken = nullptr; + if(OpenProcessToken(GetCurrentProcess(),TOKEN_QUERY,&hToken)){ + TOKEN_ELEVATION elevation; + DWORD dwSize = sizeof(elevation); + if(GetTokenInformation(hToken, TokenElevation, &elevation, sizeof(elevation), &dwSize)){ + isElevated = elevation.TokenIsElevated; + } + } + if(hToken) { + CloseHandle(hToken); + } + return isElevated; +} + +std::string maskToCpuList(uint64_t mask) +{ + std::string out = "["; + for (int i = 0; i < 64; ++i) + { + if (mask & (1ULL << i)) + { + if (out.size() > 1) out += ","; + out += std::to_string(i); + } + } + out += "]"; + return out; + +} + +std::string getLastWindowsErrorMsg(DWORD error_id) +{ + LPSTR buffer = nullptr; + + DWORD size = FormatMessageA( + FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS, + nullptr, + error_id, + MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), + (LPSTR)&buffer, + 0, + nullptr); + + if (size == 0 || buffer == nullptr) + { + return "Unknown Windows error"; + } + + std::string message(buffer, size); + LocalFree(buffer); + + return message; +}; + +bool setProcessPriority(pprocess_t& process, DWORD priority) +{ + if (!::SetPriorityClass(process, priority)) + { + DWORD err = GetLastError(); + URCL_LOG_ERROR("Unsuccessful in setting the process priority to %s (%llX). Error: %lu (%s)", processPriorityToString(priority), priority, err, getLastWindowsErrorMsg(err).c_str()); + return false; + } + + DWORD priority_applied = ::GetPriorityClass(process); + if (priority_applied == 0){ + DWORD err = GetLastError(); + URCL_LOG_ERROR("Unsuccessful in retrieving the process priority for verification. Error: %lu (%s)", err, getLastWindowsErrorMsg(err).c_str()); + return false; + } + + URCL_LOG_INFO("Process priority successfully set to %s (0x%X)", processPriorityToString(priority_applied), priority_applied); + + if (priority_applied != priority) + { + URCL_LOG_WARN("Process priority mismatch. Expected %s (0x%X), got %s (0x%X)", + processPriorityToString(priority), priority, + processPriorityToString(priority_applied), priority_applied); + return false; + } + + return true; +} + +bool setProcessAffinity(pprocess_t& process, DWORD_PTR cpu_mask) +{ + if (!::SetProcessAffinityMask(process, cpu_mask)) + { + DWORD err = GetLastError(); + URCL_LOG_ERROR("Unsuccessful in setting process affinity to CPUs %s (mask=0x%llX). Error: %lu (%s)", + maskToCpuList(cpu_mask).c_str(), + static_cast(cpu_mask), + err, + getLastWindowsErrorMsg(err).c_str()); + + return false; + } + DWORD_PTR process_mask = 0; + DWORD_PTR system_mask = 0; + if (!::GetProcessAffinityMask(process, &process_mask, &system_mask)) + { + DWORD err = GetLastError(); + URCL_LOG_ERROR("Unsuccessful in setting process affinity to %s. Error: %lu (%s)", + maskToCpuList(cpu_mask).c_str(), + err, getLastWindowsErrorMsg(err).c_str()); + return false; + } + URCL_LOG_INFO("Process affinity set to CPUs %s (mask=0x%llX)", + maskToCpuList(process_mask).c_str(), + static_cast(process_mask)); + if (process_mask != cpu_mask) + { + URCL_LOG_WARN("Process affinity mismatch. Expected %s, got %s", + maskToCpuList(cpu_mask).c_str(), + maskToCpuList(process_mask).c_str()); + return false; + } + return true; +} + +bool setThreadAffinity(pthread_t& thread, DWORD_PTR cpu_mask) +{ + DWORD_PTR result = ::SetThreadAffinityMask(thread, cpu_mask); + + if (result == 0) + { + DWORD err = GetLastError(); + URCL_LOG_ERROR("Unsuccessful in setting thread affinity to %s. Error: %lu (%s)", + maskToCpuList(cpu_mask).c_str(), + err, getLastWindowsErrorMsg(err).c_str()); + return false; + } + + URCL_LOG_INFO("Thread affinity successfully set to CPUs %s (mask=0x%llX)", + maskToCpuList(cpu_mask).c_str(), + static_cast(cpu_mask)); + return true; +} + +bool setThreadPriority(pthread_t& thread, const int priority){ + if (!::SetThreadPriority(thread, priority)) + { + DWORD err = GetLastError(); + URCL_LOG_ERROR("Unsuccessful in setting thread priority to %s (%d). Error: %lu (%s)", + threadPriorityToString(priority), priority, + err, getLastWindowsErrorMsg(err).c_str()); + + return false; + } + + int applied = ::GetThreadPriority(thread); + if (applied == THREAD_PRIORITY_ERROR_RETURN){ + DWORD err = GetLastError(); + URCL_LOG_ERROR("Unsuccessful in retrieving the thread priority for verification. Error: %lu (%s)", err, getLastWindowsErrorMsg(err).c_str()); + return false; + } + + URCL_LOG_INFO("Thread priority successfully set to %s (%d)", + threadPriorityToString(applied), applied); + + if (applied != priority) + { + URCL_LOG_WARN("Thread priority mismatch. Expected %s (%d), got %s (%d)", + threadPriorityToString(priority), priority, + threadPriorityToString(applied), applied); + return false; + } + + return true; +} + +#else + +std::string cpuSetToString(const cpu_set_t& cpuset) +{ + std::string out = "["; + + for (int i = 0; i < CPU_SETSIZE; ++i) + { + if (CPU_ISSET(i, &cpuset)) + { + if (out.size() > 1) + { + out += ","; + } + out += std::to_string(i); + } + } + + out += "]"; + return out; +} + +bool setThreadAffinity(pthread_t thread, const cpu_set_t& cpuset) +{ + int ret = pthread_setaffinity_np(thread, sizeof(cpu_set_t), &cpuset); + + if (ret != 0) + { + URCL_LOG_ERROR( + "Unsuccessful in setting thread affinity. Error: %s", + strerror(ret)); + return false; + } + + cpu_set_t applied_set; + CPU_ZERO(&applied_set); + + if (pthread_getaffinity_np( + thread, + sizeof(cpu_set_t), + &applied_set) == 0) + { + bool match = true; + + for (int i = 0; i < CPU_SETSIZE; ++i) + { + if (CPU_ISSET(i, &cpuset) != CPU_ISSET(i, &applied_set)) + { + match = false; + break; + } + } + + if (!match) + { + URCL_LOG_WARN( + "Thread affinity mismatch. Requested %s, got %s", + cpuSetToString(cpuset).c_str(), + cpuSetToString(applied_set).c_str()); + + return false; + } + } + else + { + URCL_LOG_WARN("Could not retrieve thread affinity"); + } + + URCL_LOG_INFO( + "Thread affinity successfully set to CPUs %s", + cpuSetToString(applied_set).c_str()); + + return true; +} + +#endif + +bool setFiFoScheduling(pthread_t& thread, int priority) { + #ifdef _WIN32 - return ::SetThreadPriority(thread, priority); + + if (!isProcessElevated()) + { + URCL_LOG_WARN( + "Process is not running with elevated privileges (UAC). " + "REALTIME_PRIORITY_CLASS may fail. Try 'Run as Administrator'."); + } + pprocess_t process = ::GetCurrentProcess(); + + if (!setProcessPriority(process, REALTIME_PRIORITY_CLASS)) + { + URCL_LOG_ERROR("Unsuccessful in setting process to REALTIME_PRIORITY_CLASS"); + return false; + } + + if (!setThreadPriority(thread, priority)) + { + URCL_LOG_ERROR("Unsuccessful in setting thread priority to %s (%d)", + threadPriorityToString(priority), + priority); + return false; + } + + return true; + #else // _WIN32 struct sched_param params; params.sched_priority = priority; From 551a81ebc9d6b8db54c1783467b7b11825557b93 Mon Sep 17 00:00:00 2001 From: srvald Date: Wed, 1 Jul 2026 10:18:20 +0200 Subject: [PATCH 02/25] docs: add Windows soft real-time configuration guide and helper functions documentation. --- doc/real_time.rst | 165 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 165 insertions(+) diff --git a/doc/real_time.rst b/doc/real_time.rst index 7379f1bb2..7e881ec9d 100644 --- a/doc/real_time.rst +++ b/doc/real_time.rst @@ -334,3 +334,168 @@ the ``cpufrequtils`` service. Check with ``cpufreq-info``. For further information about governors, please see the `kernel documentation `_. + +Windows soft real-time configuration +------------------------------------ + +Unlike Linux systems running a PREEMPT_RT kernel with ``SCHED_FIFO`` scheduling, Windows does not +provide the same level of scheduling behaviour. However, it can be configured for +soft real-time, where scheduling jitter is reduced and timing predictability is improved. + +According to Microsoft, hard real-time systems require deterministic execution at precise points in +time, whereas soft real-time systems tolerate a small amount of scheduling jitter and execution +latency. The real-time capabilities available on Windows fall into the latter category. + +For additional details, refer to Microsoft's documentation: +`Soft Real-Time `_. + +Preparing the system +^^^^^^^^^^^^^^^^^^^^ + +Microsoft provides guidance for configuring Windows 10 and Windows 11 systems for +soft real-time performance. The recommended device configuration includes techniques such as: + +* CPU isolation +* Routing interrupts away from real-time cores +* Disabling selected background services +* Disabling CPU idle states +* Configuring dedicated real-time CPU cores + +For detailed device configuration instructions, see `Device Configuration `_. + +Process and thread configuration +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +After the system has been configured, applications can improve scheduling behaviour by assigning +high priorities and CPU affinities to the processes and threads involved during execution. + +Microsoft recommends using: + +* ``SetPriorityClass()`` with ``REALTIME_PRIORITY_CLASS`` +* ``SetProcessAffinityMask()`` +* ``SetThreadPriority()`` with ``THREAD_PRIORITY_TIME_CRITICAL`` +* ``SetThreadAffinityMask()`` + +For detailed information about configuring real-time processes and threads on Windows, +see `Application Development `_. + +Scheduling priorities +^^^^^^^^^^^^^^^^^^^^^ + +Windows schedules threads using a combination of process priority class and thread priority level. + +The highest priority range is obtained by combining ``REALTIME_PRIORITY_CLASS`` and ``THREAD_PRIORITY_TIME_CRITICAL`` +which results in a base thread priority of 31, the highest priority available. + +For more information about Windows scheduling priorities, see `Scheduling Priorities `_. + +.. warning:: + + Microsoft recommends using ``REALTIME_PRIORITY_CLASS`` with care. Threads running at high + priorities for extended periods can negatively affect other system services and user interaction. + +Using the helper functions +^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The Universal Robots Client Library provides helper functions for configuring CPU affinity and +scheduling priorities. + +Current process and thread handles can be obtained using: + +.. code-block:: cpp + + pprocess_t process = pprocess_self(); + pthread_t thread = pthread_self(); + +Linux +~~~~~ + +On Linux, the helper functions can be used to assign CPU affinity and configure +``SCHED_FIFO`` scheduling for a thread. + +.. code-block:: cpp + + pthread_t thread = pthread_self(); + + cpu_set_t cpuset; + CPU_ZERO(&cpuset); + CPU_SET(7, &cpuset); + + setThreadAffinity(thread, cpuset); + + int max_prio = sched_get_priority_max(SCHED_FIFO); + setFiFoScheduling(thread, max_prio); + +The configured CPU affinity applies to the selected thread. The scheduling +priority is configured using ``SCHED_FIFO``. + +Windows +~~~~~~~ + +On Windows, additional helper functions are available for configuring process +priority, process affinity, thread priority and thread affinity. + +.. code-block:: cpp + + pprocess_t process = pprocess_self(); + pthread_t thread = pthread_self(); + + DWORD_PTR process_mask = (1ULL << 6) | (1ULL << 7); + setProcessAffinity(process, process_mask); + + DWORD_PTR thread_mask = (1ULL << 7); + setThreadAffinity(thread, thread_mask); + + int max_prio = sched_get_priority_max(SCHED_FIFO); + setFiFoScheduling(thread, max_prio); + + // Equivalent explicit calls + setProcessPriority(process, REALTIME_PRIORITY_CLASS); + setThreadPriority(thread, THREAD_PRIORITY_TIME_CRITICAL); + +On Windows, ``setFiFoScheduling()`` configures the process to use +``REALTIME_PRIORITY_CLASS`` and applies the requested thread priority. + +Available helper functions +~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Windows: + +.. code-block:: cpp + + bool setProcessPriority(pprocess_t& process, DWORD priority); + bool setProcessAffinity(pprocess_t& process, DWORD_PTR cpu_mask); + bool setThreadPriority(pthread_t& thread, int priority); + bool setThreadAffinity(pthread_t& thread, DWORD_PTR cpu_mask); + bool setFiFoScheduling(pthread_t& thread, int priority); + +Linux: + +.. code-block:: cpp + + bool setThreadAffinity(pthread_t& thread, const cpu_set_t& cpuset); + bool setFiFoScheduling(pthread_t& thread, int priority); + +Administrative privileges +^^^^^^^^^^^^^^^^^^^^^^^^^ + +Using ``REALTIME_PRIORITY_CLASS`` may require elevated privileges depending on the system +configuration. + +When using the real-time helper functions, it is therefore recommended to run the application with +administrative privileges. + +The helper functions verify whether the process is running with elevated privileges and will issue a +warning if this is not the case. + +If sufficient privileges are not available, Windows may reject requests to enter the real-time +priority class. + +Example usage +^^^^^^^^^^^^^ + +A complete example demonstrating how to configure process priority, thread priority and CPU affinity +using the helper functions is provided in the RTDE Client example (:ref:`rtde_client_example`). + +Refer to this example for recommended usage patterns and platform-specific recommendations. + From 54f25d01f267a1c448363b01c35c2d63cc511551 Mon Sep 17 00:00:00 2001 From: srvald Date: Wed, 1 Jul 2026 10:19:14 +0200 Subject: [PATCH 03/25] docs: helper functions example --- doc/examples/rtde_client.rst | 34 ++++++++++++++++++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/doc/examples/rtde_client.rst b/doc/examples/rtde_client.rst index 5339e7198..d84bcd55d 100644 --- a/doc/examples/rtde_client.rst +++ b/doc/examples/rtde_client.rst @@ -25,6 +25,40 @@ to initialize the RTDE client. :start-at: const std::string OUTPUT_RECIPE :end-at: const std::string INPUT_RECIPE +Configuring real-time scheduling +-------------------------------- + +To reduce scheduling jitter, the example configures CPU affinity and scheduling priorities before +starting RTDE communication. + +The Universal Robots Client Library provides helper functions that simplify configuring these +settings on both Linux and Windows. + +.. literalinclude:: ../../examples/rtde_client.cpp + :language: c++ + :caption: examples/rtde_client.cpp + :linenos: + :lineno-match: + :start-at: pprocess_t process = pprocess_self(); + :end-at: URCL_LOG_ERROR("Failed to set FIFO scheduling"); + +The example assigns dedicated CPU cores and configures the highest available scheduling priority +for the current thread. + +On Linux, ``setFiFoScheduling()`` configures ``SCHED_FIFO`` scheduling. + +On Windows, ``setFiFoScheduling()`` configures the process to use +``REALTIME_PRIORITY_CLASS`` and the thread to use +``THREAD_PRIORITY_TIME_CRITICAL``. + +For a detailed explanation of the available helper functions and platform-specific real-time +configuration recommendations, see :ref:`real time setup`. + +.. note:: + + The selected CPU indices are only examples. Applications should choose CPU affinities according + to the available hardware and system configuration. + Creating an RTDE Client ----------------------- From bcf53e18cb8094cfa2d6455194eb50a2f53ebc1c Mon Sep 17 00:00:00 2001 From: srvald Date: Wed, 1 Jul 2026 10:20:01 +0200 Subject: [PATCH 04/25] test: add unit tests for scheduling and affinity helpers. --- tests/test_helpers.cpp | 217 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 217 insertions(+) diff --git a/tests/test_helpers.cpp b/tests/test_helpers.cpp index 3499b53c2..a2b5ab071 100644 --- a/tests/test_helpers.cpp +++ b/tests/test_helpers.cpp @@ -145,3 +145,220 @@ TEST(TestHelpers, robotSeriesString) EXPECT_EQ(robotSeriesString(RobotSeries::UR_SERIES), "UR_SERIES"); EXPECT_EQ(robotSeriesString(RobotSeries::UNDEFINED), "UNDEFINED"); } + +// Thread Affinity Tests + +TEST(TestHelpers, setThreadAffinity_basic) +{ + pthread_t thread = pthread_self(); +#ifdef _WIN32 + DWORD_PTR mask = (1ULL << 0); + EXPECT_TRUE(setThreadAffinity(thread, mask)); +#else + cpu_set_t cpuset; + CPU_ZERO(&cpuset); + CPU_SET(0, &cpuset); + EXPECT_TRUE(setThreadAffinity(thread, cpuset)); +#endif +} + +TEST(TestHelpers, setThreadAffinity_mult) +{ + pthread_t thread = pthread_self(); +#ifdef _WIN32 + DWORD_PTR mask = (1ULL << 6) | (1ULL << 7); + EXPECT_TRUE(setThreadAffinity(thread, mask)); +#else + cpu_set_t cpuset; + CPU_ZERO(&cpuset); + CPU_SET(6, &cpuset); + CPU_SET(7, &cpuset); + EXPECT_TRUE(setThreadAffinity(thread, cpuset)); +#endif +} + +TEST(TestHelpers, setThreadAffinity_invalid) +{ +#ifdef _WIN32 + pthread_t thread = pthread_self(); + uint64_t invalid_mask = 1ULL << 63; + EXPECT_FALSE(setThreadAffinity(thread, invalid_mask)); +#else + cpu_set_t cpuset; + CPU_ZERO(&cpuset); + CPU_SET(63, &cpuset); + EXPECT_TRUE(setThreadAffinity(thread, cpuset)); +#endif +} + +TEST(TestHelpers, setThreadAffinity_invalidHandle) +{ +#ifdef _WIN32 + pthread_t thread = nullptr; + EXPECT_FALSE(setThreadAffinity(thread, (1ULL << 0))); +#else + pthread_t thread{}; + cpu_set_t cpuset; + CPU_ZERO(&cpuset); + CPU_SET(0, &cpuset); + EXPECT_FALSE(setThreadAffinity(thread, cpuset)); +#endif +} + +TEST(TestHelpers, setThreadAffinity_empty) +{ +#ifdef _WIN32 + pthread_t thread = pthread_self(); + EXPECT_FALSE(setThreadAffinity(thread, 0)); +#else + pthread_t thread = pthread_self(); + cpu_set_t cpuset; + CPU_ZERO(&cpuset); + EXPECT_FALSE(setThreadAffinity(thread, cpuset)); +#endif + +} + +// Thread Priority Tests + +TEST(TestHelpers, setThreadPriority_allValidPriorities) +{ +#ifdef _WIN32 + pthread_t thread = pthread_self(); + + EXPECT_TRUE(setThreadPriority(thread, THREAD_PRIORITY_LOWEST)); + EXPECT_TRUE(setThreadPriority(thread, THREAD_PRIORITY_BELOW_NORMAL)); + EXPECT_TRUE(setThreadPriority(thread, THREAD_PRIORITY_NORMAL)); + EXPECT_TRUE(setThreadPriority(thread, THREAD_PRIORITY_ABOVE_NORMAL)); + EXPECT_TRUE(setThreadPriority(thread, THREAD_PRIORITY_HIGHEST)); + EXPECT_TRUE(setThreadPriority(thread, THREAD_PRIORITY_TIME_CRITICAL)); +#endif +} + +TEST(TestHelpers, setThreadPriority_invalid) +{ +#ifdef _WIN32 + pthread_t thread = pthread_self(); + EXPECT_FALSE(setThreadPriority(thread, 999999)); +#endif +} + +TEST(TestHelpers, setThreadPriority_invalidHandle) +{ +#ifdef _WIN32 + pthread_t thread = nullptr; + EXPECT_FALSE(setThreadPriority(thread, THREAD_PRIORITY_HIGHEST)); +#endif +} + +// Process Priority tests + +TEST(TestHelpers, setProcessPriority_allValidPriorities) +{ +#ifdef _WIN32 + pprocess_t process = pprocess_self(); + + EXPECT_TRUE(setProcessPriority(process, IDLE_PRIORITY_CLASS)); + EXPECT_TRUE(setProcessPriority(process, BELOW_NORMAL_PRIORITY_CLASS)); + EXPECT_TRUE(setProcessPriority(process, NORMAL_PRIORITY_CLASS)); + EXPECT_TRUE(setProcessPriority(process, ABOVE_NORMAL_PRIORITY_CLASS)); + EXPECT_TRUE(setProcessPriority(process, HIGH_PRIORITY_CLASS)); +#endif +} + +TEST(TestHelpers, setProcessPriority_invalid) +{ +#ifdef _WIN32 + pprocess_t process = pprocess_self(); + EXPECT_FALSE(setProcessPriority(process, 999999)); +#endif +} + +TEST(TestHelpers, setProcessPriority_invalidHandle) +{ +#ifdef _WIN32 + pprocess_t process = nullptr; + EXPECT_FALSE(setProcessPriority(process, NORMAL_PRIORITY_CLASS)); +#endif +} + +// Process Affinity Tests + +TEST(TestHelpers, setProcessAffinity_basic) +{ +#ifdef _WIN32 + pprocess_t process = pprocess_self(); + DWORD_PTR mask = (1ULL << 0); + EXPECT_TRUE(setProcessAffinity(process, mask)); +#endif +} + +TEST(TestHelpers, setProcessAffinity_multi) +{ +#ifdef _WIN32 + pprocess_t process = pprocess_self(); + DWORD_PTR mask = (1ULL << 0) | (1ULL << 1); + EXPECT_TRUE(setProcessAffinity(process, mask)); +#endif +} + +TEST(TestHelpers, setProcessAffinity_invalidHandle) +{ +#ifdef _WIN32 + pprocess_t process = nullptr; + EXPECT_FALSE(setProcessAffinity(process, (1ULL << 0))); +#endif +} + +TEST(TestHelpers, setProcessAffinity_zeroMask) +{ +#ifdef _WIN32 + pprocess_t process = pprocess_self(); + EXPECT_FALSE(setProcessAffinity(process,0)); +#endif +} + +TEST(TestHelpers, setProcessAffinity_invalidMask) +{ +#ifdef _WIN32 + pprocess_t process = pprocess_self(); + DWORD_PTR mask = 0xFFFFFFFFFFFFFFFFULL; + EXPECT_FALSE(setProcessAffinity(process, mask)); +#endif +} + +// FIFO Scheduling Tests + +TEST(TestHelpers, setFiFoScheduling_normal) +{ + pthread_t thread = pthread_self(); +#ifdef _WIN32 + EXPECT_TRUE(setFiFoScheduling(thread, THREAD_PRIORITY_NORMAL)); +#else + EXPECT_TRUE(setFiFoScheduling(thread, 50)); +#endif +} + +TEST(TestHelpers, setFiFoScheduling_invalidPriority) +{ + pthread_t thread = pthread_self(); + EXPECT_FALSE(setFiFoScheduling(thread, 999)); +} + +TEST(TestHelpers, setFiFoScheduling_invalidHandle) +{ +#ifdef _WIN32 + pthread_t thread = nullptr; + EXPECT_FALSE(setFiFoScheduling(thread, THREAD_PRIORITY_NORMAL)); +#else + pthread_t thread{}; + EXPECT_FALSE(setFiFoScheduling(thread, 50)); +#endif +} + +TEST(TestHelpers, setFiFoScheduling_maxPriority) +{ + pthread_t thread = pthread_self(); + int max_prio = sched_get_priority_max(SCHED_FIFO); + EXPECT_TRUE(setFiFoScheduling(thread, max_prio)); +} From 0eb997d5686b4f4fe6cfb897ebfc30091a06d3e6 Mon Sep 17 00:00:00 2001 From: srvald Date: Wed, 1 Jul 2026 10:21:23 +0200 Subject: [PATCH 05/25] docs: show scheduling helper usage in RTDE example --- examples/rtde_client.cpp | 468 +++++++++++++++++++++++++++++++++------ 1 file changed, 402 insertions(+), 66 deletions(-) diff --git a/examples/rtde_client.cpp b/examples/rtde_client.cpp index d92605c5a..7e942bf85 100644 --- a/examples/rtde_client.cpp +++ b/examples/rtde_client.cpp @@ -26,121 +26,457 @@ //---------------------------------------------------------------------- #include - +#include +#include #include #include +#include #include -#include - +#include +#include + +#ifdef _WIN32 +#include +#endif + using namespace urcl; + +const std::string DEFAULT_ROBOT_IP = "10.54.5.169"; +const std::string OUTPUT_RECIPE = "C:/Users/MIRserv/Desktop/ur_client_library_felix/Universal_Robots_Client_Library/examples/resources/rtde_output_recipe.txt"; +const std::string INPUT_RECIPE = "C:/Users/MIRserv/Desktop/ur_client_library_felix/Universal_Robots_Client_Library/examples/resources/rtde_input_recipe.txt"; + -// In a real-world example it would be better to get those values from command line parameters / a better configuration -// system such as Boost.Program_options -const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; -const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; -const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; +const std::string KEY_TIMESTAMP = "timestamp"; +const std::string KEY_TARGET_Q = "target_q"; +const std::string KEY_TARGET_QD = "target_qd"; +const std::string KEY_TARGET_QDD = "target_qdd"; +const std::string KEY_TARGET_CURRENT = "target_current"; +const std::string KEY_TARGET_MOMENT = "target_moment"; +const std::string KEY_ACTUAL_Q = "actual_q"; +const std::string KEY_ACTUAL_QD = "actual_qd"; +const std::string KEY_ACTUAL_CURRENT = "actual_current"; +const std::string KEY_ACTUAL_CURRENT_WINDOW = "actual_current_window"; +const std::string KEY_ACTUAL_CURRENT_AS_TORQUE = "actual_current_as_torque"; +const std::string KEY_JOINT_CONTROL_OUTPUT = "joint_control_output"; +const std::string KEY_ACTUAL_TCP_POSE = "actual_TCP_pose"; +const std::string KEY_ACTUAL_TCP_SPEED = "actual_TCP_speed"; +const std::string KEY_ACTUAL_TCP_FORCE = "actual_TCP_force"; +const std::string KEY_TARGET_TCP_POSE = "target_TCP_pose"; +const std::string KEY_TARGET_TCP_SPEED = "target_TCP_speed"; +const std::string KEY_TCP_OFFSET = "tcp_offset"; +const std::string KEY_ACTUAL_TCP_ACCELERATION = "actual_TCP_acceleration"; +const std::string KEY_TARGET_TCP_ACCELERATION = "target_TCP_acceleration"; +const std::string KEY_ACTUAL_DIGITAL_INPUT_BITS = "actual_digital_input_bits"; +const std::string KEY_ACTUAL_CONFIG_DIGITAL_INPUT_BITS = "actual_configurable_digital_input_bits"; +const std::string KEY_JOINT_TEMPERATURES = "joint_temperatures"; +const std::string KEY_ACTUAL_EXECUTION_TIME = "actual_execution_time"; +const std::string KEY_TARGET_EXECUTION_TIME = "target_execution_time"; +const std::string KEY_ROBOT_MODE = "robot_mode"; +const std::string KEY_JOINT_MODE = "joint_mode"; +const std::string KEY_SAFETY_MODE = "safety_mode"; +const std::string KEY_SAFETY_STATUS = "safety_status"; +const std::string KEY_ACTUAL_TOOL_ACCELEROMETER = "actual_tool_accelerometer"; +const std::string KEY_SPEED_SCALING = "speed_scaling"; +const std::string KEY_TARGET_SPEED_FRACTION = "target_speed_fraction"; +const std::string KEY_ACTUAL_MOMENTUM = "actual_momentum"; +const std::string KEY_ACTUAL_MAIN_VOLTAGE = "actual_main_voltage"; +const std::string KEY_ACTUAL_ROBOT_VOLTAGE = "actual_robot_voltage"; +const std::string KEY_ACTUAL_ROBOT_CURRENT = "actual_robot_current"; +const std::string KEY_ACTUAL_JOINT_VOLTAGE = "actual_joint_voltage"; +const std::string KEY_ACTUAL_DIGITAL_OUTPUT_BITS = "actual_digital_output_bits"; +const std::string KEY_ACTUAL_CONFIG_DIGITAL_OUTPUT_BITS = "actual_configurable_digital_output_bits"; +const std::string KEY_RUNTIME_STATE = "runtime_state"; +const std::string KEY_ELBOW_POSITION = "elbow_position"; +const std::string KEY_ELBOW_VELOCITY = "elbow_velocity"; +const std::string KEY_ROBOT_STATUS_BITS = "robot_status_bits"; +const std::string KEY_SAFETY_STATUS_BITS = "safety_status_bits"; +const std::string KEY_ANALOG_IO_TYPES = "analog_io_types"; +const std::string KEY_STANDARD_ANALOG_INPUT0 = "standard_analog_input0"; +const std::string KEY_STANDARD_ANALOG_INPUT1 = "standard_analog_input1"; +const std::string KEY_STANDARD_ANALOG_OUTPUT0 = "standard_analog_output0"; +const std::string KEY_STANDARD_ANALOG_OUTPUT1 = "standard_analog_output1"; +const std::string KEY_IO_CURRENT = "io_current"; +const std::string KEY_INPUT_BIT_REG_0_31 = "input_bit_registers0_to_31"; +const std::string KEY_INPUT_BIT_REG_32_63 = "input_bit_registers32_to_63"; +const std::string KEY_OUTPUT_BIT_REG_0_31 = "output_bit_registers0_to_31"; +const std::string KEY_OUTPUT_BIT_REG_32_63 = "output_bit_registers32_to_63"; +const std::string KEY_ACTUAL_ROBOT_ENERGY_CONSUMED = "actual_robot_energy_consumed"; +const std::string KEY_ACTUAL_ROBOT_BRAKING_ENERGY = "actual_robot_braking_energy_dissipated"; +const std::string KEY_ENCODER0_RAW = "encoder0_raw"; +const std::string KEY_ENCODER1_RAW = "encoder1_raw"; +const std::string KEY_EUROMAP67_INPUT_BITS = "euromap67_input_bits"; +const std::string KEY_EUROMAP67_OUTPUT_BITS = "euromap67_output_bits"; +const std::string KEY_EUROMAP67_24V_VOLTAGE = "euromap67_24V_voltage"; +const std::string KEY_EUROMAP67_24V_CURRENT = "euromap67_24V_current"; +const std::string KEY_TOOL_MODE = "tool_mode"; +const std::string KEY_TOOL_ANALOG_INPUT_TYPES = "tool_analog_input_types"; +const std::string KEY_TOOL_ANALOG_INPUT0 = "tool_analog_input0"; +const std::string KEY_TOOL_ANALOG_INPUT1 = "tool_analog_input1"; +const std::string KEY_TOOL_OUTPUT_VOLTAGE = "tool_output_voltage"; +const std::string KEY_TOOL_OUTPUT_CURRENT = "tool_output_current"; +const std::string KEY_TOOL_TEMPERATURE = "tool_temperature"; +const std::string KEY_TOOL_OUTPUT_MODE = "tool_output_mode"; +const std::string KEY_TOOL_DIGITAL_OUTPUT0_MODE = "tool_digital_output0_mode"; +const std::string KEY_TOOL_DIGITAL_OUTPUT1_MODE = "tool_digital_output1_mode"; +const std::string KEY_TCP_FORCE_SCALAR = "tcp_force_scalar"; +const std::string KEY_JOINT_POS_DEV_RATIO = "joint_position_deviation_ratio"; +const std::string KEY_COLLISION_DETECT_RATIO = "collision_detection_ratio"; +const std::string KEY_FT_RAW_WRENCH = "ft_raw_wrench"; +const std::string KEY_WRENCH_CALC_FROM_CURRENTS = "wrench_calc_from_currents"; +const std::string KEY_PAYLOAD = "payload"; +const std::string KEY_PAYLOAD_COG = "payload_cog"; +const std::string KEY_PAYLOAD_INERTIA = "payload_inertia"; +const std::string KEY_SCRIPT_CONTROL_LINE = "script_control_line"; +const std::string KEY_TIME_SCALE_SOURCE = "time_scale_source"; +const std::string KEY_TARGET_GRAVITY = "target_gravity"; +const std::string KEY_TARGET_BASE_ACCEL = "target_base_acceleration"; +// const std::string KEY_CONTROLLER_STEP = "controller_step"; + + +std::thread g_writer_thread; +moodycamel::ReaderWriterQueue g_durations_queue{ 1024 }; +bool g_running = false; + +void writerThreadFunc(const std::string& filename, const std::string& column_header = "cycle_time_microseconds") +{ + std::ofstream out_file(filename, std::ios::out); + out_file << column_header << "\n"; + while (g_running || g_durations_queue.peek() != nullptr) + { + long long duration; + while (g_durations_queue.tryDequeue(duration)) + { + out_file << duration << "\n"; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + out_file.close(); +} + +int main(int argc, char* argv[]) +{ -// Preallocation of string to avoid allocation in main loop -const std::string TARGET_SPEED_FRACTION = "target_speed_fraction"; + pprocess_t process = pprocess_self(); + pthread_t thread = pthread_self(); -void printFraction(const double fraction, const std::string& label, const size_t width = 20) -{ - std::cout << "\r" << label << ": ["; - for (size_t i = 0; i < std::ceil(fraction * width); i++) +#ifdef _WIN32 + // Assign logical CPUs 6 and 7 to this process + DWORD_PTR process_mask = (1ULL << 6) | (1ULL << 7); + if (!setProcessAffinity(process, process_mask)) { - std::cout << "#"; + URCL_LOG_ERROR("Failed to set process affinity"); } - for (size_t i = 0; i < std::floor((1.0 - fraction) * width); i++) + + // Assign logical CPU 7 to this thread + DWORD_PTR thread_mask = (1ULL << 7); + if (!setThreadAffinity(thread, thread_mask)) { - std::cout << "-"; + URCL_LOG_ERROR("Failed to set thread affinity"); } - std::cout << "]" << std::flush; -} +#else + cpu_set_t cpuset; + CPU_ZERO(&cpuset); -int main(int argc, char* argv[]) -{ - // Parse the ip arguments if given - std::string robot_ip = DEFAULT_ROBOT_IP; - if (argc > 1) + // Assign logical CPU 7 to this thread + CPU_SET(7, &cpuset); + + if (!setThreadAffinity(thread, cpuset)) { - robot_ip = std::string(argv[1]); + URCL_LOG_ERROR("Failed to set thread affinity"); } +#endif - // Parse how may seconds to run - int second_to_run = -1; - if (argc > 2) + // Set thread and process to maximum priority + const int max_prio = sched_get_priority_max(SCHED_FIFO); + if (!setFiFoScheduling(thread, max_prio)) { - second_to_run = std::stoi(argv[2]); + URCL_LOG_ERROR("Failed to set FIFO scheduling"); } + std::string robot_ip = DEFAULT_ROBOT_IP; + if (argc > 1) { robot_ip = std::string(argv[1]); } + int second_to_run = -1; + if (argc > 2) { second_to_run = std::stoi(argv[2]); } + g_running = true; + g_writer_thread = std::thread(&writerThreadFunc, robot_ip + "_cycle_times.csv", "cycle_time_microseconds"); + comm::INotifier notifier; - const double rtde_frequency = 50; // Hz + const double rtde_frequency = 500; rtde_interface::RTDEClient my_client(robot_ip, notifier, OUTPUT_RECIPE, INPUT_RECIPE, rtde_frequency); my_client.init(); + + double timestamp = 0.0; + urcl::vector6d_t target_q, target_qd, target_qdd, target_current, target_moment; + urcl::vector6d_t actual_q, actual_qd, actual_current, actual_current_window, actual_current_as_torque, joint_control_output; + urcl::vector6d_t actual_TCP_pose, actual_TCP_speed, actual_TCP_force, target_TCP_pose, target_TCP_speed; + urcl::vector6d_t tcp_offset, actual_TCP_acceleration, target_TCP_acceleration; + uint64_t actual_digital_input_bits = 0, actual_configurable_digital_input_bits = 0; + urcl::vector6d_t joint_temperatures; + double actual_execution_time = 0.0, target_execution_time = 0.0; + int32_t robot_mode = 0; + std::array joint_mode; + int32_t safety_mode = 0, safety_status = 0; + urcl::vector3d_t actual_tool_accelerometer; + double speed_scaling = 0.0, actual_momentum = 0.0; + double actual_main_voltage = 0.0, actual_robot_voltage = 0.0, actual_robot_current = 0.0; + urcl::vector6d_t actual_joint_voltage; + uint64_t actual_digital_output_bits = 0, actual_configurable_digital_output_bits = 0; + uint32_t runtime_state = 0; + urcl::vector3d_t elbow_position, elbow_velocity; + uint32_t robot_status_bits = 0, safety_status_bits = 0, analog_io_types = 0; + double standard_analog_input0 = 0.0, standard_analog_input1 = 0.0, standard_analog_output0 = 0.0, standard_analog_output1 = 0.0, io_current = 0.0; + uint32_t input_bit_reg_0_31 = 0, input_bit_reg_32_63 = 0, output_bit_reg_0_31 = 0, output_bit_reg_32_63 = 0; + bool input_bit_register[64]; + int32_t input_int_register[48]; + double input_double_register[48]; + bool output_bit_register[64]; + int32_t output_int_register[48]; + double output_double_register[48]; + double actual_robot_energy_consumed = 0.0, actual_robot_braking_energy_dissipated = 0.0; + int32_t encoder0_raw = 0, encoder1_raw = 0; + uint32_t euromap67_input_bits = 0, euromap67_output_bits = 0; + double euromap67_24V_voltage = 0.0, euromap67_24V_current = 0.0; + uint32_t tool_mode = 0, tool_analog_input_types = 0; + double tool_analog_input0 = 0.0, tool_analog_input1 = 0.0; + int32_t tool_output_voltage = 0; + double tool_output_current = 0.0, tool_temperature = 0.0; + uint8_t tool_output_mode = 0, tool_digital_output0_mode = 0, tool_digital_output1_mode = 0; + double tcp_force_scalar = 0.0, joint_position_deviation_ratio = 0.0, collision_detection_ratio = 0.0; + urcl::vector6d_t ft_raw_wrench, wrench_calc_from_currents; + double payload = 0.0; + urcl::vector3d_t payload_cog; + urcl::vector6d_t payload_inertia; + uint32_t script_control_line = 0; + int32_t time_scale_source = 0; + urcl::vector3d_t target_gravity; + urcl::vector6d_t target_base_acceleration; + // uint64_t controller_step = 0; - // We will use the speed_slider_fraction as an example how to write to RTDE + double speed_slider_increment = 0.01; double speed_slider_fraction = 1.0; double target_speed_fraction = 1.0; - double speed_slider_increment = 0.01; - + double analog_val = 0.0; + double analog_increment = 0.01; + int int_register = 0; + double double_register = 0.0; + double force_val = 0.0; + double force_increment = 0.5; + uint8_t digital_values = 0; + uint8_t digital_mask = 0x03; + auto data_pkg = std::make_unique(my_client.getOutputRecipe()); - // Once RTDE communication is started, we have to make sure to read from the interface buffer, as - // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main - // loop. - my_client.start(false); // false -> do not start background read thread. - - auto start_time = std::chrono::steady_clock::now(); - while (second_to_run <= 0 || - std::chrono::duration_cast(std::chrono::steady_clock::now() - start_time).count() < - second_to_run) + my_client.start(false); + auto start_time = std::chrono::high_resolution_clock::now(); + auto cycle_start = start_time; + auto cycle_end = start_time; + int loop_counter = 0; + + while (second_to_run <= 0 || std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start_time).count() < second_to_run) { - // Wait for a DataPackage. In a real-world application this thread should be scheduled with real-time priority in - // order to ensure that this is called in time. + cycle_start = std::chrono::high_resolution_clock::now(); bool success = my_client.getDataPackageBlocking(data_pkg); if (success) { - // Data fields in the data package are accessed by their name. Only names present in the - // output recipe can be accessed. Otherwise this function will return false. - // We preallocated the string TARGET_SPEED_FRACTION to avoid allocations in the main loop. - data_pkg->getData(TARGET_SPEED_FRACTION, target_speed_fraction); - printFraction(target_speed_fraction, TARGET_SPEED_FRACTION); + // ============================================================================== + // EXTRACCIÓN DE DATOS (GET DATA) DE TODAS LAS VARIABLES + // ============================================================================== + data_pkg->getData(KEY_TIMESTAMP, timestamp); + data_pkg->getData(KEY_TARGET_Q, target_q); + data_pkg->getData(KEY_TARGET_QD, target_qd); + data_pkg->getData(KEY_TARGET_QDD, target_qdd); + data_pkg->getData(KEY_TARGET_CURRENT, target_current); + data_pkg->getData(KEY_TARGET_MOMENT, target_moment); + data_pkg->getData(KEY_ACTUAL_Q, actual_q); + data_pkg->getData(KEY_ACTUAL_QD, actual_qd); + data_pkg->getData(KEY_ACTUAL_CURRENT, actual_current); + data_pkg->getData(KEY_ACTUAL_CURRENT_WINDOW, actual_current_window); + data_pkg->getData(KEY_ACTUAL_CURRENT_AS_TORQUE, actual_current_as_torque); + data_pkg->getData(KEY_JOINT_CONTROL_OUTPUT, joint_control_output); + data_pkg->getData(KEY_ACTUAL_TCP_POSE, actual_TCP_pose); + data_pkg->getData(KEY_ACTUAL_TCP_SPEED, actual_TCP_speed); + data_pkg->getData(KEY_ACTUAL_TCP_FORCE, actual_TCP_force); + data_pkg->getData(KEY_TARGET_TCP_POSE, target_TCP_pose); + data_pkg->getData(KEY_TARGET_TCP_SPEED, target_TCP_speed); + data_pkg->getData(KEY_TCP_OFFSET, tcp_offset); + data_pkg->getData(KEY_ACTUAL_TCP_ACCELERATION, actual_TCP_acceleration); + data_pkg->getData(KEY_TARGET_TCP_ACCELERATION, target_TCP_acceleration); + data_pkg->getData(KEY_ACTUAL_DIGITAL_INPUT_BITS, actual_digital_input_bits); + data_pkg->getData(KEY_ACTUAL_CONFIG_DIGITAL_INPUT_BITS, actual_configurable_digital_input_bits); + data_pkg->getData(KEY_JOINT_TEMPERATURES, joint_temperatures); + data_pkg->getData(KEY_ACTUAL_EXECUTION_TIME, actual_execution_time); + data_pkg->getData(KEY_TARGET_EXECUTION_TIME, target_execution_time); + data_pkg->getData(KEY_ROBOT_MODE, robot_mode); + data_pkg->getData(KEY_SAFETY_MODE, safety_mode); + data_pkg->getData(KEY_SAFETY_STATUS, safety_status); + data_pkg->getData(KEY_ACTUAL_TOOL_ACCELEROMETER, actual_tool_accelerometer); + data_pkg->getData(KEY_SPEED_SCALING, speed_scaling); + data_pkg->getData(KEY_TARGET_SPEED_FRACTION, target_speed_fraction); + data_pkg->getData(KEY_ACTUAL_MOMENTUM, actual_momentum); + data_pkg->getData(KEY_ACTUAL_MAIN_VOLTAGE, actual_main_voltage); + data_pkg->getData(KEY_ACTUAL_ROBOT_VOLTAGE, actual_robot_voltage); + data_pkg->getData(KEY_ACTUAL_ROBOT_CURRENT, actual_robot_current); + data_pkg->getData(KEY_ACTUAL_JOINT_VOLTAGE, actual_joint_voltage); + data_pkg->getData(KEY_ACTUAL_DIGITAL_OUTPUT_BITS, actual_digital_output_bits); + data_pkg->getData(KEY_ACTUAL_CONFIG_DIGITAL_OUTPUT_BITS, actual_configurable_digital_output_bits); + data_pkg->getData(KEY_RUNTIME_STATE, runtime_state); + data_pkg->getData(KEY_ELBOW_POSITION, elbow_position); + data_pkg->getData(KEY_ELBOW_VELOCITY, elbow_velocity); + data_pkg->getData(KEY_ROBOT_STATUS_BITS, robot_status_bits); + data_pkg->getData(KEY_SAFETY_STATUS_BITS, safety_status_bits); + data_pkg->getData(KEY_ANALOG_IO_TYPES, analog_io_types); + data_pkg->getData(KEY_STANDARD_ANALOG_INPUT0, standard_analog_input0); + data_pkg->getData(KEY_STANDARD_ANALOG_INPUT1, standard_analog_input1); + data_pkg->getData(KEY_STANDARD_ANALOG_OUTPUT0, standard_analog_output0); + data_pkg->getData(KEY_STANDARD_ANALOG_OUTPUT1, standard_analog_output1); + data_pkg->getData(KEY_IO_CURRENT, io_current); + data_pkg->getData(KEY_INPUT_BIT_REG_0_31, input_bit_reg_0_31); + data_pkg->getData(KEY_INPUT_BIT_REG_32_63, input_bit_reg_32_63); + data_pkg->getData(KEY_OUTPUT_BIT_REG_0_31, output_bit_reg_0_31); + data_pkg->getData(KEY_OUTPUT_BIT_REG_32_63, output_bit_reg_32_63); + + for (int i = 64; i <= 127; ++i) { + data_pkg->getData("input_bit_register_" + std::to_string(i), input_bit_register[i - 64]); + data_pkg->getData("output_bit_register_" + std::to_string(i), output_bit_register[i - 64]); + } + for (int i = 0; i <= 47; ++i) { + data_pkg->getData("input_int_register_" + std::to_string(i), input_int_register[i]); + data_pkg->getData("input_double_register_" + std::to_string(i), input_double_register[i]); + data_pkg->getData("output_int_register_" + std::to_string(i), output_int_register[i]); + data_pkg->getData("output_double_register_" + std::to_string(i), output_double_register[i]); + } + + data_pkg->getData(KEY_ACTUAL_ROBOT_ENERGY_CONSUMED, actual_robot_energy_consumed); + data_pkg->getData(KEY_ACTUAL_ROBOT_BRAKING_ENERGY, actual_robot_braking_energy_dissipated); + data_pkg->getData(KEY_ENCODER0_RAW, encoder0_raw); + data_pkg->getData(KEY_ENCODER1_RAW, encoder1_raw); + data_pkg->getData(KEY_EUROMAP67_INPUT_BITS, euromap67_input_bits); + data_pkg->getData(KEY_EUROMAP67_OUTPUT_BITS, euromap67_output_bits); + data_pkg->getData(KEY_EUROMAP67_24V_VOLTAGE, euromap67_24V_voltage); + data_pkg->getData(KEY_EUROMAP67_24V_CURRENT, euromap67_24V_current); + //data_pkg->getData(KEY_TOOL_MODE, tool_mode); + //data_pkg->getData(KEY_TOOL_ANALOG_INPUT_TYPES, tool_analog_input_types); + //data_pkg->getData(KEY_TOOL_ANALOG_INPUT0, tool_analog_input0); + //data_pkg->getData(KEY_TOOL_ANALOG_INPUT1, tool_analog_input1); + //data_pkg->getData(KEY_TOOL_OUTPUT_VOLTAGE, tool_output_voltage); + //data_pkg->getData(KEY_TOOL_OUTPUT_CURRENT, tool_output_current); + //data_pkg->getData(KEY_TOOL_TEMPERATURE, tool_temperature); + //data_pkg->getData(KEY_TOOL_OUTPUT_MODE, tool_output_mode); + //data_pkg->getData(KEY_TOOL_DIGITAL_OUTPUT0_MODE, tool_digital_output0_mode); + //data_pkg->getData(KEY_TOOL_DIGITAL_OUTPUT1_MODE, tool_digital_output1_mode); + data_pkg->getData(KEY_TCP_FORCE_SCALAR, tcp_force_scalar); + data_pkg->getData(KEY_JOINT_POS_DEV_RATIO, joint_position_deviation_ratio); + data_pkg->getData(KEY_COLLISION_DETECT_RATIO, collision_detection_ratio); + data_pkg->getData(KEY_FT_RAW_WRENCH, ft_raw_wrench); + data_pkg->getData(KEY_WRENCH_CALC_FROM_CURRENTS, wrench_calc_from_currents); + data_pkg->getData(KEY_PAYLOAD, payload); + data_pkg->getData(KEY_PAYLOAD_COG, payload_cog); + data_pkg->getData(KEY_PAYLOAD_INERTIA, payload_inertia); + data_pkg->getData(KEY_SCRIPT_CONTROL_LINE, script_control_line); + data_pkg->getData(KEY_TIME_SCALE_SOURCE, time_scale_source); + data_pkg->getData(KEY_TARGET_GRAVITY, target_gravity); + data_pkg->getData(KEY_TARGET_BASE_ACCEL, target_base_acceleration); + + if (loop_counter % 500 == 0) + { + std::cout << "\r[RTDE] Todas las variables capturadas. TS: " << timestamp << "s " << std::flush; + } } else { - // The client isn't connected properly anymore / doesn't receive any data anymore. Stop the - // program. std::cout << "Could not get fresh data package from robot" << std::endl; return 1; } - - // Change the speed slider so that it will move between 0 and 1 all the time. This is for - // demonstration purposes only and gains no real value. + if (speed_slider_increment > 0) { if (speed_slider_fraction + speed_slider_increment > 1.0) - { speed_slider_increment *= -1; - } } - else if (speed_slider_fraction + speed_slider_increment < 0.0) + else { - speed_slider_increment *= -1; + if (speed_slider_fraction + speed_slider_increment < 0.0) + speed_slider_increment *= -1; } + speed_slider_fraction += speed_slider_increment; if (!my_client.getWriter().sendSpeedSlider(speed_slider_fraction)) { - // This will happen for example, when the required keys are not configured inside the input - // recipe. - std::cout << "\033[1;31mSending RTDE data failed." << "\033[0m\n" << std::endl; + std::cerr << "\033[1;31m[ERROR] Failed to send speed slider.\033[0m\n"; return 1; } - } - // Resetting the speedslider back to 100% - my_client.getWriter().sendSpeedSlider(1); + digital_values = (digital_values == 0) ? digital_mask : 0; + + if (!my_client.getWriter().sendStandardDigitalOutput(digital_mask, digital_values)) + { + std::cerr << "[ERROR] Failed to send standard digital output\n"; + } + + if (!my_client.getWriter().sendConfigurableDigitalOutput(digital_mask, digital_values)) + { + std::cerr << "[ERROR] Failed to send configurable digital output\n"; + } + + if (analog_val + analog_increment > 1.0 || analog_val + analog_increment < 0.0) + { + analog_increment *= -1; + } + analog_val += analog_increment; + + if (!my_client.getWriter().sendStandardAnalogOutput(1, analog_val)) + { + std::cerr << "[ERROR] Failed to send analog output\n"; + } + + int_register += 10; + if (int_register > 100000) int_register = 0; + + double_register += 0.1; + if (double_register > 100.0) double_register = 0.0; - URCL_LOG_INFO("Exiting RTDE read/write example."); + if (!my_client.getWriter().sendInputBitRegister(64, digital_values != 0)) + { + std::cerr << "[ERROR] Failed to send input bit register\n"; + } + + if (!my_client.getWriter().sendInputIntRegister(24, int_register)) + { + std::cerr << "[ERROR] Failed to send input int register\n"; + } + + if (!my_client.getWriter().sendInputDoubleRegister(24, double_register)) + { + std::cerr << "[ERROR] Failed to send input double register\n"; + } + if (force_val + force_increment > 10.0 || force_val + force_increment < -10.0) + { + force_increment *= -1; + } + force_val += force_increment; + + urcl::vector6d_t ext_wrench = { + 0.0, + 0.0, + force_val, + 0.0, + 0.0, + 0.0 + }; + + if (!my_client.getWriter().sendExternalForceTorque(ext_wrench)) + { + std::cerr << "[ERROR] Failed to send external wrench\n"; + } + + cycle_end = std::chrono::high_resolution_clock::now(); + g_durations_queue.enqueue(std::chrono::duration_cast(cycle_end - cycle_start).count()); + loop_counter++; + } + + g_running = false; + g_writer_thread.join(); + return 0; -} +} \ No newline at end of file From 7bbf84218779c6325e7cfd5e38f6305dccde91d5 Mon Sep 17 00:00:00 2001 From: srvald Date: Wed, 1 Jul 2026 11:10:35 +0200 Subject: [PATCH 06/25] fix: adjust wrong argument and file format --- src/helpers.cpp | 213 +++++++++++++++++++++++------------------------- 1 file changed, 104 insertions(+), 109 deletions(-) diff --git a/src/helpers.cpp b/src/helpers.cpp index ad9163058..3e327fff7 100644 --- a/src/helpers.cpp +++ b/src/helpers.cpp @@ -41,7 +41,6 @@ #include #include - // clang-format off // We want to keep the URL in one line to avoid formatting issues. This will make it easier to // extract the URL for an automatic check. @@ -57,13 +56,20 @@ const char* processPriorityToString(DWORD priority) { switch (priority) { - case IDLE_PRIORITY_CLASS: return "IDLE"; - case BELOW_NORMAL_PRIORITY_CLASS: return "BELOW_NORMAL"; - case NORMAL_PRIORITY_CLASS: return "NORMAL"; - case ABOVE_NORMAL_PRIORITY_CLASS: return "ABOVE_NORMAL"; - case HIGH_PRIORITY_CLASS: return "HIGH"; - case REALTIME_PRIORITY_CLASS: return "REALTIME"; - default: return "UNKNOWN"; + case IDLE_PRIORITY_CLASS: + return "IDLE"; + case BELOW_NORMAL_PRIORITY_CLASS: + return "BELOW_NORMAL"; + case NORMAL_PRIORITY_CLASS: + return "NORMAL"; + case ABOVE_NORMAL_PRIORITY_CLASS: + return "ABOVE_NORMAL"; + case HIGH_PRIORITY_CLASS: + return "HIGH"; + case REALTIME_PRIORITY_CLASS: + return "REALTIME"; + default: + return "UNKNOWN"; } } @@ -71,30 +77,41 @@ const char* threadPriorityToString(int priority) { switch (priority) { - case THREAD_PRIORITY_LOWEST: return "LOWEST"; - case THREAD_PRIORITY_BELOW_NORMAL: return "BELOW_NORMAL"; - case THREAD_PRIORITY_NORMAL: return "NORMAL"; - case THREAD_PRIORITY_ABOVE_NORMAL: return "ABOVE_NORMAL"; - case THREAD_PRIORITY_HIGHEST: return "HIGHEST"; - case THREAD_PRIORITY_TIME_CRITICAL: return "TIME_CRITICAL"; - default: return "UNKNOWN"; + case THREAD_PRIORITY_LOWEST: + return "LOWEST"; + case THREAD_PRIORITY_BELOW_NORMAL: + return "BELOW_NORMAL"; + case THREAD_PRIORITY_NORMAL: + return "NORMAL"; + case THREAD_PRIORITY_ABOVE_NORMAL: + return "ABOVE_NORMAL"; + case THREAD_PRIORITY_HIGHEST: + return "HIGHEST"; + case THREAD_PRIORITY_TIME_CRITICAL: + return "TIME_CRITICAL"; + default: + return "UNKNOWN"; } } -bool isProcessElevated() { - bool isElevated = false; - HANDLE hToken = nullptr; - if(OpenProcessToken(GetCurrentProcess(),TOKEN_QUERY,&hToken)){ - TOKEN_ELEVATION elevation; - DWORD dwSize = sizeof(elevation); - if(GetTokenInformation(hToken, TokenElevation, &elevation, sizeof(elevation), &dwSize)){ - isElevated = elevation.TokenIsElevated; - } - } - if(hToken) { - CloseHandle(hToken); +bool isProcessElevated() +{ + bool isElevated = false; + HANDLE hToken = nullptr; + if (OpenProcessToken(GetCurrentProcess(), TOKEN_QUERY, &hToken)) + { + TOKEN_ELEVATION elevation; + DWORD dwSize = sizeof(elevation); + if (GetTokenInformation(hToken, TokenElevation, &elevation, sizeof(elevation), &dwSize)) + { + isElevated = elevation.TokenIsElevated; } - return isElevated; + } + if (hToken) + { + CloseHandle(hToken); + } + return isElevated; } std::string maskToCpuList(uint64_t mask) @@ -104,37 +121,32 @@ std::string maskToCpuList(uint64_t mask) { if (mask & (1ULL << i)) { - if (out.size() > 1) out += ","; + if (out.size() > 1) + out += ","; out += std::to_string(i); } } out += "]"; return out; - } -std::string getLastWindowsErrorMsg(DWORD error_id) +std::string getLastWindowsErrorMsg(DWORD error_id) { - LPSTR buffer = nullptr; - - DWORD size = FormatMessageA( - FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS, - nullptr, - error_id, - MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), - (LPSTR)&buffer, - 0, - nullptr); - - if (size == 0 || buffer == nullptr) - { - return "Unknown Windows error"; - } + LPSTR buffer = nullptr; - std::string message(buffer, size); - LocalFree(buffer); + DWORD size = + FormatMessageA(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS, + nullptr, error_id, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), (LPSTR)&buffer, 0, nullptr); - return message; + if (size == 0 || buffer == nullptr) + { + return "Unknown Windows error"; + } + + std::string message(buffer, size); + LocalFree(buffer); + + return message; }; bool setProcessPriority(pprocess_t& process, DWORD priority) @@ -142,24 +154,27 @@ bool setProcessPriority(pprocess_t& process, DWORD priority) if (!::SetPriorityClass(process, priority)) { DWORD err = GetLastError(); - URCL_LOG_ERROR("Unsuccessful in setting the process priority to %s (%llX). Error: %lu (%s)", processPriorityToString(priority), priority, err, getLastWindowsErrorMsg(err).c_str()); + URCL_LOG_ERROR("Unsuccessful in setting the process priority to %s (%llX). Error: %lu (%s)", + processPriorityToString(priority), priority, err, getLastWindowsErrorMsg(err).c_str()); return false; } - + DWORD priority_applied = ::GetPriorityClass(process); - if (priority_applied == 0){ + if (priority_applied == 0) + { DWORD err = GetLastError(); - URCL_LOG_ERROR("Unsuccessful in retrieving the process priority for verification. Error: %lu (%s)", err, getLastWindowsErrorMsg(err).c_str()); + URCL_LOG_ERROR("Unsuccessful in retrieving the process priority for verification. Error: %lu (%s)", err, + getLastWindowsErrorMsg(err).c_str()); return false; } - URCL_LOG_INFO("Process priority successfully set to %s (0x%X)", processPriorityToString(priority_applied), priority_applied); + URCL_LOG_INFO("Process priority successfully set to %s (0x%X)", processPriorityToString(priority_applied), + priority_applied); if (priority_applied != priority) { - URCL_LOG_WARN("Process priority mismatch. Expected %s (0x%X), got %s (0x%X)", - processPriorityToString(priority), priority, - processPriorityToString(priority_applied), priority_applied); + URCL_LOG_WARN("Process priority mismatch. Expected %s (0x%X), got %s (0x%X)", processPriorityToString(priority), + priority, processPriorityToString(priority_applied), priority_applied); return false; } @@ -172,30 +187,25 @@ bool setProcessAffinity(pprocess_t& process, DWORD_PTR cpu_mask) { DWORD err = GetLastError(); URCL_LOG_ERROR("Unsuccessful in setting process affinity to CPUs %s (mask=0x%llX). Error: %lu (%s)", - maskToCpuList(cpu_mask).c_str(), - static_cast(cpu_mask), - err, - getLastWindowsErrorMsg(err).c_str()); + maskToCpuList(cpu_mask).c_str(), static_cast(cpu_mask), err, + getLastWindowsErrorMsg(err).c_str()); - return false; + return false; } DWORD_PTR process_mask = 0; - DWORD_PTR system_mask = 0; + DWORD_PTR system_mask = 0; if (!::GetProcessAffinityMask(process, &process_mask, &system_mask)) { DWORD err = GetLastError(); - URCL_LOG_ERROR("Unsuccessful in setting process affinity to %s. Error: %lu (%s)", - maskToCpuList(cpu_mask).c_str(), - err, getLastWindowsErrorMsg(err).c_str()); - return false; + URCL_LOG_ERROR("Unsuccessful in setting process affinity to %s. Error: %lu (%s)", maskToCpuList(cpu_mask).c_str(), + err, getLastWindowsErrorMsg(err).c_str()); + return false; } - URCL_LOG_INFO("Process affinity set to CPUs %s (mask=0x%llX)", - maskToCpuList(process_mask).c_str(), + URCL_LOG_INFO("Process affinity set to CPUs %s (mask=0x%llX)", maskToCpuList(process_mask).c_str(), static_cast(process_mask)); if (process_mask != cpu_mask) { - URCL_LOG_WARN("Process affinity mismatch. Expected %s, got %s", - maskToCpuList(cpu_mask).c_str(), + URCL_LOG_WARN("Process affinity mismatch. Expected %s, got %s", maskToCpuList(cpu_mask).c_str(), maskToCpuList(process_mask).c_str()); return false; } @@ -209,43 +219,41 @@ bool setThreadAffinity(pthread_t& thread, DWORD_PTR cpu_mask) if (result == 0) { DWORD err = GetLastError(); - URCL_LOG_ERROR("Unsuccessful in setting thread affinity to %s. Error: %lu (%s)", - maskToCpuList(cpu_mask).c_str(), - err, getLastWindowsErrorMsg(err).c_str()); + URCL_LOG_ERROR("Unsuccessful in setting thread affinity to %s. Error: %lu (%s)", maskToCpuList(cpu_mask).c_str(), + err, getLastWindowsErrorMsg(err).c_str()); return false; } - URCL_LOG_INFO("Thread affinity successfully set to CPUs %s (mask=0x%llX)", - maskToCpuList(cpu_mask).c_str(), - static_cast(cpu_mask)); + URCL_LOG_INFO("Thread affinity successfully set to CPUs %s (mask=0x%llX)", maskToCpuList(cpu_mask).c_str(), + static_cast(cpu_mask)); return true; } -bool setThreadPriority(pthread_t& thread, const int priority){ +bool setThreadPriority(pthread_t& thread, const int priority) +{ if (!::SetThreadPriority(thread, priority)) { DWORD err = GetLastError(); URCL_LOG_ERROR("Unsuccessful in setting thread priority to %s (%d). Error: %lu (%s)", - threadPriorityToString(priority), priority, - err, getLastWindowsErrorMsg(err).c_str()); + threadPriorityToString(priority), priority, err, getLastWindowsErrorMsg(err).c_str()); return false; } int applied = ::GetThreadPriority(thread); - if (applied == THREAD_PRIORITY_ERROR_RETURN){ + if (applied == THREAD_PRIORITY_ERROR_RETURN) + { DWORD err = GetLastError(); - URCL_LOG_ERROR("Unsuccessful in retrieving the thread priority for verification. Error: %lu (%s)", err, getLastWindowsErrorMsg(err).c_str()); + URCL_LOG_ERROR("Unsuccessful in retrieving the thread priority for verification. Error: %lu (%s)", err, + getLastWindowsErrorMsg(err).c_str()); return false; } - URCL_LOG_INFO("Thread priority successfully set to %s (%d)", - threadPriorityToString(applied), applied); + URCL_LOG_INFO("Thread priority successfully set to %s (%d)", threadPriorityToString(applied), applied); if (applied != priority) { - URCL_LOG_WARN("Thread priority mismatch. Expected %s (%d), got %s (%d)", - threadPriorityToString(priority), priority, + URCL_LOG_WARN("Thread priority mismatch. Expected %s (%d), got %s (%d)", threadPriorityToString(priority), priority, threadPriorityToString(applied), applied); return false; } @@ -275,25 +283,20 @@ std::string cpuSetToString(const cpu_set_t& cpuset) return out; } -bool setThreadAffinity(pthread_t thread, const cpu_set_t& cpuset) +bool setThreadAffinity(pthread_t& thread, const cpu_set_t& cpuset) { int ret = pthread_setaffinity_np(thread, sizeof(cpu_set_t), &cpuset); if (ret != 0) { - URCL_LOG_ERROR( - "Unsuccessful in setting thread affinity. Error: %s", - strerror(ret)); + URCL_LOG_ERROR("Unsuccessful in setting thread affinity. Error: %s", strerror(ret)); return false; } cpu_set_t applied_set; CPU_ZERO(&applied_set); - if (pthread_getaffinity_np( - thread, - sizeof(cpu_set_t), - &applied_set) == 0) + if (pthread_getaffinity_np(thread, sizeof(cpu_set_t), &applied_set) == 0) { bool match = true; @@ -308,10 +311,8 @@ bool setThreadAffinity(pthread_t thread, const cpu_set_t& cpuset) if (!match) { - URCL_LOG_WARN( - "Thread affinity mismatch. Requested %s, got %s", - cpuSetToString(cpuset).c_str(), - cpuSetToString(applied_set).c_str()); + URCL_LOG_WARN("Thread affinity mismatch. Requested %s, got %s", cpuSetToString(cpuset).c_str(), + cpuSetToString(applied_set).c_str()); return false; } @@ -321,9 +322,7 @@ bool setThreadAffinity(pthread_t thread, const cpu_set_t& cpuset) URCL_LOG_WARN("Could not retrieve thread affinity"); } - URCL_LOG_INFO( - "Thread affinity successfully set to CPUs %s", - cpuSetToString(applied_set).c_str()); + URCL_LOG_INFO("Thread affinity successfully set to CPUs %s", cpuSetToString(applied_set).c_str()); return true; } @@ -332,14 +331,12 @@ bool setThreadAffinity(pthread_t thread, const cpu_set_t& cpuset) bool setFiFoScheduling(pthread_t& thread, int priority) { - #ifdef _WIN32 - if (!isProcessElevated()) + if (!isProcessElevated()) { - URCL_LOG_WARN( - "Process is not running with elevated privileges (UAC). " - "REALTIME_PRIORITY_CLASS may fail. Try 'Run as Administrator'."); + URCL_LOG_WARN("Process is not running with elevated privileges (UAC). " + "REALTIME_PRIORITY_CLASS may fail. Try 'Run as Administrator'."); } pprocess_t process = ::GetCurrentProcess(); @@ -351,9 +348,7 @@ bool setFiFoScheduling(pthread_t& thread, int priority) if (!setThreadPriority(thread, priority)) { - URCL_LOG_ERROR("Unsuccessful in setting thread priority to %s (%d)", - threadPriorityToString(priority), - priority); + URCL_LOG_ERROR("Unsuccessful in setting thread priority to %s (%d)", threadPriorityToString(priority), priority); return false; } From ec350f494ee76a4ffcb41201a560075cfeaf479f Mon Sep 17 00:00:00 2001 From: srvald Date: Wed, 1 Jul 2026 11:11:36 +0200 Subject: [PATCH 07/25] fix: adjust format and wrong test expected return --- tests/test_helpers.cpp | 38 ++++++++------------------------------ 1 file changed, 8 insertions(+), 30 deletions(-) diff --git a/tests/test_helpers.cpp b/tests/test_helpers.cpp index a2b5ab071..b42a8756b 100644 --- a/tests/test_helpers.cpp +++ b/tests/test_helpers.cpp @@ -179,15 +179,15 @@ TEST(TestHelpers, setThreadAffinity_mult) TEST(TestHelpers, setThreadAffinity_invalid) { -#ifdef _WIN32 pthread_t thread = pthread_self(); - uint64_t invalid_mask = 1ULL << 63; +#ifdef _WIN32 + uint64_t invalid_mask = 1ULL << 63; EXPECT_FALSE(setThreadAffinity(thread, invalid_mask)); #else cpu_set_t cpuset; CPU_ZERO(&cpuset); CPU_SET(63, &cpuset); - EXPECT_TRUE(setThreadAffinity(thread, cpuset)); + EXPECT_FALSE(setThreadAffinity(thread, cpuset)); #endif } @@ -201,29 +201,27 @@ TEST(TestHelpers, setThreadAffinity_invalidHandle) cpu_set_t cpuset; CPU_ZERO(&cpuset); CPU_SET(0, &cpuset); - EXPECT_FALSE(setThreadAffinity(thread, cpuset)); + EXPECT_DEATH(setThreadAffinity(thread, cpuset), ""); #endif } TEST(TestHelpers, setThreadAffinity_empty) { -#ifdef _WIN32 pthread_t thread = pthread_self(); +#ifdef _WIN32 EXPECT_FALSE(setThreadAffinity(thread, 0)); #else - pthread_t thread = pthread_self(); cpu_set_t cpuset; CPU_ZERO(&cpuset); EXPECT_FALSE(setThreadAffinity(thread, cpuset)); #endif - } // Thread Priority Tests +#ifdef _WIN32 TEST(TestHelpers, setThreadPriority_allValidPriorities) { -#ifdef _WIN32 pthread_t thread = pthread_self(); EXPECT_TRUE(setThreadPriority(thread, THREAD_PRIORITY_LOWEST)); @@ -232,30 +230,24 @@ TEST(TestHelpers, setThreadPriority_allValidPriorities) EXPECT_TRUE(setThreadPriority(thread, THREAD_PRIORITY_ABOVE_NORMAL)); EXPECT_TRUE(setThreadPriority(thread, THREAD_PRIORITY_HIGHEST)); EXPECT_TRUE(setThreadPriority(thread, THREAD_PRIORITY_TIME_CRITICAL)); -#endif } TEST(TestHelpers, setThreadPriority_invalid) { -#ifdef _WIN32 pthread_t thread = pthread_self(); EXPECT_FALSE(setThreadPriority(thread, 999999)); -#endif } TEST(TestHelpers, setThreadPriority_invalidHandle) { -#ifdef _WIN32 pthread_t thread = nullptr; EXPECT_FALSE(setThreadPriority(thread, THREAD_PRIORITY_HIGHEST)); -#endif } // Process Priority tests TEST(TestHelpers, setProcessPriority_allValidPriorities) { -#ifdef _WIN32 pprocess_t process = pprocess_self(); EXPECT_TRUE(setProcessPriority(process, IDLE_PRIORITY_CLASS)); @@ -263,69 +255,55 @@ TEST(TestHelpers, setProcessPriority_allValidPriorities) EXPECT_TRUE(setProcessPriority(process, NORMAL_PRIORITY_CLASS)); EXPECT_TRUE(setProcessPriority(process, ABOVE_NORMAL_PRIORITY_CLASS)); EXPECT_TRUE(setProcessPriority(process, HIGH_PRIORITY_CLASS)); -#endif } TEST(TestHelpers, setProcessPriority_invalid) { -#ifdef _WIN32 pprocess_t process = pprocess_self(); EXPECT_FALSE(setProcessPriority(process, 999999)); -#endif } TEST(TestHelpers, setProcessPriority_invalidHandle) { -#ifdef _WIN32 pprocess_t process = nullptr; EXPECT_FALSE(setProcessPriority(process, NORMAL_PRIORITY_CLASS)); -#endif } // Process Affinity Tests TEST(TestHelpers, setProcessAffinity_basic) { -#ifdef _WIN32 pprocess_t process = pprocess_self(); DWORD_PTR mask = (1ULL << 0); EXPECT_TRUE(setProcessAffinity(process, mask)); -#endif } TEST(TestHelpers, setProcessAffinity_multi) { -#ifdef _WIN32 pprocess_t process = pprocess_self(); DWORD_PTR mask = (1ULL << 0) | (1ULL << 1); EXPECT_TRUE(setProcessAffinity(process, mask)); -#endif } TEST(TestHelpers, setProcessAffinity_invalidHandle) { -#ifdef _WIN32 pprocess_t process = nullptr; EXPECT_FALSE(setProcessAffinity(process, (1ULL << 0))); -#endif } TEST(TestHelpers, setProcessAffinity_zeroMask) { -#ifdef _WIN32 pprocess_t process = pprocess_self(); - EXPECT_FALSE(setProcessAffinity(process,0)); -#endif + EXPECT_FALSE(setProcessAffinity(process, 0)); } TEST(TestHelpers, setProcessAffinity_invalidMask) { -#ifdef _WIN32 pprocess_t process = pprocess_self(); DWORD_PTR mask = 0xFFFFFFFFFFFFFFFFULL; EXPECT_FALSE(setProcessAffinity(process, mask)); -#endif } +#endif // FIFO Scheduling Tests From 85f289649158dfda3053654752de4dc2cb9209c8 Mon Sep 17 00:00:00 2001 From: srvald Date: Wed, 1 Jul 2026 11:14:44 +0200 Subject: [PATCH 08/25] fix: example back to normal adding the scheduling helper functions --- examples/rtde_client.cpp | 442 +++++++-------------------------------- 1 file changed, 72 insertions(+), 370 deletions(-) diff --git a/examples/rtde_client.cpp b/examples/rtde_client.cpp index 7e942bf85..aef19caf7 100644 --- a/examples/rtde_client.cpp +++ b/examples/rtde_client.cpp @@ -26,140 +26,44 @@ //---------------------------------------------------------------------- #include -#include -#include + #include #include -#include #include -#include -#include - -#ifdef _WIN32 -#include -#endif - +#include + using namespace urcl; - -const std::string DEFAULT_ROBOT_IP = "10.54.5.169"; -const std::string OUTPUT_RECIPE = "C:/Users/MIRserv/Desktop/ur_client_library_felix/Universal_Robots_Client_Library/examples/resources/rtde_output_recipe.txt"; -const std::string INPUT_RECIPE = "C:/Users/MIRserv/Desktop/ur_client_library_felix/Universal_Robots_Client_Library/examples/resources/rtde_input_recipe.txt"; - -const std::string KEY_TIMESTAMP = "timestamp"; -const std::string KEY_TARGET_Q = "target_q"; -const std::string KEY_TARGET_QD = "target_qd"; -const std::string KEY_TARGET_QDD = "target_qdd"; -const std::string KEY_TARGET_CURRENT = "target_current"; -const std::string KEY_TARGET_MOMENT = "target_moment"; -const std::string KEY_ACTUAL_Q = "actual_q"; -const std::string KEY_ACTUAL_QD = "actual_qd"; -const std::string KEY_ACTUAL_CURRENT = "actual_current"; -const std::string KEY_ACTUAL_CURRENT_WINDOW = "actual_current_window"; -const std::string KEY_ACTUAL_CURRENT_AS_TORQUE = "actual_current_as_torque"; -const std::string KEY_JOINT_CONTROL_OUTPUT = "joint_control_output"; -const std::string KEY_ACTUAL_TCP_POSE = "actual_TCP_pose"; -const std::string KEY_ACTUAL_TCP_SPEED = "actual_TCP_speed"; -const std::string KEY_ACTUAL_TCP_FORCE = "actual_TCP_force"; -const std::string KEY_TARGET_TCP_POSE = "target_TCP_pose"; -const std::string KEY_TARGET_TCP_SPEED = "target_TCP_speed"; -const std::string KEY_TCP_OFFSET = "tcp_offset"; -const std::string KEY_ACTUAL_TCP_ACCELERATION = "actual_TCP_acceleration"; -const std::string KEY_TARGET_TCP_ACCELERATION = "target_TCP_acceleration"; -const std::string KEY_ACTUAL_DIGITAL_INPUT_BITS = "actual_digital_input_bits"; -const std::string KEY_ACTUAL_CONFIG_DIGITAL_INPUT_BITS = "actual_configurable_digital_input_bits"; -const std::string KEY_JOINT_TEMPERATURES = "joint_temperatures"; -const std::string KEY_ACTUAL_EXECUTION_TIME = "actual_execution_time"; -const std::string KEY_TARGET_EXECUTION_TIME = "target_execution_time"; -const std::string KEY_ROBOT_MODE = "robot_mode"; -const std::string KEY_JOINT_MODE = "joint_mode"; -const std::string KEY_SAFETY_MODE = "safety_mode"; -const std::string KEY_SAFETY_STATUS = "safety_status"; -const std::string KEY_ACTUAL_TOOL_ACCELEROMETER = "actual_tool_accelerometer"; -const std::string KEY_SPEED_SCALING = "speed_scaling"; -const std::string KEY_TARGET_SPEED_FRACTION = "target_speed_fraction"; -const std::string KEY_ACTUAL_MOMENTUM = "actual_momentum"; -const std::string KEY_ACTUAL_MAIN_VOLTAGE = "actual_main_voltage"; -const std::string KEY_ACTUAL_ROBOT_VOLTAGE = "actual_robot_voltage"; -const std::string KEY_ACTUAL_ROBOT_CURRENT = "actual_robot_current"; -const std::string KEY_ACTUAL_JOINT_VOLTAGE = "actual_joint_voltage"; -const std::string KEY_ACTUAL_DIGITAL_OUTPUT_BITS = "actual_digital_output_bits"; -const std::string KEY_ACTUAL_CONFIG_DIGITAL_OUTPUT_BITS = "actual_configurable_digital_output_bits"; -const std::string KEY_RUNTIME_STATE = "runtime_state"; -const std::string KEY_ELBOW_POSITION = "elbow_position"; -const std::string KEY_ELBOW_VELOCITY = "elbow_velocity"; -const std::string KEY_ROBOT_STATUS_BITS = "robot_status_bits"; -const std::string KEY_SAFETY_STATUS_BITS = "safety_status_bits"; -const std::string KEY_ANALOG_IO_TYPES = "analog_io_types"; -const std::string KEY_STANDARD_ANALOG_INPUT0 = "standard_analog_input0"; -const std::string KEY_STANDARD_ANALOG_INPUT1 = "standard_analog_input1"; -const std::string KEY_STANDARD_ANALOG_OUTPUT0 = "standard_analog_output0"; -const std::string KEY_STANDARD_ANALOG_OUTPUT1 = "standard_analog_output1"; -const std::string KEY_IO_CURRENT = "io_current"; -const std::string KEY_INPUT_BIT_REG_0_31 = "input_bit_registers0_to_31"; -const std::string KEY_INPUT_BIT_REG_32_63 = "input_bit_registers32_to_63"; -const std::string KEY_OUTPUT_BIT_REG_0_31 = "output_bit_registers0_to_31"; -const std::string KEY_OUTPUT_BIT_REG_32_63 = "output_bit_registers32_to_63"; -const std::string KEY_ACTUAL_ROBOT_ENERGY_CONSUMED = "actual_robot_energy_consumed"; -const std::string KEY_ACTUAL_ROBOT_BRAKING_ENERGY = "actual_robot_braking_energy_dissipated"; -const std::string KEY_ENCODER0_RAW = "encoder0_raw"; -const std::string KEY_ENCODER1_RAW = "encoder1_raw"; -const std::string KEY_EUROMAP67_INPUT_BITS = "euromap67_input_bits"; -const std::string KEY_EUROMAP67_OUTPUT_BITS = "euromap67_output_bits"; -const std::string KEY_EUROMAP67_24V_VOLTAGE = "euromap67_24V_voltage"; -const std::string KEY_EUROMAP67_24V_CURRENT = "euromap67_24V_current"; -const std::string KEY_TOOL_MODE = "tool_mode"; -const std::string KEY_TOOL_ANALOG_INPUT_TYPES = "tool_analog_input_types"; -const std::string KEY_TOOL_ANALOG_INPUT0 = "tool_analog_input0"; -const std::string KEY_TOOL_ANALOG_INPUT1 = "tool_analog_input1"; -const std::string KEY_TOOL_OUTPUT_VOLTAGE = "tool_output_voltage"; -const std::string KEY_TOOL_OUTPUT_CURRENT = "tool_output_current"; -const std::string KEY_TOOL_TEMPERATURE = "tool_temperature"; -const std::string KEY_TOOL_OUTPUT_MODE = "tool_output_mode"; -const std::string KEY_TOOL_DIGITAL_OUTPUT0_MODE = "tool_digital_output0_mode"; -const std::string KEY_TOOL_DIGITAL_OUTPUT1_MODE = "tool_digital_output1_mode"; -const std::string KEY_TCP_FORCE_SCALAR = "tcp_force_scalar"; -const std::string KEY_JOINT_POS_DEV_RATIO = "joint_position_deviation_ratio"; -const std::string KEY_COLLISION_DETECT_RATIO = "collision_detection_ratio"; -const std::string KEY_FT_RAW_WRENCH = "ft_raw_wrench"; -const std::string KEY_WRENCH_CALC_FROM_CURRENTS = "wrench_calc_from_currents"; -const std::string KEY_PAYLOAD = "payload"; -const std::string KEY_PAYLOAD_COG = "payload_cog"; -const std::string KEY_PAYLOAD_INERTIA = "payload_inertia"; -const std::string KEY_SCRIPT_CONTROL_LINE = "script_control_line"; -const std::string KEY_TIME_SCALE_SOURCE = "time_scale_source"; -const std::string KEY_TARGET_GRAVITY = "target_gravity"; -const std::string KEY_TARGET_BASE_ACCEL = "target_base_acceleration"; -// const std::string KEY_CONTROLLER_STEP = "controller_step"; - - -std::thread g_writer_thread; -moodycamel::ReaderWriterQueue g_durations_queue{ 1024 }; -bool g_running = false; - -void writerThreadFunc(const std::string& filename, const std::string& column_header = "cycle_time_microseconds") +// In a real-world example it would be better to get those values from command line parameters / a better configuration +// system such as Boost.Program_options +const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; +const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; +const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; + +// Preallocation of string to avoid allocation in main loop +const std::string TARGET_SPEED_FRACTION = "target_speed_fraction"; + +void printFraction(const double fraction, const std::string& label, const size_t width = 20) { - std::ofstream out_file(filename, std::ios::out); - out_file << column_header << "\n"; - while (g_running || g_durations_queue.peek() != nullptr) + std::cout << "\r" << label << ": ["; + for (size_t i = 0; i < std::ceil(fraction * width); i++) { - long long duration; - while (g_durations_queue.tryDequeue(duration)) - { - out_file << duration << "\n"; - } - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + std::cout << "#"; } - out_file.close(); + for (size_t i = 0; i < std::floor((1.0 - fraction) * width); i++) + { + std::cout << "-"; + } + std::cout << "]" << std::flush; } - + int main(int argc, char* argv[]) { - - pprocess_t process = pprocess_self(); pthread_t thread = pthread_self(); #ifdef _WIN32 + pprocess_t process = pprocess_self(); + // Assign logical CPUs 6 and 7 to this process DWORD_PTR process_mask = (1ULL << 6) | (1ULL << 7); if (!setProcessAffinity(process, process_mask)) @@ -193,290 +97,88 @@ int main(int argc, char* argv[]) URCL_LOG_ERROR("Failed to set FIFO scheduling"); } + // Parse the ip arguments if given std::string robot_ip = DEFAULT_ROBOT_IP; - if (argc > 1) { robot_ip = std::string(argv[1]); } + if (argc > 1) + { + robot_ip = std::string(argv[1]); + } + + // Parse how may seconds to run int second_to_run = -1; - if (argc > 2) { second_to_run = std::stoi(argv[2]); } - g_running = true; - g_writer_thread = std::thread(&writerThreadFunc, robot_ip + "_cycle_times.csv", "cycle_time_microseconds"); - + if (argc > 2) + { + second_to_run = std::stoi(argv[2]); + } + comm::INotifier notifier; - const double rtde_frequency = 500; + const double rtde_frequency = 50; // Hz rtde_interface::RTDEClient my_client(robot_ip, notifier, OUTPUT_RECIPE, INPUT_RECIPE, rtde_frequency); my_client.init(); - - double timestamp = 0.0; - urcl::vector6d_t target_q, target_qd, target_qdd, target_current, target_moment; - urcl::vector6d_t actual_q, actual_qd, actual_current, actual_current_window, actual_current_as_torque, joint_control_output; - urcl::vector6d_t actual_TCP_pose, actual_TCP_speed, actual_TCP_force, target_TCP_pose, target_TCP_speed; - urcl::vector6d_t tcp_offset, actual_TCP_acceleration, target_TCP_acceleration; - uint64_t actual_digital_input_bits = 0, actual_configurable_digital_input_bits = 0; - urcl::vector6d_t joint_temperatures; - double actual_execution_time = 0.0, target_execution_time = 0.0; - int32_t robot_mode = 0; - std::array joint_mode; - int32_t safety_mode = 0, safety_status = 0; - urcl::vector3d_t actual_tool_accelerometer; - double speed_scaling = 0.0, actual_momentum = 0.0; - double actual_main_voltage = 0.0, actual_robot_voltage = 0.0, actual_robot_current = 0.0; - urcl::vector6d_t actual_joint_voltage; - uint64_t actual_digital_output_bits = 0, actual_configurable_digital_output_bits = 0; - uint32_t runtime_state = 0; - urcl::vector3d_t elbow_position, elbow_velocity; - uint32_t robot_status_bits = 0, safety_status_bits = 0, analog_io_types = 0; - double standard_analog_input0 = 0.0, standard_analog_input1 = 0.0, standard_analog_output0 = 0.0, standard_analog_output1 = 0.0, io_current = 0.0; - uint32_t input_bit_reg_0_31 = 0, input_bit_reg_32_63 = 0, output_bit_reg_0_31 = 0, output_bit_reg_32_63 = 0; - bool input_bit_register[64]; - int32_t input_int_register[48]; - double input_double_register[48]; - bool output_bit_register[64]; - int32_t output_int_register[48]; - double output_double_register[48]; - double actual_robot_energy_consumed = 0.0, actual_robot_braking_energy_dissipated = 0.0; - int32_t encoder0_raw = 0, encoder1_raw = 0; - uint32_t euromap67_input_bits = 0, euromap67_output_bits = 0; - double euromap67_24V_voltage = 0.0, euromap67_24V_current = 0.0; - uint32_t tool_mode = 0, tool_analog_input_types = 0; - double tool_analog_input0 = 0.0, tool_analog_input1 = 0.0; - int32_t tool_output_voltage = 0; - double tool_output_current = 0.0, tool_temperature = 0.0; - uint8_t tool_output_mode = 0, tool_digital_output0_mode = 0, tool_digital_output1_mode = 0; - double tcp_force_scalar = 0.0, joint_position_deviation_ratio = 0.0, collision_detection_ratio = 0.0; - urcl::vector6d_t ft_raw_wrench, wrench_calc_from_currents; - double payload = 0.0; - urcl::vector3d_t payload_cog; - urcl::vector6d_t payload_inertia; - uint32_t script_control_line = 0; - int32_t time_scale_source = 0; - urcl::vector3d_t target_gravity; - urcl::vector6d_t target_base_acceleration; - // uint64_t controller_step = 0; - double speed_slider_increment = 0.01; + // We will use the speed_slider_fraction as an example how to write to RTDE double speed_slider_fraction = 1.0; double target_speed_fraction = 1.0; - double analog_val = 0.0; - double analog_increment = 0.01; - int int_register = 0; - double double_register = 0.0; - double force_val = 0.0; - double force_increment = 0.5; - uint8_t digital_values = 0; - uint8_t digital_mask = 0x03; - + double speed_slider_increment = 0.01; + auto data_pkg = std::make_unique(my_client.getOutputRecipe()); - my_client.start(false); - auto start_time = std::chrono::high_resolution_clock::now(); - auto cycle_start = start_time; - auto cycle_end = start_time; - int loop_counter = 0; - - while (second_to_run <= 0 || std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start_time).count() < second_to_run) + // Once RTDE communication is started, we have to make sure to read from the interface buffer, as + // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main + // loop. + my_client.start(false); // false -> do not start background read thread. + + auto start_time = std::chrono::steady_clock::now(); + while (second_to_run <= 0 || + std::chrono::duration_cast(std::chrono::steady_clock::now() - start_time).count() < + second_to_run) { - cycle_start = std::chrono::high_resolution_clock::now(); + // Wait for a DataPackage. In a real-world application this thread should be scheduled with real-time priority in + // order to ensure that this is called in time. bool success = my_client.getDataPackageBlocking(data_pkg); if (success) { - // ============================================================================== - // EXTRACCIÓN DE DATOS (GET DATA) DE TODAS LAS VARIABLES - // ============================================================================== - data_pkg->getData(KEY_TIMESTAMP, timestamp); - data_pkg->getData(KEY_TARGET_Q, target_q); - data_pkg->getData(KEY_TARGET_QD, target_qd); - data_pkg->getData(KEY_TARGET_QDD, target_qdd); - data_pkg->getData(KEY_TARGET_CURRENT, target_current); - data_pkg->getData(KEY_TARGET_MOMENT, target_moment); - data_pkg->getData(KEY_ACTUAL_Q, actual_q); - data_pkg->getData(KEY_ACTUAL_QD, actual_qd); - data_pkg->getData(KEY_ACTUAL_CURRENT, actual_current); - data_pkg->getData(KEY_ACTUAL_CURRENT_WINDOW, actual_current_window); - data_pkg->getData(KEY_ACTUAL_CURRENT_AS_TORQUE, actual_current_as_torque); - data_pkg->getData(KEY_JOINT_CONTROL_OUTPUT, joint_control_output); - data_pkg->getData(KEY_ACTUAL_TCP_POSE, actual_TCP_pose); - data_pkg->getData(KEY_ACTUAL_TCP_SPEED, actual_TCP_speed); - data_pkg->getData(KEY_ACTUAL_TCP_FORCE, actual_TCP_force); - data_pkg->getData(KEY_TARGET_TCP_POSE, target_TCP_pose); - data_pkg->getData(KEY_TARGET_TCP_SPEED, target_TCP_speed); - data_pkg->getData(KEY_TCP_OFFSET, tcp_offset); - data_pkg->getData(KEY_ACTUAL_TCP_ACCELERATION, actual_TCP_acceleration); - data_pkg->getData(KEY_TARGET_TCP_ACCELERATION, target_TCP_acceleration); - data_pkg->getData(KEY_ACTUAL_DIGITAL_INPUT_BITS, actual_digital_input_bits); - data_pkg->getData(KEY_ACTUAL_CONFIG_DIGITAL_INPUT_BITS, actual_configurable_digital_input_bits); - data_pkg->getData(KEY_JOINT_TEMPERATURES, joint_temperatures); - data_pkg->getData(KEY_ACTUAL_EXECUTION_TIME, actual_execution_time); - data_pkg->getData(KEY_TARGET_EXECUTION_TIME, target_execution_time); - data_pkg->getData(KEY_ROBOT_MODE, robot_mode); - data_pkg->getData(KEY_SAFETY_MODE, safety_mode); - data_pkg->getData(KEY_SAFETY_STATUS, safety_status); - data_pkg->getData(KEY_ACTUAL_TOOL_ACCELEROMETER, actual_tool_accelerometer); - data_pkg->getData(KEY_SPEED_SCALING, speed_scaling); - data_pkg->getData(KEY_TARGET_SPEED_FRACTION, target_speed_fraction); - data_pkg->getData(KEY_ACTUAL_MOMENTUM, actual_momentum); - data_pkg->getData(KEY_ACTUAL_MAIN_VOLTAGE, actual_main_voltage); - data_pkg->getData(KEY_ACTUAL_ROBOT_VOLTAGE, actual_robot_voltage); - data_pkg->getData(KEY_ACTUAL_ROBOT_CURRENT, actual_robot_current); - data_pkg->getData(KEY_ACTUAL_JOINT_VOLTAGE, actual_joint_voltage); - data_pkg->getData(KEY_ACTUAL_DIGITAL_OUTPUT_BITS, actual_digital_output_bits); - data_pkg->getData(KEY_ACTUAL_CONFIG_DIGITAL_OUTPUT_BITS, actual_configurable_digital_output_bits); - data_pkg->getData(KEY_RUNTIME_STATE, runtime_state); - data_pkg->getData(KEY_ELBOW_POSITION, elbow_position); - data_pkg->getData(KEY_ELBOW_VELOCITY, elbow_velocity); - data_pkg->getData(KEY_ROBOT_STATUS_BITS, robot_status_bits); - data_pkg->getData(KEY_SAFETY_STATUS_BITS, safety_status_bits); - data_pkg->getData(KEY_ANALOG_IO_TYPES, analog_io_types); - data_pkg->getData(KEY_STANDARD_ANALOG_INPUT0, standard_analog_input0); - data_pkg->getData(KEY_STANDARD_ANALOG_INPUT1, standard_analog_input1); - data_pkg->getData(KEY_STANDARD_ANALOG_OUTPUT0, standard_analog_output0); - data_pkg->getData(KEY_STANDARD_ANALOG_OUTPUT1, standard_analog_output1); - data_pkg->getData(KEY_IO_CURRENT, io_current); - data_pkg->getData(KEY_INPUT_BIT_REG_0_31, input_bit_reg_0_31); - data_pkg->getData(KEY_INPUT_BIT_REG_32_63, input_bit_reg_32_63); - data_pkg->getData(KEY_OUTPUT_BIT_REG_0_31, output_bit_reg_0_31); - data_pkg->getData(KEY_OUTPUT_BIT_REG_32_63, output_bit_reg_32_63); - - for (int i = 64; i <= 127; ++i) { - data_pkg->getData("input_bit_register_" + std::to_string(i), input_bit_register[i - 64]); - data_pkg->getData("output_bit_register_" + std::to_string(i), output_bit_register[i - 64]); - } - for (int i = 0; i <= 47; ++i) { - data_pkg->getData("input_int_register_" + std::to_string(i), input_int_register[i]); - data_pkg->getData("input_double_register_" + std::to_string(i), input_double_register[i]); - data_pkg->getData("output_int_register_" + std::to_string(i), output_int_register[i]); - data_pkg->getData("output_double_register_" + std::to_string(i), output_double_register[i]); - } - - data_pkg->getData(KEY_ACTUAL_ROBOT_ENERGY_CONSUMED, actual_robot_energy_consumed); - data_pkg->getData(KEY_ACTUAL_ROBOT_BRAKING_ENERGY, actual_robot_braking_energy_dissipated); - data_pkg->getData(KEY_ENCODER0_RAW, encoder0_raw); - data_pkg->getData(KEY_ENCODER1_RAW, encoder1_raw); - data_pkg->getData(KEY_EUROMAP67_INPUT_BITS, euromap67_input_bits); - data_pkg->getData(KEY_EUROMAP67_OUTPUT_BITS, euromap67_output_bits); - data_pkg->getData(KEY_EUROMAP67_24V_VOLTAGE, euromap67_24V_voltage); - data_pkg->getData(KEY_EUROMAP67_24V_CURRENT, euromap67_24V_current); - //data_pkg->getData(KEY_TOOL_MODE, tool_mode); - //data_pkg->getData(KEY_TOOL_ANALOG_INPUT_TYPES, tool_analog_input_types); - //data_pkg->getData(KEY_TOOL_ANALOG_INPUT0, tool_analog_input0); - //data_pkg->getData(KEY_TOOL_ANALOG_INPUT1, tool_analog_input1); - //data_pkg->getData(KEY_TOOL_OUTPUT_VOLTAGE, tool_output_voltage); - //data_pkg->getData(KEY_TOOL_OUTPUT_CURRENT, tool_output_current); - //data_pkg->getData(KEY_TOOL_TEMPERATURE, tool_temperature); - //data_pkg->getData(KEY_TOOL_OUTPUT_MODE, tool_output_mode); - //data_pkg->getData(KEY_TOOL_DIGITAL_OUTPUT0_MODE, tool_digital_output0_mode); - //data_pkg->getData(KEY_TOOL_DIGITAL_OUTPUT1_MODE, tool_digital_output1_mode); - data_pkg->getData(KEY_TCP_FORCE_SCALAR, tcp_force_scalar); - data_pkg->getData(KEY_JOINT_POS_DEV_RATIO, joint_position_deviation_ratio); - data_pkg->getData(KEY_COLLISION_DETECT_RATIO, collision_detection_ratio); - data_pkg->getData(KEY_FT_RAW_WRENCH, ft_raw_wrench); - data_pkg->getData(KEY_WRENCH_CALC_FROM_CURRENTS, wrench_calc_from_currents); - data_pkg->getData(KEY_PAYLOAD, payload); - data_pkg->getData(KEY_PAYLOAD_COG, payload_cog); - data_pkg->getData(KEY_PAYLOAD_INERTIA, payload_inertia); - data_pkg->getData(KEY_SCRIPT_CONTROL_LINE, script_control_line); - data_pkg->getData(KEY_TIME_SCALE_SOURCE, time_scale_source); - data_pkg->getData(KEY_TARGET_GRAVITY, target_gravity); - data_pkg->getData(KEY_TARGET_BASE_ACCEL, target_base_acceleration); - - if (loop_counter % 500 == 0) - { - std::cout << "\r[RTDE] Todas las variables capturadas. TS: " << timestamp << "s " << std::flush; - } + // Data fields in the data package are accessed by their name. Only names present in the + // output recipe can be accessed. Otherwise this function will return false. + // We preallocated the string TARGET_SPEED_FRACTION to avoid allocations in the main loop. + data_pkg->getData(TARGET_SPEED_FRACTION, target_speed_fraction); + printFraction(target_speed_fraction, TARGET_SPEED_FRACTION); } else { + // The client isn't connected properly anymore / doesn't receive any data anymore. Stop the + // program. std::cout << "Could not get fresh data package from robot" << std::endl; return 1; } - + + // Change the speed slider so that it will move between 0 and 1 all the time. This is for + // demonstration purposes only and gains no real value. if (speed_slider_increment > 0) { if (speed_slider_fraction + speed_slider_increment > 1.0) + { speed_slider_increment *= -1; + } } - else + else if (speed_slider_fraction + speed_slider_increment < 0.0) { - if (speed_slider_fraction + speed_slider_increment < 0.0) - speed_slider_increment *= -1; + speed_slider_increment *= -1; } - speed_slider_fraction += speed_slider_increment; if (!my_client.getWriter().sendSpeedSlider(speed_slider_fraction)) { - std::cerr << "\033[1;31m[ERROR] Failed to send speed slider.\033[0m\n"; + // This will happen for example, when the required keys are not configured inside the input + // recipe. + std::cout << "\033[1;31mSending RTDE data failed." << "\033[0m\n" << std::endl; return 1; } + } - digital_values = (digital_values == 0) ? digital_mask : 0; - - if (!my_client.getWriter().sendStandardDigitalOutput(digital_mask, digital_values)) - { - std::cerr << "[ERROR] Failed to send standard digital output\n"; - } - - if (!my_client.getWriter().sendConfigurableDigitalOutput(digital_mask, digital_values)) - { - std::cerr << "[ERROR] Failed to send configurable digital output\n"; - } - - if (analog_val + analog_increment > 1.0 || analog_val + analog_increment < 0.0) - { - analog_increment *= -1; - } - analog_val += analog_increment; - - if (!my_client.getWriter().sendStandardAnalogOutput(1, analog_val)) - { - std::cerr << "[ERROR] Failed to send analog output\n"; - } - - int_register += 10; - if (int_register > 100000) int_register = 0; - - double_register += 0.1; - if (double_register > 100.0) double_register = 0.0; - - if (!my_client.getWriter().sendInputBitRegister(64, digital_values != 0)) - { - std::cerr << "[ERROR] Failed to send input bit register\n"; - } - - if (!my_client.getWriter().sendInputIntRegister(24, int_register)) - { - std::cerr << "[ERROR] Failed to send input int register\n"; - } - - if (!my_client.getWriter().sendInputDoubleRegister(24, double_register)) - { - std::cerr << "[ERROR] Failed to send input double register\n"; - } - - if (force_val + force_increment > 10.0 || force_val + force_increment < -10.0) - { - force_increment *= -1; - } - force_val += force_increment; + // Resetting the speedslider back to 100% + my_client.getWriter().sendSpeedSlider(1); - urcl::vector6d_t ext_wrench = { - 0.0, - 0.0, - force_val, - 0.0, - 0.0, - 0.0 - }; + URCL_LOG_INFO("Exiting RTDE read/write example."); - if (!my_client.getWriter().sendExternalForceTorque(ext_wrench)) - { - std::cerr << "[ERROR] Failed to send external wrench\n"; - } - - cycle_end = std::chrono::high_resolution_clock::now(); - g_durations_queue.enqueue(std::chrono::duration_cast(cycle_end - cycle_start).count()); - loop_counter++; - } - - g_running = false; - g_writer_thread.join(); - return 0; } \ No newline at end of file From 64e07745dd76314d22b86ec2594f5ef698ec26c0 Mon Sep 17 00:00:00 2001 From: srvald Date: Wed, 1 Jul 2026 11:50:17 +0200 Subject: [PATCH 09/25] fix: show UAC warning only when REALTIME priority. --- src/helpers.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/helpers.cpp b/src/helpers.cpp index 3e327fff7..4cd48a4ac 100644 --- a/src/helpers.cpp +++ b/src/helpers.cpp @@ -151,6 +151,12 @@ std::string getLastWindowsErrorMsg(DWORD error_id) bool setProcessPriority(pprocess_t& process, DWORD priority) { + if (!isProcessElevated() && priority == REALTIME_PRIORITY_CLASS) + { + URCL_LOG_WARN("Process is not running with elevated privileges (UAC). " + "REALTIME_PRIORITY_CLASS may fail. Try 'Run as Administrator'."); + } + if (!::SetPriorityClass(process, priority)) { DWORD err = GetLastError(); @@ -332,13 +338,7 @@ bool setThreadAffinity(pthread_t& thread, const cpu_set_t& cpuset) bool setFiFoScheduling(pthread_t& thread, int priority) { #ifdef _WIN32 - - if (!isProcessElevated()) - { - URCL_LOG_WARN("Process is not running with elevated privileges (UAC). " - "REALTIME_PRIORITY_CLASS may fail. Try 'Run as Administrator'."); - } - pprocess_t process = ::GetCurrentProcess(); + pprocess_t process = pprocess_self(); if (!setProcessPriority(process, REALTIME_PRIORITY_CLASS)) { From 4fc6d350ad0ecdee3ec4d028267b2700a3adc7bb Mon Sep 17 00:00:00 2001 From: srvald Date: Thu, 2 Jul 2026 10:45:07 +0200 Subject: [PATCH 10/25] fix: remove GNU SOURCE --- src/helpers.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/helpers.cpp b/src/helpers.cpp index 4cd48a4ac..1ebab5606 100644 --- a/src/helpers.cpp +++ b/src/helpers.cpp @@ -25,7 +25,6 @@ * */ //---------------------------------------------------------------------- -#define _GNU_SOURCE #include #include From 012ae048e53bd06f5d4b810baa483edf4be9ace6 Mon Sep 17 00:00:00 2001 From: srvald Date: Thu, 2 Jul 2026 10:52:52 +0200 Subject: [PATCH 11/25] fix: avoid cpu_set_t on apple --- include/ur_client_library/helpers.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ur_client_library/helpers.h b/include/ur_client_library/helpers.h index 64f0b1531..26015c382 100644 --- a/include/ur_client_library/helpers.h +++ b/include/ur_client_library/helpers.h @@ -154,7 +154,7 @@ bool setThreadAffinity(pthread_t& thread, DWORD_PTR cpu_mask); */ bool setThreadPriority(pthread_t& thread, const int priority); -#else +#elif __linux__ /*! * \brief Restrict a thread to a set of CPUs. From 161f4de833d2852b5acbfffecc7257f4527c6fff Mon Sep 17 00:00:00 2001 From: srvald Date: Thu, 2 Jul 2026 10:59:50 +0200 Subject: [PATCH 12/25] fix: return false if cannot get affinity thread ubuntu --- src/helpers.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/helpers.cpp b/src/helpers.cpp index 1ebab5606..5c09b8280 100644 --- a/src/helpers.cpp +++ b/src/helpers.cpp @@ -325,6 +325,7 @@ bool setThreadAffinity(pthread_t& thread, const cpu_set_t& cpuset) else { URCL_LOG_WARN("Could not retrieve thread affinity"); + return false; } URCL_LOG_INFO("Thread affinity successfully set to CPUs %s", cpuSetToString(applied_set).c_str()); From d246b084ae24e131d34fd7657b99e39a7a361736 Mon Sep 17 00:00:00 2001 From: srvald Date: Thu, 2 Jul 2026 11:05:41 +0200 Subject: [PATCH 13/25] fix: add elif linux to avoid function to be used with Apple --- src/helpers.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/helpers.cpp b/src/helpers.cpp index 5c09b8280..d71474c15 100644 --- a/src/helpers.cpp +++ b/src/helpers.cpp @@ -266,7 +266,7 @@ bool setThreadPriority(pthread_t& thread, const int priority) return true; } -#else +#elif __linux__ std::string cpuSetToString(const cpu_set_t& cpuset) { From 0666c9fd18c751a4cb7d3483233bc39b513c3469 Mon Sep 17 00:00:00 2001 From: srvald Date: Thu, 2 Jul 2026 11:18:23 +0200 Subject: [PATCH 14/25] fix: apply old process priority if thread priority fails at FiFo scheduling --- src/helpers.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/helpers.cpp b/src/helpers.cpp index d71474c15..5ad3b1224 100644 --- a/src/helpers.cpp +++ b/src/helpers.cpp @@ -340,6 +340,15 @@ bool setFiFoScheduling(pthread_t& thread, int priority) #ifdef _WIN32 pprocess_t process = pprocess_self(); + DWORD old_process_priority = ::GetPriorityClass(process); + if (old_process_priority == 0) + { + DWORD err = GetLastError(); + URCL_LOG_ERROR("Unsuccessful in retrieving the current process priority. Error: %lu (%s)", err, + getLastWindowsErrorMsg(err).c_str()); + return false; + } + if (!setProcessPriority(process, REALTIME_PRIORITY_CLASS)) { URCL_LOG_ERROR("Unsuccessful in setting process to REALTIME_PRIORITY_CLASS"); @@ -349,6 +358,14 @@ bool setFiFoScheduling(pthread_t& thread, int priority) if (!setThreadPriority(thread, priority)) { URCL_LOG_ERROR("Unsuccessful in setting thread priority to %s (%d)", threadPriorityToString(priority), priority); + + if (!setProcessPriority(process, old_process_priority)) + { + URCL_LOG_ERROR("Failed to restore previous process priority %s (0x%X)", + processPriorityToString(old_process_priority), + old_process_priority); + } + return false; } From 633483d24372ccab15d2be7089725e29aa12a435 Mon Sep 17 00:00:00 2001 From: srvald Date: Thu, 2 Jul 2026 11:19:42 +0200 Subject: [PATCH 15/25] tests: compatible cpu affinity mask for github actions --- tests/test_helpers.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_helpers.cpp b/tests/test_helpers.cpp index b42a8756b..04464cb66 100644 --- a/tests/test_helpers.cpp +++ b/tests/test_helpers.cpp @@ -166,7 +166,7 @@ TEST(TestHelpers, setThreadAffinity_mult) { pthread_t thread = pthread_self(); #ifdef _WIN32 - DWORD_PTR mask = (1ULL << 6) | (1ULL << 7); + DWORD_PTR mask = (1ULL << 0) | (1ULL << 1); EXPECT_TRUE(setThreadAffinity(thread, mask)); #else cpu_set_t cpuset; From e91a534c14b70f20d346ea08b7512094551897be Mon Sep 17 00:00:00 2001 From: srvald Date: Thu, 2 Jul 2026 11:23:49 +0200 Subject: [PATCH 16/25] tests: remove FIFO scheduling tests --- tests/test_helpers.cpp | 36 ------------------------------------ 1 file changed, 36 deletions(-) diff --git a/tests/test_helpers.cpp b/tests/test_helpers.cpp index 04464cb66..859618e7e 100644 --- a/tests/test_helpers.cpp +++ b/tests/test_helpers.cpp @@ -304,39 +304,3 @@ TEST(TestHelpers, setProcessAffinity_invalidMask) EXPECT_FALSE(setProcessAffinity(process, mask)); } #endif - -// FIFO Scheduling Tests - -TEST(TestHelpers, setFiFoScheduling_normal) -{ - pthread_t thread = pthread_self(); -#ifdef _WIN32 - EXPECT_TRUE(setFiFoScheduling(thread, THREAD_PRIORITY_NORMAL)); -#else - EXPECT_TRUE(setFiFoScheduling(thread, 50)); -#endif -} - -TEST(TestHelpers, setFiFoScheduling_invalidPriority) -{ - pthread_t thread = pthread_self(); - EXPECT_FALSE(setFiFoScheduling(thread, 999)); -} - -TEST(TestHelpers, setFiFoScheduling_invalidHandle) -{ -#ifdef _WIN32 - pthread_t thread = nullptr; - EXPECT_FALSE(setFiFoScheduling(thread, THREAD_PRIORITY_NORMAL)); -#else - pthread_t thread{}; - EXPECT_FALSE(setFiFoScheduling(thread, 50)); -#endif -} - -TEST(TestHelpers, setFiFoScheduling_maxPriority) -{ - pthread_t thread = pthread_self(); - int max_prio = sched_get_priority_max(SCHED_FIFO); - EXPECT_TRUE(setFiFoScheduling(thread, max_prio)); -} From 7a4887711a8ecea8e2a61a9a4ec74c5c9ae5952a Mon Sep 17 00:00:00 2001 From: srvald Date: Thu, 2 Jul 2026 11:32:31 +0200 Subject: [PATCH 17/25] test: correct CPUs at thread affinity mask for ubuntu and some tests removed from being executed with Apple --- tests/test_helpers.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/tests/test_helpers.cpp b/tests/test_helpers.cpp index 859618e7e..77d49931b 100644 --- a/tests/test_helpers.cpp +++ b/tests/test_helpers.cpp @@ -154,7 +154,7 @@ TEST(TestHelpers, setThreadAffinity_basic) #ifdef _WIN32 DWORD_PTR mask = (1ULL << 0); EXPECT_TRUE(setThreadAffinity(thread, mask)); -#else +#elif __linux__ cpu_set_t cpuset; CPU_ZERO(&cpuset); CPU_SET(0, &cpuset); @@ -168,11 +168,11 @@ TEST(TestHelpers, setThreadAffinity_mult) #ifdef _WIN32 DWORD_PTR mask = (1ULL << 0) | (1ULL << 1); EXPECT_TRUE(setThreadAffinity(thread, mask)); -#else +#elif __linux__ cpu_set_t cpuset; CPU_ZERO(&cpuset); - CPU_SET(6, &cpuset); - CPU_SET(7, &cpuset); + CPU_SET(0, &cpuset); + CPU_SET(1, &cpuset); EXPECT_TRUE(setThreadAffinity(thread, cpuset)); #endif } @@ -183,7 +183,7 @@ TEST(TestHelpers, setThreadAffinity_invalid) #ifdef _WIN32 uint64_t invalid_mask = 1ULL << 63; EXPECT_FALSE(setThreadAffinity(thread, invalid_mask)); -#else +#elif __linux__ cpu_set_t cpuset; CPU_ZERO(&cpuset); CPU_SET(63, &cpuset); @@ -196,7 +196,7 @@ TEST(TestHelpers, setThreadAffinity_invalidHandle) #ifdef _WIN32 pthread_t thread = nullptr; EXPECT_FALSE(setThreadAffinity(thread, (1ULL << 0))); -#else +#elif __linux__ pthread_t thread{}; cpu_set_t cpuset; CPU_ZERO(&cpuset); @@ -210,7 +210,7 @@ TEST(TestHelpers, setThreadAffinity_empty) pthread_t thread = pthread_self(); #ifdef _WIN32 EXPECT_FALSE(setThreadAffinity(thread, 0)); -#else +#elif __linux__ cpu_set_t cpuset; CPU_ZERO(&cpuset); EXPECT_FALSE(setThreadAffinity(thread, cpuset)); From 6c3f64c296f8078c900c180a2e7268c05b90c5e0 Mon Sep 17 00:00:00 2001 From: srvald Date: Thu, 2 Jul 2026 11:50:58 +0200 Subject: [PATCH 18/25] fix: fix format --- src/helpers.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/helpers.cpp b/src/helpers.cpp index 5ad3b1224..5e88ff0c3 100644 --- a/src/helpers.cpp +++ b/src/helpers.cpp @@ -362,8 +362,7 @@ bool setFiFoScheduling(pthread_t& thread, int priority) if (!setProcessPriority(process, old_process_priority)) { URCL_LOG_ERROR("Failed to restore previous process priority %s (0x%X)", - processPriorityToString(old_process_priority), - old_process_priority); + processPriorityToString(old_process_priority), old_process_priority); } return false; From e62bd2c53b8127d4fd63511af9f8f02dfe503ddf Mon Sep 17 00:00:00 2001 From: srvald Date: Thu, 2 Jul 2026 11:53:00 +0200 Subject: [PATCH 19/25] tests: resolve unused variable using Apple --- tests/test_helpers.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/tests/test_helpers.cpp b/tests/test_helpers.cpp index 77d49931b..0220a5123 100644 --- a/tests/test_helpers.cpp +++ b/tests/test_helpers.cpp @@ -150,11 +150,12 @@ TEST(TestHelpers, robotSeriesString) TEST(TestHelpers, setThreadAffinity_basic) { - pthread_t thread = pthread_self(); #ifdef _WIN32 + pthread_t thread = pthread_self(); DWORD_PTR mask = (1ULL << 0); EXPECT_TRUE(setThreadAffinity(thread, mask)); #elif __linux__ + pthread_t thread = pthread_self(); cpu_set_t cpuset; CPU_ZERO(&cpuset); CPU_SET(0, &cpuset); @@ -164,11 +165,12 @@ TEST(TestHelpers, setThreadAffinity_basic) TEST(TestHelpers, setThreadAffinity_mult) { - pthread_t thread = pthread_self(); #ifdef _WIN32 + pthread_t thread = pthread_self(); DWORD_PTR mask = (1ULL << 0) | (1ULL << 1); EXPECT_TRUE(setThreadAffinity(thread, mask)); #elif __linux__ + pthread_t thread = pthread_self(); cpu_set_t cpuset; CPU_ZERO(&cpuset); CPU_SET(0, &cpuset); @@ -179,11 +181,12 @@ TEST(TestHelpers, setThreadAffinity_mult) TEST(TestHelpers, setThreadAffinity_invalid) { - pthread_t thread = pthread_self(); #ifdef _WIN32 + pthread_t thread = pthread_self(); uint64_t invalid_mask = 1ULL << 63; EXPECT_FALSE(setThreadAffinity(thread, invalid_mask)); #elif __linux__ + pthread_t thread = pthread_self(); cpu_set_t cpuset; CPU_ZERO(&cpuset); CPU_SET(63, &cpuset); @@ -207,10 +210,11 @@ TEST(TestHelpers, setThreadAffinity_invalidHandle) TEST(TestHelpers, setThreadAffinity_empty) { - pthread_t thread = pthread_self(); #ifdef _WIN32 + pthread_t thread = pthread_self(); EXPECT_FALSE(setThreadAffinity(thread, 0)); #elif __linux__ + pthread_t thread = pthread_self(); cpu_set_t cpuset; CPU_ZERO(&cpuset); EXPECT_FALSE(setThreadAffinity(thread, cpuset)); From d09ed572eb835e12b5dca99af79e691bc6ace29c Mon Sep 17 00:00:00 2001 From: srvald Date: Fri, 3 Jul 2026 08:07:28 +0200 Subject: [PATCH 20/25] fix: adjust code format --- src/helpers.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/helpers.cpp b/src/helpers.cpp index 5e88ff0c3..d29a4cfba 100644 --- a/src/helpers.cpp +++ b/src/helpers.cpp @@ -362,7 +362,7 @@ bool setFiFoScheduling(pthread_t& thread, int priority) if (!setProcessPriority(process, old_process_priority)) { URCL_LOG_ERROR("Failed to restore previous process priority %s (0x%X)", - processPriorityToString(old_process_priority), old_process_priority); + processPriorityToString(old_process_priority), old_process_priority); } return false; From c3a202a01e41a0155f4b99f50095b3e109e931ad Mon Sep 17 00:00:00 2001 From: srvald Date: Fri, 3 Jul 2026 08:08:00 +0200 Subject: [PATCH 21/25] example: compatible with Apple --- examples/rtde_client.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/examples/rtde_client.cpp b/examples/rtde_client.cpp index aef19caf7..60b673e66 100644 --- a/examples/rtde_client.cpp +++ b/examples/rtde_client.cpp @@ -59,9 +59,9 @@ void printFraction(const double fraction, const std::string& label, const size_t int main(int argc, char* argv[]) { - pthread_t thread = pthread_self(); #ifdef _WIN32 + pthread_t thread = pthread_self(); pprocess_t process = pprocess_self(); // Assign logical CPUs 6 and 7 to this process @@ -77,7 +77,8 @@ int main(int argc, char* argv[]) { URCL_LOG_ERROR("Failed to set thread affinity"); } -#else +#elif __linux__ + pthread_t thread = pthread_self(); cpu_set_t cpuset; CPU_ZERO(&cpuset); From 6fe9f2cf967eabb8aeb6151f58cb03d01351b6c6 Mon Sep 17 00:00:00 2001 From: srvald Date: Fri, 3 Jul 2026 08:16:57 +0200 Subject: [PATCH 22/25] docs: Remove whitespace --- doc/real_time.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/real_time.rst b/doc/real_time.rst index 7e881ec9d..b4cc6a9f9 100644 --- a/doc/real_time.rst +++ b/doc/real_time.rst @@ -376,7 +376,7 @@ Microsoft recommends using: * ``SetThreadPriority()`` with ``THREAD_PRIORITY_TIME_CRITICAL`` * ``SetThreadAffinityMask()`` -For detailed information about configuring real-time processes and threads on Windows, +For detailed information about configuring real-time processes and threads on Windows, see `Application Development `_. Scheduling priorities From 37599d1ee51cd576affc864c7ee5ab9c43fa5c4c Mon Sep 17 00:00:00 2001 From: srvald Date: Fri, 3 Jul 2026 08:18:17 +0200 Subject: [PATCH 23/25] fix: adjust format and thread missing with Apple --- examples/rtde_client.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/examples/rtde_client.cpp b/examples/rtde_client.cpp index 60b673e66..4ceb55fa6 100644 --- a/examples/rtde_client.cpp +++ b/examples/rtde_client.cpp @@ -59,9 +59,8 @@ void printFraction(const double fraction, const std::string& label, const size_t int main(int argc, char* argv[]) { - -#ifdef _WIN32 pthread_t thread = pthread_self(); +#ifdef _WIN32 pprocess_t process = pprocess_self(); // Assign logical CPUs 6 and 7 to this process @@ -78,7 +77,6 @@ int main(int argc, char* argv[]) URCL_LOG_ERROR("Failed to set thread affinity"); } #elif __linux__ - pthread_t thread = pthread_self(); cpu_set_t cpuset; CPU_ZERO(&cpuset); From b56bdb0c5bb63bd441f94439f050eca8e8cf1644 Mon Sep 17 00:00:00 2001 From: srvald Date: Fri, 3 Jul 2026 10:37:52 +0200 Subject: [PATCH 24/25] refactor: if failure, set previos priority back --- src/helpers.cpp | 64 ++++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 61 insertions(+), 3 deletions(-) diff --git a/src/helpers.cpp b/src/helpers.cpp index d29a4cfba..6043f0bd8 100644 --- a/src/helpers.cpp +++ b/src/helpers.cpp @@ -156,6 +156,31 @@ bool setProcessPriority(pprocess_t& process, DWORD priority) "REALTIME_PRIORITY_CLASS may fail. Try 'Run as Administrator'."); } + DWORD old_process_priority = ::GetPriorityClass(process); + if (old_process_priority == 0) + { + DWORD err = GetLastError(); + URCL_LOG_ERROR("Unsuccessful in retrieving the current process priority. Error: %lu (%s)", err, + getLastWindowsErrorMsg(err).c_str()); + return false; + } + + auto restore_priority = [&]() + { + if (!::SetPriorityClass(process, old_process_priority)) + { + DWORD err = GetLastError(); + URCL_LOG_ERROR("Failed to restore previous process priority %s (0x%X). Error: %lu (%s)", + processPriorityToString(old_process_priority), old_process_priority, + err, getLastWindowsErrorMsg(err).c_str()); + } + else + { + URCL_LOG_INFO("Previous process priority successfully restored to %s (0x%X)", + processPriorityToString(old_process_priority), old_process_priority); + } + }; + if (!::SetPriorityClass(process, priority)) { DWORD err = GetLastError(); @@ -170,6 +195,7 @@ bool setProcessPriority(pprocess_t& process, DWORD priority) DWORD err = GetLastError(); URCL_LOG_ERROR("Unsuccessful in retrieving the process priority for verification. Error: %lu (%s)", err, getLastWindowsErrorMsg(err).c_str()); + restore_priority(); return false; } @@ -180,6 +206,7 @@ bool setProcessPriority(pprocess_t& process, DWORD priority) { URCL_LOG_WARN("Process priority mismatch. Expected %s (0x%X), got %s (0x%X)", processPriorityToString(priority), priority, processPriorityToString(priority_applied), priority_applied); + restore_priority(); return false; } @@ -236,6 +263,31 @@ bool setThreadAffinity(pthread_t& thread, DWORD_PTR cpu_mask) bool setThreadPriority(pthread_t& thread, const int priority) { + int old_priority = ::GetThreadPriority(thread); + if (old_priority == THREAD_PRIORITY_ERROR_RETURN) + { + DWORD err = GetLastError(); + URCL_LOG_ERROR("Unsuccessful in retrieving the current thread priority. Error: %lu (%s)", + err, getLastWindowsErrorMsg(err).c_str()); + return false; + } + + auto restore_priority = [&]() + { + if (!::SetThreadPriority(thread, old_priority)) + { + DWORD err = GetLastError(); + URCL_LOG_ERROR("Failed to restore previous thread priority %s (%d). Error: %lu (%s)", + threadPriorityToString(old_priority), old_priority, + err, getLastWindowsErrorMsg(err).c_str()); + } + else + { + URCL_LOG_INFO("Previous thread priority successfully restored to %s (%d)", + threadPriorityToString(old_priority), old_priority); + } + }; + if (!::SetThreadPriority(thread, priority)) { DWORD err = GetLastError(); @@ -251,6 +303,7 @@ bool setThreadPriority(pthread_t& thread, const int priority) DWORD err = GetLastError(); URCL_LOG_ERROR("Unsuccessful in retrieving the thread priority for verification. Error: %lu (%s)", err, getLastWindowsErrorMsg(err).c_str()); + restore_priority(); return false; } @@ -260,6 +313,7 @@ bool setThreadPriority(pthread_t& thread, const int priority) { URCL_LOG_WARN("Thread priority mismatch. Expected %s (%d), got %s (%d)", threadPriorityToString(priority), priority, threadPriorityToString(applied), applied); + restore_priority(); return false; } @@ -358,11 +412,15 @@ bool setFiFoScheduling(pthread_t& thread, int priority) if (!setThreadPriority(thread, priority)) { URCL_LOG_ERROR("Unsuccessful in setting thread priority to %s (%d)", threadPriorityToString(priority), priority); + URCL_LOG_INFO("Restoring previous process priority %s (0x%X)", + processPriorityToString(old_process_priority), old_process_priority); - if (!setProcessPriority(process, old_process_priority)) + if (!::SetPriorityClass(process, old_process_priority)) { - URCL_LOG_ERROR("Failed to restore previous process priority %s (0x%X)", - processPriorityToString(old_process_priority), old_process_priority); + DWORD err = GetLastError(); + URCL_LOG_ERROR("Failed to restore previous process priority %s (0x%X). Error: %lu (%s)", + processPriorityToString(old_process_priority), old_process_priority, + err, getLastWindowsErrorMsg(err).c_str()); } return false; From 41f371c9f9c795618d4e90f367801d76c2fa008d Mon Sep 17 00:00:00 2001 From: srvald Date: Fri, 3 Jul 2026 11:29:14 +0200 Subject: [PATCH 25/25] fix: adjust format --- src/helpers.cpp | 31 ++++++++++++++----------------- 1 file changed, 14 insertions(+), 17 deletions(-) diff --git a/src/helpers.cpp b/src/helpers.cpp index 6043f0bd8..d4cdce0bc 100644 --- a/src/helpers.cpp +++ b/src/helpers.cpp @@ -165,14 +165,13 @@ bool setProcessPriority(pprocess_t& process, DWORD priority) return false; } - auto restore_priority = [&]() - { + auto restore_priority = [&]() { if (!::SetPriorityClass(process, old_process_priority)) { DWORD err = GetLastError(); URCL_LOG_ERROR("Failed to restore previous process priority %s (0x%X). Error: %lu (%s)", - processPriorityToString(old_process_priority), old_process_priority, - err, getLastWindowsErrorMsg(err).c_str()); + processPriorityToString(old_process_priority), old_process_priority, err, + getLastWindowsErrorMsg(err).c_str()); } else { @@ -267,24 +266,22 @@ bool setThreadPriority(pthread_t& thread, const int priority) if (old_priority == THREAD_PRIORITY_ERROR_RETURN) { DWORD err = GetLastError(); - URCL_LOG_ERROR("Unsuccessful in retrieving the current thread priority. Error: %lu (%s)", - err, getLastWindowsErrorMsg(err).c_str()); + URCL_LOG_ERROR("Unsuccessful in retrieving the current thread priority. Error: %lu (%s)", err, + getLastWindowsErrorMsg(err).c_str()); return false; } - - auto restore_priority = [&]() - { + + auto restore_priority = [&]() { if (!::SetThreadPriority(thread, old_priority)) { DWORD err = GetLastError(); URCL_LOG_ERROR("Failed to restore previous thread priority %s (%d). Error: %lu (%s)", - threadPriorityToString(old_priority), old_priority, - err, getLastWindowsErrorMsg(err).c_str()); + threadPriorityToString(old_priority), old_priority, err, getLastWindowsErrorMsg(err).c_str()); } else { - URCL_LOG_INFO("Previous thread priority successfully restored to %s (%d)", - threadPriorityToString(old_priority), old_priority); + URCL_LOG_INFO("Previous thread priority successfully restored to %s (%d)", threadPriorityToString(old_priority), + old_priority); } }; @@ -412,15 +409,15 @@ bool setFiFoScheduling(pthread_t& thread, int priority) if (!setThreadPriority(thread, priority)) { URCL_LOG_ERROR("Unsuccessful in setting thread priority to %s (%d)", threadPriorityToString(priority), priority); - URCL_LOG_INFO("Restoring previous process priority %s (0x%X)", - processPriorityToString(old_process_priority), old_process_priority); + URCL_LOG_INFO("Restoring previous process priority %s (0x%X)", processPriorityToString(old_process_priority), + old_process_priority); if (!::SetPriorityClass(process, old_process_priority)) { DWORD err = GetLastError(); URCL_LOG_ERROR("Failed to restore previous process priority %s (0x%X). Error: %lu (%s)", - processPriorityToString(old_process_priority), old_process_priority, - err, getLastWindowsErrorMsg(err).c_str()); + processPriorityToString(old_process_priority), old_process_priority, err, + getLastWindowsErrorMsg(err).c_str()); } return false;