Merge pull request #91 from cactusdynamics/ros2

Added ROS2 integration by allowing a real-time thread to publish

.gitignore

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

CMakeLists.txt

@@ -6,6 +6,7 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 option(ENABLE_CLANG_TIDY "Run clang-tidy" OFF)
 option(ENABLE_EXAMPLES "Build example programs" ON)
 option(ENABLE_TRACING "Enable runtime tracing support" ON)
+option(ENABLE_ROS2 "Enables ROS2 support" OFF)
 option(BUILD_DOCS "Build documentations" OFF)
 
 # Used for building cactus-rt when all dependencies are vendored
@@ -104,8 +105,10 @@ endfunction()
 # Cactus RT library #
 #####################
 
-if (ENABLE_TRACING)
-  add_subdirectory(protos)
+if(${CMAKE_PROJECT_NAME} STREQUAL ${PROJECT_NAME})
+  if (ENABLE_TRACING)
+    add_subdirectory(protos)
+  endif()
 endif()
 
 add_library(cactus_rt
@@ -133,35 +136,40 @@ target_link_libraries(cactus_rt
 # Use a bounded queue
 target_compile_definitions(cactus_rt PUBLIC QUILL_USE_BOUNDED_QUEUE)
 
-if (ENABLE_TRACING)
-  target_sources(cactus_rt
-    PRIVATE
-    src/cactus_rt/tracing/sink.cc
-    src/cactus_rt/tracing/thread_tracer.cc
-    src/cactus_rt/tracing/trace_aggregator.cc
-    src/cactus_rt/tracing/tracing_enabled.cc
-    src/cactus_rt/tracing/utils/string_interner.cc
-  )
+if(${CMAKE_PROJECT_NAME} STREQUAL ${PROJECT_NAME})
+  if (ENABLE_TRACING)
+    target_sources(cactus_rt
+      PRIVATE
+      src/cactus_rt/tracing/sink.cc
+      src/cactus_rt/tracing/thread_tracer.cc
+      src/cactus_rt/tracing/trace_aggregator.cc
+      src/cactus_rt/tracing/tracing_enabled.cc
+      src/cactus_rt/tracing/utils/string_interner.cc
+    )
 
-  target_link_libraries(cactus_rt
-    PUBLIC
-    cactus_tracing_embedded_perfetto_protos
-  )
+    target_link_libraries(cactus_rt
+      PUBLIC
+      cactus_tracing_embedded_perfetto_protos
+    )
 
-  target_compile_definitions(cactus_rt
-    PUBLIC
-    CACTUS_RT_TRACING_ENABLED=1
-  )
-endif()
+    target_compile_definitions(cactus_rt
+      PUBLIC
+      CACTUS_RT_TRACING_ENABLED=1
+    )
+  endif()
 
-if(${CMAKE_PROJECT_NAME} STREQUAL ${PROJECT_NAME})
   if (ENABLE_CLANG_TIDY)
     find_program(CLANG_TIDY clang-tidy clang-tidy-18 clang-tidy-17 clang-tidy-16 clang-tidy-15 clang-tidy-14)
   else()
     message(STATUS "Not running clang-tidy. Use ENABLE_CLANG_TIDY=ON to run clang-tidy.")
   endif()
 endif()
 
+# 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

cmake/ros2.cmake

@@ -0,0 +1,17 @@
+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
+)
+
+add_subdirectory(examples/ros2/publisher)
+
+ament_package()

colcon.pkg

@@ -0,0 +1,4 @@
+---
+cmake-args:
+  - "-DENABLE_ROS2=ON"
+  - "-DENABLE_EXAMPLES=OFF"

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/publisher/CMakeLists.txt

@@ -0,0 +1,22 @@
+find_package(std_msgs REQUIRED)
+
+add_executable(rt_ros2_publisher
+  main.cc
+)
+
+target_link_libraries(rt_ros2_publisher
+  PRIVATE
+  cactus_rt
+)
+
+setup_cactus_rt_target_options(rt_ros2_publisher)
+
+ament_target_dependencies(rt_ros2_publisher
+  PUBLIC
+  std_msgs
+)
+
+install(
+  TARGETS rt_ros2_publisher
+  DESTINATION lib/${PROJECT_NAME}
+)

examples/ros2/publisher/main.cc

@@ -0,0 +1,87 @@
+#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;
+
+  explicit 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
+
+class RealtimeROS2PublisherThread : 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:
+  RealtimeROS2PublisherThread(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.CreateROS2EnabledThread<RealtimeROS2PublisherThread>("RTROS2PublisherThread");
+  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,68 @@
+#ifndef CACTUS_RT_ROS2_APP_H_
+#define CACTUS_RT_ROS2_APP_H_
+#include <memory>
+#include <optional>
+#include <type_traits>
+
+#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> CreateROS2EnabledThread(Args&&... args) {
+    static_assert(std::is_base_of_v<Ros2ThreadMixin, ThreadT>, "Must derive ROS2 thread from Ros2ThreadMixin");
+    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