ROS2 adapter WIP

.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,11 @@
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 
+target_sources(cactus_rt
+  PRIVATE
+  src/cactus_rt/ros2/ros2_adapter.cc
+)
+
 ament_target_dependencies(cactus_rt
   PUBLIC
   rclcpp

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,90 @@
-int main() {
+#include <cactus_rt/ros2/ros2_adapter.h>
+#include <cactus_rt/rt.h>
+
+#include <iostream>
+#include <memory>
+#include <std_msgs/msg/int64.hpp>
+
+using cactus_rt::App;
+using cactus_rt::CyclicThread;
+
+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 {
+  int64_t                                                            loop_counter_ = 0;
+  std::shared_ptr<cactus_rt::ros2::Ros2Adapter>                      ros2_adapter_;
+  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, std::shared_ptr<cactus_rt::ros2::Ros2Adapter> ros2_adapter) : CyclicThread(name, CreateThreadConfig()), ros2_adapter_(ros2_adapter) {
+    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_);
+      publisher_->Publish(loop_counter_);
+    }
+    return false;
+  }
+};
+
+int main(int argc, char* argv[]) {
+  rclcpp::init(argc, argv);
+
+  App app;
+  app.StartTraceSession("build/data.perfetto");
+
+  auto ros2_adapter = std::make_shared<cactus_rt::ros2::Ros2Adapter>("cactus_rt_ros2_example", cactus_rt::ros2::Ros2Adapter::Config{});
+  auto thread = std::make_shared<ExampleRTThread>("ExampleRTThread", ros2_adapter);
+
+  app.RegisterThread(thread);
+
+  constexpr unsigned int time = 5;
+  std::cout << "Testing RT loop for " << time << " seconds.\n";
+
+  app.Start();
+
+  rclcpp::spin(ros2_adapter->Node());
+  rclcpp::shutdown();
+
+  // 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/ros2/app.h

@@ -0,0 +1,17 @@
+#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 App : cactus_rt::App {
+  std::shared_ptr<Ros2Adapter> ros2_adapter;
+
+ public:
+  explicit App(std::string name = "RTROS2App", cactus_rt::AppConfig config = cactus_rt::AppConfig());
+  ~App() override;
+};
+}  // namespace cactus_rt::ros2
+#endif

include/cactus_rt/ros2/ros2_adapter.h

@@ -1,4 +1,152 @@
+#ifndef CACTUS_RT_ROS2_ROS2_ADAPTER_H_
+#define CACTUS_RT_ROS2_ROS2_ADAPTER_H_
+
+#include <readerwriterqueue.h>
+
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <optional>
+#include <rclcpp/rclcpp.hpp>
+#include <stdexcept>
+#include <string>
+#include <type_traits>
+#include <utility>
+#include <vector>
+
 namespace cactus_rt::ros2 {
-class Ros2Adapter {
+
+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;
+
+  // TODO: there are other rclcpp::Publisher::publish methods that can implement.
+  virtual void                               PublishToRos(const void* rt_msg) const = 0;
+  virtual std::pair<bool, const void* const> DequeueFromRT() = 0;
+
+ public:
+  virtual ~IPublisher();
+};
+
+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_;
+
+  // This is used in dequeue from RT to
+  RealtimeT data_;
+
+  void PublishToRos(const void* rt_msg) const override {
+    const auto* rt_msg_typed = static_cast<const RealtimeT*>(rt_msg);
+
+    if (converter_) {
+      auto loaned_msg = publisher_->borrow_loaned_message();
+      converter_.value()(*rt_msg_typed, loaned_msg.get());
+      publisher_->publish(std::move(loaned_msg));
+    } else {
+      if constexpr (std::is_same_v<RealtimeT, RosT>) {
+        const auto* ros_msg_typed = static_cast<const RosT*>(rt_msg_typed);
+        publisher_->publish(*ros_msg_typed);
+      } else {
+        throw std::invalid_argument{"converter not specified but RealtimeT and RosT are not the same?!"};
+      }
+    }
+  }
+
+  std::pair<bool, const void* const> DequeueFromRT() override {
+    // One copy
+    bool ok = queue_.try_dequeue(data_);
+    return std::make_pair(ok, &data_);
+  }
+
+ 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)) {}
+
+  template <typename... Args>
+  bool Publish(Args&&... args) noexcept {
+    bool success = queue_.try_emplace(std::forward<Args>(args)...);
+    // TODO: Keep track of success/failed messages and expose that to be queried
+    return success;
+  }
+};
+
+class Ros2Adapter : private rclcpp::Node {
+ public:
+  struct Config {
+    /**
+     * The time interval for which the adapter node is polling for publisher data.
+     */
+    std::chrono::microseconds timer_interval = std::chrono::microseconds(200'000);
+  };
+
+ private:
+  std::string node_name_;
+
+  // Protects the vector of subscribers, publishers, and services
+  // This means that during the timer callback, no subscribers, publishers, services, and etc. can be changed.
+  std::mutex mut_;
+
+  // Timer callback id
+
+  // Publisher data
+  // TODO: the shared_ptr is supposed to give this to the real-time thread, but not sure if this is the best idea.
+  std::vector<std::shared_ptr<IPublisher>> publishers_;
+
+ public:
+  Ros2Adapter(const std::string& name_, const Config& config);
+
+  std::shared_ptr<rclcpp::Node> Node() {
+    return shared_from_this();
+  }
+
+  template <typename RealtimeT, typename RosT>
+  std::shared_ptr<Publisher<RealtimeT, RosT>> CreatePublisher(
+    const std::string&                                      topic_name,
+    const rclcpp::QoS&                                      qos,
+    std::optional<RealtimeToROS2Converter<RealtimeT, RosT>> converter,
+    size_t                                                  rt_queue_size = 1000
+  ) {
+    if (!converter) {
+      if constexpr (!(std::is_trivial_v<RosT> && std::is_standard_layout_v<RosT> && std::is_same_v<RosT, RealtimeT>)) {
+        throw std::invalid_argument("RosT and RealtimeT must be the same and must be a plain object for converter not to be specified");
+      }
+    }
+
+    typename rclcpp::Publisher<RosT>::SharedPtr       publisher = this->create_publisher<RosT>(topic_name, qos);
+    typename moodycamel::ReaderWriterQueue<RealtimeT> queue{rt_queue_size};
+
+    auto publisher_bundle = std::make_shared<Publisher<RealtimeT, RosT>>(
+      std::move(publisher),
+      converter,
+      std::move(queue)
+    );
+
+    const std::scoped_lock lock(mut_);
+    publishers_.push_back(publisher_bundle);
+    return publisher_bundle;
+  }
+
+ private:
+  void TimerCallback();
 };
 }  // namespace cactus_rt::ros2
+
+#endif

package.xml

@@ -11,6 +11,8 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+
+  <build_depend>std_msgs</build_depend> <!-- Needed for the example -->
   <build_depend>protobuf-dev</build_depend>
   <exec_depend>protobuf</exec_depend>
   <test_depend>gtest</test_depend>

src/cactus_rt/ros2/ros2_adapter.cc

@@ -0,0 +1,27 @@
+#include "cactus_rt/ros2/ros2_adapter.h"
+
+#include <readerwriterqueue.h>
+
+namespace cactus_rt::ros2 {
+
+Ros2Adapter::Ros2Adapter(const std::string& name, const Ros2Adapter::Config& config) : rclcpp::Node(name + "_ros_adapter") {
+  this->create_wall_timer(config.timer_interval, [this] { TimerCallback(); });
+}
+
+void Ros2Adapter::TimerCallback() {
+  for (const auto& publisher : publishers_) {
+    // TODO: hopefully the thread is not publishing so quickly that a single
+    // publisher monopolizes all resources. That said, if that happens the
+    // program is likely in bigger trouble anyway.
+    while (true) {
+      const auto [has_data, rt_msg_ptr] = publisher->DequeueFromRT();
+      if (!has_data) {
+        break;
+      }
+
+      publisher->PublishToRos(rt_msg_ptr);
+    }
+  }
+}
+
+}  // namespace cactus_rt::ros2