Ability to publish to ROS2 from a real-time thread

.gitignore

@@ -2,3 +2,5 @@ build/
 .cache/
 
 .vscode/settings.json
+log/
+install/

CMakeLists.txt

@@ -165,13 +165,13 @@ if(${CMAKE_PROJECT_NAME} STREQUAL ${PROJECT_NAME})
   endif()
 endif()
 
-setup_cactus_rt_target_options(cactus_rt)
-
 # ROS 2 build support
 if (ENABLE_ROS2)
   include(cmake/ros2.cmake)
 endif()
 
+setup_cactus_rt_target_options(cactus_rt)
+
 # Build tests, examples, docs, only if this project is not embedded in another
 # project.
 if(${CMAKE_PROJECT_NAME} STREQUAL ${PROJECT_NAME})

cmake/ros2.cmake

@@ -1,6 +1,12 @@
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 
+target_sources(cactus_rt
+  PRIVATE
+  src/cactus_rt/ros2/ros2_adapter.cc
+  src/cactus_rt/ros2/app.cc
+)
+
 ament_target_dependencies(cactus_rt
   PUBLIC
   rclcpp

docs/ros2.md

@@ -0,0 +1,6 @@
+ROS2 integration with cactus-rt
+===============================
+
+![cactus-rt ROS2 integration object ownership diagram](imgs/ROS2Ownership.svg)
+
+![cactus-rt ROS2 publisher data flow](imgs/ROS2Publisher.svg)

examples/ros2_example/CMakeLists.txt

@@ -1,3 +1,5 @@
+find_package(std_msgs REQUIRED)
+
 add_executable(rt_ros2_example
   main.cc
 )
@@ -9,6 +11,11 @@ target_link_libraries(rt_ros2_example
 
 setup_cactus_rt_target_options(rt_ros2_example)
 
+ament_target_dependencies(rt_ros2_example
+  PUBLIC
+  std_msgs
+)
+
 install(
   TARGETS rt_ros2_example
   DESTINATION lib/${PROJECT_NAME}

examples/ros2_example/main.cc

@@ -1,3 +1,91 @@
-int main() {
+#include <cactus_rt/ros2/app.h>
+#include <cactus_rt/rt.h>
+
+#include <chrono>
+#include <iostream>
+#include <memory>
+#include <std_msgs/msg/int64.hpp>
+#include <thread>
+
+using cactus_rt::CyclicThread;
+using cactus_rt::ros2::App;
+
+struct RealtimeData {
+  int64_t data;
+
+  RealtimeData() = default;
+  RealtimeData(int64_t d) : data(d) {}
+};
+using RosData = std_msgs::msg::Int64;
+
+namespace {
+void RealtimeToROS2ConverterFunc(const RealtimeData& rt_data, RosData& ros_data) {
+  // A bit of a silly example, but gets the point across.
+  ros_data.data = rt_data.data;
+}
+}  // namespace
+
+/**
+ * This is a no-op thread that does nothing at 1 kHz.
+ */
+class ExampleRTThread : public CyclicThread, public cactus_rt::ros2::Ros2ThreadMixin {
+  int64_t                                                            loop_counter_ = 0;
+  std::shared_ptr<cactus_rt::ros2::Publisher<RealtimeData, RosData>> publisher_;
+
+  static cactus_rt::CyclicThreadConfig CreateThreadConfig() {
+    cactus_rt::CyclicThreadConfig thread_config;
+    thread_config.period_ns = 1'000'000;
+    thread_config.cpu_affinity = std::vector<size_t>{2};
+    thread_config.SetFifoScheduler(80);
+
+    // thread_config.tracer_config.trace_sleep = true;
+    thread_config.tracer_config.trace_wakeup_latency = true;
+    return thread_config;
+  }
+
+ public:
+  ExampleRTThread(const char* name) : CyclicThread(name, CreateThreadConfig()) {}
+
+  void InitializeForRos2() override {
+    publisher_ = ros2_adapter_->CreatePublisher<RealtimeData, RosData>("/hello", rclcpp::QoS(10), RealtimeToROS2ConverterFunc);
+  }
+
+  int64_t GetLoopCounter() const {
+    return loop_counter_;
+  }
+
+ protected:
+  bool Loop(int64_t /*now*/) noexcept final {
+    loop_counter_++;
+    if (loop_counter_ % 1000 == 0) {
+      LOG_INFO(Logger(), "Loop {}", loop_counter_);
+
+      const auto span = Tracer().WithSpan("Publish");
+      publisher_->Publish(loop_counter_);
+    }
+    return false;
+  }
+};
+
+int main(int argc, char* argv[]) {
+  rclcpp::init(argc, argv);
+
+  App app;
+  app.StartTraceSession("build/data.perfetto");
+
+  auto thread = app.CreateThread<ExampleRTThread>("ExampleRTThread");
+  app.RegisterThread(thread);
+
+  constexpr unsigned int time = 30;
+  std::cout << "Testing RT loop for " << time << " seconds.\n";
+
+  app.Start();
+
+  std::this_thread::sleep_for(std::chrono::seconds(time));
+
+  app.RequestStop();
+  app.Join();
+
+  std::cout << "Number of loops executed: " << thread->GetLoopCounter() << "\n";
   return 0;
 }

include/cactus_rt/app.h

@@ -70,11 +70,22 @@ class App {
    */
   void RegisterThread(std::shared_ptr<Thread> thread);
 
+  /**
+   * @brief Sets up the trace aggregator. Call this before starting the thread
+   * if you don't want to call RegisterThread and maintain tracing capabilities.
+   */
+  void SetupTraceAggregator(Thread& thread);
+
   /**
    * @brief Starts the app by locking the memory and reserving the memory. Also
    * start all the threads in registration order.
+   *
+   * @param start_monotonic_time_ns The start time of the monotonic clock, if
+   * you prefer to set a time. Mostly used for child class of App to keep a
+   * consistent start time. Leave as default or pass -1 for the function to
+   * automatically determine a time (i.e. most situations).
    */
-  virtual void Start();
+  virtual void Start(int64_t start_monotonic_time_ns = -1);
 
   /**
    * @brief sends RequestStop to all threads in registration order.

include/cactus_rt/ros2/app.h

@@ -0,0 +1,65 @@
+#ifndef CACTUS_RT_ROS2_APP_H_
+#define CACTUS_RT_ROS2_APP_H_
+#include <memory>
+
+#include "../app.h"
+#include "ros2_adapter.h"
+
+namespace cactus_rt::ros2 {
+
+class Ros2ThreadMixin {
+ protected:
+  std::shared_ptr<Ros2Adapter> ros2_adapter_;
+
+ public:
+  void SetRos2Adapter(std::shared_ptr<Ros2Adapter> ros2_adapter) {
+    ros2_adapter_ = ros2_adapter;
+  }
+
+  virtual void InitializeForRos2() = 0;
+  virtual ~Ros2ThreadMixin() = 0;
+};
+
+class Ros2ExecutorThread : public cactus_rt::Thread, public cactus_rt::ros2::Ros2ThreadMixin {
+  std::optional<rclcpp::executors::SingleThreadedExecutor> executor_;
+
+  static cactus_rt::ThreadConfig CreateThreadConfig();
+
+ public:
+  Ros2ExecutorThread();
+
+  void Run() override;
+
+  void InitializeForRos2() override {}
+};
+
+class App : public cactus_rt::App {
+  std::shared_ptr<Ros2Adapter>        ros2_adapter_;
+  std::shared_ptr<Ros2ExecutorThread> ros2_executor_thread_;
+
+ public:
+  explicit App(
+    std::string          name = "RTROS2App",
+    cactus_rt::AppConfig config = cactus_rt::AppConfig(),
+    Ros2Adapter::Config  ros2_adapter_config = Ros2Adapter::Config()
+  );
+
+  template <typename ThreadT, typename... Args>
+  std::shared_ptr<ThreadT> CreateThread(Args&&... args) {
+    std::shared_ptr<ThreadT> thread = std::make_shared<ThreadT>(std::forward<Args>(args)...);
+
+    thread->SetRos2Adapter(ros2_adapter_);
+    thread->InitializeForRos2();
+
+    return thread;
+  }
+
+  void Start(int64_t start_monotonic_time_ns = -1) override;
+
+  void RequestStop() override;
+
+  void Join() override;
+};
+
+}  // namespace cactus_rt::ros2
+#endif