source: flair-src/branches/sanscv/tools/VrpnLite/src/VrpnLite.cpp@ 452

Last change on this file since 452 was 324, checked in by Sanahuja Guillaume, 3 years ago

removing opencv dependency

File size: 2.8 KB
Line 
1// created: 2019/03/12
2// filename: VrpnLite.cpp
3//
4// author: Guillaume Sanahuja
5// Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7// version: $Id: $
8//
9// purpose: vrpnlite, to forward it to bth for exemple
10// usefull to reduce vrpn frame size
11//
12/*********************************************************************/
13
14#include "VrpnLite.h"
15#include <VrpnClient.h>
16#include <VrpnObject.h>
17#include <Thread.h>
18#include <UdpSocket.h>
19#include <FrameworkManager.h>
20#include <Quaternion.h>
21#include <string.h>
22
23using namespace std;
24using namespace flair::core;
25using namespace flair::sensor;
26
27VrpnLite::VrpnLite(string clientAddress,string serveurAddress): Thread(getFrameworkManager(),"VrpnLite",90) {
28 vrpnclient=new VrpnClient("vrpn", serveurAddress,80);
29 VrpnObject* vrpnobject = new VrpnObject("x4_0",vrpnclient->GetTabWidget(),vrpnclient);
30 //VrpnObject* vrpnobject2 = new VrpnObject("target",vrpnclient->GetTabWidget(),vrpnclient);
31
32 vrpnobjects.push_back(vrpnobject);
33 //vrpnobjects.push_back(vrpnobject2);
34
35 dataSocket = new UdpSocket(this,"client socket",clientAddress);
36
37 vrpnclient->Start();
38}
39
40VrpnLite::~VrpnLite() {
41}
42
43void VrpnLite::Run(void) {
44 while (!ToBeStopped()) {
45
46 WaitUpdate(vrpnobjects.at(0));//tood improve this wait, to be sure all vrpnobject are up to date; or one thread by object?
47 //could be also lighter to send only one frame with all objects
48 int i=0;
49 for (vector<VrpnObject*>::iterator it = vrpnobjects.begin();it < vrpnobjects.end(); it++) {
50 SendObject(*it,i);
51 i++;
52 }
53
54 }
55}
56
57void VrpnLite::SendObject(const VrpnObject* vrpnobject,uint8_t id) const{
58 Vector3Df objectPosition;
59 Quaternion objectQuaternion;
60
61 vrpnobject->GetPosition(objectPosition);
62 vrpnobject->GetQuaternion(objectQuaternion);
63 Time time=vrpnobject->GetLastPacketTime();
64
65 float position[3];
66 position[0]=objectPosition.x;
67 position[1]=objectPosition.y;
68 position[2]=objectPosition.z;
69 float quaternion[4];
70 quaternion[0]=objectQuaternion.q0;
71 quaternion[1]=objectQuaternion.q1;
72 quaternion[2]=objectQuaternion.q2;
73 quaternion[3]=objectQuaternion.q3;
74
75 for(int i=0;i<3;i++) dataSocket->HostToNetwork((char*)(&position[i]),sizeof(position[i]));
76 for(int i=0;i<4;i++) dataSocket->HostToNetwork((char*)(&quaternion[i]),sizeof(quaternion[i]));
77 dataSocket->HostToNetwork((char*)(&time),sizeof(Time));
78
79 char datas[sizeof(id) + sizeof(position)+sizeof(quaternion)+ sizeof(time)];
80
81 datas[0]=id;
82 memcpy(datas+sizeof(id),position, sizeof(position));
83 memcpy(datas +sizeof(id)+ sizeof(position),quaternion, sizeof(quaternion));
84 memcpy(datas+sizeof(id) + sizeof(position)+sizeof(quaternion),&time, sizeof(time));
85
86 dataSocket->SendMessage(datas,sizeof(datas));
87}
Note: See TracBrowser for help on using the repository browser.