vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Tracker_JsonNet.C
Go to the documentation of this file.
1 #include "vrpn_Tracker_JsonNet.h"
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
28 static 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 
32 static const char* const MSG_KEY_TRACKER_ID = "id";
33 static const char* const MSG_KEY_TRACKER_QUAT = "quat";
34 static const char* const MSG_KEY_TRACKER_POS = "pos";
35 
36 static const char* const MSG_KEY_BUTTON_ID = "button";
37 static const char* const MSG_KEY_BUTTON_STATUS = "state";
38 
39 static const char* const MSG_KEY_ANALOG_CHANNEL = "num";
40 static const char* const MSG_KEY_ANALOG_DATA = "data";
41 
42 static const char* const MSG_KEY_TEXT_DATA = "data";
43 
44 // Message types (values for MSG_KEY_TYPE)
45 static const int MSG_TYPE_TRACKER = 1;
46 static const int MSG_TYPE_BUTTON = 2;
47 static const int MSG_TYPE_ANALOG = 3;
48 static const int MSG_TYPE_TEXT = 4;
49 
50 vrpn_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  _pJsonReader(0),
57  _do_tracker_report(false)
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 
72 vrpn_Tracker_JsonNet::~vrpn_Tracker_JsonNet(void)
73 {
74  if (_pJsonReader != 0) {
75  delete _pJsonReader;
76  _pJsonReader = 0;
77  }
78  _network_release();
79 }
80 
81 
82 void vrpn_Tracker_JsonNet::mainloop() {
83  server_mainloop();
84  /*
85  * The original Dtrack code uses blocking call to select() in _network_receive with
86  * a 1 sec timeout. In Dtrack, the data is supposed to be continuously flowing (app. 60 Hz),
87  * so the timeout is unlikely to happen. However, the data from the Android device flow at a lower
88  * frequency and may not flow at all if the tilt tracker is disabled.
89  * Thus a 1 sec timeout here causes latency and jerky movements in Dtrack
90  */
91  const int timeout_us = 10 * 1000;
92  int received_length = _network_receive(_network_buffer, _NETWORK_BUFFER_SIZE, timeout_us);
93 
94  if (received_length < 0) {
95  //fprintf(stderr, "vrpn_Tracker_JsonNet : receive error %d\n", received_length);
96  return;
97  }
98  _network_buffer[received_length] = '\0';
99  //fprintf(stderr, "got data : %.*s\n", received_length, _network_buffer);
100  if (!_parse(_network_buffer, received_length)) {
101  // whatever error
102 
103  return;
104  }
105 
106  // report trackerchanges
107  // TODO really use timestamps
108  struct timeval ts ;
109  vrpn_gettimeofday(&ts, NULL);
110  // from vrpn_Tracker_DTrack::dtrack2vrpnbody
111  if (d_connection && _do_tracker_report) {
112  char msgbuf[1000];
113  // Encode pos and d_quat
114  int len = vrpn_Tracker::encode_to(msgbuf);
115  if (d_connection->pack_message(len, ts, position_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY)) {
116  // error
117  }
118  _do_tracker_report = false;
119  //fprintf(stderr, "Packed and sent\n");
120  }
121 
124 
125 }
126 
127 bool vrpn_Tracker_JsonNet::_parse(const char* buffer, int length) {
128  Json::Value root; // will contains the root value after parsing.
129  // Beware collectcomment = true crashes
130  bool parsingSuccessful = _pJsonReader->parse( buffer, root , false);
131  if ( !parsingSuccessful ) {
132  // report to the user the failure and their locations in the document.
133  fprintf(stderr, "vrpn_Tracker_JsonNet parse error :%s\n",
134  _pJsonReader->getFormatedErrorMessages().c_str());
135  fprintf(stderr, "%s\n",buffer);
136  return false;
137  }
138 
139  const Json::Value& constRoot = root;
140  // Find MessageType
141  const Json::Value& type = constRoot[MSG_KEY_TYPE];
142  int messageType;
143  if (!type.empty() && type.isConvertibleTo(Json::intValue)) {
144  messageType = type.asInt();
145  // HACK
146  } else {
147  fprintf(stderr, "vrpn_Tracker_JsonNet parse error : missing message type\n");
148  return false;
149  }
150  switch (messageType) {
151  // TODO cleanup
152  case MSG_TYPE_TRACKER:
153  return _parse_tracker_data(root);
154  break;
155  case MSG_TYPE_BUTTON:
156  return _parse_button(root);
157  break;
158  case MSG_TYPE_ANALOG:
159  return _parse_analog(root);
160  break;
161  case MSG_TYPE_TEXT:
162  return _parse_text(root);
163  break;
164  default:
165  ;
166  }
167  return false;
168 
169 
170 }
171 
180 bool vrpn_Tracker_JsonNet::_parse_tracker_data(const Json::Value& root) {
181  const Json::Value& constRoot = root;
182 
183  // Id of the current tracker
184  const Json::Value& sensorId = constRoot[MSG_KEY_TRACKER_ID];
185  if (!sensorId.empty() && sensorId.isConvertibleTo(Json::intValue)){
186  this->d_sensor = sensorId.asInt();
187  } else {
188  return false;
189  }
190 
191  /*
192  * mainloop calls vrpn_Tracker::encode_to, that will send d_sensor, d_pos and d_quat.
193  * velocity and acceleration are curretly not handled
194  */
195 
196  const Json::Value& quatData = constRoot[MSG_KEY_TRACKER_QUAT];
197  if (!quatData.empty() && quatData.isArray() && quatData.size() == 4) {
198  this->d_quat[0] = quatData[0u].asDouble();
199  this->d_quat[1] = quatData[1].asDouble();
200  this->d_quat[2] = quatData[2].asDouble();
201  this->d_quat[3] = quatData[3].asDouble();
202  //q_vec_type ypr;
203  //q_to_euler(ypr, d_quat);
204  //fprintf(stderr, "yaw-rY %.3f pitch-rX %.3f roll-rZ %.3f \n", ypr[0], ypr[1], ypr[2]);
205  }
206 
207  /*
208  * Look for a position
209  */
210  const Json::Value& posData = constRoot[MSG_KEY_TRACKER_POS];
211  if (!posData.empty() && posData.isArray() && posData.size() == 3) {
212  this->pos[0] = posData[0u].asDouble();
213  this->pos[1]= posData[1].asDouble();
214  this->pos[2]= posData[2].asDouble();
215  }
216 
217  _do_tracker_report = true;
218  return true;
219 }
220 
229 bool vrpn_Tracker_JsonNet::_parse_text(const Json::Value& root) {
230  const Json::Value& valueTextStatus = root[MSG_KEY_TEXT_DATA];
231  const char *msg = "";
232  if (!valueTextStatus.empty() && valueTextStatus.isConvertibleTo(Json::stringValue)) {
233  send_text_message(vrpn_TEXT_NORMAL) << valueTextStatus.asString();
234  return true;
235  }
236  fprintf(stderr, "vrpn_Tracker_JsonNet::_parse_text parse error : missing text");
237  return false;
238 }
247 bool vrpn_Tracker_JsonNet::_parse_button(const Json::Value& root) {
248  const Json::Value& valueButtonStatus = root[MSG_KEY_BUTTON_STATUS];
249  bool buttonStatus;
250  if (!valueButtonStatus.empty() && valueButtonStatus.isConvertibleTo(Json::booleanValue)) {
251  buttonStatus = valueButtonStatus.asBool();
252  } else {
253  fprintf(stderr, "vrpn_Tracker_JsonNet::_parse_button parse error : missing status");
254  return false;
255  }
256  const Json::Value& valueButtonId = root[MSG_KEY_BUTTON_ID];
257  int buttonId; // buttonId embedded in the message.
258  if (!valueButtonId.empty() && valueButtonId.isConvertibleTo(Json::intValue)) {
259  buttonId = valueButtonId.asInt();
260  } else {
261  fprintf(stderr, "vrpn_Tracker_JsonNet::_parse_button parse error : missing id\n");
262  return false;
263  }
264 
265  if (buttonId < 0 || buttonId > num_buttons) {
266  fprintf(stderr, "invalid button Id %d (max : %d)\n", buttonId, num_buttons);
267  } else {
268  buttons[buttonId] = (int)buttonStatus;
269  }
270 
271  return true;
272 }
273 
283 bool vrpn_Tracker_JsonNet::_parse_analog(const Json::Value& root) {
284  const Json::Value& valueData = root[MSG_KEY_ANALOG_DATA];
285  double data;
286  if (!valueData.empty() && valueData.isConvertibleTo(Json::realValue)) {
287  data = valueData.asDouble();
288  } else {
289  fprintf(stderr, "vrpn_Tracker_JsonNet::_parse_analog parse error : missing status");
290  return false;
291  }
292 
293 
294  const Json::Value& channelNumberId = root[MSG_KEY_ANALOG_CHANNEL];
295  int channelNumber;
296  if (!channelNumberId.empty() && channelNumberId.isConvertibleTo(Json::intValue)) {
297  channelNumber = channelNumberId.asInt();
298  } else {
299  fprintf(stderr, "vrpn_Tracker_JsonNet::_parse_analog parse error : missing id\n");
300  return false;
301  }
302 
303  if (channelNumber < 0 || channelNumber >= num_channel) {
304  fprintf(stderr, "vrpn_Tracker_JsonNet::_parse_analog id out of bounds %d/%d\n", channelNumber, num_channel);
305  } else {
306  channel[channelNumber] = data;
307  }
308 
309  return true;
310 }
311 
316 bool vrpn_Tracker_JsonNet::_network_init(int udp_port) {
317  int iResult;
318 #ifdef _WIN32
319  {
320  // Initialize Winsock
321  WORD versionRequested = MAKEWORD(2,2);
322  WSADATA wsaData;
323 
324  iResult = WSAStartup(versionRequested, &wsaData);
325  if (iResult != 0) {
326  printf("WSAStartup failed with error: %d\n", iResult);
327  return false;
328  }
329  }
330 #endif
331 
332 #ifdef _WIN32
333  {
334  // Create a SOCKET for connecting to server
335  _socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
336  if (_socket == INVALID_SOCKET) {
337  printf("socket failed with error: %ld\n", WSAGetLastError());
338  //freeaddrinfo(result);
339  WSACleanup();
340  return false;
341  }
342  }
343 #else
344  {
345  int usock;
346 
347  usock = socket(PF_INET, SOCK_DGRAM, 0);
348 
349  if (usock < 0){
350  return false;
351  }
352  _socket = usock;
353  }
354 #endif
355  struct sockaddr_in localSocketAddress;
356  localSocketAddress.sin_family = AF_INET;
357  localSocketAddress.sin_addr.s_addr = htonl(INADDR_ANY);
358  localSocketAddress.sin_port = htons(udp_port);
359 
360  // Setup the listening socket
361  iResult = bind( _socket, (struct sockaddr*)&localSocketAddress, sizeof(localSocketAddress));
362  if (iResult < 0) {
363 #ifdef _WIN32
364  printf("bind failed with error: %d\n", WSAGetLastError());
365 #else
366  printf("bind failed.");
367 #endif
368  //freeaddrinfo(result);
369  _network_release();
370  return false;
371  }
372 
373  //freeaddrinfo(result);
374  return true;
375 
376 }
377 
387 int vrpn_Tracker_JsonNet::_network_receive(void *buffer, int maxlen, int tout_us)
388 {
389  int nbytes, err;
390  fd_set set;
391  struct timeval tout;
392 
393  // waiting for data:
394 
395  FD_ZERO(&set);
396  FD_SET(_socket, &set);
397 
398  tout.tv_sec = tout_us / 1000000;
399  tout.tv_usec = tout_us % 1000000;
400 
401  switch((err = select(FD_SETSIZE, &set, NULL, NULL, &tout))){
402  case 1:
403  break; // data available
404  case 0:
405  //fprintf(stderr, "net_receive: select timeout (err = 0)\n");
406  return -1; // timeout
407  break;
408  default:
409  //fprintf(stderr, "net_receive: select error %d\n", err);
410  return -2; // error
411 
412  }
413 
414  // receiving packet:
415  while(1){
416 
417  // receive one packet:
418  nbytes = recv(_socket, (char *)buffer, maxlen, 0);
419  if(nbytes < 0){ // receive error
420  //fprintf(stderr, "recv_receive: select error %d\n", err);
421  return -3;
422  }
423 
424  // check, if more data available: if so, receive another packet
425  FD_ZERO(&set);
426  FD_SET(_socket, &set);
427 
428  tout.tv_sec = 0; // timeout with value of zero, thus no waiting
429  tout.tv_usec = 0;
430 
431  if(select(FD_SETSIZE, &set, NULL, NULL, &tout) != 1){
432  // no more data available: check length of received packet and return
433  if(nbytes >= maxlen){ // buffer overflow
434  return -4;
435  }
436  return nbytes;
437  }
438  }
439 }
440 
444 void vrpn_Tracker_JsonNet::_network_release() {
445 #ifdef _WIN32
446  closesocket(_socket);
447  WSACleanup();
448 #else
449  close(_socket);
450 #endif
451 }
452 
453 #endif // defined VRPN_USE_JSONNET
454 
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
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
Allows a user to send text messages from a device (usually,.
Definition: vrpn_Text.h:39
Generic connection class not specific to the transport mechanism.
#define INVALID_SOCKET
virtual void report_changes(void)
Definition: vrpn_Button.C:422
virtual int encode_to(char *buf)
Definition: vrpn_Tracker.C:533
const int vrpn_BUTTON_MAX_BUTTONS
Definition: vrpn_Button.h:12
Header allowing use of a output stream-style method of sending text messages from devices...
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
#define vrpn_CHANNEL_MAX
Definition: vrpn_Analog.h:16
All button servers should derive from this class, which provides the ability to turn any of the butto...
Definition: vrpn_Button.h:65