Loop validated cache

include/kodlab_mjbots_sdk/common_header.h

@@ -44,7 +44,7 @@ template<typename T>
 class ValidatedCache
 {
 
-private:
+protected:
   /**
    * @brief Validation status of \c data_
    */
@@ -71,11 +71,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,19 +93,28 @@ 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;
     }

include/kodlab_mjbots_sdk/loop_cache.h

@@ -0,0 +1,103 @@
+/**
+ * @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 "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.
+ * 
+ */
+class LoopId
+{
+ public:
+  /**
+   * @brief Returns the current loop id
+   * 
+   * @return uint32_t 
+   */
+  static uint32_t get(){
+    return loop_id_;
+  }
+
+  /**
+   * @brief Override the current loop id
+   * 
+   * @param id_des 
+   */
+  static void set(uint32_t id_des){
+    loop_id_ = id_des;
+  }
+
+  /**
+   * @brief increment and return the loop id, note 
+   * @note there can only be ONE loop using this type of cache at once to avoid 
+   * prematurely invalidating caches, MAKE SURE THIS IS WHAT YOU WANT
+   * 
+   * @return uint32_t 
+   */
+  static uint32_t increment(){
+      return (++loop_id_);
+  }
+
+ protected:
+  /**
+   * @brief Loop id shared between all instances acts a "global" loop id
+   * 
+   */
+  static 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::set(-1); // 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,14 @@
+/**
+ * @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"
+
+uint32_t kodlab::LoopId::loop_id_ = 0; //Initialize the static variable