robotInterface.h

#ifndef ROBOTINTERFACE_H
#define ROBOTINTERFACE_H

#include <iostream>
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/WrenchStamped.h>
#include <sensor_msgs/JointState.h>
#include <boost/circular_buffer.hpp>

#include <Eigen/Core>
#include "../../support/header/conversions.h"
#include "../../support/header/defines.h"
#include "../../support/header/formulas.h"


#define NELEMENTQUEUEFOR 10 //number of element in queue for force
#define NELEMENTQUEUETOR 10

class RobotInterface
{
public:
  RobotInterface(ros::NodeHandle nh, std::string robotName);
  int init();
  int getwTv(Eigen::Matrix4d* wTv_eigen);
  int getJointState(std::vector<double>* jState);
  int sendyDot(std::vector<double> yDot);

  int getForceTorque(Eigen::Vector3d* force, Eigen::Vector3d* torque);

  //int getwTjoints(std::vector<Eigen::Matrix4d> *wTjoints);


private:
  std::string robotName; //for tf listener (girona500_A,B)
  std::string topicRoot; //for publishing yDot and subscribing to sensors ("/uwsim/g500_A/")
  tf::TransformListener tfListener;
  ros::Publisher pubTwist;
  ros::Publisher pubJoint;
  ros::Subscriber subJointState;
  ros::Subscriber subForceTorque;

  std::vector<double> jState_priv;
  //std::vector<double> force_priv; with queues they dont need to be member of class
  //std::vector<double> torque_priv;
  void subJointStateCallback(const sensor_msgs::JointState& js);
  void subForceTorqueCallback(const geometry_msgs::WrenchStamped& msg);

  std::vector<boost::circular_buffer<double>> vectorForceQueue;
  std::vector<boost::circular_buffer<double>> vectorTorqueQueue;



};


#endif // ROBOTINTERFACE_H