source: flair-src/trunk/lib/FlairSensorActuator/src/VrpnClient_impl.cpp@ 320

// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2013/04/03
//  filename:   VrpnClient_impl.cpp
//
//  author:     César Richard, Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    objet se connectant au serveur vrpn
//
//
/*********************************************************************/

#include "VrpnClient.h"
#include "VrpnClient_impl.h"
#include "VrpnObject.h"
#include "VrpnObject_impl.h"
#include <SerialPort.h>
#include <vrpn_Connection.h>
#include <FrameworkManager.h>
#include <TabWidget.h>
#include <Tab.h>
#include <GridLayout.h>
#include <OneAxisRotation.h>
#include <Vector3D.h>
#include <Quaternion.h>
#include <Mutex.h>
#include <UdpSocket.h>
#include <math.h>

using std::string;
using std::vector;
using namespace flair::core;
using namespace flair::gui;
using namespace flair::sensor;

VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
                                 std::string address) {
  this->self = self;
  this->address = address;
  isConnected=false;
  connectionType=VrpnClient::Vrpn;

  connection = vrpn_get_connection_by_name(address.c_str());

  CommonConstructor(name);
  
  Printf("Connecting to VRPN server on %s\n",address.c_str());
}

VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
                                 uint16_t port) {
  this->self = self;
        isConnected=false;
  connectionType=VrpnClient::VrpnLite;

  dataSocket =new UdpSocket(getFrameworkManager(), "data_socket", port);

  CommonConstructor(name);
  
  Printf("Connecting to VRPN-lite server on port %i\n",port);
}

VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
                                 SerialPort *serialport, uint16_t us_period) {
  this->us_period = us_period;
  this->self = self;
  this->serialport = serialport;
  connectionType=VrpnClient::Xbee;

  serialport->SetBaudrate(111111);
  serialport->SetRxTimeout(us_period * 1000);

  CommonConstructor(name);
  
  Printf("Connecting to VRPN server through xbee on %s\n",serialport->ObjectName().c_str());
}

void VrpnClient_impl::CommonConstructor(std::string name) {
  mutex = new Mutex(self, name);
  
  // station sol
  main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
  tab = new TabWidget(main_tab->NewRow(), name);
  setup_tab = new Tab(tab, "Reglages");

  rotation_1 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 1",OneAxisRotation::PreRotation);
  rotation_2 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 2",OneAxisRotation::PreRotation);
}

VrpnClient_impl::~VrpnClient_impl() {
  if (connectionType==VrpnClient::Vrpn) {
    // on fait une copie car le delete touche a trackables_copy via
    // RemoveTrackable
    vector<VrpnObject_impl *> trackables_copy = trackables;
    for (unsigned int i = 0; i < trackables_copy.size(); i++)
      delete trackables_copy.at(i)->self;
    // trackables.clear();
  } else if(connectionType==VrpnClient::Xbee || connectionType==VrpnClient::VrpnLite) {
    // on fait une copie car le delete touche a xbee_objects_copy via
    // RemoveTrackable
    vector<liteObject_t> liteObjects_copy = liteObjects;
    for (unsigned int i = 0; i < liteObjects_copy.size(); i++)
      delete liteObjects_copy.at(i).vrpnobject->self;
  }

  delete main_tab;

  if (connectionType==VrpnClient::Vrpn) {
    // it will automatically delete connection
    connection->removeReference();
  }
}

void VrpnClient_impl::ComputeRotations(Vector3Df &point) {
  rotation_1->ComputeRotation(point);
  rotation_2->ComputeRotation(point);
}

void VrpnClient_impl::ComputeRotations(Quaternion &quat) {
  rotation_1->ComputeRotation(quat);
  rotation_2->ComputeRotation(quat);
}

void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj) {
  if (connectionType==VrpnClient::Vrpn) {
    mutex->GetMutex();
    trackables.push_back(obj);
    mutex->ReleaseMutex();
  } else {
    self->Warn("AddTrackable called but not in vrpn mode\n");
  }
}

void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj, uint8_t id) {
  if (connectionType==VrpnClient::VrpnLite || connectionType==VrpnClient::Xbee) {
    liteObject_t tmp;
    tmp.vrpnobject = obj;
    tmp.id = id;
    mutex->GetMutex();
    liteObjects.push_back(tmp);
    mutex->ReleaseMutex();
  } else {
    self->Warn("AddTrackable called but not in vrpnlite nor in xbee mode\n");
  }
}

void VrpnClient_impl::RemoveTrackable(VrpnObject_impl *obj) {
  mutex->GetMutex();
  if (connectionType==VrpnClient::Vrpn) {
    for (vector<VrpnObject_impl *>::iterator it = trackables.begin();
         it < trackables.end(); it++) {
      if (*it == obj) {
        trackables.erase(it);
        break;
      }
    }
  }
  if (connectionType==VrpnClient::VrpnLite || connectionType==VrpnClient::Xbee) {
    for (vector<liteObject_t>::iterator it = liteObjects.begin();
         it < liteObjects.end(); it++) {
      if ((*it).vrpnobject == obj) {
        liteObjects.erase(it);
        break;
      }
    }
  }
  mutex->ReleaseMutex();
}

void VrpnClient_impl::Run(void) {
        struct timeval timeout;
        timeout.tv_sec=0;
        timeout.tv_usec=100000;
                        
  while (!self->ToBeStopped()) {
    if (connectionType==VrpnClient::Xbee) {
      ssize_t read = 0;
      uint8_t response[38] = {0};

      read = serialport->Read(response, sizeof(response));
      if (read > 0 && read != sizeof(response))
        read += serialport->Read(&response[read], sizeof(response) - read);
      // int temps=(float)self->GetTime()/(1000*1000);
      // self->Printf("%i %i %i\n",temps-last,temps,last);
      // last=temps;
      if (read < 0) {
        // self->Warn("erreur rt_dev_read (%s)\n",strerror(-read));
      } else if (read != sizeof(response)) {
        self->Warn("serial read error %i/%i\n", read, sizeof(response));
      } else {
        // for(ssize_t i=0;i<read;i++) printf("%x ",response[i]);
        // printf("\n");
        uint8_t checksum = 0;
        for (ssize_t i = 3; i < read; i++)
          checksum += response[i];
        if (checksum != 255) {
          self->Err("checksum error\n");
        } else {
          vrpn_TRACKERCB t;
          float pos[3];
          float quat[4];
          uint8_t id = response[8];

          mutex->GetMutex();
          if (id < liteObjects.size()) {
            memcpy(pos, &response[9], sizeof(pos));
            memcpy(quat, &response[9] + sizeof(pos), sizeof(quat));
            for (int i = 0; i < 3; i++)
              t.pos[i] = pos[i];
            for (int i = 0; i < 4; i++)
              t.quat[i] = quat[i];
            if (fabs(pos[0] > 10) || fabs(pos[1] > 10) || fabs(pos[2] > 10)) {
              Printf("prob pos %f %f %f\n", pos[0], pos[1], pos[2]);
            } else {
              // self->Printf("%i %f %f %f
              // %f\n",id,pos[0],pos[1],pos[2],(float)self->GetTime()/(1000*1000));
              VrpnObject_impl::handle_pos(liteObjects.at(id).vrpnobject, t);
            }
          }
          mutex->ReleaseMutex();
        }
      }
    } else if(connectionType==VrpnClient::Vrpn) {
        if(connection->connected()==vrpn_true && !isConnected) {
            isConnected=true;
            Printf("VRPN connected to %s\n",address.c_str());
        }
        if(connection->connected()==vrpn_false && isConnected) {
            isConnected=false;
            Printf("VRPN disconnected to %s\n",address.c_str());
        }
        //timeout in mainloop is not well handled if not connected...
        if(isConnected) {
            //this is when trackables callbacks are called:
            if(connection->mainloop(&timeout)!=0) {
                self->Warn("connection dropped from\n",address.c_str());
            }
            //printf("%lld\n",GetTime()/(1000*1000));
            mutex->GetMutex();
            for (unsigned int i = 0; i < trackables.size(); i++)
                trackables.at(i)->tracker->mainloop();
            mutex->ReleaseMutex();
        } else {
            connection->mainloop();
            self->SleepMS(10);
        }
    }else if(connectionType==VrpnClient::VrpnLite) {
      vrpn_TRACKERCB t;
      float pos[3];
      float quat[4];
      Time time;
      uint8_t id;
      char datas[sizeof(id) + sizeof(pos)+sizeof(quat)+ sizeof(time)];
      int rcv=dataSocket->RecvMessage(datas,sizeof(datas),50*1000*1000);
      if(rcv!=sizeof(datas)) continue;  
      id = datas[0];
      memcpy(pos, datas+sizeof(id), sizeof(pos));
      memcpy(quat, datas +sizeof(id)+ sizeof(pos), sizeof(quat));
      memcpy(&time, datas+sizeof(id) + sizeof(pos)+sizeof(quat), sizeof(time));
      
      for(int i=0;i<3;i++) dataSocket->NetworkToHost((char*)(&pos[i]),sizeof(pos[i]));
      for(int i=0;i<4;i++) dataSocket->NetworkToHost((char*)(&quat[i]),sizeof(quat[i]));
      dataSocket->NetworkToHost((char*)(&time),sizeof(time));

      mutex->GetMutex();
      if (id < liteObjects.size()) {
        for (int i = 0; i < 3; i++) t.pos[i] = pos[i];
        // warning: t.quat is defined as (qx,qy,qz,qw), which is different from
        // flair::core::Quaternion
        t.quat[0] = quat[1];
        t.quat[1] = quat[2];
        t.quat[2] = quat[3];
        t.quat[3] = quat[0];
        t.msg_time.tv_sec=time/((Time)1000000000);
        t.msg_time.tv_usec=(time%((Time)1000000000))/((Time)1000);
        //Printf("%i %lld %lld %lld\n",id,time,t.msg_time.tv_sec,t.msg_time.tv_usec);
        VrpnObject_impl::handle_pos(liteObjects.at(id).vrpnobject, t);
      }
      mutex->ReleaseMutex();
    }
  }
}