Added ROS2 publishing example

.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,11 +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_example)
+add_subdirectory(examples/ros2/publisher)
 
 ament_package()

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,88 @@
+#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
+
+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,67 @@
+#ifndef CACTUS_RT_ROS2_APP_H_
+#define CACTUS_RT_ROS2_APP_H_
+#include <memory>
+#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

include/cactus_rt/ros2/publisher.h

@@ -0,0 +1,92 @@
+#ifndef CACTUS_RT_ROS2_PUBLISHER_H_
+#define CACTUS_RT_ROS2_PUBLISHER_H_
+
+#include <readerwriterqueue.h>
+
+#include <functional>
+#include <optional>
+#include <rclcpp/rclcpp.hpp>
+
+namespace cactus_rt::ros2 {
+class Ros2Adapter;
+
+template <typename RealtimeT, typename RosT>
+using RealtimeToROS2Converter = std::function<void(const RealtimeT&, RosT&)>;
+
+template <typename RealtimeT, typename RosT>
+using Ros2ToRealtimeConverter = std::function<RealtimeT(const RosT&)>;
+
+class IPublisher {
+  friend class Ros2Adapter;
+
+  virtual bool DequeueAndPublishToRos() = 0;
+  virtual void FullyDrainAndPublishToRos() = 0;
+
+ public:
+  virtual ~IPublisher() = 0;
+};
+
+template <typename RealtimeT, typename RosT>
+class Publisher : public IPublisher {
+  typename rclcpp::Publisher<RosT>::SharedPtr             publisher_;
+  std::optional<RealtimeToROS2Converter<RealtimeT, RosT>> converter_;
+  moodycamel::ReaderWriterQueue<RealtimeT>                queue_;
+
+  bool DequeueAndPublishToRos() override {
+    RealtimeT rt_msg;
+
+    const bool has_data = queue_.try_dequeue(rt_msg);
+    if (!has_data) {
+      return false;
+    }
+
+    if (converter_) {
+      auto loaned_msg = publisher_->borrow_loaned_message();
+      converter_.value()(rt_msg, loaned_msg.get());
+      publisher_->publish(std::move(loaned_msg));
+    } else {
+      if constexpr (std::is_same_v<RealtimeT, RosT>) {
+        const auto* ros_msg = static_cast<const RosT*>(rt_msg);
+        publisher_->publish(*ros_msg);
+      } else {
+        throw std::invalid_argument{"converter not specified but RealtimeT and RosT are not the same?!"};
+      }
+    }
+
+    return true;
+  }
+
+  void FullyDrainAndPublishToRos() override {
+    while (true) {
+      const auto has_data = DequeueAndPublishToRos();
+      if (!has_data) {
+        break;
+      }
+    }
+  }
+
+ public:
+  /**
+   * Constructs a publisher. Shouldn't be called directly. Only public to enable make_shared.
+   *
+   * @private
+   */
+  Publisher(
+    typename rclcpp::Publisher<RosT>::SharedPtr             publisher,
+    std::optional<RealtimeToROS2Converter<RealtimeT, RosT>> converter,
+    moodycamel::ReaderWriterQueue<RealtimeT>&&              queue
+  ) : publisher_(publisher), converter_(converter), queue_(std::move(queue)) {}
+
+  ~Publisher() override = default;
+
+  template <typename... Args>
+  bool Publish(Args&&... args) noexcept {
+    const bool success = queue_.try_emplace(std::forward<Args>(args)...);
+    // TODO: Keep track of success/failed messages and expose that to be queried
+    return success;
+  }
+};
+
+}  // namespace cactus_rt::ros2
+
+#endif