From 57763a00d6bcd76c2a6d8b804b00838bc1c3d06c Mon Sep 17 00:00:00 2001
From: Martin Mayer <martin.mayer@gmx.de>
Date: Wed, 4 Jan 2023 18:34:38 +0100
Subject: [PATCH 1/9] added thread.h

Signed-off-by: Martin Mayer <martin.mayer@gmx.de>
---
 CMakeLists.txt           |   1 +
 include/rcutils/thread.h |  66 +++++++++++++++++++++++
 src/thread.c             | 113 +++++++++++++++++++++++++++++++++++++++
 3 files changed, 180 insertions(+)
 create mode 100644 include/rcutils/thread.h
 create mode 100644 src/thread.c

diff --git a/CMakeLists.txt b/CMakeLists.txt
index ee7a2c69..50309eb9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -72,6 +72,7 @@ set(rcutils_sources
   src/string_array.c
   src/string_map.c
   src/testing/fault_injection.c
+  src/thread.c
   src/time.c
   ${time_impl_c}
   src/uint8_array.c
diff --git a/include/rcutils/thread.h b/include/rcutils/thread.h
new file mode 100644
index 00000000..54dd0992
--- /dev/null
+++ b/include/rcutils/thread.h
@@ -0,0 +1,66 @@
+// Copyright (c) 2020 Robert Bosch GmbH
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCUTILS__THREAD_H_
+#define RCUTILS__THREAD_H_
+
+#include "rcutils/visibility_control.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/// Enum for simple configuration of threads in two priority classes.
+enum ThreadPriority
+{
+  THREAD_PRIORITY_LOW,
+  THREAD_PRIORITY_MEDIUM,
+  THREAD_PRIORITY_HIGH
+};
+
+/// Calculates an OS specific thread priority from a ThreadPriority value.
+/**
+ * \param[in] thread_priority thread priority of type ThreadPriority
+ * \param[out] os_priority OS specific thread priority
+ * \return 1 on systems that support POSIX, else 0
+ */
+RCUTILS_LOCAL
+int calculate_os_thread_priority(
+  const int thread_priority,
+  int * os_priority);
+
+/// Sets the priority and cpu affinity of the given native thread.
+/**
+ * This function intentionally only works on operating systems which support a FIFO thread scheduler.
+ * Note for Linux: using this function requires elevated privileges and a kernel with realtime patch.
+ *
+ * Implementation note: For setting thread priorities which are intended for a non-realtime/fair thread
+ * scheduler a new utility function should be implemented in order to not mix up different use cases.
+ *
+ * \param[in] native_handle native thread handle
+ * \param[in] priority priority of type ThreadPriority to be set for the given thread
+ * \param[in] cpu_bitmask cpu core bitmask for the given thread; use (unsigned) -1 for all cores
+ * \return 1 on success, 0 on error
+ */
+RCUTILS_PUBLIC
+int configure_native_realtime_thread(
+  unsigned long int native_handle, const int priority,
+  const unsigned int cpu_bitmask);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // RCUTILS__THREAD_H_
diff --git a/src/thread.c b/src/thread.c
new file mode 100644
index 00000000..057b6d32
--- /dev/null
+++ b/src/thread.c
@@ -0,0 +1,113 @@
+// Copyright (c) 2020 Robert Bosch GmbH
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rcutils/thread.h"
+
+#ifdef _WIN32  // i.e., Windows platform.
+// #include <windows.h>
+#elif __APPLE__  // i.e., macOS platform.
+// #include <pthread.h>
+// #include <mach/mach_init.h>
+// #include <mach/mach_port.h>
+// #include <mach/mach_time.h>
+// #include <mach/thread_act.h>
+// #include <mach/thread_policy.h>
+// #include <sys/sysctl.h>
+#else  // POSIX platforms
+  #include <pthread.h>
+  #ifdef __QNXNTO__
+    #include <sys/neutrino.h>
+    #include <sys/syspage.h>
+  #endif  // __QNXNTO__
+#endif
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+int calculate_os_thread_priority(
+  const int thread_priority,
+  int * os_priority)
+{
+#ifdef _WIN32
+  return false;
+#elif __APPLE__
+  return false;
+#else
+  if (thread_priority == THREAD_PRIORITY_HIGH) {
+    *os_priority = sched_get_priority_max(SCHED_FIFO);
+  } else if (thread_priority == THREAD_PRIORITY_LOW) {
+    *os_priority = sched_get_priority_min(SCHED_FIFO);
+  } else if (thread_priority == THREAD_PRIORITY_MEDIUM) {
+    // Should be a value of 49 on standard Linux platforms, which is just below
+    // the default priority of 50 for threaded interrupt handling.
+    *os_priority =
+      (sched_get_priority_min(SCHED_FIFO) + sched_get_priority_max(SCHED_FIFO)) / 2 - 1;
+  } else {  // unhandled priority
+    return 0;
+  }
+  return 1;
+#endif
+}
+
+int configure_native_realtime_thread(
+  unsigned long int native_handle, const int priority,
+  const unsigned int cpu_bitmask)
+{
+  int success = 1;
+#ifdef _WIN32
+  return false;
+#elif __APPLE__
+  return false;
+#else  // POSIX systems
+  struct sched_param params;
+  int policy;
+  success &= (pthread_getschedparam(native_handle, &policy, &params) == 0);
+  success &= calculate_os_thread_priority(priority, &params.sched_priority);
+  success &= (pthread_setschedparam(native_handle, SCHED_FIFO, &params) == 0);
+
+#ifdef __QNXNTO__
+  // run_mask is a bit mask to set which cpu a thread runs on
+  // where each bit corresponds to a cpu core
+  int64_t run_mask = cpu_bitmask;
+
+  // Function used to change thread affinity of thread associated with native_handle
+  if (ThreadCtlExt(
+      0, native_handle, _NTO_TCTL_RUNMASK,
+      reinterpret_cast<void *>(run_mask)) == -1)
+  {
+    success &= 0;
+  } else {
+    success &= 1;
+  }
+#else
+  cpu_set_t cpuset;
+  CPU_ZERO(&cpuset);
+  for (unsigned int i = 0; i < sizeof(cpu_bitmask) * 8; i++) {
+    if ( (cpu_bitmask & (1 << i)) != 0) {
+      CPU_SET(i, &cpuset);
+    }
+  }
+  success &= (pthread_setaffinity_np(native_handle, sizeof(cpu_set_t), &cpuset) == 0);
+#endif  // __QNXNTO__
+#endif
+
+  return success;
+}
+
+#ifdef __cplusplus
+}
+#endif
+

From 3888633ac5b763ac0faff0b640d9ccbc82f27dc4 Mon Sep 17 00:00:00 2001
From: Martin Mayer <martin.mayer@gmx.de>
Date: Wed, 4 Jan 2023 19:17:41 +0100
Subject: [PATCH 2/9] modified return type

Signed-off-by: Martin Mayer <martin.mayer@gmx.de>
---
 include/rcutils/thread.h |  9 +++++----
 src/thread.c             | 21 +++++++++++----------
 2 files changed, 16 insertions(+), 14 deletions(-)

diff --git a/include/rcutils/thread.h b/include/rcutils/thread.h
index 54dd0992..a96d732f 100644
--- a/include/rcutils/thread.h
+++ b/include/rcutils/thread.h
@@ -16,6 +16,7 @@
 #define RCUTILS__THREAD_H_
 
 #include "rcutils/visibility_control.h"
+#include "rcutils/types/rcutils_ret.h"
 
 #ifdef __cplusplus
 extern "C"
@@ -34,10 +35,10 @@ enum ThreadPriority
 /**
  * \param[in] thread_priority thread priority of type ThreadPriority
  * \param[out] os_priority OS specific thread priority
- * \return 1 on systems that support POSIX, else 0
+ * \return RCUTILS_RET_OK on systems that support POSIX
  */
 RCUTILS_LOCAL
-int calculate_os_thread_priority(
+rcutils_ret_t calculate_os_thread_priority(
   const int thread_priority,
   int * os_priority);
 
@@ -52,10 +53,10 @@ int calculate_os_thread_priority(
  * \param[in] native_handle native thread handle
  * \param[in] priority priority of type ThreadPriority to be set for the given thread
  * \param[in] cpu_bitmask cpu core bitmask for the given thread; use (unsigned) -1 for all cores
- * \return 1 on success, 0 on error
+ * \return RCUTILS_RET_OK on success
  */
 RCUTILS_PUBLIC
-int configure_native_realtime_thread(
+rcutils_ret_t configure_native_realtime_thread(
   unsigned long int native_handle, const int priority,
   const unsigned int cpu_bitmask);
 
diff --git a/src/thread.c b/src/thread.c
index 057b6d32..100532f6 100644
--- a/src/thread.c
+++ b/src/thread.c
@@ -37,14 +37,14 @@ extern "C"
 {
 #endif
 
-int calculate_os_thread_priority(
+rcutils_ret_t calculate_os_thread_priority(
   const int thread_priority,
   int * os_priority)
 {
 #ifdef _WIN32
-  return false;
+  return RCUTILS_RET_ERROR;
 #elif __APPLE__
-  return false;
+  return RCUTILS_RET_ERROR;
 #else
   if (thread_priority == THREAD_PRIORITY_HIGH) {
     *os_priority = sched_get_priority_max(SCHED_FIFO);
@@ -56,26 +56,27 @@ int calculate_os_thread_priority(
     *os_priority =
       (sched_get_priority_min(SCHED_FIFO) + sched_get_priority_max(SCHED_FIFO)) / 2 - 1;
   } else {  // unhandled priority
-    return 0;
+    return RCUTILS_RET_ERROR;
   }
-  return 1;
+  return RCUTILS_RET_OK;
 #endif
 }
 
-int configure_native_realtime_thread(
+rcutils_ret_t configure_native_realtime_thread(
   unsigned long int native_handle, const int priority,
   const unsigned int cpu_bitmask)
 {
   int success = 1;
 #ifdef _WIN32
-  return false;
+  return RCUTILS_RET_ERROR;
 #elif __APPLE__
-  return false;
+  return RCUTILS_RET_ERROR;
 #else  // POSIX systems
   struct sched_param params;
   int policy;
   success &= (pthread_getschedparam(native_handle, &policy, &params) == 0);
-  success &= calculate_os_thread_priority(priority, &params.sched_priority);
+  success &= (calculate_os_thread_priority(priority, &params.sched_priority) ==
+              RCUTILS_RET_OK ? 1 : 0);
   success &= (pthread_setschedparam(native_handle, SCHED_FIFO, &params) == 0);
 
 #ifdef __QNXNTO__
@@ -104,7 +105,7 @@ int configure_native_realtime_thread(
 #endif  // __QNXNTO__
 #endif
 
-  return success;
+  return success ? RCUTILS_RET_OK : RCUTILS_RET_ERROR;
 }
 
 #ifdef __cplusplus

From 1c86cb717ab84729f03b2a0cfe09aebec07782a2 Mon Sep 17 00:00:00 2001
From: Martin Mayer <martin.mayer@gmx.de>
Date: Wed, 4 Jan 2023 19:27:44 +0100
Subject: [PATCH 3/9] style fix

Signed-off-by: Martin Mayer <martin.mayer@gmx.de>
---
 src/thread.c | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/thread.c b/src/thread.c
index 100532f6..9a885ada 100644
--- a/src/thread.c
+++ b/src/thread.c
@@ -76,7 +76,7 @@ rcutils_ret_t configure_native_realtime_thread(
   int policy;
   success &= (pthread_getschedparam(native_handle, &policy, &params) == 0);
   success &= (calculate_os_thread_priority(priority, &params.sched_priority) ==
-              RCUTILS_RET_OK ? 1 : 0);
+    RCUTILS_RET_OK ? 1 : 0);
   success &= (pthread_setschedparam(native_handle, SCHED_FIFO, &params) == 0);
 
 #ifdef __QNXNTO__
@@ -87,7 +87,7 @@ rcutils_ret_t configure_native_realtime_thread(
   // Function used to change thread affinity of thread associated with native_handle
   if (ThreadCtlExt(
       0, native_handle, _NTO_TCTL_RUNMASK,
-      reinterpret_cast<void *>(run_mask)) == -1)
+      (void *)run_mask) == -1)
   {
     success &= 0;
   } else {
@@ -111,4 +111,3 @@ rcutils_ret_t configure_native_realtime_thread(
 #ifdef __cplusplus
 }
 #endif
-

From bb4460355afce4142fda296ce383b1c4d16c241b Mon Sep 17 00:00:00 2001
From: Martin Mayer <martin.mayer@gmx.de>
Date: Wed, 4 Jan 2023 19:39:44 +0100
Subject: [PATCH 4/9] comment

Signed-off-by: Martin Mayer <martin.mayer@gmx.de>
---
 include/rcutils/thread.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/include/rcutils/thread.h b/include/rcutils/thread.h
index a96d732f..14769162 100644
--- a/include/rcutils/thread.h
+++ b/include/rcutils/thread.h
@@ -42,7 +42,7 @@ rcutils_ret_t calculate_os_thread_priority(
   const int thread_priority,
   int * os_priority);
 
-/// Sets the priority and cpu affinity of the given native thread.
+/// Sets a realtime priority and a cpu affinity for the given native thread.
 /**
  * This function intentionally only works on operating systems which support a FIFO thread scheduler.
  * Note for Linux: using this function requires elevated privileges and a kernel with realtime patch.

From 61c09b2c2321b0c33e32a56c574aac87ed121bff Mon Sep 17 00:00:00 2001
From: Martin Mayer <martin.mayer@gmx.de>
Date: Thu, 5 Jan 2023 15:03:36 +0100
Subject: [PATCH 5/9] extended thread priorities

Signed-off-by: Martin Mayer <martin.mayer@gmx.de>
---
 include/rcutils/thread.h | 22 +++++++++++++++++++++-
 src/thread.c             | 25 +++++++++++++++----------
 2 files changed, 36 insertions(+), 11 deletions(-)

diff --git a/include/rcutils/thread.h b/include/rcutils/thread.h
index 14769162..b8cffb8b 100644
--- a/include/rcutils/thread.h
+++ b/include/rcutils/thread.h
@@ -23,11 +23,31 @@ extern "C"
 {
 #endif
 
-/// Enum for simple configuration of threads in two priority classes.
+/// Enum for OS independent thread priorities
 enum ThreadPriority
 {
   THREAD_PRIORITY_LOW,
+  THREAD_PRIORITY_LOW_PLUS1,
+  THREAD_PRIORITY_LOW_PLUS2,
+  THREAD_PRIORITY_LOW_PLUS3,
+  THREAD_PRIORITY_LOW_PLUS4,
+  THREAD_PRIORITY_LOW_PLUS5,
+  THREAD_PRIORITY_MEDIUM_MINUS5,
+  THREAD_PRIORITY_MEDIUM_MINUS4,
+  THREAD_PRIORITY_MEDIUM_MINUS3,
+  THREAD_PRIORITY_MEDIUM_MINUS2,
+  THREAD_PRIORITY_MEDIUM_MINUS1,
   THREAD_PRIORITY_MEDIUM,
+  THREAD_PRIORITY_MEDIUM_PLUS1,
+  THREAD_PRIORITY_MEDIUM_PLUS2,
+  THREAD_PRIORITY_MEDIUM_PLUS3,
+  THREAD_PRIORITY_MEDIUM_PLUS4,
+  THREAD_PRIORITY_MEDIUM_PLUS5,
+  THREAD_PRIORITY_HIGH_MINUS5,
+  THREAD_PRIORITY_HIGH_MINUS4,
+  THREAD_PRIORITY_HIGH_MINUS3,
+  THREAD_PRIORITY_HIGH_MINUS2,
+  THREAD_PRIORITY_HIGH_MINUS1,
   THREAD_PRIORITY_HIGH
 };
 
diff --git a/src/thread.c b/src/thread.c
index 9a885ada..f152c7a0 100644
--- a/src/thread.c
+++ b/src/thread.c
@@ -46,18 +46,23 @@ rcutils_ret_t calculate_os_thread_priority(
 #elif __APPLE__
   return RCUTILS_RET_ERROR;
 #else
-  if (thread_priority == THREAD_PRIORITY_HIGH) {
-    *os_priority = sched_get_priority_max(SCHED_FIFO);
-  } else if (thread_priority == THREAD_PRIORITY_LOW) {
-    *os_priority = sched_get_priority_min(SCHED_FIFO);
-  } else if (thread_priority == THREAD_PRIORITY_MEDIUM) {
-    // Should be a value of 49 on standard Linux platforms, which is just below
-    // the default priority of 50 for threaded interrupt handling.
-    *os_priority =
-      (sched_get_priority_min(SCHED_FIFO) + sched_get_priority_max(SCHED_FIFO)) / 2 - 1;
-  } else {  // unhandled priority
+  if (thread_priority > THREAD_PRIORITY_HIGH || thread_priority < THREAD_PRIORITY_LOW) {
     return RCUTILS_RET_ERROR;
   }
+  const int max_prio = sched_get_priority_max(SCHED_FIFO);
+  const int min_prio = sched_get_priority_min(SCHED_FIFO);
+  const int range_prio = max_prio - min_prio;
+
+  int priority = min_prio + (thread_priority - THREAD_PRIORITY_LOW) *
+    range_prio / (THREAD_PRIORITY_HIGH - THREAD_PRIORITY_LOW);
+  if (priority > min_prio && priority < max_prio) {
+    // on Linux systems THREAD_PRIORITY_MEDIUM should be prio 49 instead of 50
+    // in order to not block any interrupt handlers
+    priority--;
+  }
+
+  *os_priority = priority;
+
   return RCUTILS_RET_OK;
 #endif
 }

From a8eb930557373181dd0c901a471e385dfb20349e Mon Sep 17 00:00:00 2001
From: Martin Mayer <martin.mayer@gmx.de>
Date: Thu, 5 Jan 2023 20:01:26 +0100
Subject: [PATCH 6/9] changed thread enum

Signed-off-by: Martin Mayer <martin.mayer@gmx.de>
---
 include/rcutils/thread.h | 26 ++++++++++++++++----------
 src/thread.c             | 10 +++++-----
 2 files changed, 21 insertions(+), 15 deletions(-)

diff --git a/include/rcutils/thread.h b/include/rcutils/thread.h
index b8cffb8b..379a7f81 100644
--- a/include/rcutils/thread.h
+++ b/include/rcutils/thread.h
@@ -26,14 +26,17 @@ extern "C"
 /// Enum for OS independent thread priorities
 enum ThreadPriority
 {
+  THREAD_PRIORITY_LOWEST,
+  THREAD_PRIORITY_LOWEST_PLUS1,
+  THREAD_PRIORITY_LOWEST_PLUS2,
+  THREAD_PRIORITY_LOWEST_PLUS3,
+  THREAD_PRIORITY_LOW_MINUS3,
+  THREAD_PRIORITY_LOW_MINUS2,
+  THREAD_PRIORITY_LOW_MINUS1,
   THREAD_PRIORITY_LOW,
   THREAD_PRIORITY_LOW_PLUS1,
   THREAD_PRIORITY_LOW_PLUS2,
   THREAD_PRIORITY_LOW_PLUS3,
-  THREAD_PRIORITY_LOW_PLUS4,
-  THREAD_PRIORITY_LOW_PLUS5,
-  THREAD_PRIORITY_MEDIUM_MINUS5,
-  THREAD_PRIORITY_MEDIUM_MINUS4,
   THREAD_PRIORITY_MEDIUM_MINUS3,
   THREAD_PRIORITY_MEDIUM_MINUS2,
   THREAD_PRIORITY_MEDIUM_MINUS1,
@@ -41,14 +44,17 @@ enum ThreadPriority
   THREAD_PRIORITY_MEDIUM_PLUS1,
   THREAD_PRIORITY_MEDIUM_PLUS2,
   THREAD_PRIORITY_MEDIUM_PLUS3,
-  THREAD_PRIORITY_MEDIUM_PLUS4,
-  THREAD_PRIORITY_MEDIUM_PLUS5,
-  THREAD_PRIORITY_HIGH_MINUS5,
-  THREAD_PRIORITY_HIGH_MINUS4,
   THREAD_PRIORITY_HIGH_MINUS3,
   THREAD_PRIORITY_HIGH_MINUS2,
   THREAD_PRIORITY_HIGH_MINUS1,
-  THREAD_PRIORITY_HIGH
+  THREAD_PRIORITY_HIGH,
+  THREAD_PRIORITY_HIGH_PLUS1,
+  THREAD_PRIORITY_HIGH_PLUS2,
+  THREAD_PRIORITY_HIGH_PLUS3,
+  THREAD_PRIORITY_HIGHEST_MINUS3,
+  THREAD_PRIORITY_HIGHEST_MINUS2,
+  THREAD_PRIORITY_HIGHEST_MINUS1,
+  THREAD_PRIORITY_HIGHEST
 };
 
 /// Calculates an OS specific thread priority from a ThreadPriority value.
@@ -58,7 +64,7 @@ enum ThreadPriority
  * \return RCUTILS_RET_OK on systems that support POSIX
  */
 RCUTILS_LOCAL
-rcutils_ret_t calculate_os_thread_priority(
+rcutils_ret_t calculate_os_fifo_thread_priority(
   const int thread_priority,
   int * os_priority);
 
diff --git a/src/thread.c b/src/thread.c
index f152c7a0..60f81cb3 100644
--- a/src/thread.c
+++ b/src/thread.c
@@ -37,7 +37,7 @@ extern "C"
 {
 #endif
 
-rcutils_ret_t calculate_os_thread_priority(
+rcutils_ret_t calculate_os_fifo_thread_priority(
   const int thread_priority,
   int * os_priority)
 {
@@ -46,15 +46,15 @@ rcutils_ret_t calculate_os_thread_priority(
 #elif __APPLE__
   return RCUTILS_RET_ERROR;
 #else
-  if (thread_priority > THREAD_PRIORITY_HIGH || thread_priority < THREAD_PRIORITY_LOW) {
+  if (thread_priority > THREAD_PRIORITY_HIGHEST || thread_priority < THREAD_PRIORITY_LOWEST) {
     return RCUTILS_RET_ERROR;
   }
   const int max_prio = sched_get_priority_max(SCHED_FIFO);
   const int min_prio = sched_get_priority_min(SCHED_FIFO);
   const int range_prio = max_prio - min_prio;
 
-  int priority = min_prio + (thread_priority - THREAD_PRIORITY_LOW) *
-    range_prio / (THREAD_PRIORITY_HIGH - THREAD_PRIORITY_LOW);
+  int priority = min_prio + (thread_priority - THREAD_PRIORITY_LOWEST) *
+    range_prio / (THREAD_PRIORITY_HIGHEST - THREAD_PRIORITY_LOWEST);
   if (priority > min_prio && priority < max_prio) {
     // on Linux systems THREAD_PRIORITY_MEDIUM should be prio 49 instead of 50
     // in order to not block any interrupt handlers
@@ -80,7 +80,7 @@ rcutils_ret_t configure_native_realtime_thread(
   struct sched_param params;
   int policy;
   success &= (pthread_getschedparam(native_handle, &policy, &params) == 0);
-  success &= (calculate_os_thread_priority(priority, &params.sched_priority) ==
+  success &= (calculate_os_fifo_thread_priority(priority, &params.sched_priority) ==
     RCUTILS_RET_OK ? 1 : 0);
   success &= (pthread_setschedparam(native_handle, SCHED_FIFO, &params) == 0);
 

From 05f21924a9d44531c99321757f294ab5aaeaef05 Mon Sep 17 00:00:00 2001
From: Martin Mayer <martin.mayer@gmx.de>
Date: Thu, 5 Jan 2023 20:50:03 +0100
Subject: [PATCH 7/9] added test_thread

Signed-off-by: Martin Mayer <martin.mayer@gmx.de>
---
 CMakeLists.txt       |  8 ++++++
 test/test_thread.cpp | 67 ++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 75 insertions(+)
 create mode 100644 test/test_thread.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 50309eb9..98b4bfa5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -562,6 +562,14 @@ if(BUILD_TESTING)
     test/test_macros.cpp
   )
 
+  ament_add_gtest(test_thread
+    test/test_thread.cpp
+    src/thread.c # to test RCUTILS_LOCAL functions
+  )
+  if(TARGET test_thread)
+    target_link_libraries(test_thread ${PROJECT_NAME})
+  endif()
+
   add_performance_test(benchmark_logging test/benchmark/benchmark_logging.cpp)
   if(TARGET benchmark_logging)
     target_link_libraries(benchmark_logging ${PROJECT_NAME})
diff --git a/test/test_thread.cpp b/test/test_thread.cpp
new file mode 100644
index 00000000..79ba1b8e
--- /dev/null
+++ b/test/test_thread.cpp
@@ -0,0 +1,67 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "rcutils/thread.h"
+#include "rcutils/error_handling.h"
+
+#ifdef __linux__
+#include <pthread.h>
+#endif  // __linux__
+
+TEST(test_thread, config_rt_thread) {
+#ifdef __linux__
+  const unsigned cpu_id = 0;
+  const unsigned long cpu_bitmask = 1 << cpu_id;
+  const int priority = THREAD_PRIORITY_MEDIUM;
+  if (configure_native_realtime_thread(pthread_self(), priority, cpu_bitmask) != RCUTILS_RET_OK) {
+    GTEST_SKIP() << "Unable to set realtime thread priority.";
+    return;
+  }
+
+  struct sched_param params_self;
+  int policy_self;
+  EXPECT_EQ(0, pthread_getschedparam(pthread_self(), &policy_self, &params_self));
+  int prio_calculated;
+  EXPECT_EQ(RCUTILS_RET_OK, calculate_os_fifo_thread_priority(priority, &prio_calculated));
+  EXPECT_EQ(prio_calculated, params_self.sched_priority);
+  EXPECT_EQ(SCHED_FIFO, policy_self);
+
+  cpu_set_t cpuset_self;
+  EXPECT_EQ(0, pthread_getaffinity_np(pthread_self(), sizeof(cpuset_self), &cpuset_self));
+  cpu_set_t cpuset;
+  CPU_ZERO(&cpuset);
+  CPU_SET(cpu_id, &cpuset);
+  EXPECT_EQ(0, memcmp(&cpuset, &cpuset_self, sizeof(cpu_set_t)));
+#else
+  GTEST_SKIP() << "Testcase not implemented for this platform."
+#endif  // __linux__
+}
+
+TEST(test_thread, calculate_rt_priorities) {
+#ifdef __linux__
+  int prio = -1;
+  const int max_prio = sched_get_priority_max(SCHED_FIFO);
+  const int min_prio = sched_get_priority_min(SCHED_FIFO);
+  EXPECT_EQ(RCUTILS_RET_OK, calculate_os_fifo_thread_priority(THREAD_PRIORITY_LOWEST, &prio));
+  EXPECT_EQ(min_prio, prio);
+  EXPECT_EQ(RCUTILS_RET_OK, calculate_os_fifo_thread_priority(THREAD_PRIORITY_MEDIUM, &prio));
+  EXPECT_EQ((max_prio + min_prio) / 2 - 1, prio);
+  EXPECT_EQ(RCUTILS_RET_OK, calculate_os_fifo_thread_priority(THREAD_PRIORITY_HIGHEST, &prio));
+  EXPECT_EQ(max_prio, prio);
+#elif
+  GTEST_SKIP() << "Testcase not implemented for this platform."
+#endif  // __linux__
+}

From 90d719c84c232749563f9f82d45b9e1a450205a3 Mon Sep 17 00:00:00 2001
From: Martin Mayer <martin.mayer@gmx.de>
Date: Thu, 5 Jan 2023 20:51:55 +0100
Subject: [PATCH 8/9] vaariable renamed

Signed-off-by: Martin Mayer <martin.mayer@gmx.de>
---
 test/test_thread.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/test/test_thread.cpp b/test/test_thread.cpp
index 79ba1b8e..3f4dc040 100644
--- a/test/test_thread.cpp
+++ b/test/test_thread.cpp
@@ -34,9 +34,9 @@ TEST(test_thread, config_rt_thread) {
   struct sched_param params_self;
   int policy_self;
   EXPECT_EQ(0, pthread_getschedparam(pthread_self(), &policy_self, &params_self));
-  int prio_calculated;
-  EXPECT_EQ(RCUTILS_RET_OK, calculate_os_fifo_thread_priority(priority, &prio_calculated));
-  EXPECT_EQ(prio_calculated, params_self.sched_priority);
+  int os_prio_calculated;
+  EXPECT_EQ(RCUTILS_RET_OK, calculate_os_fifo_thread_priority(priority, &os_prio_calculated));
+  EXPECT_EQ(os_prio_calculated, params_self.sched_priority);
   EXPECT_EQ(SCHED_FIFO, policy_self);
 
   cpu_set_t cpuset_self;

From af38e6d81da1c14a612e5baef8beddc2d4960418 Mon Sep 17 00:00:00 2001
From: Martin Mayer <martin.mayer@gmx.de>
Date: Thu, 5 Jan 2023 21:13:47 +0100
Subject: [PATCH 9/9] added some nolint statements

Signed-off-by: Martin Mayer <martin.mayer@gmx.de>
---
 include/rcutils/thread.h | 2 +-
 src/thread.c             | 2 +-
 test/test_thread.cpp     | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/include/rcutils/thread.h b/include/rcutils/thread.h
index 379a7f81..55c16425 100644
--- a/include/rcutils/thread.h
+++ b/include/rcutils/thread.h
@@ -83,7 +83,7 @@ rcutils_ret_t calculate_os_fifo_thread_priority(
  */
 RCUTILS_PUBLIC
 rcutils_ret_t configure_native_realtime_thread(
-  unsigned long int native_handle, const int priority,
+  unsigned long int native_handle, const int priority, // NOLINT
   const unsigned int cpu_bitmask);
 
 #ifdef __cplusplus
diff --git a/src/thread.c b/src/thread.c
index 60f81cb3..7a958b55 100644
--- a/src/thread.c
+++ b/src/thread.c
@@ -68,7 +68,7 @@ rcutils_ret_t calculate_os_fifo_thread_priority(
 }
 
 rcutils_ret_t configure_native_realtime_thread(
-  unsigned long int native_handle, const int priority,
+  unsigned long int native_handle, const int priority, // NOLINT
   const unsigned int cpu_bitmask)
 {
   int success = 1;
diff --git a/test/test_thread.cpp b/test/test_thread.cpp
index 3f4dc040..c2f5b360 100644
--- a/test/test_thread.cpp
+++ b/test/test_thread.cpp
@@ -24,7 +24,7 @@
 TEST(test_thread, config_rt_thread) {
 #ifdef __linux__
   const unsigned cpu_id = 0;
-  const unsigned long cpu_bitmask = 1 << cpu_id;
+  const unsigned long cpu_bitmask = 1 << cpu_id;  // NOLINT
   const int priority = THREAD_PRIORITY_MEDIUM;
   if (configure_native_realtime_thread(pthread_self(), priority, cpu_bitmask) != RCUTILS_RET_OK) {
     GTEST_SKIP() << "Unable to set realtime thread priority.";