qbrobotics_research_api.h

Go to the documentation of this file.

 
#ifndef QBROBOTICS_DRIVER_QBROBOTICS_RESEARCH_API_H
#define QBROBOTICS_DRIVER_QBROBOTICS_RESEARCH_API_H
 
// standard libraries
#include <iomanip>
#include <map>
#include <memory>
#include <thread>
#include <vector>
// project libraries
#include <serial/serial.h>
#include <qbrobotics_research_api/qbrobotics_research_commands.h>
 
namespace qbrobotics_research_api {
 
class Communication {
 public:
  struct ConnectedDeviceInfo {
    uint8_t id {0};
    std::string serial_number {};
    std::string type {};
    std::string sub_type {};
  };
 
  Communication();
  explicit Communication(uint32_t baud_rate);
  explicit Communication(const serial::Serial::Timeout &timeout);
  explicit Communication(uint32_t baud_rate, const serial::Serial::Timeout &timeout);
  virtual ~Communication() = default;
 
  virtual int listConnectedDevices();
  virtual int listConnectedDevices(const std::string &serial_port_name, std::vector<ConnectedDeviceInfo> &device_ids);
  virtual int listSerialPorts(std::vector<serial::PortInfo> &serial_ports_info);
 
  virtual int closeSerialPort(const std::string &serial_port_name);
 
  virtual int createSerialPort(const std::string &serial_port_name);
 
  virtual int createSerialPort(const std::string &serial_port_name, uint32_t baud_rate);
 
  virtual int createSerialPort(const std::string &serial_port_name, const serial::Serial::Timeout &timeout);
 
  virtual int createSerialPort(const std::string &serial_port_name, uint32_t baud_rate, const serial::Serial::Timeout &timeout);
 
  virtual int openSerialPort(const std::string &serial_port_name);
 
  virtual int openSerialPort(const std::string &serial_port_name, uint32_t baud_rate);
 
  virtual int openSerialPort(const std::string &serial_port_name, serial::Serial::Timeout &timeout);
 
  virtual int openSerialPort(const std::string &serial_port_name, uint32_t baud_rate, serial::Serial::Timeout &timeout);
 
  /*
    -----------------
    parse package, send commands and other utilities to control qbrobotics devices
    -----------------
  */
 
  virtual int parsePackage(const std::string &serial_port_name, uint8_t device_id, uint8_t command);
  virtual int parsePackage(const std::string &serial_port_name, uint8_t device_id, uint8_t command, std::vector<int8_t> &data_in);
  virtual int sendCommand(const std::string &serial_port_name, uint8_t device_id, uint8_t command);
  virtual int sendCommand(const std::string &serial_port_name, uint8_t device_id, uint8_t command, const std::vector<int8_t> &data_out);
  virtual int sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command);
  virtual int sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, uint8_t max_repeats);
  virtual int sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, std::vector<int8_t> &data_in);
  virtual int sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, uint8_t max_repeats, std::vector<int8_t> &data_in);
  virtual int sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, const std::vector<int8_t> &data_out);
  virtual int sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, uint8_t max_repeats, const std::vector<int8_t> &data_out);
  virtual int sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, const std::vector<int8_t> &data_out, std::vector<int8_t> &data_in);
  virtual int sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, uint8_t max_repeats, const std::vector<int8_t> &data_out, std::vector<int8_t> &data_in);
  virtual int sendCommandBroadcast(const std::string &serial_port_name, uint8_t command);
  virtual int sendCommandBroadcast(const std::string &serial_port_name, uint8_t command, const std::vector<int8_t> &data_out);
  //WARN: broadcast should be used only for non-returning methods (it is unreliable for returning methods)
 
  virtual int deserializePackage(const std::vector<uint8_t> &package_in, uint8_t &device_id, uint8_t &command);
  virtual int deserializePackage(const std::vector<uint8_t> &package_in, uint8_t &device_id, uint8_t &command, std::vector<int8_t> &data);
  virtual int readPackage(const std::string &serial_port_name, std::vector<uint8_t> &package_in);
  virtual int readPackage(const std::string &serial_port_name, uint8_t &device_id, uint8_t &command);
  virtual int readPackage(const std::string &serial_port_name, uint8_t &device_id, uint8_t &command, std::vector<int8_t> &data);
  virtual int serializePackage(uint8_t device_id, uint8_t command, std::vector<uint8_t> &package_out);
  virtual int serializePackage(uint8_t device_id, uint8_t command, const std::vector<int8_t> &data, std::vector<uint8_t> &package_out);
  virtual int writePackage(const std::string &serial_port_name, const std::vector<uint8_t> &package_out);
  virtual int writePackage(const std::string &serial_port_name, uint8_t device_id, uint8_t command);
  virtual int writePackage(const std::string &serial_port_name, uint8_t device_id, uint8_t command, const std::vector<int8_t> &data);
 
  virtual uint8_t checksum(const std::vector<uint8_t> &data, uint32_t size);
 
  template<class T>
  static void swapBytes(T &x) {
    auto a = reinterpret_cast<uint8_t*>(std::addressof(x));
    for(auto b = a+sizeof(T)-1; a<b; ++a,--b) {
      std::swap(*a, *b);
    }
  }
  template<class T>
  static void swapBytes(std::vector<T> &vector) {
    for (auto &&item : vector) {  // auto && is required by std::vector<bool> which returns a prvalueL
      swapBytes(item);
    }
  }
  template<class S, class T>
  static std::vector<S> vectorCast(std::vector<T> vector) {
    auto data_in = reinterpret_cast<S*>(vector.data());
    std::vector<S> data_out(data_in, data_in+vector.size()*sizeof(T)/sizeof(S));
    return data_out;
  }
  template<class S, class T>
  static std::vector<S> vectorCastAndSwap(std::vector<T> vector) {
    auto data_out = vectorCast<S>(vector);
    swapBytes(data_out);
    return data_out;
  }
  template<class S, class T>
  static std::vector<S> vectorSwapAndCast(std::vector<T> vector) {
    swapBytes(vector);
    return vectorCast<S>(vector);
  }
 
  std::map<std::string, std::shared_ptr<serial::Serial>> getSerialPorts() { return serial_ports_; }
 
 protected:
  explicit Communication(const Communication &communication);
  explicit Communication(const Communication &communication, const serial::Serial::Timeout &timeout);
 
  std::map<std::string, std::vector<ConnectedDeviceInfo>> connected_devices_;
  std::map<std::string, serial::PortInfo> serial_ports_info_;
  std::map<std::string, std::shared_ptr<serial::Serial>> serial_ports_;
 
  uint32_t serial_ports_baud_rate_;
  serial::Serial::Timeout serial_ports_timeout_;
 
  bool isInSerialPorts(const std::string &serial_port_name);
  bool isInSerialPortsInfo(const std::string &serial_port_name);
};
 
class CommunicationLegacy : public Communication {
 public:
  CommunicationLegacy() = default;
  explicit CommunicationLegacy(const Communication &communication);
  explicit CommunicationLegacy(const Communication &communication, const serial::Serial::Timeout &timeout);
  virtual ~CommunicationLegacy() = default;
 
  int parsePackage(const std::string &serial_port_name, uint8_t device_id, uint8_t command, std::vector<int8_t> &data_in) override;
  int sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, uint8_t max_repeats, const std::vector<int8_t> &data_out, std::vector<int8_t> &data_in) override;
 
  int deserializePackage(const std::vector<uint8_t> &package_in, uint8_t &device_id, uint8_t &command, std::vector<int8_t> &data) override;
  int readLongPackage(const std::string &serial_port_name, std::vector<uint8_t> &package_in);
  int readPackage(const std::string &serial_port_name, uint8_t &device_id, uint8_t &command, std::vector<int8_t> &data) override;
  int readPackage(const std::string &serial_port_name, std::vector<uint8_t> &package_in) override;
};
 
class Device {
 public:
  class Params {
   public:
    Params() = default;
    virtual ~Params() = default;
 
    virtual void initParams(const std::vector<int8_t> &param_buffer);
 
    template<class T>
    static void getParameter(uint8_t param_id, const std::vector<int8_t> &param_buffer, std::vector<T> &param_vector) {
      auto number_of_values = static_cast<uint8_t>(param_buffer.at((param_id-1)*PARAM_BYTE_SLOT + 2));
      auto param_iter = param_buffer.begin() + (param_id-1)*PARAM_BYTE_SLOT + 3;
      std::vector<int8_t> param_bytes(param_iter, param_iter + number_of_values*sizeof(T));
      param_vector = Communication::vectorCastAndSwap<T>(param_bytes);
    }
    template<class T>
    static void getParameter(uint8_t param_id, const std::vector<int8_t> &param_buffer, T &param) {
      std::vector<T> param_vector;
      getParameter(param_id, param_buffer, param_vector);
      param = param_vector.front();
    }
 
    std::string serial_number {};
    std::string type {};
    std::string sub_type {};
 
    uint8_t id {0};
    std::vector<float> position_pid;
    std::vector<float> current_pid;
    uint8_t startup_activation {false};  // should be bool
    uint8_t input_mode {0};
    uint8_t control_mode {0};
    std::vector<uint8_t> encoder_resolutions;
    std::vector<int16_t> encoder_offsets;
    std::vector<float> encoder_multipliers;
    uint8_t use_position_limits {false};  // should be bool
    std::vector<int32_t> position_limits;
    std::vector<int32_t> position_max_steps;
    int16_t current_limit {0};
    uint8_t rate_limiter {0};  // param id after softhand params
  };
 
  explicit Device(std::shared_ptr<Communication> communication, std::string name, std::string serial_port, uint8_t id);
  explicit Device(std::shared_ptr<Communication> communication, std::string name, std::string serial_port, uint8_t id, bool init_params);
  explicit Device(std::shared_ptr<Communication> communication, std::string name, std::string serial_port, uint8_t id, bool init_params, std::unique_ptr<Params> params);
  virtual ~Device() = default;
 
  virtual int ping();
 
  virtual int getControlReferences(std::vector<int16_t> &control_references);
 
  virtual int getCurrents(std::vector<int16_t> &currents);
 
  virtual int getCurrentsAndPositions(std::vector<int16_t> &currents, std::vector<int16_t> &positions);
 
  virtual int getPositions(std::vector<int16_t> &positions);
 
  virtual int getVelocities(std::vector<int16_t> &velocities);
 
  virtual int getAccelerations(std::vector<int16_t> &accelerations);
 
  virtual int getMotorStates(bool &motor_state);
  virtual int getCycleTime(int16_t &cycle_time);
 
  virtual int setControlReferences(const std::vector<int16_t> &control_references);
 
  virtual int setControlReferencesAndWait(const std::vector<int16_t> &control_references);
 
  virtual int setMotorStates(bool motor_state);
 
  virtual int getInfo(std::string &info);
 
  virtual int getInfo(uint16_t info_type, std::string &info);
 
  virtual int getParameters(std::vector<int8_t> &param_buffer);
 
  virtual int getParameters(uint8_t id, std::vector<int8_t> &param_buffer);
 
  virtual int getParamId();
 
  virtual int getParamId(uint8_t &id);
 
  virtual int getParamPositionPID();
 
  virtual int getParamPositionPID(std::vector<float> &position_pid);
 
  virtual int getParamCurrentPID();
 
  virtual int getParamCurrentPID(std::vector<float> &current_pid);
 
  virtual int getParamStartupActivation();
 
  virtual int getParamStartupActivation(uint8_t &startup_activation);
 
  virtual int getParamInputMode();
 
  virtual int getParamInputMode(uint8_t &input_mode);
 
  virtual int getParamControlMode();
 
  virtual int getParamControlMode(uint8_t &control_mode);
 
  virtual int getParamEncoderResolutions();
 
  virtual int getParamEncoderResolutions(std::vector<uint8_t> &encoder_resolutions);
 
  virtual int getParamEncoderOffsets();
 
  virtual int getParamEncoderOffsets(std::vector<int16_t> &encoder_offsets);
 
  virtual int getParamEncoderMultipliers();
 
  virtual int getParamEncoderMultipliers(std::vector<float> &encoder_multipliers);
 
  virtual int getParamUsePositionLimits();
 
  virtual int getParamUsePositionLimits(uint8_t &use_position_limits);
 
  virtual int getParamPositionLimits();
 
  virtual int getParamPositionLimits(std::vector<int32_t> &position_limits);
 
  virtual int getParamPositionMaxSteps();
 
  virtual int getParamPositionMaxSteps(std::vector<int32_t> &position_max_steps);
 
  virtual int getParamCurrentLimit();
 
  virtual int getParamCurrentLimit(int16_t &current_limit);
 
  virtual int setParameter(uint16_t param_type, const std::vector<int8_t> &param_data);
 
  virtual int setParamId(uint8_t id);
 
  virtual int setParamPositionPID(const std::vector<float> &position_pid);
 
  virtual int setParamCurrentPID(const std::vector<float> &current_pid);
 
  virtual int setParamStartupActivation(bool startup_activation);
 
  virtual int setParamInputMode(uint8_t input_mode);
 
  virtual int setParamControlMode(uint8_t control_mode);
 
  virtual int setParamEncoderResolutions(const std::vector<uint8_t> &encoder_resolutions);
 
  virtual int setParamEncoderOffsets(const std::vector<int16_t> &encoder_offsets);
 
  virtual int setParamEncoderMultipliers(const std::vector<float> &encoder_multipliers);
 
  virtual int setParamUsePositionLimits(bool use_position_limits);
 
  virtual int setParamPositionLimits(const std::vector<int32_t> &position_limits);
 
  virtual int setParamPositionMaxSteps(const std::vector<int32_t> &position_max_steps);
 
  virtual int setParamCurrentLimit(int16_t current_limit);
 
  virtual int setParamZeros();
 
  virtual int setParamBaudrate(uint8_t prescaler_divider);
 
  virtual int restoreFactoryDataMemory();
 
  virtual int restoreUserDataMemory();
 
  virtual int storeFactoryDataMemory();
 
  virtual int storeUserDataMemory();
 
  virtual int setBootloaderMode();
 
  std::shared_ptr<Params> getParams() { return params_; }
  bool isQbmove() { return params_->type == "001"; }
  bool isSHPRO()  { return params_->type == "006" && params_->sub_type == "001"; }
  bool isSH2M() { return params_->type == "006" && params_->sub_type == "020"; }
 
 protected:
  std::string name_;
  std::string serial_port_;
  std::shared_ptr<Params> params_;
  std::shared_ptr<Communication> communication_;
};
 
}  // namespace qbrobotics_research_api
 
#endif  // QBROBOTICS_DRIVER_QBROBOTICS_RESEARCH_API_H