RokubiminiEthercat.hpp

Go to the documentation of this file.

#pragma once

#include <rokubimini/Rokubimini.hpp>
#include <rokubimini_ethercat/RokubiminiEthercatSlave.hpp>
#include <utility>
#include <rokubimini_msgs/FirmwareUpdateEthercat.h>
#include <rokubimini_msgs/ResetWrench.h>
#include <Eigen/Dense>
namespace rokubimini
{
namespace ethercat
{
class RokubiminiEthercat : public Rokubimini
{
public:
  RokubiminiEthercat() = default;

  RokubiminiEthercat(const std::string& name, NodeHandlePtr nh) : Rokubimini(name, std::move(nh)){ /* do nothing */ };
  // RokubiminiEthercat() = default;
  ~RokubiminiEthercat() override = default;

  void preSetupConfiguration() override;

  void postSetupConfiguration() override;

  void updateProcessReading() override;

  bool deviceIsMissing() const override;

  void shutdownWithCommunication() override;

  void setSlavePointer(const RokubiminiEthercatSlavePtr& slavePtr)
  {
    slavePtr_ = slavePtr;
  }

  void setState(const uint16_t state)
  {
    slavePtr_->setState(state);
  }

  bool waitForState(const uint16_t state)
  {
    return slavePtr_->waitForState(state);
  }

  // Missing: Methods for calling SDO
  bool getSerialNumber(unsigned int& samplingRate) override;

  bool getForceTorqueSamplingRate(int& samplingRate) override;

  bool setForceTorqueFilter(const configuration::ForceTorqueFilter& filter) override;

  bool setAccelerationFilter(const unsigned int filter) override;

  bool setAngularRateFilter(const unsigned int filter) override;

  bool setAccelerationRange(const unsigned int range) override;

  bool setAngularRateRange(const unsigned int range) override;

  bool setForceTorqueOffset(const Eigen::Matrix<double, 6, 1>& forceTorqueOffset) override;

  bool setSensorConfiguration(const configuration::SensorConfiguration& sensorConfiguration) override;

  bool setSensorCalibration(const calibration::SensorCalibration& sensorCalibration) override;

  bool setConfigMode();

  bool setRunMode();
  bool saveConfigParameter() override;

  void createRosPublishers() override;

  void signalShutdown();

  bool firmwareUpdateCallback(rokubimini_msgs::FirmwareUpdateEthercat::Request& request,
                              rokubimini_msgs::FirmwareUpdateEthercat::Response& response);

  bool resetWrenchCallback(rokubimini_msgs::ResetWrench::Request& request,
                           rokubimini_msgs::ResetWrench::Response& response);
  void createRosServices() override;

  void publishRosMessages() override;

  void createRosDiagnostics() override;

  void publishRosDiagnostics() override;

  void updateConnectionStatus(diagnostic_updater::DiagnosticStatusWrapper& stat);

  void setRunsAsync(bool runsAsync);

  void publishConnectionStatusDiagnostics();

  void publishDataFlagsDiagnostics(const ros::TimerEvent& event);

  template <typename Value>
  bool sendSdoRead(const uint16_t index, const uint8_t subindex, const bool completeAccess, Value& value);

  bool sendSdoReadGeneric(const std::string& indexString, const std::string& subindexString,
                          const std::string& valueTypeString, std::string& valueString);

  template <typename Value>
  bool sendSdoWrite(const uint16_t index, const uint8_t subindex, const bool completeAccess, const Value value);

  bool sendSdoWriteGeneric(const std::string& indexString, const std::string& subindexString,
                           const std::string& valueTypeString, const std::string& valueString);

protected:
  RokubiminiEthercatSlavePtr slavePtr_{ nullptr };

  using RosPublisherPtr = std::shared_ptr<ros::Publisher>;

  RosPublisherPtr readingPublisher_;

  RosPublisherPtr wrenchPublisher_;

  RosPublisherPtr imuPublisher_;

  RosPublisherPtr temperaturePublisher_;

  ros::ServiceServer firmwareUpdateService_;

  ros::ServiceServer resetWrenchService_;

  std::atomic<bool> computeMeanWrenchFlag_{ false };

  std::atomic<uint32_t> wrenchMessageCount_{ 0 };

  mutable std::recursive_mutex meanWrenchOffsetMutex_;

  const static uint32_t TOTAL_NUMBER_OF_WRENCH_MESSAGES = 50;

  Eigen::Matrix<double, 6, Eigen::Dynamic> resetServiceWrenchSamples_;
};

template <>
bool RokubiminiEthercat::sendSdoRead(const uint16_t index, const uint8_t subindex, const bool completeAccess,
                                     int8_t& value);
template <>
bool RokubiminiEthercat::sendSdoRead(const uint16_t index, const uint8_t subindex, const bool completeAccess,
                                     int16_t& value);
template <>
bool RokubiminiEthercat::sendSdoRead(const uint16_t index, const uint8_t subindex, const bool completeAccess,
                                     int32_t& value);
template <>
bool RokubiminiEthercat::sendSdoRead(const uint16_t index, const uint8_t subindex, const bool completeAccess,
                                     int64_t& value);
template <>
bool RokubiminiEthercat::sendSdoRead(const uint16_t index, const uint8_t subindex, const bool completeAccess,
                                     uint8_t& value);
template <>
bool RokubiminiEthercat::sendSdoRead(const uint16_t index, const uint8_t subindex, const bool completeAccess,
                                     uint16_t& value);
template <>
bool RokubiminiEthercat::sendSdoRead(const uint16_t index, const uint8_t subindex, const bool completeAccess,
                                     uint32_t& value);
template <>
bool RokubiminiEthercat::sendSdoRead(const uint16_t index, const uint8_t subindex, const bool completeAccess,
                                     uint64_t& value);
template <>
bool RokubiminiEthercat::sendSdoRead(const uint16_t index, const uint8_t subindex, const bool completeAccess,
                                     float& value);
template <>
bool RokubiminiEthercat::sendSdoRead(const uint16_t index, const uint8_t subindex, const bool completeAccess,
                                     double& value);

template <>
bool RokubiminiEthercat::sendSdoWrite(const uint16_t index, const uint8_t subindex, const bool completeAccess,
                                      const int8_t value);
template <>
bool RokubiminiEthercat::sendSdoWrite(const uint16_t index, const uint8_t subindex, const bool completeAccess,
                                      const int16_t value);
template <>
bool RokubiminiEthercat::sendSdoWrite(const uint16_t index, const uint8_t subindex, const bool completeAccess,
                                      const int32_t value);
template <>
bool RokubiminiEthercat::sendSdoWrite(const uint16_t index, const uint8_t subindex, const bool completeAccess,
                                      const int64_t value);
template <>
bool RokubiminiEthercat::sendSdoWrite(const uint16_t index, const uint8_t subindex, const bool completeAccess,
                                      const uint8_t value);
template <>
bool RokubiminiEthercat::sendSdoWrite(const uint16_t index, const uint8_t subindex, const bool completeAccess,
                                      const uint16_t value);
template <>
bool RokubiminiEthercat::sendSdoWrite(const uint16_t index, const uint8_t subindex, const bool completeAccess,
                                      const uint32_t value);
template <>
bool RokubiminiEthercat::sendSdoWrite(const uint16_t index, const uint8_t subindex, const bool completeAccess,
                                      const uint64_t value);
template <>
bool RokubiminiEthercat::sendSdoWrite(const uint16_t index, const uint8_t subindex, const bool completeAccess,
                                      const float value);
template <>
bool RokubiminiEthercat::sendSdoWrite(const uint16_t index, const uint8_t subindex, const bool completeAccess,
                                      const double value);

}  // namespace ethercat
}  // namespace rokubimini