feat: Thread configuration prototype

rclcpp/CMakeLists.txt

@@ -110,6 +110,16 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/waitable.cpp
 )
 
+if(${CMAKE_SYSTEM_NAME} MATCHES "Linux")
+  list(APPEND ${PROJECT_NAME}_SRCS
+    src/rclcpp/threads/posix_thread.cpp
+  )
+elseif(${CMAKE_SYSTEM_NAME} MATCHES "Windows")
+  list(APPEND ${PROJECT_NAME}_SRCS
+    src/rclcpp/threads/windows_thread.cpp
+  )
+endif()
+
 find_package(Python3 REQUIRED COMPONENTS Interpreter)
 
 # "watch" template for changes

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -18,6 +18,7 @@
 #include <chrono>
 #include <memory>
 #include <mutex>
+#include <vector>
 #include <set>
 #include <thread>
 #include <unordered_map>
@@ -26,6 +27,7 @@
 #include "rclcpp/macros.hpp"
 #include "rclcpp/memory_strategies.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/threads.hpp"
 
 namespace rclcpp
 {
@@ -79,12 +81,23 @@ class MultiThreadedExecutor : public rclcpp::Executor
   run(size_t this_thread_number);
 
 private:
+  void run_on_rclcpp_thread(
+    rclcpp::detail::ThreadAttribute & thread_attr,
+    std::vector<rclcpp::Thread> & threads,
+    size_t thread_id);
+
+  void run_on_this_thread(
+    rclcpp::detail::ThreadAttribute & thread_attr,
+    size_t thread_id);
+
   RCLCPP_DISABLE_COPY(MultiThreadedExecutor)
 
   std::mutex wait_mutex_;
   size_t number_of_threads_;
   bool yield_before_execute_;
   std::chrono::nanoseconds next_exec_timeout_;
+  rcl_thread_attrs_t * thread_attributes_;
+  bool use_thread_attrs_;
 };
 
 }  // namespace executors

rclcpp/include/rclcpp/threads.hpp

@@ -0,0 +1,26 @@
+// Copyright 2023 eSOL Co.,Ltd.
+//
+// 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 RCLCPP__THREADS_HPP_
+#define RCLCPP__THREADS_HPP_
+
+#if defined(__linux__)
+#include "rclcpp/threads/posix/thread.hpp"
+#elif defined(_WIN32)
+#include "rclcpp/threads/win32/thread.hpp"
+#else
+#include "rclcpp/threads/std/thread.hpp"
+#endif
+
+#endif  // RCLCPP__THREADS_HPP_

rclcpp/include/rclcpp/threads/posix/linux/cpu_set.hpp

@@ -0,0 +1,119 @@
+// Copyright 2023 eSOL Co.,Ltd.
+//
+// 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 RCLCPP__THREADS__POSIX__LINUX__CPU_SET_HPP_
+#define RCLCPP__THREADS__POSIX__LINUX__CPU_SET_HPP_
+
+#include <pthread.h>
+#include <vector>
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+namespace detail
+{
+
+struct CpuSet
+{
+  using NativeCpuSetType = cpu_set_t;
+  CpuSet()
+  {
+    int processor_num = static_cast<int>(sysconf(_SC_NPROCESSORS_ONLN));
+    cpu_size_ = CPU_ALLOC_SIZE(processor_num);
+    cpu_set_ = CPU_ALLOC(cpu_size_);
+    CPU_ZERO_S(cpu_size_, cpu_set_);
+  }
+  explicit CpuSet(int num_cpu)
+  {
+    int processor_num = static_cast<int>(sysconf(_SC_NPROCESSORS_ONLN));
+    cpu_size_ = CPU_ALLOC_SIZE(processor_num);
+    cpu_set_ = CPU_ALLOC(cpu_size_);
+    CPU_ZERO_S(cpu_size_, cpu_set_);
+    CPU_SET_S(num_cpu, cpu_size_, cpu_set_);
+  }
+  CpuSet(const CpuSet & cpuset)
+  {
+    int processor_num = static_cast<int>(sysconf(_SC_NPROCESSORS_ONLN));
+    cpu_size_ = cpuset.get_cpu_size();
+    cpu_set_ = CPU_ALLOC(cpu_size_);
+    CPU_ZERO_S(cpu_size_, cpu_set_);
+    for (int i = 0; i < processor_num; i++) {
+      if (CPU_ISSET_S(i, cpu_size_, cpuset.native_cpu_set())) {
+        CPU_SET_S(i, cpu_size_, cpu_set_);
+      }
+    }
+  }
+  CpuSet & operator=(CpuSet const & cpuset)
+  {
+    int processor_num = static_cast<int>(sysconf(_SC_NPROCESSORS_ONLN));
+    cpu_size_ = cpuset.get_cpu_size();
+    cpu_set_ = CPU_ALLOC(cpu_size_);
+    CPU_ZERO_S(cpu_size_, cpu_set_);
+    for (int i = 0; i < processor_num; i++) {
+      if (CPU_ISSET_S(i, cpu_size_, cpuset.native_cpu_set())) {
+        CPU_SET_S(i, cpu_size_, cpu_set_);
+      }
+    }
+    return *this;
+  }
+  CpuSet(CpuSet &&) = delete;
+  CpuSet & operator=(CpuSet &&) = delete;
+  ~CpuSet()
+  {
+    CPU_FREE(cpu_set_);
+    cpu_size_ = 0;
+  }
+  void set(int cpu)
+  {
+    int processor_num = static_cast<int>(sysconf(_SC_NPROCESSORS_ONLN));
+    if (0 > cpu || processor_num <= cpu) {
+      auto ec = std::make_error_code(std::errc::invalid_argument);
+      throw std::system_error{ec, "cpu number is invaild"};
+    }
+    CPU_SET_S(cpu, cpu_size_, cpu_set_);
+  }
+  void unset(int cpu)
+  {
+    int processor_num = static_cast<int>(sysconf(_SC_NPROCESSORS_ONLN));
+    if (0 > cpu || processor_num <= cpu) {
+      auto ec = std::make_error_code(std::errc::invalid_argument);
+      throw std::system_error{ec, "cpu number is invaild"};
+    }
+    CPU_CLR_S(cpu, cpu_size_, cpu_set_);
+  }
+  void clear()
+  {
+    CPU_ZERO_S(cpu_size_, cpu_set_);
+  }
+  bool is_set(int cpu)
+  {
+    return CPU_ISSET_S(cpu, cpu_size_, cpu_set_);
+  }
+  size_t get_cpu_size() const
+  {
+    return cpu_size_;
+  }
+  NativeCpuSetType * native_cpu_set() const {return cpu_set_;}
+
+private:
+  NativeCpuSetType * cpu_set_;
+  size_t cpu_size_;
+};
+}  // namespace detail
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__THREADS__POSIX__LINUX__CPU_SET_HPP_

rclcpp/include/rclcpp/threads/posix/thread.hpp

@@ -0,0 +1,141 @@
+// Copyright 2023 eSOL Co.,Ltd.
+//
+// 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 RCLCPP__THREADS__POSIX__THREAD_HPP_
+#define RCLCPP__THREADS__POSIX__THREAD_HPP_
+
+#include <pthread.h>
+#include <unistd.h>
+#include <condition_variable>
+#include <cstdio>
+#include <memory>
+#include <mutex>
+#include <system_error>
+#include <tuple>
+#include <type_traits>
+#include <utility>
+
+#include "rclcpp/threads/posix/thread_attribute.hpp"
+#include "rclcpp/threads/posix/thread_func.hpp"
+#include "rclcpp/threads/posix/thread_id.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+struct Thread
+{
+  using NativeHandleType = pthread_t;
+  using Attribute = detail::ThreadAttribute;
+  using Id = detail::ThreadId;
+
+  // Assume pthread_t is an invalid handle if it's 0
+  Thread() noexcept
+  : handle_{} {}
+  Thread(Thread && other)
+  : handle_(other.handle_) {other.handle_ = NativeHandleType{};}
+  template<typename F, typename ... Args,
+    typename = std::enable_if_t<!std::is_same<std::decay_t<F>, Attribute>::value>>
+  explicit Thread(F && f, Args && ... args)
+  : Thread(static_cast<Attribute *>(nullptr),
+      make_thread_func(nullptr, std::forward<F>(f), std::forward<Args>(args)...))
+  {}
+  template<typename F, typename ... Args>
+  Thread(Attribute & attr, F && f, Args && ... args)
+  : Thread(&attr, make_thread_func(&attr, std::forward<F>(f), std::forward<Args>(args)...))
+  {}
+  Thread(Thread const &) = delete;
+  ~Thread()
+  {
+    if (handle_) {std::terminate();}
+  }
+
+  Thread & operator=(Thread && other) noexcept
+  {
+    if (handle_) {std::terminate();}
+    swap(other);
+    return *this;
+  }
+
+  Thread & operator=(Thread const &) = delete;
+
+  void swap(Thread & other)
+  {
+    using std::swap;
+    swap(handle_, other.handle_);
+  }
+
+  void join()
+  {
+    void * p;
+    int r = pthread_join(handle_, &p);
+    if (r != 0) {throw std::system_error(r, std::system_category(), "Error in pthread_join ");}
+    handle_ = NativeHandleType{};
+  }
+
+  bool joinable() const noexcept
+  {
+    return 0 == pthread_equal(handle_, NativeHandleType{});
+  }
+
+  void detach()
+  {
+    int r = pthread_detach(handle_);
+    if (r != 0) {throw std::system_error(r, std::system_category(), "Error in detach ");}
+    handle_ = NativeHandleType{};
+  }
+
+  NativeHandleType native_handle() const
+  {
+    return handle_;
+  }
+
+  Id get_id() const noexcept
+  {
+    return Id{handle_};
+  }
+
+  static int hardware_concurrency() noexcept
+  {
+    return static_cast<int>(sysconf(_SC_NPROCESSORS_ONLN));
+  }
+
+private:
+  using ThreadFuncUniquePtr = detail::ThreadFuncUniquePtr;
+
+  Thread(Attribute * attr, ThreadFuncUniquePtr func);
+
+  template<typename F, typename ... Args>
+  static ThreadFuncUniquePtr make_thread_func(Attribute * attr, F && f, Args && ... args)
+  {
+    using detail::ThreadFunc;
+    using detail::ThreadFuncImplementation;
+    ThreadFunc * func = new ThreadFuncImplementation<std::decay_t<F>, std::decay_t<Args>...>(
+      attr, std::forward<F>(f), std::forward<Args>(args)...
+    );
+
+    return ThreadFuncUniquePtr{func};
+  }
+
+  NativeHandleType handle_;
+};
+
+inline void swap(Thread & t1, Thread & t2)
+{
+  t1.swap(t2);
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__THREADS__POSIX__THREAD_HPP_