RokubiminiSerial.hpp

Go to the documentation of this file.

#pragma once
 
#include <rokubimini/Rokubimini.hpp>
#include <rokubimini_serial/RokubiminiSerialImpl.hpp>
#include <utility>
#include <rokubimini_msgs/FirmwareUpdateSerial.h>
#include <rokubimini_msgs/ResetWrench.h>
#include <geometry_msgs/Wrench.h>
#include <thread>
#include <Eigen/Dense>
namespace rokubimini
{
namespace serial
{
class RokubiminiSerial : public Rokubimini
{
public:
  RokubiminiSerial() = default;
 
  RokubiminiSerial(const std::string& name, NodeHandlePtr nh) : Rokubimini(name, std::move(nh)){ /* do nothing */ };
  ~RokubiminiSerial() override = default;
 
  void preSetupConfiguration() override;
 
  void postSetupConfiguration() override;
 
  bool setPublishMode(double timeStep);
 
  bool init();
 
  void updateProcessReading() override;
 
  bool deviceIsMissing() const override;
 
  void shutdownWithCommunication() override;
 
  void setImplPointer(const RokubiminiSerialImplPtr& implPtr)
  {
    implPtr_ = implPtr;
  }
 
  // 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 createRosDiagnostics() override;
 
  void publishRosDiagnostics() override;
 
  void updateConnectionStatus(diagnostic_updater::DiagnosticStatusWrapper& stat);
 
  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;
 
  void update();
 
  void parseCommunicationMsgs();
 
  void publishDataFlagsDiagnostics(const ros::TimerEvent& event);
 
  void publishConnectionStatusDiagnostics();
 
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 = 50;
 
  Eigen::Matrix<double, 6, Eigen::Dynamic> resetServiceWrenchSamples_;
 
  std::thread publishingThread_;
 
  bool publishersSet_ = false;
 
  bool rosDiagnosticsSet_ = false;
};
 
}  // namespace serial
}  // namespace rokubimini