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