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