vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
vrpn_Tracker_DTrack.C
Go to the documentation of this file.
1// vrpn_Tracker_DTrack.C
2//
3// Advanced Realtime Tracking GmbH's (http://www.ar-tracking.de) DTrack/DTrack2 client
4
5// developed by David Nahon for Virtools VR Pack (http://www.virtools.com)
6// (07/20/2004) improved by Advanced Realtime Tracking GmbH (http://www.ar-tracking.de)
7// (07/02/2007, 06/29/2009) upgraded by Advanced Realtime Tracking GmbH to support new devices
8// (08/25/2010) a correction added by Advanced Realtime Tracking GmbH
9// (12/01/2010) support of 3dof objects added by Advanced Realtime Tracking GmbH
10//
11// Recommended settings within DTrack's 'Settings / Network' or DTrack2's 'Settings / Output' dialog:
12// 'ts', '6d', '6df' or '6df2', '6dcal', '3d' (optional)
13
14/* Configuration file:
15
16################################################################################
17# Advanced Realtime Tracking GmbH (http://www.ar-tracking.de) DTrack/DTrack2 client
18#
19# creates as many vrpn_Tracker as there are bodies or Flysticks, starting with the bodies
20# creates 2 analogs per Flystick
21# creates 8 buttons per Flystick
22#
23# NOTE: when using DTrack's older output format for Flystick data ('6df'), the numbering
24# of Flystick buttons differs from DTrack documentation (for compatibility with
25# older vrpn releases)
26#
27# Arguments:
28# char name_of_this_device[]
29# int udp_port (DTrack sends data to this UDP port)
30#
31# Optional arguments:
32# float time_to_reach_joy (in seconds; see below)
33# int number_of_bodies, number_of_flysticks (fixed numbers of bodies and Flysticks)
34# int renumbered_ids[] (vrpn_Tracker IDs of bodies and Flysticks)
35# char "3d" (activates 3dof marker output if available;
36# always last argument if "-" is not present)
37# char "-" (activates tracing; always last argument)
38#
39# NOTE: time_to_reach_joy is the time needed to reach the maximum value (1.0 or -1.0) of the
40# joystick of older 'Flystick' devices when the corresponding button is pressed
41# (one of the last buttons amongst the 8); not necessary for newer 'Flystick2' devices
42# with its analog joystick
43#
44# NOTE: if fixed numbers of bodies and Flysticks should be used, both arguments
45# number_of_bodies and number_of_flysticks have to be set
46#
47# NOTE: renumbering of tracker IDs is only possible, if fixed numbers of bodies and
48# Flysticks are set; there has to be an argument present for each body/Flystick
49
50#vrpn_Tracker_DTrack DTrack 5000
51#vrpn_Tracker_DTrack DTrack 5000 -
52#vrpn_Tracker_DTrack DTrack 5000 3d
53#vrpn_Tracker_DTrack DTrack 5000 3d -
54#vrpn_Tracker_DTrack DTrack 5000 0.5
55#vrpn_Tracker_DTrack DTrack 5000 0.5 2 2
56#vrpn_Tracker_DTrack DTrack 5000 0.5 2 2 2 1 0 3
57#vrpn_Tracker_DTrack DTrack 5000 0.5 2 2 2 1 0 3 3d -
58
59################################################################################
60*/
61
62// usually the following should work:
63
64#ifndef _WIN32
65 #define OS_UNIX // for Unix (Linux, Irix, ...)
66#else
67 #define OS_WIN // for MS Windows (2000, XP, ...)
68#endif
69
70#include <stdio.h> // for NULL, printf, fprintf, etc
71#include <stdlib.h> // for strtod, exit, free, malloc, etc
72#include <string.h> // for strncmp, memset, strcat, etc
73
74#include "quat.h" // for Q_RAD_TO_DEG, etc
75#include "vrpn_Connection.h" // for vrpn_CONNECTION_LOW_LATENCY, etc
76#include "vrpn_Shared.h" // for timeval, INVALID_SOCKET, etc
77#include "vrpn_Tracker_DTrack.h"
78#include "vrpn_Types.h" // for vrpn_float64
79#include "vrpn_MessageMacros.h" // for VRPN_MSG_INFO, VRPN_MSG_WARNING, VRPN_MSG_ERROR
80
81#ifdef OS_WIN
82 #ifdef VRPN_USE_WINSOCK2
83 #include <winsock2.h> // struct timeval is defined here
84 #else
85 #include <winsock.h> // struct timeval is defined here
86 #endif
87#endif
88#ifdef OS_UNIX
89 #include <netinet/in.h> // for sockaddr_in, INADDR_ANY, etc
90 #include <sys/socket.h> // for bind, recv, socket, AF_INET, etc
91 #include <unistd.h> // for close
92 #include <sys/select.h> // for select, FD_SET, FD_SETSIZE, etc
93#endif
94
95
96// There is a problem with linking on SGIs related to standard libraries.
97#ifndef sgi
98
99// --------------------------------------------------------------------------
100// Globals:
101
102#define DTRACK2VRPN_BUTTONS_PER_FLYSTICK 8 // number of vrpn buttons per Flystick (fixed)
103#define DTRACK2VRPN_ANALOGS_PER_FLYSTICK 2 // number of vrpn analogs per Flystick (fixed)
104
105#define UDPRECEIVE_BUFSIZE 20000 // size of udp buffer for DTrack data (one frame; in bytes)
106
107// --------------------------------------------------------------------------
108
109// Local error codes:
110
111#define DTRACK_ERR_NONE 0 // no error
112#define DTRACK_ERR_TIMEOUT 1 // timeout while receiving data
113#define DTRACK_ERR_UDP 2 // UDP receive error
114#define DTRACK_ERR_PARSE 3 // error in UDP packet
115
116// Local prototypes:
117
118static char* string_nextline(char* str, char* start, int len);
119static char* string_get_i(char* str, int* i);
120static char* string_get_ui(char* str, unsigned int* ui);
121static char* string_get_d(char* str, double* d);
122static char* string_get_f(char* str, float* f);
123static char* string_get_block(char* str, const char* fmt, int* idat, float* fdat);
124
125static vrpn_Tracker_DTrack::socket_type udp_init(unsigned short port);
126static int udp_exit(vrpn_Tracker_DTrack::socket_type sock);
127static int udp_receive(vrpn_Tracker_DTrack::socket_type sock, void *buffer, int maxlen, int tout_us);
128
129
130// --------------------------------------------------------------------------
131// Constructor:
132// name (i): device name
133// c (i): vrpn_Connection
134// dtrackPort (i): DTrack UDP port
135// timeToReachJoy (i): time needed to reach the maximum value of the joystick
136// fixNbody, fixNflystick (i): fixed numbers of DTrack bodies and Flysticks (-1 if not wanted)
137// fixId (i): renumbering of targets; must have exactly (fixNbody + fixNflystick) elements (NULL if not wanted)
138// actTracing (i): activate trace output
139
141 int dtrackPort, float timeToReachJoy,
142 int fixNbody, int fixNflystick, int* fixId,
143 bool act3DOFout, bool actTracing) :
144 vrpn_Tracker(name, c),
145 vrpn_Button_Filter(name, c),
146 vrpn_Analog(name, c)
147{
148
149 int i;
150
151 // Dunno how many of these there are yet...
152
153 num_sensors = 0;
154 num_channel = 0;
155 num_buttons = 0;
156
157 // init variables: general
158 output_3dof_marker = act3DOFout;
159 tracing = actTracing;
160 tracing_frames = 0;
161
162 tim_first.tv_sec = tim_first.tv_usec = 0; // also used to recognize the first frame
163 tim_last.tv_sec = tim_last.tv_usec = 0;
164
165 // init variables: DTrack data
166
167 if(fixNbody >= 0 && fixNflystick >= 0){ // fixed numbers of bodies and Flysticks should be used
168 use_fix_numbering = true;
169
170 fix_nbody = fixNbody;
171 fix_nflystick = fixNflystick;
172
173 fix_idbody.resize(fix_nbody + fix_nflystick);
174 fix_idflystick.resize(fix_nflystick);
175
176 if(fixId){ // take the renumbering information for bodies and Flysticks
177 for(i=0; i<fix_nbody + fix_nflystick; i++){ // take the renumbering information for bodies
178 fix_idbody[i] = fixId[i];
179 }
180
181 for(i=0; i<fix_nflystick; i++){ // take the renumbering information for Flysticks (vrpn button data)
182 fix_idflystick[i] = fixId[i + fix_nbody];
183 }
184
185 for(i=0; i<fix_nbody; i++){ // remove all bodies from the Flystick renumbering data
186 for(int j=0; j<fix_nflystick; j++){
187 if(fixId[i] < fixId[j + fix_nbody] && fix_idflystick[j] > 0){ // be sure to avoid crazy numbers...
188 fix_idflystick[j]--;
189 }
190 }
191 }
192 }else{ // take identity numbering for bodies and Flysticks
193 for(i=0; i<fix_nbody + fix_nflystick; i++){
194 fix_idbody[i] = i;
195 }
196 for(i=0; i<fix_nflystick; i++){
197 fix_idflystick[i] = i;
198 }
199 }
200 }else{ // no fixed numbers
201 use_fix_numbering = false;
202 }
203
204 warning_nbodycal = false;
205
206 // init variables: preparing data for VRPN
207
208 if(timeToReachJoy > 1e-20){
209 joy_incPerSec = 1.f / timeToReachJoy; // increase of 'joystick' channel
210 }else{
211 joy_incPerSec = 1e20f; // so it reaches immediately
212 }
213
214 // init: communicating with DTrack
215
216 if(!dtrack_init(dtrackPort)){
217 exit(EXIT_FAILURE);
218 }
219}
220
221// Destructor:
222
224{
225
226 dtrack_exit();
227}
228
229
230// --------------------------------------------------------------------------
231// Main loop:
232
233// This function should be called each time through the main loop
234// of the server code. It checks for a report from the tracker and
235// sends it if there is one.
236
238{
239 struct timeval timestamp;
240 long tts, ttu;
241 float dt;
242 int nbody, nflystick, i;
243 int newid;
244
245 // call the generic server mainloop, since we are a server:
246
248
249 // get data from DTrack:
250
251 if(!dtrack_receive()){
252 if(d_lasterror != DTRACK_ERR_TIMEOUT){
253 fprintf(stderr, "vrpn_Tracker_DTrack: Receive Error from DTrack.\n");
254 }
255 return;
256 }
257
258 tracing_frames++;
259
260 // get time stamp:
261
263
264 if(act_timestamp >= 0){ // use DTrack time stamp if available
265 tts = (long )act_timestamp;
266 ttu = (long )((act_timestamp - tts) * 1000000);
267 tts += timestamp.tv_sec - timestamp.tv_sec % 86400; // add day part of vrpn time stamp
268
269 if(tts >= timestamp.tv_sec + 43200 - 1800){ // shift closer to vrpn time stamp
270 tts -= 86400;
271 }else if(tts <= timestamp.tv_sec - 43200 - 1800){
272 tts += 86400;
273 }
274
275 timestamp.tv_sec = tts;
276 timestamp.tv_usec = ttu;
277 }
278
279 if(tim_first.tv_sec == 0 && tim_first.tv_usec == 0){
280 tim_first = tim_last = timestamp;
281 }
282
283 dt = (float )vrpn_TimevalDurationSeconds(timestamp, tim_last);
284 tim_last = timestamp;
285
286 if(tracing && ((tracing_frames % 10) == 0)){
287 printf("framenr %u time %.3lf\n", act_framecounter, vrpn_TimevalDurationSeconds(timestamp, tim_first));
288 }
289
290 // find number of targets visible for vrpn to choose the correct vrpn ID numbers:
291 // (1) takes fixed number of bodies and Flysticks, if defined in the configuration file
292 // (2) otherwise uses the '6dcal' line in DTrack's output, that gives the total number of
293 // calibrated targets, if available
294 // (3) otherwise tracks the maximum number of appeared targets
295
296 if(use_fix_numbering){ // fixed numbers should be used
297 nbody = fix_nbody; // number of bodies visible for vrpn
298 nflystick = fix_nflystick; // number of Flysticks visible for vrpn
299 }else if(act_has_bodycal_format){ // DTrack/DTrack2 sent information about the number of calibrated targets
300 nbody = act_num_bodycal; // number of bodies visible for vrpn
301 nflystick = act_num_flystick; // number of Flysticks visible for vrpn
302 }else{ // else track the maximum number of appeared targets (at least)
303 if(!warning_nbodycal){ // mention warning (once)
304 fprintf(stderr, "vrpn_Tracker_DTrack warning: no DTrack '6dcal' data available.\n");
305 warning_nbodycal = true;
306 }
307
308 nbody = act_num_body; // number of bodies visible for vrpn
309 nflystick = act_num_flystick; // number of Flysticks visible for vrpn
310 }
311
312 // report tracker data to vrpn:
313
314 num_sensors = nbody + nflystick; // total number of targets visible for vrpn
315 num_buttons = nflystick * DTRACK2VRPN_BUTTONS_PER_FLYSTICK; // 8 buttons per Flystick
316 num_channel = nflystick * DTRACK2VRPN_ANALOGS_PER_FLYSTICK; // 2 channels per joystick/Flystick
317
318 for(i=0; i<act_num_body; i++){ // DTrack standard bodies
319 if(act_body[i].id < nbody){ // there might be more DTrack standard bodies than wanted
320 if(act_body[i].quality >= 0){ // report position only if body is tracked
321 if(use_fix_numbering){
322 newid = fix_idbody[act_body[i].id]; // renumbered ID
323 }else{
324 newid = act_body[i].id;
325 }
326
327 dtrack2vrpn_body(newid, "", act_body[i].id, act_body[i].loc, act_body[i].rot, timestamp);
328 }
329 }
330 }
331
332 if(num_channel >= static_cast<int>(joy_last.size())){ // adjust length of vector for current joystick value
333 size_t j0 = joy_last.size();
334
335 joy_simulate.resize(num_channel);
336 joy_last.resize(num_channel);
337
338 for(size_t j=j0; j< static_cast<size_t>(num_channel); j++){
339 joy_simulate[j] = false;
340 joy_last[j] = 0;
341 }
342 }
343
344 for(i=0; i<(int)act_num_flystick; i++){ // DTrack Flysticks
345 if(act_flystick[i].id < nflystick){ // there might be more DTrack Flysticks than wanted
346 if(act_flystick[i].quality >= 0){ // report position only if Flystick is tracked
347 if(use_fix_numbering){
348 newid = fix_idbody[act_flystick[i].id + nbody]; // renumbered ID for position
349 }else{
350 newid = act_flystick[i].id + nbody;
351 }
352
353 dtrack2vrpn_body(newid, "f", act_flystick[i].id, act_flystick[i].loc, act_flystick[i].rot,
354 timestamp);
355 }
356
357 if(use_fix_numbering){
358 newid = fix_idflystick[act_flystick[i].id]; // renumbered ID for buttons and analogs
359 }else{
360 newid = act_flystick[i].id;
361 }
362
363 dtrack2vrpn_flystickbuttons(newid, act_flystick[i].id,
364 act_flystick[i].num_button, act_flystick[i].button, timestamp);
365
366 dtrack2vrpn_flystickanalogs(newid, act_flystick[i].id,
367 act_flystick[i].num_joystick, act_flystick[i].joystick, dt, timestamp);
368 }
369 }
370
371 if (output_3dof_marker) {
372 int offset = num_sensors;
373 num_sensors += act_num_marker;
374 for(i=0; i<act_num_marker; i++){ // DTrack 3dof marker
375 dtrack2vrpn_marker(offset + i, "m", act_marker[i].id, act_marker[i].loc, timestamp);
376 }
377 }
378
379 // finish main loop:
380
381 vrpn_Analog::report_changes(); // report any analog event;
382 vrpn_Button::report_changes(); // report any button event;
383}
384
385
386// -----------------------------------------------------------------------------------------
387// Helpers:
388
389
390// ---------------------------------------------------------------------------------------------------
391// ---------------------------------------------------------------------------------------------------
392// Preparing data for VRPN:
393// these functions convert DTrack data to vrpn data
394
395// Preparing marker data:
396// id (i): VRPN body ID
397// str_dtrack (i): DTrack marker name (just used for trace output)
398// id_dtrack (i): DTrack marker ID (just used for trace output)
399// loc (i): position
400// timestamp (i): timestamp for body data
401// return value (o): 0 ok, -1 error
402
403int vrpn_Tracker_DTrack::dtrack2vrpn_marker(int id, const char* str_dtrack, int id_dtrack,
404 const float* loc, struct timeval timestamp)
405{
406
407 d_sensor = id;
408
409 // position (plus converting to unit meter):
410
411 pos[0] = loc[0] / 1000.;
412 pos[1] = loc[1] / 1000.;
413 pos[2] = loc[2] / 1000.;
414
415 // orientation: none
416
417 q_make(d_quat, 1, 0, 0, 0);
418
419 // pack and deliver tracker report:
420
421 if(d_connection){
422 char msgbuf[1000];
423 int len = vrpn_Tracker::encode_to(msgbuf);
424
427 {
428 fprintf(stderr, "vrpn_Tracker_DTrack: cannot write message: tossing.\n");
429 }
430 }
431
432 // tracing:
433
434 if(tracing && ((tracing_frames % 10) == 0)){
435
436 printf("marker id (DTrack vrpn): %s%d %d pos (x y z): %.4f %.4f %.4f\n",
437 str_dtrack, id_dtrack, id, pos[0], pos[1], pos[2]);
438 }
439
440 return 0;
441}
442
443
444// Preparing body data:
445// id (i): VRPN body ID
446// str_dtrack (i): DTrack body name ('body' or 'Flystick'; just used for trace output)
447// id_dtrack (i): DTrack body ID (just used for trace output)
448// loc (i): position
449// rot (i): orientation (3x3 rotation matrix)
450// timestamp (i): timestamp for body data
451// return value (o): 0 ok, -1 error
452
453int vrpn_Tracker_DTrack::dtrack2vrpn_body(int id, const char* str_dtrack, int id_dtrack,
454 const float* loc, const float* rot, struct timeval timestamp)
455{
456
457 d_sensor = id;
458
459 // position (plus converting to unit meter):
460
461 pos[0] = loc[0] / 1000.;
462 pos[1] = loc[1] / 1000.;
463 pos[2] = loc[2] / 1000.;
464
465 // orientation:
466
467 q_matrix_type destMatrix; // if this is not good, just build the matrix and do a matrixToQuat
468
469 destMatrix[0][0] = rot[0];
470 destMatrix[0][1] = rot[1];
471 destMatrix[0][2] = rot[2];
472 destMatrix[0][3] = 0.0;
473
474 destMatrix[1][0] = rot[3];
475 destMatrix[1][1] = rot[4];
476 destMatrix[1][2] = rot[5];
477 destMatrix[1][3] = 0.0;
478
479 destMatrix[2][0] = rot[6];
480 destMatrix[2][1] = rot[7];
481 destMatrix[2][2] = rot[8];
482 destMatrix[2][3] = 0.0;
483
484 destMatrix[3][0] = 0.0;
485 destMatrix[3][1] = 0.0;
486 destMatrix[3][2] = 0.0;
487 destMatrix[3][3] = 1.0;
488
489 q_from_row_matrix(d_quat, destMatrix);
490
491 // pack and deliver tracker report:
492
493 if(d_connection){
494 char msgbuf[1000];
495 int len = vrpn_Tracker::encode_to(msgbuf);
496
499 {
500 fprintf(stderr, "vrpn_Tracker_DTrack: cannot write message: tossing.\n");
501 }
502 }
503
504 // tracing:
505
506 if(tracing && ((tracing_frames % 10) == 0)){
507 q_vec_type yawPitchRoll;
508
509 q_to_euler(yawPitchRoll, d_quat);
510
511 printf("body id (DTrack vrpn): %s%d %d pos (x y z): %.4f %.4f %.4f euler (y p r): %.3f %.3f %.3f\n",
512 str_dtrack, id_dtrack, id, pos[0], pos[1], pos[2],
513 Q_RAD_TO_DEG(yawPitchRoll[0]), Q_RAD_TO_DEG(yawPitchRoll[1]), Q_RAD_TO_DEG(yawPitchRoll[2]));
514 }
515
516 return 0;
517}
518
519
520// Preparing Flystick button data:
521// id (i): VRPN Flystick ID
522// id_dtrack (i): DTrack Flystick ID (just used for trace output)
523// num_but (i): number of buttons
524// but (i): button state (1 pressed, 0 not pressed)
525// timestamp (i): timestamp for button data
526// return value (o): 0 ok, -1 error
527
528int vrpn_Tracker_DTrack::dtrack2vrpn_flystickbuttons(int id, int id_dtrack,
529 int num_but, const int* but, struct timeval timestamp)
530{
531 int n, i, ind;
532
534
535 // buttons:
536 // NOTE: numbering of two buttons (B2 and B4) differs from DTrack documentation, as long as the 'older'
537 // output format '6df' is used by DTrack!
538
540 i = 0;
541 while(i < n){
542 buttons[ind++] = but[i];
543 i++;
544 }
545 while(i < DTRACK2VRPN_BUTTONS_PER_FLYSTICK){ // fill remaining buttons
546 buttons[ind++] = 0;
547 i++;
548 }
549
550 if(act_has_old_flystick_format){ // for backward compatibility!
551 // NOTE: numbering of two buttons (button 2 and button 4) differs from DTrack documentation!
552 buttons[id * DTRACK2VRPN_BUTTONS_PER_FLYSTICK + 1] = but[3];
553 buttons[id * DTRACK2VRPN_BUTTONS_PER_FLYSTICK + 3] = but[1];
554 }
555
556 vrpn_Button::timestamp = timestamp; // timestamp for button event (explicitly necessary)
557
558 // tracing:
559
560 if(tracing && ((tracing_frames % 10) == 0)){
561 printf("flystick id (DTrack vrpn): f%d %d but ", id_dtrack, id);
562 for(i=0; i<n; i++){
563 printf(" %d", but[i]);
564 }
565 printf("\n");
566 }
567
568 return 0;
569}
570
571
572// Preparing Flystick analog data:
573// id (i): VRPN Flystick ID
574// id_dtrack (i): DTrack Flystick ID (just used for trace output)
575// num_ana (i): number of analogs
576// ana (i): analog state (-1 <= ana <= 1)
577// dt (i): time since last change
578// timestamp (i): timestamp for analog data
579// return value (o): 0 ok, -1 error
580
581int vrpn_Tracker_DTrack::dtrack2vrpn_flystickanalogs(int id, int id_dtrack,
582 int num_ana, const float* ana, float dt, struct timeval timestamp)
583{
584 int n, i, ind;
585 float f;
586
588
589 // analogs:
590 // NOTE: simulation of time varying floating values, if a joystick action of an older
591 // 'Flystick' device is recognized!
592
594 i = 0;
595 while(i < n){
596 f = ana[i];
597
598 // simulation of time varying floating values (actually just necessary for older
599 // 'Flystick' devices and for backward compatibility):
600
601 if(f == 0){ // zero position: reset everything
602 joy_simulate[ind] = false;
603 }else if((f > 0.99 || f < -0.99) && joy_last[ind] == 0){ // extreme change: start simulation
604 joy_simulate[ind] = true;
605 }
606
607 if(joy_simulate[ind]){ // simulation of time varying floating values
608 if(f > 0){
609 f = joy_last[ind] + joy_incPerSec * dt;
610
611 if(f >= 1){
612 f = 1;
613 joy_simulate[ind] = false;
614 }
615 }else{
616 f = joy_last[ind] - joy_incPerSec * dt;
617
618 if(f <= -1){
619 f = -1;
620 joy_simulate[ind] = false;
621 }
622 }
623 }
624
625 joy_last[ind] = f;
626 channel[ind++] = f;
627 i++;
628 }
629 while(i < DTRACK2VRPN_ANALOGS_PER_FLYSTICK){ // fill remaining analogs
630 channel[ind++] = 0;
631 i++;
632 }
633
634 vrpn_Analog::timestamp = timestamp; // timestamp for analog event (explicitly necessary)
635
636 // tracing:
637
638 if(tracing && ((tracing_frames % 10) == 0)){
639 printf("flystick id (DTrack vrpn): f%d %d ana ", id_dtrack, id);
640 for(i=0; i<n; i++){
641 printf(" %.2f", ana[i]);
642 }
643 printf("\n");
644 }
645
646 return 0;
647}
648
649
650// ---------------------------------------------------------------------------------------------------
651// ---------------------------------------------------------------------------------------------------
652// Communication with DTrack:
653// these functions receive and parse data packets from DTrack
654
655// Initializing communication with DTrack:
656//
657// udpport (i): UDP port number to receive data from DTrack
658//
659// return value (o): initialization was successful (boolean)
660
661bool vrpn_Tracker_DTrack::dtrack_init(int udpport)
662{
663
664 d_udpbuf = NULL;
665 d_lasterror = DTRACK_ERR_NONE;
666
667 // create UDP socket:
668
669 if(udpport <= 0 || udpport > 65535){
670 fprintf(stderr, "vrpn_Tracker_DTrack: Illegal UDP port %d.\n", udpport);
671 return false;
672 }
673
674 d_udpsock = udp_init((unsigned short )udpport);
675
676 if(d_udpsock == INVALID_SOCKET){
677 fprintf(stderr, "vrpn_Tracker_DTrack: Cannot Initialize UDP Socket.\n");
678 return false;
679 }
680
681 d_udptimeout_us = 0;
682
683 // create UDP buffer:
684
685 d_udpbufsize = UDPRECEIVE_BUFSIZE;
686
687 d_udpbuf = (char *)malloc(d_udpbufsize);
688
689 if(d_udpbuf == NULL){
690 udp_exit(d_udpsock);
691 d_udpsock = INVALID_SOCKET;
692 fprintf(stderr, "vrpn_Tracker_DTrack: Cannot Allocate Memory for UDP Buffer.\n");
693 return false;
694 }
695
696 // reset actual DTrack data:
697
698 act_framecounter = 0;
699 act_timestamp = -1;
700
701 act_num_marker = act_num_body = act_num_flystick = 0;
702 act_has_bodycal_format = false;
703 act_has_old_flystick_format = false;
704
705 return true;
706}
707
708
709// Deinitializing communication with DTrack:
710//
711// return value (o): deinitialization was successful (boolean)
712
713bool vrpn_Tracker_DTrack::dtrack_exit(void)
714{
715
716 // release buffer:
717
718 if(d_udpbuf != NULL){
719 free(d_udpbuf);
720 }
721
722 // release UDP socket:
723
724 if(d_udpsock != INVALID_SOCKET){
725 udp_exit(d_udpsock);
726 }
727
728 return true;
729}
730
731
732// ---------------------------------------------------------------------------------------------------
733// Receive and process one DTrack data packet (UDP; ASCII protocol):
734//
735// return value (o): receiving was successful (boolean)
736
737bool vrpn_Tracker_DTrack::dtrack_receive(void)
738{
739 char* s;
740 int i, j, k, l, n, len, id;
741 char sfmt[20];
742 int iarr[3];
743 float f;
744 int loc_num_bodycal, loc_num_flystick1, loc_num_meatool;
745
746 if(d_udpsock == INVALID_SOCKET){
747 d_lasterror = DTRACK_ERR_UDP;
748 return false;
749 }
750
751 // defaults:
752
753 act_framecounter = 0;
754 act_timestamp = -1;
755
756 loc_num_bodycal = -1; // i.e. not available
757 loc_num_flystick1 = loc_num_meatool = 0;
758
759 act_has_bodycal_format = false;
760
761 // receive UDP packet:
762
763 len = udp_receive(d_udpsock, d_udpbuf, d_udpbufsize-1, d_udptimeout_us);
764
765 if(len == -1){
766 d_lasterror = DTRACK_ERR_TIMEOUT;
767 return false;
768 }
769 if(len <= 0){
770 d_lasterror = DTRACK_ERR_UDP;
771 return false;
772 }
773
774 s = d_udpbuf;
775 s[len] = '\0';
776
777 // process lines:
778
779 d_lasterror = DTRACK_ERR_PARSE;
780
781 do{
782 // line for frame counter:
783
784 if(!strncmp(s, "fr ", 3)){
785 s += 3;
786
787 if(!(s = string_get_ui(s, &act_framecounter))){ // get frame counter
788 act_framecounter = 0;
789 return false;
790 }
791
792 continue;
793 }
794
795 // line for timestamp:
796
797 if(!strncmp(s, "ts ", 3)){
798 s += 3;
799
800 if(!(s = string_get_d(s, &act_timestamp))){ // get time stamp
801 act_timestamp = -1;
802 return false;
803 }
804
805 continue;
806 }
807
808 // line for additional information about number of calibrated bodies:
809
810 if(!strncmp(s, "6dcal ", 6)){
811 s += 6;
812
813 act_has_bodycal_format = true;
814
815 if(!(s = string_get_i(s, &loc_num_bodycal))){ // get number of calibrated bodies
816 return false;
817 }
818
819 continue;
820 }
821
822 // line for 3dof marker data:
823
824 if(!strncmp(s, "3d ", 3)){
825 s += 3;
826 act_num_marker = 0;
827
828 if(!(s = string_get_i(s, &n))){ // get number of standard bodies (in line)
829 return false;
830 }
831
832 if (static_cast<unsigned>(n) > act_marker.size()) {
833 act_marker.resize(n);
834 }
835
836 for(i=0; i<n; i++){ // get data of standard bodies
837 if(!(s = string_get_block(s, "if", &id, &f))){
838 return false;
839 }
840
841 act_marker[act_num_marker].id = id;
842
843 if(!(s = string_get_block(s, "fff", NULL, act_marker[act_num_marker].loc))){
844 return false;
845 }
846
847 act_num_marker++;
848 }
849
850 continue;
851 }
852
853 // line for standard body data:
854
855 if(!strncmp(s, "6d ", 3)){
856 s += 3;
857
858 for(i=0; i<act_num_body; i++){ // disable all existing data
859 memset(&act_body[i], 0, sizeof(vrpn_dtrack_body_type));
860 act_body[i].id = i;
861 act_body[i].quality = -1;
862 }
863
864 if(!(s = string_get_i(s, &n))){ // get number of standard bodies (in line)
865 return false;
866 }
867
868 for(i=0; i<n; i++){ // get data of standard bodies
869 if(!(s = string_get_block(s, "if", &id, &f))){
870 return false;
871 }
872
873 if(id >= act_num_body){ // adjust length of vector
874 act_body.resize(id + 1);
875
876 for(j=act_num_body; j<=id; j++){
877 memset(&act_body[j], 0, sizeof(vrpn_dtrack_body_type));
878 act_body[j].id = j;
879 act_body[j].quality = -1;
880 }
881
882 act_num_body = id + 1;
883 }
884
885 act_body[id].id = id;
886 act_body[id].quality = f;
887
888 if(!(s = string_get_block(s, "fff", NULL, act_body[id].loc))){
889 return false;
890 }
891
892 if(!(s = string_get_block(s, "fffffffff", NULL, act_body[id].rot))){
893 return false;
894 }
895 }
896
897 continue;
898 }
899
900 // line for Flystick data (older format):
901
902 if(!strncmp(s, "6df ", 4)){
903 s += 4;
904
905 act_has_old_flystick_format = true;
906
907 if(!(s = string_get_i(s, &n))){ // get number of calibrated Flysticks
908 return false;
909 }
910
911 loc_num_flystick1 = n;
912
913 if(n > act_num_flystick){ // adjust length of vector
914 act_flystick.resize(n);
915
916 act_num_flystick = n;
917 }
918
919 for(i=0; i<n; i++){ // get data of Flysticks
920 if(!(s = string_get_block(s, "ifi", iarr, &f))){
921 return false;
922 }
923
924 if(iarr[0] != i){ // not expected
925 return false;
926 }
927
928 act_flystick[i].id = iarr[0];
929 act_flystick[i].quality = f;
930
931 act_flystick[i].num_button = 8;
932 k = iarr[1];
933 for(j=0; j<8; j++){
934 act_flystick[i].button[j] = k & 0x01;
935 k >>= 1;
936 }
937
938 act_flystick[i].num_joystick = 2; // additionally to buttons 5-8
939 if(iarr[1] & 0x20){
940 act_flystick[i].joystick[0] = -1;
941 }else if(iarr[1] & 0x80){
942 act_flystick[i].joystick[0] = 1;
943 }else{
944 act_flystick[i].joystick[0] = 0;
945 }
946 if(iarr[1] & 0x10){
947 act_flystick[i].joystick[1] = -1;
948 }else if(iarr[1] & 0x40){
949 act_flystick[i].joystick[1] = 1;
950 }else{
951 act_flystick[i].joystick[1] = 0;
952 }
953
954 if(!(s = string_get_block(s, "fff", NULL, act_flystick[i].loc))){
955 return false;
956 }
957
958 if(!(s = string_get_block(s, "fffffffff", NULL, act_flystick[i].rot))){
959 return false;
960 }
961 }
962
963 continue;
964 }
965
966 // line for Flystick data (newer format):
967
968 if(!strncmp(s, "6df2 ", 5)){
969 s += 5;
970
971 act_has_old_flystick_format = false;
972
973 if(!(s = string_get_i(s, &n))){ // get number of calibrated Flysticks
974 return false;
975 }
976
977 if(n > act_num_flystick){ // adjust length of vector
978 act_flystick.resize(n);
979
980 act_num_flystick = n;
981 }
982
983 if(!(s = string_get_i(s, &n))){ // get number of Flysticks
984 return false;
985 }
986
987 for(i=0; i<n; i++){ // get data of Flysticks
988 if(!(s = string_get_block(s, "ifii", iarr, &f))){
989 return false;
990 }
991
992 if(iarr[0] != i){ // not expected
993 return false;
994 }
995
996 act_flystick[i].id = iarr[0];
997 act_flystick[i].quality = f;
998
1000 return false;
1001 }
1003 return false;
1004 }
1005 act_flystick[i].num_button = iarr[1];
1006 act_flystick[i].num_joystick = iarr[2];
1007
1008 if(!(s = string_get_block(s, "fff", NULL, act_flystick[i].loc))){
1009 return false;
1010 }
1011
1012 if(!(s = string_get_block(s, "fffffffff", NULL, act_flystick[i].rot))){
1013 return false;
1014 }
1015
1016 strcpy(sfmt, "");
1017 j = 0;
1018 while(j < act_flystick[i].num_button){
1019 strcat(sfmt, "i");
1020 j += 32;
1021 }
1022 j = 0;
1023 while(j < act_flystick[i].num_joystick){
1024 strcat(sfmt, "f");
1025 j++;
1026 }
1027
1028 if(!(s = string_get_block(s, sfmt, iarr, act_flystick[i].joystick))){
1029 return false;
1030 }
1031
1032 k = l = 0;
1033 for(j=0; j<act_flystick[i].num_button; j++){
1034 act_flystick[i].button[j] = iarr[k] & 0x01;
1035 iarr[k] >>= 1;
1036
1037 l++;
1038 if(l == 32){
1039 k++;
1040 l = 0;
1041 }
1042 }
1043 }
1044
1045 continue;
1046 }
1047
1048 // line for measurement tool data:
1049
1050 if(!strncmp(s, "6dmt ", 5)){
1051 s += 5;
1052
1053 if(!(s = string_get_i(s, &n))){ // get number of calibrated measurement tools
1054 return false;
1055 }
1056
1057 loc_num_meatool = n;
1058
1059 continue;
1060 }
1061
1062 // ignore unknown line identifiers (could be valid in future DTracks)
1063
1064 }while((s = string_nextline(d_udpbuf, s, d_udpbufsize)) != NULL);
1065
1066 // set number of calibrated standard bodies, if necessary:
1067
1068 if(loc_num_bodycal >= 0){ // '6dcal' information was available
1069 act_num_bodycal = loc_num_bodycal - loc_num_flystick1 - loc_num_meatool;
1070 }
1071
1072 d_lasterror = DTRACK_ERR_NONE;
1073 return true;
1074}
1075
1076
1077// ---------------------------------------------------------------------------------------------------
1078// ---------------------------------------------------------------------------------------------------
1079// Parsing DTrack data:
1080
1081// Search next line in buffer:
1082// str (i): buffer (total)
1083// start (i): start position within buffer
1084// len (i): buffer length in bytes
1085// return (i): begin of line, NULL if no new line in buffer
1086
1087static char* string_nextline(char* str, char* start, int len)
1088{
1089 char* s = start;
1090 char* se = str + len;
1091 int crlffound = 0;
1092
1093 while(s < se){
1094 if(*s == '\r' || *s == '\n'){ // crlf
1095 crlffound = 1;
1096 }else{
1097 if(crlffound){ // begin of new line found
1098 return (*s) ? s : NULL; // first character is '\0': end of buffer
1099 }
1100 }
1101
1102 s++;
1103 }
1104
1105 return NULL; // no new line found in buffer
1106}
1107
1108
1109// Read next 'int' value from string:
1110// str (i): string
1111// i (o): read value
1112// return value (o): pointer behind read value in str; NULL in case of error
1113
1114static char* string_get_i(char* str, int* i)
1115{
1116 char* s;
1117
1118 *i = (int )strtol(str, &s, 0);
1119 return (s == str) ? NULL : s;
1120}
1121
1122
1123// Read next 'unsigned int' value from string:
1124// str (i): string
1125// ui (o): read value
1126// return value (o): pointer behind read value in str; NULL in case of error
1127
1128static char* string_get_ui(char* str, unsigned int* ui)
1129{
1130 char* s;
1131
1132 *ui = (unsigned int )strtoul(str, &s, 0);
1133 return (s == str) ? NULL : s;
1134}
1135
1136
1137// Read next 'double' value from string:
1138// str (i): string
1139// d (o): read value
1140// return value (o): pointer behind read value in str; NULL in case of error
1141
1142static char* string_get_d(char* str, double* d)
1143{
1144 char* s;
1145
1146 *d = strtod(str, &s);
1147 return (s == str) ? NULL : s;
1148}
1149
1150
1151// Read next 'float' value from string:
1152// str (i): string
1153// f (o): read value
1154// return value (o): pointer behind read value in str; NULL in case of error
1155
1156static char* string_get_f(char* str, float* f)
1157{
1158 char* s;
1159
1160 *f = (float )strtod(str, &s); // strtof() only available in GNU-C
1161 return (s == str) ? NULL : s;
1162}
1163
1164
1165// Process next block '[...]' in string:
1166// str (i): string
1167// fmt (i): format string ('i' for 'int', 'f' for 'float')
1168// idat (o): array for 'int' values (long enough due to fmt)
1169// fdat (o): array for 'float' values (long enough due to fmt)
1170// return value (o): pointer behind read value in str; NULL in case of error
1171
1172static char* string_get_block(char* str, const char* fmt, int* idat, float* fdat)
1173{
1174 char* strend;
1175 int index_i, index_f;
1176
1177 if((str = strchr(str, '[')) == NULL){ // search begin of block
1178 return NULL;
1179 }
1180 if((strend = strchr(str, ']')) == NULL){ // search end of block
1181 return NULL;
1182 }
1183
1184 str++; // (temporarily) remove delimiters
1185 *strend = '\0';
1186
1187 index_i = index_f = 0;
1188
1189 while(*fmt){
1190 switch(*fmt++){
1191 case 'i':
1192 if((str = string_get_i(str, &idat[index_i++])) == NULL){
1193 *strend = ']';
1194 return NULL;
1195 }
1196 break;
1197
1198 case 'f':
1199 if((str = string_get_f(str, &fdat[index_f++])) == NULL){
1200 *strend = ']';
1201 return NULL;
1202 }
1203 break;
1204
1205 default: // unknown format character
1206 *strend = ']';
1207 return NULL;
1208 }
1209 }
1210
1211 // ignore additional data inside the block
1212
1213 *strend = ']';
1214 return strend + 1;
1215}
1216
1217
1218// ---------------------------------------------------------------------------------------------------
1219// ---------------------------------------------------------------------------------------------------
1220// Handling UDP data:
1221
1222#ifdef OS_UNIX
1223#define INVALID_SOCKET -1
1224#endif
1225
1226// Initialize UDP socket:
1227// port (i): port number
1228// return value (o): socket number, NULL if error
1229
1230static vrpn_Tracker_DTrack::socket_type udp_init(unsigned short port)
1231{
1233 struct sockaddr_in name;
1234
1235 // initialize socket dll (only Windows):
1236
1237#ifdef OS_WIN
1238 {
1239 WORD vreq;
1240 WSADATA wsa;
1241
1242 vreq = MAKEWORD(2, 0);
1243
1244 if(WSAStartup(vreq, &wsa) != 0){
1245 return NULL;
1246 }
1247 }
1248#endif
1249
1250 // create socket:
1251
1252#ifdef OS_UNIX
1253 {
1254 int usock;
1255
1256 usock = socket(PF_INET, SOCK_DGRAM, 0);
1257
1258 if(usock < 0){
1259 return INVALID_SOCKET;
1260 }
1261 sock = usock;
1262 }
1263#endif
1264#ifdef OS_WIN
1265 {
1266 SOCKET wsock;
1267
1268 wsock = socket(PF_INET, SOCK_DGRAM, 0);
1269
1270 if(wsock == INVALID_SOCKET){
1271 WSACleanup();
1272 return INVALID_SOCKET;
1273 }
1274
1275 sock = wsock;
1276 }
1277#endif
1278
1279 // name socket:
1280
1281 name.sin_family = AF_INET;
1282 name.sin_port = htons(port);
1283 name.sin_addr.s_addr = htonl(INADDR_ANY);
1284
1285 if(bind(sock, (struct sockaddr *) &name, sizeof(name)) < 0){
1286 udp_exit(sock);
1287 return INVALID_SOCKET;
1288 }
1289
1290 return sock;
1291}
1292
1293
1294// Deinitialize UDP socket:
1295// sock (i): socket number
1296// return value (o): 0 ok, -1 error
1297
1298static int udp_exit(vrpn_Tracker_DTrack::socket_type sock)
1299{
1300 int err;
1301
1302#ifdef OS_UNIX
1303 err = close(sock);
1304#endif
1305#ifdef OS_WIN
1306 err = closesocket(sock);
1307 WSACleanup();
1308#endif
1309
1310 if(err < 0){
1311 return -1;
1312 }
1313
1314 return 0;
1315}
1316
1317
1318// Receive UDP data:
1319// - tries to receive packets, as long as data are available
1320// sock (i): socket number
1321// buffer (o): buffer for UDP data
1322// maxlen (i): length of buffer
1323// tout_us (i): timeout in us (micro sec)
1324// return value (o): number of received bytes, <0 if error/timeout occurred
1325
1326// Don't tell us about the FD_SET causing a conditional expression to be constant
1327#ifdef _WIN32
1328#pragma warning ( disable : 4127 )
1329#endif
1330
1331static int udp_receive(vrpn_Tracker_DTrack::socket_type sock, void *buffer, int maxlen, int tout_us)
1332{
1333 int nbytes, err;
1334 fd_set set;
1335 struct timeval tout;
1336
1337 // waiting for data:
1338
1339 FD_ZERO(&set);
1340 FD_SET(sock, &set);
1341
1342 tout.tv_sec = tout_us / 1000000;
1343 tout.tv_usec = tout_us % 1000000;
1344
1345 switch((err = select(FD_SETSIZE, &set, NULL, NULL, &tout))){
1346 case 1:
1347 break; // data available
1348 case 0:
1349 return -1; // timeout
1350 default:
1351 return -2; // error
1352 }
1353
1354 // receiving packet:
1355
1356 while(1){
1357
1358 // receive one packet:
1359
1360 nbytes = recv(sock, (char *)buffer, maxlen, 0);
1361
1362 if(nbytes < 0){ // receive error
1363 return -3;
1364 }
1365
1366 // check, if more data available: if so, receive another packet
1367
1368 FD_ZERO(&set);
1369 FD_SET(sock, &set);
1370
1371 tout.tv_sec = 0; // timeout with value of zero, thus no waiting
1372 tout.tv_usec = 0;
1373
1374 if(select(FD_SETSIZE, &set, NULL, NULL, &tout) != 1){
1375
1376 // no more data available: check length of received packet and return
1377
1378 if(nbytes >= maxlen){ // buffer overflow
1379 return -4;
1380 }
1381
1382 return nbytes;
1383 }
1384 }
1385}
1386
1387#endif
1388
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition vrpn_Analog.h:38
struct timeval timestamp
Definition vrpn_Analog.h:41
vrpn_int32 num_channel
Definition vrpn_Analog.h:40
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
vrpn_Connection * d_connection
Connection that this object talks to.
vrpn_int32 d_sender_id
Sender ID registered with the connection.
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
All button servers should derive from this class, which provides the ability to turn any of the butto...
Definition vrpn_Button.h:66
vrpn_int32 num_buttons
Definition vrpn_Button.h:48
struct timeval timestamp
Definition vrpn_Button.h:49
virtual void report_changes(void)
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition vrpn_Button.h:45
Generic connection class not specific to the transport mechanism.
virtual int pack_message(vrpn_uint32 len, struct timeval time, vrpn_int32 type, vrpn_int32 sender, const char *buffer, vrpn_uint32 class_of_service)
Pack a message that will be sent the next time mainloop() is called. Turn off the RELIABLE flag if yo...
virtual void mainloop()
This function should be called each time through the main loop of the server code....
vrpn_Tracker_DTrack(const char *name, vrpn_Connection *c, int dtrackPort, float timeToReachJoy=0.f, int fixNbody=-1, int fixNflystick=-1, int *fixId=NULL, bool act3DOFout=false, bool actTracing=false)
virtual int encode_to(char *buf)
vrpn_float64 d_quat[4]
vrpn_int32 d_sensor
vrpn_float64 pos[3]
vrpn_int32 num_sensors
vrpn_int32 position_m_id
#define INVALID_SOCKET
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
Header containing macros formerly duplicated in a lot of implementation files.
double vrpn_TimevalDurationSeconds(struct timeval endT, struct timeval startT)
Return the number of seconds between startT and endT as a floating-point value.
#define vrpn_gettimeofday
Definition vrpn_Shared.h:99
#define SOCKET
Definition vrpn_Shared.h:52
#define DTRACK2VRPN_ANALOGS_PER_FLYSTICK
#define DTRACK_ERR_TIMEOUT
#define DTRACK_ERR_UDP
#define INVALID_SOCKET
#define DTRACK2VRPN_BUTTONS_PER_FLYSTICK
#define DTRACK_ERR_PARSE
#define UDPRECEIVE_BUFSIZE
#define DTRACK_ERR_NONE
#define vrpn_DTRACK_FLYSTICK_MAX_BUTTON
#define vrpn_DTRACK_FLYSTICK_MAX_JOYSTICK