RokubiminiSerial.hpp

Go to the documentation of this file.

#pragma once

#include <rokubimini/Rokubimini.hpp>
#include <rokubimini_serial/RokubiminiSerialImpl.hpp>
#include <rokubimini_msgs/FirmwareUpdateSerial.h>
#include <rokubimini_msgs/ResetWrench.h>
#include <geometry_msgs/Wrench.h>

namespace rokubimini
{
namespace serial
{
class RokubiminiSerial : public Rokubimini
{
public:
  RokubiminiSerial() = default;
  ~RokubiminiSerial() override = default;

  void doStartupWithCommunication() override;

  bool init();

  void updateProcessReading() override;

  bool deviceIsMissing() const override;

  void shutdownWithCommunication() override;

  void setImplPointer(const RokubiminiSerialImplPtr& implPtr)
  {
    implPtr_ = implPtr;
  }

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

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

  // Missing: Methods for calling SDO
  bool getSerialNumber(unsigned int& serialNumber) 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 setHardwareReset();

  bool setInitMode();

  bool saveConfigParameter() override;

  bool loadConfig();

  bool printUserConfig();

  void createRosPublishers() override;

  void signalShutdown();

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

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

  void publishRosMessages() override;

protected:
  RokubiminiSerialImplPtr implPtr_{ nullptr };

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

  RosPublisherPtr wrenchPublisher_;

  RosPublisherPtr temperaturePublisher_;

  ros::ServiceServer firmwareUpdateService_;

  ros::ServiceServer resetWrenchService_;

  uint64_t noFrameSyncCounter_{ 0 };

  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 = 200;

  geometry_msgs::Wrench meanWrenchOffset_;
};

}  // namespace serial
}  // namespace rokubimini