WIP lockless example

CMakeLists.txt

@@ -174,8 +174,9 @@ if(${CMAKE_PROJECT_NAME} STREQUAL ${PROJECT_NAME})
 
   if (ENABLE_EXAMPLES)
     message(STATUS "Building example programs. Turn it off via ENABLE_EXAMPLES=OFF")
-    add_subdirectory(examples/message_passing_example)
+    add_subdirectory(examples/lockless_example)
     add_subdirectory(examples/logging_example)
+    add_subdirectory(examples/message_passing_example)
     add_subdirectory(examples/mutex_example)
     add_subdirectory(examples/signal_handling_example)
     add_subdirectory(examples/simple_deadline_example)

examples/lockless_example/CMakeLists.txt

@@ -0,0 +1,10 @@
+add_executable(rt_lockless_example
+  main.cc
+)
+
+target_link_libraries(rt_lockless_example
+  PRIVATE
+  cactus_rt
+)
+
+setup_cactus_rt_target_options(rt_lockless_example)

examples/lockless_example/main.cc

@@ -0,0 +1,145 @@
+#include <cactus_rt/experimental/lockless.h>
+#include <cactus_rt/rt.h>
+
+#include <chrono>
+#include <iostream>
+
+using cactus_rt::App;
+using cactus_rt::CyclicThread;
+using cactus_rt::Thread;
+using cactus_rt::experimental::lockless::AtomicMessage;
+using cactus_rt::experimental::lockless::spsc::RealtimeReadableValue;
+using namespace std::chrono_literals;
+
+struct Pose {
+  // We want default constructed values to have a flag showing it is default
+  // constructed. This is because the RealtimeReadableValue will default
+  // construct a value and it can immediately be read. We need to tell the writer
+  // it is invalid. It may not be necessary to do this in general.
+  bool   valid = false;
+  double x = 0.0;
+  double y = 0.0;
+  double z = 0.0;
+  double roll = 0.0;
+  double pitch = 0.0;
+  double yaw = 0.0;
+
+  Pose() {}
+
+  Pose(double xx, double yy, double zz, double ro, double pi, double ya) : valid(true),
+                                                                           x(xx),
+                                                                           y(yy),
+                                                                           z(zz),
+                                                                           roll(ro),
+                                                                           pitch(pi),
+                                                                           yaw(ya) {}
+};
+
+bool operator==(const Pose& p1, const Pose& p2) {
+  return p1.x == p2.x &&
+         p1.y == p2.y &&
+         p1.z == p2.z &&
+         p1.roll == p2.roll &&
+         p1.pitch == p2.pitch &&
+         p1.yaw == p2.yaw;
+}
+
+bool operator!=(const Pose& p1, const Pose& p2) {
+  return !(p1 == p2);
+}
+
+/**
+ * A struct that holds all the shared data so it can be passed to both the real-time and non-real-time threads
+ */
+struct Context {
+  AtomicMessage<bool>         done = false;
+  RealtimeReadableValue<Pose> target_pose = {};
+};
+
+/**
+ * This is a real-time thread
+ */
+class RTThread : public CyclicThread {
+  Context& ctx_;
+  Pose     current_target_pose_ = {};
+
+  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);
+
+    return thread_config;
+  }
+
+ public:
+  RTThread(Context& ctx) : CyclicThread("RTThread", CreateThreadConfig()), ctx_(ctx) {}
+
+ protected:
+  bool Loop(int64_t /*now*/) noexcept final {
+    if (ctx_.done.Read()) {
+      return true;
+    }
+
+    Pose new_pose = ctx_.target_pose.Read();
+    if (!new_pose.valid) {
+      return false;
+    }
+
+    if (new_pose != current_target_pose_) {
+      current_target_pose_ = new_pose;
+      LOG_INFO(
+        Logger(),
+        "detected new pose: {} {} {} {} {} {}",
+        current_target_pose_.x,
+        current_target_pose_.y,
+        current_target_pose_.z,
+        current_target_pose_.roll,
+        current_target_pose_.pitch,
+        current_target_pose_.yaw
+      );
+    }
+
+    return false;
+  }
+};
+
+class NonRTThread : public Thread {
+  Context& ctx_;
+
+  static cactus_rt::ThreadConfig CreateThreadConfig() {
+    cactus_rt::ThreadConfig thread_config;
+    thread_config.SetOtherScheduler();
+    return thread_config;
+  }
+
+ public:
+  NonRTThread(Context& ctx) : Thread("NonRTThread", CreateThreadConfig()), ctx_(ctx) {}
+
+  void Run() final {
+    ctx_.target_pose.Write(Pose(1.5, 2.5, 3.5, 4.5, 5.5, 6.5));
+    std::this_thread::sleep_for(1s);
+
+    // Realistically only one of these values should be visible on the real-time thread.
+    ctx_.target_pose.Write(Pose(1.5, 2.5, 3.5, 4.5, 5.5, 7.5));
+    ctx_.target_pose.Write(Pose(1.5, 2.5, 3.5, 4.5, 5.5, 8.5));
+    std::this_thread::sleep_for(1s);
+
+    ctx_.done.Write(true);
+  }
+};
+
+int main() {
+  Context ctx;
+  auto    rt_thread = std::make_shared<RTThread>(ctx);
+  auto    non_rt_thread = std::make_shared<NonRTThread>(ctx);
+
+  App app;
+  app.RegisterThread(rt_thread);
+  app.RegisterThread(non_rt_thread);
+
+  app.Start();
+  app.Join();
+
+  return 0;
+}

include/cactus_rt/experimental/lockless.h

@@ -0,0 +1,8 @@
+#ifndef CACTUS_RT_EXPERIMENTAL_LOCKLESS_H_
+#define CACTUS_RT_EXPERIMENTAL_LOCKLESS_H_
+
+#include "lockless/atomic_message.h"
+#include "lockless/spsc/realtime_readable_value.h"
+#include "lockless/spsc/realtime_writable_value.h"
+
+#endif

include/cactus_rt/experimental/lockless/spsc/realtime_readable_value.h

@@ -0,0 +1,47 @@
+#ifndef CACTUS_RT_EXPERIMENTAL_LOCKLESS_SPSC_REALTIME_READABLE_VALUE_
+#define CACTUS_RT_EXPERIMENTAL_LOCKLESS_SPSC_REALTIME_READABLE_VALUE_
+
+#include <atomic>
+#include <memory>
+
+namespace cactus_rt::experimental::lockless::spsc {
+
+/**
+ * This uses the CAS exchange algorithm to allow a single (real-time) thread to
+ * be able to atomically read a value shared by a different (non-real-time)
+ * thread.
+ *
+ * The reader for this algorithm is wait-free while the writer is lock-free. The
+ * reader is unable to modify the value and transmit it back to the writer with
+ * this algorithm.
+ */
+template <typename T>
+class RealtimeReadableValue {
+  std::unique_ptr<T> storage_ptr_ = std::make_unique<T>();
+  std::atomic<T*>    atomic_ptr_ = storage_ptr_.get();
+
+ public:
+  T Read() {
+    // TODO: need to figure out the atomic memory order here!
+    T* data_ptr = atomic_ptr_.exchange(nullptr);
+    T  data = *data_ptr;
+    atomic_ptr_.store(data_ptr);
+    return data;
+  }
+
+  void Write(const T& new_value) {
+    auto new_ptr = std::make_unique<T>(new_value);
+    T*   expected;
+
+    do {
+      expected = storage_ptr_.get();
+      // TODO: sequential consistency is probably wrong here. Need to understand if acq_rel is sufficient.
+    } while (!atomic_ptr_.compare_exchange_weak(expected, new_ptr.get()));
+
+    storage_ptr_ = std::move(new_ptr);
+  }
+};
+
+}  // namespace cactus_rt::experimental::lockless::spsc
+
+#endif

include/cactus_rt/experimental/lockless/spsc/realtime_writable_value.h

@@ -0,0 +1,8 @@
+#ifndef CACTUS_RT_EXPERIMENTAL_LOCKLESS_SPSC_REALTIME_WRITABLE_VALUE_
+#define CACTUS_RT_EXPERIMENTAL_LOCKLESS_SPSC_REALTIME_WRITABLE_VALUE_
+
+namespace cactus_rt::experimental::lockless::spsc {
+
+}
+
+#endif