Loop validated cache

include/kodlab_mjbots_sdk/common_header.h

@@ -4,6 +4,7 @@
 #include <atomic>
 #include <signal.h>  // manage the ctrl+c signal
 #include <type_traits>
+#include <typeinfo>
 #include <iostream>
 #include <memory>
 #include <vector>
@@ -44,7 +45,7 @@ template<typename T>
 class ValidatedCache
 {
 
-private:
+protected:
   /**
    * @brief Validation status of \c data_
    */
@@ -71,11 +72,17 @@ class ValidatedCache
    */
   ValidatedCache(const T &data, const bool &valid) : data_(data), valid_(valid) {}
 
+  /**
+   * @brief Destroy the ValidatedCache object. Virtual destructor for proper 
+   * derived pointer destruction.
+   */
+   virtual ~ValidatedCache(){};
+
   /**
    * @brief Returns the data status
    * @return \c true if data is valid, \c false otherwise
    */
-  bool valid() const { return valid_; }
+  virtual bool valid() const { return valid_; }
 
   /**
    * @brief Marks the class invalid so that future calls will change the cached
@@ -87,21 +94,30 @@ class ValidatedCache
    * @brief Sets the class data and marks it valid
    * @param data valid data
    */
-  void set(const T &data)
+  virtual void set(const T &data)
   {
     data_ = data;
     valid_ = true;
   }
 
+  /**
+   * @brief Uses operator= to set the class data and marks it valid
+   * @param data valid data
+   */
+  void operator=(const T &data)
+  {
+    set(data);
+  }
+
   /**
    * @brief Accessor for data (<b>does not check validity</b>)
    * @return data
    */
   T get()
   {
-    if (!valid_)
+    if (!valid())
     {
-      std::cerr << "[WARN] Returning invalid data." << std::endl;
+      std::cerr << "[WARN] Returning invalid data of type " << typeid(data_).name() << std::endl;
     }
     return data_;
   }

include/kodlab_mjbots_sdk/loop_cache.h

@@ -0,0 +1,139 @@
+/**
+ * @file loop_cache.h
+ * @author J. Diego Caporale (jdcap@seas.upenn.edu)
+ * @brief Control loop validated cache and id for automatic invalidation synced 
+ * with loop
+ * @date 2023-03-30
+ * 
+ * @copyright Copyright (c) 2023 The Trustees of the University of Pennsylvania. 
+ * All Rights Reserved
+ * BSD 3-Clause License
+ */
+
+#pragma once
+#include <type_traits>
+#include <memory>
+#include <atomic>
+
+#include "kodlab_mjbots_sdk/common_header.h"
+
+namespace kodlab{
+
+/**
+ * @brief Simple loop id counter class that allows the user to tell when the 
+ * control loop has looped. Useful for forcing recalculations and/or timing.
+ * @warning This is a singleton, with all the pros and cons that come with that
+ * @warning This is not thread safe and should only be used in the control loop
+ * 
+ */
+class LoopId{
+
+ public:
+
+  /**
+   * @brief Get the LoopId object
+   * 
+   * @return LoopId& 
+   */
+  static LoopId& getInstance() {
+    static LoopId loop_id_obj;
+    return loop_id_obj;
+  }
+
+  /**
+   * @brief Returns the current loop id
+   * 
+   * @return uint32_t 
+   */
+  static uint32_t get(){
+    return getInstance().loop_id_;
+  }
+
+  /**
+   * @brief Override the current loop id
+   * 
+   * @param id_des 
+   */
+  static void set(uint32_t id_des){
+    getInstance().loop_id_ = id_des;
+  }
+
+  /**
+   * @brief Initialize the loop id (sets it to uint32_t(-1))
+   */
+  static void init(){
+    getInstance().set(-1); // Set loop to pre-loop value
+  }
+
+  /**
+   * @brief Increment and return the loop id 
+   * @note LoopId is a singleton, there can only be one, only one control loop
+   * at a time should be incrementing it otherwise you may be prematurely 
+   * invalidating caches.
+   * 
+   * @return uint32_t 
+   */
+  static uint32_t increment(){
+      return (++getInstance().loop_id_);
+  }
+
+  // Delete copy and assignment, singletons should not do either
+  LoopId(const LoopId&) = delete; ///< deleted
+  LoopId& operator=(const LoopId&) = delete; ///< deleted
+
+ private:
+  /**
+   * @brief Construct a new Loop Id object.
+   */
+  LoopId() {}
+  /**
+   * @brief Destroy the Loop Id object
+   * 
+   */
+  ~LoopId() {}
+
+  /**
+   * @brief Loop id shared between all instances acts a "global" loop id
+   * 
+   */
+  std::atomic<std::uint32_t> loop_id_;
+};
+
+/**
+ * @brief Subclass of the ValidatedCache which uses the loop id to invalidate 
+ * data, while still allowing the user to invalidate manually if so desired.
+ * 
+ * @tparam T, data type
+ */
+template<typename T>
+class ValidatedLoopCache : public ValidatedCache<T>
+{
+ public:
+  using ValidatedCache<T>::ValidatedCache;
+  using ValidatedCache<T>::valid_;
+  using ValidatedCache<T>::data_;
+  using ValidatedCache<T>::operator=;
+
+  /**
+   * @brief Returns the data status
+   * @return \c true if data is valid, \c false otherwise
+   */
+  bool valid() const override{ return (valid_ && loop_id_.get() == last_loop_id_); }
+
+  /**
+   * @brief Sets the class data and marks it valid and saves loop id
+   * @param data valid data
+   */
+  void set(const T &data) override
+  {
+    data_ = data;
+    valid_ = true;
+    last_loop_id_ = loop_id_.get();
+  }
+
+ protected:
+  uint32_t last_loop_id_ = -1; // loop id from the last set event, used to invalidate
+  LoopId loop_id_; // Loop id, tied to control loop
+};
+
+}// namespace kodlab

include/kodlab_mjbots_sdk/mjbots_control_loop.h

@@ -19,6 +19,7 @@
 #include "real_time_tools/hard_spinner.hpp"
 #include "VoidLcm.hpp"
 #include "common_header.h"
+#include "kodlab_mjbots_sdk/loop_cache.h" // Set up loop id for loop-tied caches
 
 namespace kodlab::mjbots {
 /*!
@@ -250,6 +251,8 @@ void MjbotsControlLoop<log_type, input_type, robot_type>::Run() {
   lcm_sub_->Init();
   Init();
   SoftStart::InitializeTimer();
+  kodlab::LoopId::init(); // Set loop id/count to uint(-1), first update 
+                          // will be in loop 0
 
   float prev_msg_duration = 0;
 
@@ -284,6 +287,7 @@ void MjbotsControlLoop<log_type, input_type, robot_type>::Run() {
     }
 
     // Calculate torques and log
+    kodlab::LoopId::increment(); // invalidate any caches that are tied to the loop by incrementing the id
     Update();      //TODO should we give full control to the robot_ instead of feeding update thorugh?
     // robot_->Update();
     PrepareLog();

src/CMakeLists.txt

@@ -15,6 +15,7 @@ set(HEADER_LIST "${kodlab_mjbots_sdk_SOURCE_DIR}/include/kodlab_mjbots_sdk/pi3ha
                 "${kodlab_mjbots_sdk_SOURCE_DIR}/include/kodlab_mjbots_sdk/lcm_publisher.h"
                 "${kodlab_mjbots_sdk_SOURCE_DIR}/include/kodlab_mjbots_sdk/abstract_realtime_object.h"
                 "${kodlab_mjbots_sdk_SOURCE_DIR}/include/kodlab_mjbots_sdk/mjbots_control_loop.h"
+                "${kodlab_mjbots_sdk_SOURCE_DIR}/include/kodlab_mjbots_sdk/loop_cache.h"
                 "${kodlab_mjbots_sdk_SOURCE_DIR}/include/kodlab_mjbots_sdk/joint_base.h"
                 "${kodlab_mjbots_sdk_SOURCE_DIR}/include/kodlab_mjbots_sdk/joint_moteus.h"
                 "${kodlab_mjbots_sdk_SOURCE_DIR}/include/kodlab_mjbots_sdk/math.h"
@@ -41,6 +42,7 @@ set(SOURCE_LIST "pi3hat.cpp"
                 "robot_base.cpp"
                 "joint_base.cpp"
                 "cartesian_leg.cpp"
+                "loop_cache.cpp"
         )
 
 # Make an automatic library - will be static or dynamic based on user setting

src/loop_cache.cpp

@@ -0,0 +1,13 @@
+/**
+ * @file loop_cache.cpp
+ * @author J. Diego Caporale (jdcap@seas.upenn.edu)
+ * @brief Control loop validated cache and id for automatic invalidation synced 
+ * with loop
+ * @date 2023-03-30
+ * 
+ * @copyright Copyright (c) 2023 The Trustees of the University of Pennsylvania. 
+ * All Rights Reserved
+ * BSD 3-Clause License
+ */
+#include "kodlab_mjbots_sdk/loop_cache.h"
+