spirit_interface.h

#ifndef SPIRIT_INTERFACE_H
#define SPIRIT_INTERFACE_H
 
#include <quad_msgs/LegCommandArray.h>
#include <robot_driver/hardware_interfaces/hardware_interface.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/JointState.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
#include <eigen3/Eigen/Eigen>
#include <mblink/mblink.hpp>
 
using gr::MBLink;
 
struct LimbCmd_t {
  Eigen::Vector3f pos, vel, tau;
  short kp[3];
  short kd[3];
  bool restart_flag;
};
 
typedef std::unordered_map<std::string, Eigen::VectorXf> MBData_t;
 
 
class SpiritInterface : public HardwareInterface {
 public:
  SpiritInterface();
 
  virtual void loadInterface(int argc, char** argv);
 
  virtual void unloadInterface();
 
  virtual bool send(const quad_msgs::LegCommandArray& leg_command_array_msg,
                    const Eigen::VectorXd& user_tx_data);
 
  virtual bool recv(sensor_msgs::JointState& joint_state_msg,
                    sensor_msgs::Imu& imu_msg, Eigen::VectorXd& user_rx_data);
 
  MBLink mblink_;
 
  MBData_t mbdata_;
 
  std::vector<std::string> joint_names_ = {"8",  "0", "1", "9",  "2", "3",
                                           "10", "4", "5", "11", "6", "7"};
 
  std::vector<int> joint_indices_ = {8, 0, 1, 9, 2, 3, 10, 4, 5, 11, 6, 7};
 
  std::vector<double> kt_vec_ = {0.546, 0.546, 1.092, 0.546, 0.546, 1.092,
                                 0.546, 0.546, 1.092, 0.546, 0.546, 1.092};
};
 
#endif  // SPIRIT_INTERFACE_H