rfly_vrpn.h

#pragma once
 
#include ".\vrpn\vrpn_Tracker.h"
#include ".\vrpn\vrpn_Connection.h"
#include <string>
#include <vector>
#include "rfly_quat.h"
 
#if defined (_DLL)
#pragma comment(lib,"vrpn.lib")
#pragma comment(lib,"quat.lib")
#else
#pragma comment(lib,"vrpn_mt.lib")
#pragma comment(lib,"quat_mt.lib")
#endif
 
typedef std::shared_ptr<vrpn_Connection> ConnectionPtr;
typedef std::shared_ptr<vrpn_Tracker_Remote> TrackerRemotePtr;
 
class VrpnTrackerRfly
{
public:
 bool has_data = false;
 bool has_pos = false;
 bool has_vel = false;
 bool has_acc = false;
 
 vrpn_TRACKERCB vrpn_pose;
 vrpn_TRACKERVELCB vrpn_vel;
 vrpn_TRACKERACCCB vrpn_acc;
 
  std::string tracker_name;
 
 // typedef std::shared_ptr<VrpnTrackerRfly> Ptr;
 
 void CreateTracker(std::string tracker_name, ConnectionPtr connection){
    tracker_remote_ = std::make_shared<vrpn_Tracker_Remote>(tracker_name.c_str(), connection.get());
 
    init(tracker_name);
  }
 
 VrpnTrackerRfly()
  {
 for(int i=0;i<3;i++){
      vrpn_pose.pos[i]=0;
      vrpn_pose.quat[i]=0;
      vrpn_vel.vel[i]=0;
      vrpn_vel.vel_quat[i]=0;
      vrpn_acc.acc[i]=0;
      vrpn_acc.acc_quat[i]=0;
    }
    vrpn_pose.quat[3]=1;
    vrpn_vel.vel_quat[3]=1;
    vrpn_vel.vel_quat_dt=0;
    vrpn_acc.acc_quat[3]=1;
    vrpn_acc.acc_quat_dt=0;
    vrpn_pose.msg_time.tv_sec=0;
    vrpn_pose.msg_time.tv_usec=0;
    vrpn_vel.msg_time.tv_sec=0;
    vrpn_vel.msg_time.tv_usec=0;
    vrpn_acc.msg_time.tv_sec=0;
    vrpn_acc.msg_time.tv_usec=0;
  }
 
 ~VrpnTrackerRfly()
  {
    std::cout << "Destroying tracker " << std::endl;
    tracker_remote_->unregister_change_handler(this, &VrpnTrackerRfly::handle_pose);
    tracker_remote_->unregister_change_handler(this, &VrpnTrackerRfly::handle_twist);
    tracker_remote_->unregister_change_handler(this, &VrpnTrackerRfly::handle_accel);
  }
 
 void output(double y[30])
  {
    tracker_remote_->mainloop();
 
 // y0 -> Time stamp sec for pos msg
 // y1 -> Time stamp usec for pos msg
 // t_pos_msg = y0 + y1 / 1000000.0
 // y2-y4 -> Pos_x,Pos_y,Pos_z in ned, unit m
 // y5-y8 -> Quat (w,x,y,z) for vehicle attitude
 // y9 -> Time stamp sec for vel msg
 // y10 -> Time stamp usec for vel msg
 // t_vel_msg = y9 + y10 / 1000000.0
 // y11-y13 -> vel_x,vel_y,vel_z in ned, unit m/s
 // y14 -> Time stamp sec for acc msg
 // y15 -> Time stamp usec for acc msg
 // y16-y18 -> acc_x,acc_y,acc_z in ned, unit m/s^2
 // y19-y21 -> Euler (roll pitch yaw) ang from vehicle Quat
 // y22 -> tracker->has_data, is there any vrpn message received.
 // y23 -> tracker->has_pos, is the vrpn pos message received.
 // 24-29 -> reserve for future use
 
 // !!! Translate Z-UP to Z-Down
    y[0] = vrpn_pose.msg_time.tv_sec;  
    y[1] = vrpn_pose.msg_time.tv_usec; 
 for (int j = 0; j<3; j++)
        y[j+2] = vrpn_pose.pos[j];     
    y[3] = -y[3];
    y[4] = -y[4];
 
 // Coordinate Transformation for y,z axis
    y[5]=vrpn_pose.quat[3]; //w
    y[6]=vrpn_pose.quat[0]; //x
    y[7]=-vrpn_pose.quat[1]; //y
    y[8]=-vrpn_pose.quat[2]; //z
 
 
    y[9] = vrpn_vel.msg_time.tv_sec;
    y[10] = vrpn_vel.msg_time.tv_usec;
 for (int j = 0; j<3; j++)
        y[j+11] = vrpn_vel.vel[j];
    y[12] = -y[12];
    y[13] = -y[13];
 
    y[14] = vrpn_acc.msg_time.tv_sec;
    y[15] = vrpn_acc.msg_time.tv_usec;
 for (int j = 0; j<3; j++)
        y[j+16] = vrpn_acc.acc[j];
    y[17] = -y[17];
    y[18] = -y[18];
 
 double yawPitchRoll[3];
    q_to_euler(yawPitchRoll, vrpn_pose.quat);
    y[19] = yawPitchRoll[2];   // Roll
    y[20] = -yawPitchRoll[1];  // pitch 
    y[21] = -yawPitchRoll[0];  // yaw
    y[22] = has_data;
    y[23] = has_pos;
  }
 
private:
  TrackerRemotePtr tracker_remote_;
 
 void init(std::string tracker_name)
  {
    std::cout << "Creating new tracker " << tracker_name << std::endl;
 
    tracker_remote_->register_change_handler(this, &VrpnTrackerRfly::handle_pose);
    tracker_remote_->register_change_handler(this, &VrpnTrackerRfly::handle_twist);
    tracker_remote_->register_change_handler(this, &VrpnTrackerRfly::handle_accel);
    tracker_remote_->shutup = true;
 
    this->tracker_name = tracker_name;
  }
 
 static void VRPN_CALLBACK handle_pose(void *userData, const vrpn_TRACKERCB tracker_pose)
  {
 VrpnTrackerRfly *tracker = static_cast<VrpnTrackerRfly *>(userData);
 
    memcpy(&tracker->vrpn_pose, &tracker_pose, sizeof(tracker_pose));
    tracker->has_pos = true;
    tracker->has_data = true;
 //   std::cout <<"Name:"<<tracker->tracker_name<<"\tx: "<< tracker->pose[1] << "\ty: " << tracker->pose[2] << std::endl;
  }
 
 static void VRPN_CALLBACK handle_twist(void *userData, const vrpn_TRACKERVELCB tracker_twist)
  {
 VrpnTrackerRfly *tracker = static_cast<VrpnTrackerRfly *>(userData);
 
    memcpy(&tracker->vrpn_vel, &tracker_twist, sizeof(tracker_twist));
    tracker->has_vel = true;
    tracker->has_data = true;
  }
 
 static void VRPN_CALLBACK handle_accel(void *userData, const vrpn_TRACKERACCCB tracker_accel)
  {
 VrpnTrackerRfly *tracker = static_cast<VrpnTrackerRfly *>(userData);
 
    memcpy(&tracker->vrpn_acc, &tracker_accel, sizeof(tracker_accel));
    tracker->has_acc = true;
    tracker->has_data = true;
  }
 
};
 
class VrpnConnectionRfly
{
public:
 // typedef std::shared_ptr<VrpnConnectionRfly> Ptr;
 
 VrpnConnectionRfly()
  {
 
  }
 
 void mainloop()
  {
 connection_->mainloop();
 if (!connection_->doing_okay())
    {
      std::cout << "VRPN connection is not 'doing okay'" << std::endl;
    }
  }
 
 void CreateConnect(std::string host,int port=3883){
    host_ = host + ":"+std::to_string(port);
    std::cout << "Connecting to VRPN server at host:" << host_ << std::endl;
 connection_ = std::shared_ptr<vrpn_Connection>(vrpn_get_connection_by_name(host_.c_str()));
    std::cout << "Connection established" << std::endl;
  }
 
  std::string host_;
 
  ConnectionPtr connection_;
};