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