Rokubimini.hpp

Go to the documentation of this file.

#pragma once
 
#include <atomic>
#include <functional>
#include <mutex>
#include <string>
 
#include <rokubimini/Reading.hpp>
#include <rokubimini/Statusword.hpp>
#include <rokubimini/configuration/Configuration.hpp>
#include <rokubimini/configuration/SensorConfiguration.hpp>
#include <rokubimini/configuration/ForceTorqueFilter.hpp>
#include <utility>
 
#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>
#include <ros/node_handle.h>
 
namespace rokubimini
{
class Rokubimini
{
public:
  using NodeHandlePtr = std::shared_ptr<ros::NodeHandle>;
  using DiagnosticsUpdaterPtr = std::shared_ptr<diagnostic_updater::Updater>;
  using RosPublisherPtr = std::shared_ptr<ros::Publisher>;
  using TimerPtr = std::shared_ptr<ros::Timer>;
  using ReadingCb = std::function<void(const std::string&, const Reading&)>;
  using ErrorCb = std::function<void(const std::string&)>;
  using ErrorRecoveredCb = std::function<void(const std::string&)>;
  using FatalCb = std::function<void(const std::string&)>;
  using FatalRecoveredCb = std::function<void(const std::string&)>;
  using DeviceDisconnectedCb = std::function<void(const std::string&)>;
  using DeviceReconnectedCb = std::function<void(const std::string&)>;
 
  // Anonymous callbacks do not include the name of the device.
  using AnonymousReadingCb = std::function<void(const Reading&)>;
  using AnonymousErrorCb = std::function<void(void)>;
 
  Rokubimini() = default;
 
  Rokubimini(const std::string& name, NodeHandlePtr nh) : name_(name), nh_(std::move(nh)){ /* do nothing */ };
  virtual ~Rokubimini() = default;
 
  std::string getName() const
  {
    return name_;
  }
 
  configuration::Configuration& getConfiguration()
  {
    return configuration_;
  }
 
  const configuration::Configuration& getConfiguration() const
  {
    return configuration_;
  }
 
  void addReadingCb(const ReadingCb& readingCb, const int priority = 0)
  {
    readingCbs_.insert({ priority, readingCb });
  }
 
  void addErrorCb(const ErrorCb& errorCb, const int priority = 0)
  {
    errorCbs_.insert({ priority, errorCb });
  }
 
  void addErrorRecoveredCb(const ErrorRecoveredCb& errorRecoveredCb, const int priority = 0)
  {
    errorRecoveredCbs_.insert({ priority, errorRecoveredCb });
  }
 
  void addFatalCb(const FatalCb& fatalCb, const int priority = 0)
  {
    fatalCbs_.insert({ priority, fatalCb });
  }
 
  void addFatalRecoveredCb(const FatalRecoveredCb& fatalRecoveredCb, const int priority = 0)
  {
    fatalRecoveredCbs_.insert({ priority, fatalRecoveredCb });
  }
 
  void addDeviceDisconnectedCb(const DeviceDisconnectedCb& deviceDisconnectedCb, const int priority = 0)
  {
    deviceDisconnectedCbs_.insert({ priority, deviceDisconnectedCb });
  }
 
  void addDeviceReconnectedCb(const DeviceReconnectedCb& deviceReconnectedCb, const int priority = 0)
  {
    deviceReconnectedCbs_.insert({ priority, deviceReconnectedCb });
  }
 
  void startupWithoutCommunication();
 
  void startupWithCommunication();
 
  virtual void updateProcessReading() = 0;
 
  virtual void shutdownWithCommunication() = 0;
 
  virtual void shutdownWithoutCommunication(){ /* do nothing */ };
 
  void clearGoalStateEnum();
 
  void errorCb();
 
  void errorRecoveredCb();
 
  bool deviceIsInErrorState();
 
  void fatalCb();
 
  void fatalRecoveredCb();
 
  bool deviceIsInFatalState();
 
  void deviceDisconnectedCb();
 
  void deviceReconnectedCb();
 
  virtual bool deviceIsMissing() const = 0;
 
  Statusword getStatusword() const
  {
    return statusword_;
  }
 
  virtual bool getSerialNumber(unsigned int& serialNumber) = 0;
 
  virtual bool getForceTorqueSamplingRate(int& samplingRate) = 0;
 
  virtual bool setForceTorqueFilter(const configuration::ForceTorqueFilter& filter) = 0;
 
  virtual bool setAccelerationFilter(const unsigned int filter) = 0;
 
  virtual bool setAngularRateFilter(const unsigned int filter) = 0;
 
  virtual bool setAccelerationRange(const unsigned int range) = 0;
 
  virtual bool setAngularRateRange(const unsigned int range) = 0;
 
  virtual bool setForceTorqueOffset(const Eigen::Matrix<double, 6, 1>& forceTorqueOffset) = 0;
 
  virtual bool setSensorConfiguration(const configuration::SensorConfiguration& sensorConfiguration) = 0;
 
  virtual bool setSensorCalibration(const calibration::SensorCalibration& sensorCalibration) = 0;
 
  virtual bool saveConfigParameter() = 0;
 
  Reading getReading() const;
 
  void getReading(Reading& reading) const;
 
  void setNodeHandle(const NodeHandlePtr& nh)
  {
    nh_ = nh;
  }
 
  virtual void createRosPublishers() = 0;
 
  virtual void createRosServices() = 0;
 
  virtual void publishRosMessages() = 0;
 
  virtual void load();
 
  std::string getProductName() const
  {
    return productName_;
  }
 
  virtual void createRosDiagnostics() = 0;
 
  virtual void publishRosDiagnostics() = 0;
 
protected:
  virtual void preSetupConfiguration() = 0;
 
  virtual void postSetupConfiguration() = 0;
 
  void setStatusword(Statusword& statusword);
 
  static constexpr double NAN_VALUE = std::numeric_limits<double>::quiet_NaN();
 
  std::string name_;
 
  configuration::Configuration configuration_;
 
  Statusword statusword_;
 
  std::atomic<bool> statuswordRequested_{ false };
 
  mutable std::recursive_mutex readingMutex_;
 
  Reading reading_;
 
  NodeHandlePtr nh_;
 
  std::string productName_;
 
  std::multimap<int, ReadingCb, std::greater<int>> readingCbs_;
 
  std::multimap<int, ErrorCb, std::greater<int>> errorCbs_;
 
  std::multimap<int, ErrorRecoveredCb, std::greater<int>> errorRecoveredCbs_;
 
  std::multimap<int, FatalCb, std::greater<int>> fatalCbs_;
 
  std::multimap<int, FatalRecoveredCb, std::greater<int>> fatalRecoveredCbs_;
 
  std::multimap<int, DeviceDisconnectedCb, std::greater<int>> deviceDisconnectedCbs_;
 
  std::multimap<int, DeviceReconnectedCb, std::greater<int>> deviceReconnectedCbs_;
 
  DiagnosticsUpdaterPtr connectionStatusUpdater_;
 
  TimerPtr dataFlagsDiagnosticsTimer_;
 
  RosPublisherPtr dataFlagsDiagnosticsPublisher_;
};
 
}  // namespace rokubimini