dynamics_limit.hpp

Go to the documentation of this file.

#pragma once
 
#include <functional>
#include <memory>
#include <mutex>
#include <sstream>
#include <stdexcept>
#include <string>
#include <utility>
 
namespace franky {
 
template <typename LimitType>
class DynamicsLimit {
 public:
  DynamicsLimit(
      const std::string &desc, const LimitType &max_val, const std::shared_ptr<std::mutex> &write_mutex,
      const std::function<bool()> &can_write_condition)
      : DynamicsLimit(desc, max_val, write_mutex, can_write_condition, max_val) {}
 
  DynamicsLimit(
      std::string desc, const LimitType &max_val, const std::shared_ptr<std::mutex> &write_mutex,
      const std::function<bool()> &can_write_condition, const LimitType &default_val)
      : max(max_val),
        desc(std::move(desc)),
        write_mutex_(write_mutex),
        value_(default_val),
        can_write_condition_(can_write_condition) {}
 
  template <typename AlternativeType>
  void setFrom(const AlternativeType &value);
 
  void set(const LimitType &value) {
    std::unique_lock lock(*write_mutex_);
    if (!can_write_condition_()) {
      std::stringstream ss;
      ss << "Cannot set " << desc << " limit while robot is in control.";
      throw std::runtime_error(ss.str());
    }
    check(value);
    value_ = value;
  }
 
  [[nodiscard]] LimitType get() const { return value_; }
 
  const LimitType max;
 
  const std::string desc;
 
  template <typename LimitTypeStream>
  friend std::ostream &operator<<(std::ostream &os, const DynamicsLimit<LimitTypeStream> &dynamics_limit);
 
 private:
  void check(const LimitType &value) const;
 
  std::shared_ptr<std::mutex> write_mutex_;   
  LimitType value_;                           
  std::function<bool()> can_write_condition_; 
};
 
template <typename LimitTypeStream>
std::ostream &operator<<(std::ostream &os, const DynamicsLimit<LimitTypeStream> &dynamics_limit) {
  os << dynamics_limit.value_ << " (max: " << dynamics_limit.max << ")";
  return os;
}
 
}  // namespace franky