BufferManager.hpp

 
#pragma once
 
#include <condition_variable>
#include <memory>
#include <mutex>
#include <shared_mutex>
#include <string>
#include <vector>
 
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <rclcpp/rclcpp.hpp>
 
#include "event_detector/DifferenceBasedBuffer.hpp"
#include "event_detector/common.hpp"
 
namespace event_detector {
 
template <typename T>
using Buffer = DifferenceBasedBuffer<T, rclcpp::Time, rclcpp::Duration>;
 
class BufferManager {
 public:
  template <typename T>
  class DataBuffers {
   public:
    DataBuffers();
 
    void setBufferManager(BufferManager* manager);
 
    void update(const std::vector<ClientConnection>& clients);
 
    void insert(const typename std::shared_ptr<const T>& sample, const rclcpp::Time& stamp,
                const std::string& client_id, const std::string& topic,
                const bool clear_if_time_is_running_backwards = true);
 
    std::vector<std::string> getClients();
 
    std::vector<std::string> getFrames(const std::string& client_id);
 
    std::vector<Stamped<T>> get(const std::string& client_id = "", const std::string& frame_id = "",
                                const std::string& topic = "");
 
    std::vector<Stamped<T>> get(const int k, const std::string& client_id = "", const std::string& frame_id = "",
                                const std::string& topic = "");
 
    std::vector<Stamped<T>> get(const rclcpp::Time& start_time, const rclcpp::Time& end_time,
                                const std::string& client_id = "", const std::string& frame_id = "",
                                const std::string& topic = "");
 
    std::vector<Stamped<T>> get(const int k, const rclcpp::Time& start_time, const rclcpp::Time& end_time,
                                const std::string& client_id = "", const std::string& frame_id = "",
                                const std::string& topic = "");
 
    string_map<std::vector<Stamped<T>>> getWithTopic(const int k, const rclcpp::Time& start_time,
                                                     const rclcpp::Time& end_time, const std::string& client_id = "",
                                                     const std::string& frame_id = "", const std::string& topic = "");
 
    std::string getInfo() const;
 
    void clearBuffer(const std::string& client_id, const std::string& topic);
 
    bool timeIsRunningBackwards(const std::string& client_id, const std::string& topic, const rclcpp::Time& stamp);
 
   protected:
    BufferManager* manager_ = nullptr;
 
    string_map_2d<Buffer<std::shared_ptr<const T>>> buffer_by_client_topic_;
 
    string_map_2d<std::string> frame_by_client_topic_;
 
    string_map_2d<rclcpp::Time> last_stamp_by_client_topic_;
 
    string_map_2d<std::shared_ptr<std::shared_mutex>> mutex_by_client_topic_;
 
    mutable std::shared_mutex reconfigure_mutex_;
  };
 
 public:
  BufferManager();
 
  void lockBuffers(const bool is_write);
 
  void unlockBuffers(const bool is_write);
 
  std::string getInfo() const;
 
 public:
  mutable std::mutex buffer_lock_mutex_;
 
  std::condition_variable buffer_lock_condition_variable_;
 
  int buffer_lock_n_threads_ = 0;
 
#define DATATYPE(TYPE, VAR)    \
 \
  DataBuffers<TYPE> VAR;
#include "event_detector/datatypes.macro"
#undef DATATYPE
};
 
}  // namespace event_detector
 
#include "event_detector/BufferManager.tpp"