source: flair-src/trunk/tools/VrpnLite/src/VrpnLite.cpp@ 446

Last change on this file since 446 was 446, checked in by Sanahuja Guillaume, 4 months ago

vrpnlite: add headless mode and kill button

File size: 4.9 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 <TabWidget.h>
20#include <Tab.h>
21#include <PushButton.h>
22#include <FrameworkManager.h>
23#include <Quaternion.h>
24#include <string.h>
25
26using namespace std;
27using namespace flair::core;
28using namespace flair::sensor;
29using namespace flair::gui;
30
31VrpnLite::VrpnLite(int vrpnLitePort,string vrpnServerAddress): Thread(getFrameworkManager(),"VrpnLite",90) {
32 vrpnclient=new VrpnClient("vrpn", vrpnServerAddress,80);
33 Tab* tab=new Tab(vrpnclient->GetTabWidget(),"Main");
34 killButton=new PushButton(tab->NewRow(),"kill");
35 dataSocket = new UdpSocket(this,"client socket",vrpnLitePort);
36 vrpnclient->Start();
37}
38
39VrpnLite::~VrpnLite() {
40}
41
42void VrpnLite::Run(void) {
43 Time dataSocketTimeout;
44 char msg[256];
45 int src_id;
46
47 while (!ToBeStopped()) {
48 if(killButton->Clicked()) SafeStop();
49
50 if(vrpnobjects.size()>0) {
51 //wait for last one to be sure all are up to date
52 //but if object is not tracked we send nothing... send it unsynchronized???
53 if(WaitUpdate(vrpnobjects.at(vrpnobjects.size()-1),100000000)) {
54 SendObjects();
55 }
56 dataSocketTimeout=TIME_NONBLOCK;
57 } else {
58 dataSocketTimeout=100000000;
59 }
60
61 ssize_t rcv=dataSocket->RecvMessage(msg,sizeof(msg),dataSocketTimeout,NULL,NULL,&src_id);
62 if(rcv>0) {
63 string object=msg;
64 uint16_t id;
65 memcpy(&id,&msg[rcv-2],sizeof(id));
66 dataSocket->HostToNetwork((char*)&id,sizeof(id));
67
68 if(src_id==vrpnobjects_list.size()) {
69 vector<VrpnObject*> vect;
70 vrpnobjects_list.push_back(vect);
71 //Printf("add vect %i\n",src_id);
72 }
73 //assume we receive it in the good order
74 if(id==vrpnobjects_list.at(src_id).size()) {
75 Printf("adding object %s with id %i from %i\n",object.c_str(),id,src_id);
76 VrpnObject* vrpnobject = new VrpnObject(object,vrpnclient->GetTabWidget());
77 vrpnobjects.push_back(vrpnobject);
78 vrpnobjects_list.at(src_id).push_back(vrpnobject);
79 }else {
80 Err("adding object %s failed, expected id %i, got %i\n",object.c_str(),vrpnobjects.size(),id);
81 }
82 }
83 }
84}
85
86void VrpnLite::SendObjects(void) const{
87
88 int16_t position[3];
89 int16_t quaternion[4];
90 Time time;
91
92 //for (vector<vector<VrpnObject*>>::const_iterator connection= vrpnobjects_list.begin();connection < vrpnobjects_list.end(); connection++) {
93 for (int i=0;i<vrpnobjects_list.size();i++) {
94 char datas[vrpnobjects_list.at(i).size()*(sizeof(position)+sizeof(quaternion))+ sizeof(time)];
95 char *datasPtr=datas;
96 for (vector<VrpnObject*>::const_iterator it = vrpnobjects_list.at(i).begin();it < vrpnobjects_list.at(i).end(); it++) {
97 Vector3Df objectPosition;
98 Quaternion objectQuaternion;
99 const VrpnObject* vrpnobject=*it;
100
101 vrpnobject->GetPosition(objectPosition);
102 vrpnobject->GetQuaternion(objectQuaternion);
103 time=vrpnobject->GetLastPacketTime();
104
105 position[0]=ConvertPosition(objectPosition.x);
106 position[1]=ConvertPosition(objectPosition.y);
107 position[2]=ConvertPosition(objectPosition.z);
108 quaternion[0]=ConvertQuaternion(objectQuaternion.q0);
109 quaternion[1]=ConvertQuaternion(objectQuaternion.q1);
110 quaternion[2]=ConvertQuaternion(objectQuaternion.q2);
111 quaternion[3]=ConvertQuaternion(objectQuaternion.q3);
112
113 for(int i=0;i<3;i++) dataSocket->HostToNetwork((char*)(&position[i]),sizeof(position[i]));
114 for(int i=0;i<4;i++) dataSocket->HostToNetwork((char*)(&quaternion[i]),sizeof(quaternion[i]));
115
116 memcpy(datasPtr,position, sizeof(position));
117 datasPtr+=sizeof(position);
118 memcpy(datasPtr,quaternion, sizeof(quaternion));
119 datasPtr+=sizeof(quaternion);
120 }
121 dataSocket->HostToNetwork((char*)(&time),sizeof(Time));
122 memcpy(datasPtr,&time, sizeof(time));//only one time for all VrpnObject; suppose it is the same!
123 dataSocket->SendMessage(datas,sizeof(datas),i);
124 }
125}
126
127int16_t VrpnLite::ConvertQuaternion(float value) const{
128 int16_t tmp;
129 tmp=value*32767.;
130 if(value<-1) {
131 tmp=-32767;
132 Warn("position value is %f, saturating it to %i\n",value,tmp);
133 }
134 if(value>1) {
135 tmp=32767;
136 Warn("position value is %f, saturating it to %i\n",value,tmp);
137 }
138 return tmp;
139}
140
141int16_t VrpnLite::ConvertPosition(float value) const{
142 int16_t tmp;
143 tmp=value*1000;
144 if(value<-32.768) {
145 tmp=-32768;
146 Warn("position value is %f, saturating it to %i\n",value,tmp);
147 }
148 if(value>32.767) {
149 tmp=32767;
150 Warn("position value is %f, saturating it to %i\n",value,tmp);
151 }
152 return tmp;
153}
Note: See TracBrowser for help on using the repository browser.