vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
vrpn_Tracker_JsonNet.C
Go to the documentation of this file.
2
3#if defined(VRPN_USE_JSONNET)
4
5#ifdef _WIN32
6 #ifdef VRPN_USE_WINSOCK2
7 #include <winsock2.h> // struct timeval is defined here
8 #else
9 #include <winsock.h> // struct timeval is defined here
10 #endif
11#else
12 #include <sys/socket.h>
13 #include <sys/time.h>
14 #include <netinet/in.h>
15 #include <unistd.h>
16 #define INVALID_SOCKET -1
17#endif
18
19#include "json/json.h"
20
21#include "quat.h"
22
24
25#include <stdlib.h> // for exit
26
27// These must match definitions in eu.ensam.ii.vrpn.Vrpn
28static const char* const MSG_KEY_TYPE = "type";
29//static const char* const MSG_KEY_SEQUENCE_NUMBER = "sn";
30//static const char* const MSG_KEY_TIMESTAMP = "ts";
31
32static const char* const MSG_KEY_TRACKER_ID = "id";
33static const char* const MSG_KEY_TRACKER_QUAT = "quat";
34static const char* const MSG_KEY_TRACKER_POS = "pos";
35
36static const char* const MSG_KEY_BUTTON_ID = "button";
37static const char* const MSG_KEY_BUTTON_STATUS = "state";
38
39static const char* const MSG_KEY_ANALOG_CHANNEL = "num";
40static const char* const MSG_KEY_ANALOG_DATA = "data";
41
42static const char* const MSG_KEY_TEXT_DATA = "data";
43
44// Message types (values for MSG_KEY_TYPE)
45static const int MSG_TYPE_TRACKER = 1;
46static const int MSG_TYPE_BUTTON = 2;
47static const int MSG_TYPE_ANALOG = 3;
48static const int MSG_TYPE_TEXT = 4;
49
50vrpn_Tracker_JsonNet::vrpn_Tracker_JsonNet(const char* name,vrpn_Connection* c,int udp_port) :
51 vrpn_Tracker(name, c),
52 vrpn_Button_Filter(name, c),
53 vrpn_Analog(name, c),
54 vrpn_Text_Sender(name, c),
55 _socket(INVALID_SOCKET),
56 _do_tracker_report(false),
57 _pJsonReader(0)
58{
59 fprintf(stderr, "vrpn_Tracker_JsonNet : Device %s listen on port udp port %d\n", name, udp_port);
60 if (! _network_init(udp_port)) {
61 exit(EXIT_FAILURE);
62 }
63
64 // Buttons part
65
66 num_buttons = vrpn_BUTTON_MAX_BUTTONS;
67 num_channel = vrpn_CHANNEL_MAX;
68
69 _pJsonReader = new Json::Reader();
70}
71
72vrpn_Tracker_JsonNet::~vrpn_Tracker_JsonNet(void)
73{
74 if (_pJsonReader != 0) {
75 try {
76 delete _pJsonReader;
77 } catch (...) {
78 fprintf(stderr, "vrpn_Tracker_JsonNet::~vrpn_Tracker_JsonNet(): delete failed\n");
79 return;
80 }
81 _pJsonReader = 0;
82 }
83 _network_release();
84}
85
86
87void vrpn_Tracker_JsonNet::mainloop()
88{
89 server_mainloop();
90 /*
91 * The original Dtrack code uses blocking call to select() in _network_receive with
92 * a 1 sec timeout. In Dtrack, the data is supposed to be continuously flowing (app. 60 Hz),
93 * so the timeout is unlikely to happen. However, the data from the Android device flow at a lower
94 * frequency and may not flow at all if the tilt tracker is disabled.
95 * Thus a 1 sec timeout here causes latency and jerky movements in Dtrack
96 */
97 const int timeout_us = 10 * 1000;
98 int received_length = _network_receive(_network_buffer, _NETWORK_BUFFER_SIZE, timeout_us);
99
100 if (received_length < 0) {
101 //fprintf(stderr, "vrpn_Tracker_JsonNet : receive error %d\n", received_length);
102 return;
103 }
104 _network_buffer[received_length] = '\0';
105 //fprintf(stderr, "got data : %.*s\n", received_length, _network_buffer);
106 if (!_parse(_network_buffer, received_length)) {
107 // whatever error
108
109 return;
110 }
111
112 // report trackerchanges
113 // TODO really use timestamps
114 struct timeval ts ;
115 vrpn_gettimeofday(&ts, NULL);
116 // from vrpn_Tracker_DTrack::dtrack2vrpnbody
117 if (d_connection && _do_tracker_report) {
118 char msgbuf[1000];
119 // Encode pos and d_quat
120 int len = vrpn_Tracker::encode_to(msgbuf);
121 if (d_connection->pack_message(len, ts, position_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY)) {
122 // error
123 }
124 _do_tracker_report = false;
125 //fprintf(stderr, "Packed and sent\n");
126 }
127
130}
131
132bool vrpn_Tracker_JsonNet::_parse(const char* buffer, int /*length*/)
133{
134 Json::Value root; // will contains the root value after parsing.
135 // Beware collectcomment = true crashes
136 bool parsingSuccessful = _pJsonReader->parse( buffer, root , false);
137 if ( !parsingSuccessful ) {
138 // report to the user the failure and their locations in the document.
139 fprintf(stderr, "vrpn_Tracker_JsonNet parse error :%s\n",
140 _pJsonReader->getFormatedErrorMessages().c_str());
141 fprintf(stderr, "%s\n",buffer);
142 return false;
143 }
144
145 const Json::Value& constRoot = root;
146 // Find MessageType
147 const Json::Value& type = constRoot[MSG_KEY_TYPE];
148 int messageType;
149 if (!type.empty() && type.isConvertibleTo(Json::intValue)) {
150 messageType = type.asInt();
151 // HACK
152 } else {
153 fprintf(stderr, "vrpn_Tracker_JsonNet parse error : missing message type\n");
154 return false;
155 }
156 switch (messageType) {
157 // TODO cleanup
158 case MSG_TYPE_TRACKER:
159 return _parse_tracker_data(root);
160 break;
161 case MSG_TYPE_BUTTON:
162 return _parse_button(root);
163 break;
164 case MSG_TYPE_ANALOG:
165 return _parse_analog(root);
166 break;
167 case MSG_TYPE_TEXT:
168 return _parse_text(root);
169 break;
170 default:
171 break;
172 }
173 return false;
174}
175
184bool vrpn_Tracker_JsonNet::_parse_tracker_data(const Json::Value& root)
185{
186 const Json::Value& constRoot = root;
187
188 // Id of the current tracker
189 const Json::Value& sensorId = constRoot[MSG_KEY_TRACKER_ID];
190 if (!sensorId.empty() && sensorId.isConvertibleTo(Json::intValue)){
191 this->d_sensor = sensorId.asInt();
192 } else {
193 return false;
194 }
195
196 /*
197 * mainloop calls vrpn_Tracker::encode_to, that will send d_sensor, d_pos and d_quat.
198 * velocity and acceleration are curretly not handled
199 */
200
201 const Json::Value& quatData = constRoot[MSG_KEY_TRACKER_QUAT];
202 if (!quatData.empty() && quatData.isArray() && quatData.size() == 4) {
203 this->d_quat[0] = quatData[0u].asDouble();
204 this->d_quat[1] = quatData[1].asDouble();
205 this->d_quat[2] = quatData[2].asDouble();
206 this->d_quat[3] = quatData[3].asDouble();
207 //q_vec_type ypr;
208 //q_to_euler(ypr, d_quat);
209 //fprintf(stderr, "yaw-rY %.3f pitch-rX %.3f roll-rZ %.3f \n", ypr[0], ypr[1], ypr[2]);
210 }
211
212 /*
213 * Look for a position
214 */
215 const Json::Value& posData = constRoot[MSG_KEY_TRACKER_POS];
216 if (!posData.empty() && posData.isArray() && posData.size() == 3) {
217 this->pos[0] = posData[0u].asDouble();
218 this->pos[1]= posData[1].asDouble();
219 this->pos[2]= posData[2].asDouble();
220 }
221
222 _do_tracker_report = true;
223 return true;
224}
225
234bool vrpn_Tracker_JsonNet::_parse_text(const Json::Value& root)
235{
236 const Json::Value& valueTextStatus = root[MSG_KEY_TEXT_DATA];
237 if (!valueTextStatus.empty() && valueTextStatus.isConvertibleTo(Json::stringValue)) {
238 send_text_message(vrpn_TEXT_NORMAL) << valueTextStatus.asString();
239 return true;
240 }
241 fprintf(stderr, "vrpn_Tracker_JsonNet::_parse_text parse error : missing text");
242 return false;
243}
252bool vrpn_Tracker_JsonNet::_parse_button(const Json::Value& root)
253{
254 const Json::Value& valueButtonStatus = root[MSG_KEY_BUTTON_STATUS];
255 bool buttonStatus;
256 if (!valueButtonStatus.empty() && valueButtonStatus.isConvertibleTo(Json::booleanValue)) {
257 buttonStatus = valueButtonStatus.asBool();
258 } else {
259 fprintf(stderr, "vrpn_Tracker_JsonNet::_parse_button parse error : missing status");
260 return false;
261 }
262 const Json::Value& valueButtonId = root[MSG_KEY_BUTTON_ID];
263 int buttonId; // buttonId embedded in the message.
264 if (!valueButtonId.empty() && valueButtonId.isConvertibleTo(Json::intValue)) {
265 buttonId = valueButtonId.asInt();
266 } else {
267 fprintf(stderr, "vrpn_Tracker_JsonNet::_parse_button parse error : missing id\n");
268 return false;
269 }
270
271 if (buttonId < 0 || buttonId > num_buttons) {
272 fprintf(stderr, "invalid button Id %d (max : %d)\n", buttonId, num_buttons);
273 } else {
274 buttons[buttonId] = (int)buttonStatus;
275 }
276
277 return true;
278}
279
289bool vrpn_Tracker_JsonNet::_parse_analog(const Json::Value& root)
290{
291 const Json::Value& valueData = root[MSG_KEY_ANALOG_DATA];
292 double data;
293 if (!valueData.empty() && valueData.isConvertibleTo(Json::realValue)) {
294 data = valueData.asDouble();
295 } else {
296 fprintf(stderr, "vrpn_Tracker_JsonNet::_parse_analog parse error : missing status");
297 return false;
298 }
299
300 const Json::Value& channelNumberId = root[MSG_KEY_ANALOG_CHANNEL];
301 int channelNumber;
302 if (!channelNumberId.empty() && channelNumberId.isConvertibleTo(Json::intValue)) {
303 channelNumber = channelNumberId.asInt();
304 } else {
305 fprintf(stderr, "vrpn_Tracker_JsonNet::_parse_analog parse error : missing id\n");
306 return false;
307 }
308
309 if (channelNumber < 0 || channelNumber >= num_channel) {
310 fprintf(stderr, "vrpn_Tracker_JsonNet::_parse_analog id out of bounds %d/%d\n", channelNumber, num_channel);
311 } else {
312 channel[channelNumber] = data;
313 }
314
315 return true;
316}
317
322bool vrpn_Tracker_JsonNet::_network_init(int udp_port)
323{
324 int iResult;
325#ifdef _WIN32
326 {
327 // Initialize Winsock
328 WORD versionRequested = MAKEWORD(2,2);
329 WSADATA wsaData;
330
331 iResult = WSAStartup(versionRequested, &wsaData);
332 if (iResult != 0) {
333 printf("WSAStartup failed with error: %d\n", iResult);
334 return false;
335 }
336 }
337#endif
338
339#ifdef _WIN32
340 {
341 // Create a SOCKET for connecting to server
342 _socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
343 if (_socket == INVALID_SOCKET) {
344 printf("socket failed with error: %ld\n", WSAGetLastError());
345 //freeaddrinfo(result);
346 WSACleanup();
347 return false;
348 }
349 }
350#else
351 {
352 int usock;
353
354 usock = socket(PF_INET, SOCK_DGRAM, 0);
355
356 if (usock < 0){
357 return false;
358 }
359 _socket = usock;
360 }
361#endif
362 struct sockaddr_in localSocketAddress;
363 memset((void *)&localSocketAddress, 0, sizeof(localSocketAddress));
364 localSocketAddress.sin_family = AF_INET;
365 localSocketAddress.sin_addr.s_addr = htonl(INADDR_ANY);
366 localSocketAddress.sin_port = htons(udp_port);
367
368 // Setup the listening socket
369 iResult = bind( _socket, (struct sockaddr*)&localSocketAddress, sizeof(localSocketAddress));
370 if (iResult < 0) {
371#ifdef _WIN32
372 printf("bind failed with error: %d\n", WSAGetLastError());
373#else
374 printf("bind failed.");
375#endif
376 //freeaddrinfo(result);
377 _network_release();
378 return false;
379 }
380
381 //freeaddrinfo(result);
382 return true;
383
384}
385
395int vrpn_Tracker_JsonNet::_network_receive(void *buffer, int maxlen, int tout_us)
396{
397 int nbytes, err;
398 fd_set set;
399 struct timeval tout;
400
401 // waiting for data:
402
403 FD_ZERO(&set);
404 FD_SET(_socket, &set);
405
406 tout.tv_sec = tout_us / 1000000;
407 tout.tv_usec = tout_us % 1000000;
408
409 switch((err = select(FD_SETSIZE, &set, NULL, NULL, &tout))){
410 case 1:
411 break; // data available
412 case 0:
413 //fprintf(stderr, "net_receive: select timeout (err = 0)\n");
414 return -1; // timeout
415 break;
416 default:
417 //fprintf(stderr, "net_receive: select error %d\n", err);
418 return -2; // error
419
420 }
421
422 // receiving packet:
423 while(1){
424
425 // receive one packet:
426 nbytes = recv(_socket, (char *)buffer, maxlen, 0);
427 if(nbytes < 0){ // receive error
428 //fprintf(stderr, "recv_receive: select error %d\n", err);
429 return -3;
430 }
431
432 // check, if more data available: if so, receive another packet
433 FD_ZERO(&set);
434 FD_SET(_socket, &set);
435
436 tout.tv_sec = 0; // timeout with value of zero, thus no waiting
437 tout.tv_usec = 0;
438
439 if(select(FD_SETSIZE, &set, NULL, NULL, &tout) != 1){
440 // no more data available: check length of received packet and return
441 if(nbytes >= maxlen){ // buffer overflow
442 return -4;
443 }
444 return nbytes;
445 }
446 }
447}
448
452void vrpn_Tracker_JsonNet::_network_release()
453{
454#ifdef _WIN32
455 closesocket(_socket);
456 WSACleanup();
457#else
458 close(_socket);
459#endif
460}
461
462#endif // defined VRPN_USE_JSONNET
463
virtual void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Send a report only if something has changed (for servers) Optionally, tell what time to stamp the val...
Definition: vrpn_Analog.C:71
All button servers should derive from this class, which provides the ability to turn any of the butto...
Definition: vrpn_Button.h:66
virtual void report_changes(void)
Definition: vrpn_Button.C:423
Generic connection class not specific to the transport mechanism.
Allows a user to send text messages from a device (usually,.
Definition: vrpn_Text.h:40
virtual int encode_to(char *buf)
Definition: vrpn_Tracker.C:552
#define vrpn_CHANNEL_MAX
Definition: vrpn_Analog.h:16
@ vrpn_TEXT_NORMAL
const int vrpn_BUTTON_MAX_BUTTONS
Definition: vrpn_Button.h:13
#define INVALID_SOCKET
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
Header allowing use of a output stream-style method of sending text messages from devices.
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:99