vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Tracker.C
Go to the documentation of this file.
1 #include <ctype.h> // for isspace
2 #include <stdio.h> // for fprintf, stderr, NULL, etc
3 #include <string.h> // for memcpy, strlen, strncmp, etc
4 
5 // NOTE: a vrpn tracker must call user callbacks with tracker data (pos and
6 // ori info) which represent the transformation xfSourceFromSensor.
7 // This means that the pos info is the position of the origin of
8 // the sensor coord sys in the source coord sys space, and the
9 // quat represents the orientation of the sensor relative to the
10 // source space (ie, its value rotates the source's axes so that
11 // they coincide with the sensor's)
12 
13 // Include vrpn_Shared.h _first_ to avoid conflicts with sys/time.h
14 // and unistd.h
15 #include "vrpn_Shared.h" // for timeval, vrpn_buffer, etc
16 
17 #ifdef _WIN32
18 #ifndef _WIN32_WCE
19 #include <io.h>
20 #endif
21 #endif
22 
23 #include "vrpn_RedundantTransmission.h" // for vrpn_RedundantTransmission
24 #include "vrpn_Tracker.h"
25 
26 #if defined(VRPN_USE_LIBUSB_1_0)
27 #include <libusb.h> // for libusb_close, etc
28 #endif
29 
30 #ifndef VRPN_CLIENT_ONLY
31 #include "vrpn_Serial.h" // for vrpn_close_commport, etc
32 #endif
33 
34 static const char *default_tracker_cfg_file_name = "vrpn_Tracker.cfg";
35 // this is used unless we pass in a path & file name into the vrpn_TRACKER
36 // constructor
37 
38 #define vrpn_ser_tkr_MAX_TIME_INTERVAL \
39  (2000000) // max time between reports (usec)
40 
41 //#define VERBOSE
42 // #define READ_HISTOGRAM
43 
45  const char *tracker_cfg_file_name)
46  : vrpn_BaseClass(name, c)
47  , unit2sensor(NULL)
48  , unit2sensor_quat(NULL)
49  , num_unit2sensors(0)
50 {
51  FILE *config_file;
53 
54  // Set the current time to zero, just to have something there
55  timestamp.tv_sec = 0;
56  timestamp.tv_usec = 0;
57 
58  // Set the watchdog time to zero, which will disable it unless it is used
59  watchdog_timestamp.tv_sec = 0;
60  watchdog_timestamp.tv_usec = 0;
61 
62  // Set the sensor to 0 just to have something in there.
63  d_sensor = 0;
64 
65  // Set the position to the origin and the orientation to identity
66  // just to have something there in case nobody fills them in later
67  pos[0] = pos[1] = pos[2] = 0.0;
68  d_quat[0] = d_quat[1] = d_quat[2] = 0.0;
69  d_quat[3] = 1.0;
70 
71  // Set the velocity to zero and the orientation to identity
72  // just to have something there in case nobody fills them in later
73  vel[0] = vel[1] = vel[2] = 0.0;
74  vel_quat[0] = vel_quat[1] = vel_quat[2] = 0.0;
75  vel_quat[3] = 1.0;
76  vel_quat_dt = 1;
77 
78  // Set the acceleration to zero and the orientation to identity
79  // just to have something there in case nobody fills them in later
80  acc[0] = acc[1] = acc[2] = 0.0;
81  acc_quat[0] = acc_quat[1] = acc_quat[2] = 0.0;
82  acc_quat[3] = 1.0;
83  acc_quat_dt = 1;
84 
85 #ifdef DESKTOP_PHANTOM_DEFAULTS
86  // Defaults for Desktop Phantom.
87  tracker2room[0] = tracker2room[1] = 0.0;
88  tracker2room[2] = -0.28;
89 #else
90  // Set the room to tracker and sensor to unit transforms to identity
91  tracker2room[0] = tracker2room[1] = tracker2room[2] = 0.0;
92 #endif
94  tracker2room_quat[3] = 1.0;
95 
96  num_sensors = 1;
97 
98 #ifdef DESKTOP_PHANTOM_DEFAULTS
99  // Defaults for Desktop Phantom.
100  workspace_min[0] = workspace_min[1] = -0.2;
101  workspace_min[2] = -0.1;
102  workspace_max[0] = workspace_max[1] = workspace_max[2] = 0.2;
103 #else
104  workspace_min[0] = workspace_min[1] = workspace_min[2] = -0.5;
105  workspace_max[0] = workspace_max[1] = workspace_max[2] = 0.5;
106 #endif
107  // replace defaults with values from tracker config file
108  // if it exists
109  if (tracker_cfg_file_name == NULL) { // the default argument value
110  tracker_cfg_file_name = default_tracker_cfg_file_name;
111  }
112  if ((config_file = fopen(tracker_cfg_file_name, "r")) == NULL) {
113  // Can't find the config file
114  // Complain only if we are using the a non-default config file
115  // (Since most people don't use any config file at all,
116  // and would be confused to see an error about not being
117  // able to open the default config file)
118  if (tracker_cfg_file_name != default_tracker_cfg_file_name) {
119  fprintf(stderr, "vrpn_Tracker: Can't find config file %s\n",
120  tracker_cfg_file_name);
121  }
122  }
123  else if (read_config_file(config_file, name)) {
124  fprintf(
125  stderr,
126  "vrpn_Tracker: Found config file %s, but cannot read info for %s\n",
127  tracker_cfg_file_name, name);
128  fclose(config_file);
129  }
130  else { // no problems
131  fprintf(stderr, "vrpn_Tracker: Read room and sensor info from %s\n",
132  tracker_cfg_file_name);
133  fclose(config_file);
134  }
135 }
136 
138 {
139  // Register this tracker device and the needed message types
140  if (d_connection) {
141  position_m_id =
142  d_connection->register_message_type("vrpn_Tracker Pos_Quat");
143  velocity_m_id =
144  d_connection->register_message_type("vrpn_Tracker Velocity");
145  accel_m_id =
146  d_connection->register_message_type("vrpn_Tracker Acceleration");
148  d_connection->register_message_type("vrpn_Tracker To_Room");
150  d_connection->register_message_type("vrpn_Tracker Unit_To_Sensor");
152  "vrpn_Tracker Request_Tracker_To_Room");
154  "vrpn_Tracker Request_Unit_To_Sensor");
156  d_connection->register_message_type("vrpn_Tracker Workspace");
158  "vrpn_Tracker Request_Tracker_Workspace");
160  d_connection->register_message_type("vrpn_Tracker set_update_rate");
162  d_connection->register_message_type("vrpn_Tracker Reset_Origin");
163  }
164  return 0;
165 }
166 
167 // virtual
168 // Delete all of the unit2sensor position and quaternion entries.
170 {
171  if (unit2sensor != NULL) {
172  delete[] unit2sensor;
173  }
174  if (unit2sensor_quat != NULL) {
175  delete[] unit2sensor_quat;
176  }
177  num_unit2sensors = 0;
178 }
179 
180 // Make sure we have enough unit2sensor elements in the array.
181 // Returns false if we run out of memory, true otherwise.
183 {
184  unsigned i;
185  ++num; // Just to make sure we don't fall prey to off-by-one indexing
186  // errors.
187  if (num > num_unit2sensors) {
188  // Make sure we allocate in large chunks, rather than one at a time.
189  if (num < 2 * num_unit2sensors) {
190  num = 2 * num_unit2sensors;
191  }
192 
193  // Allocate new space for the two lists.
194  vrpn_Tracker_Pos *newlist = new vrpn_Tracker_Pos[num];
195  if (newlist == NULL) {
196  return false;
197  }
198  vrpn_Tracker_Quat *newqlist = new vrpn_Tracker_Quat[num];
199  if (newqlist == NULL) {
200  return false;
201  }
202 
203  // Copy all of the existing elements.
204  for (i = 0; i < num_unit2sensors; i++) {
205  memcpy(newlist[i], unit2sensor[i], sizeof(vrpn_Tracker_Pos));
206  memcpy(newqlist[i], unit2sensor_quat[i], sizeof(vrpn_Tracker_Quat));
207  }
208 
209  // Initialize all of the new elements
210  for (i = num_unit2sensors; i < num; i++) {
211  newlist[i][0] = newlist[i][1] = newlist[i][2] = 0.0;
212  newqlist[i][0] = 0.0;
213  newqlist[i][1] = 0.0;
214  newqlist[i][2] = 0.0;
215  newqlist[i][3] = 1.0;
216  }
217 
218  // Switch the new lists in for the old, and delete the old.
219  if (unit2sensor != NULL) {
220  delete[] unit2sensor;
221  }
222  if (unit2sensor_quat != NULL) {
223  delete[] unit2sensor_quat;
224  }
225  unit2sensor = newlist;
226  unit2sensor_quat = newqlist;
227  num_unit2sensors = num;
228  }
229  return true;
230 }
231 
232 int vrpn_Tracker::read_config_file(FILE *config_file, const char *tracker_name)
233 {
234 
235  char line[512]; // line read from input file
236  vrpn_int32 num_sens;
237  vrpn_int32 which_sensor;
238  float f[14];
239  int i, j;
240 
241  // Read lines from the file until we run out
242  while (fgets(line, sizeof(line), config_file) != NULL) {
243  // Make sure the line wasn't too long
244  if (strlen(line) >= sizeof(line) - 1) {
245  fprintf(stderr, "Line too long in config file: %s\n", line);
246  return -1;
247  }
248  // find tracker name in file
249  if ((!(strncmp(line, tracker_name, strlen(tracker_name)))) &&
250  (isspace(line[strlen(tracker_name)]))) {
251  // get the tracker2room xform and the workspace volume
252  if (fgets(line, sizeof(line), config_file) == NULL) break;
253  if (sscanf(line, "%f%f%f", &f[0], &f[1], &f[2]) != 3) break;
254  if (fgets(line, sizeof(line), config_file) == NULL) break;
255  if (sscanf(line, "%f%f%f%f", &f[3], &f[4], &f[5], &f[6]) != 4)
256  break;
257  if (fgets(line, sizeof(line), config_file) == NULL) break;
258  if (sscanf(line, "%f%f%f%f%f%f", &f[7], &f[8], &f[9], &f[10],
259  &f[11], &f[12]) != 6)
260  break;
261 
262  for (i = 0; i < 3; i++) {
263  tracker2room[i] = f[i];
264  workspace_min[i] = f[i + 7];
265  workspace_max[i] = f[i + 10];
266  }
267  for (i = 0; i < 4; i++)
268  tracker2room_quat[i] = f[i + 3];
269  // get the number of sensors
270  if (fgets(line, sizeof(line), config_file) == NULL) break;
271  if (sscanf(line, "%d", &num_sens) != 1) break;
272  if (!ensure_enough_unit2sensors(num_sens + 1)) {
273  fprintf(stderr, "Out of memory\n");
274  return -1;
275  }
276  for (i = 0; i < num_sens; i++) {
277  // get which sensor this xform is for
278  if (fgets(line, sizeof(line), config_file) == NULL) break;
279  if (sscanf(line, "%d", &which_sensor) != 1) break;
280  if (!ensure_enough_unit2sensors(which_sensor + 1)) {
281  fprintf(stderr, "Out of memory\n");
282  return -1;
283  }
284  // get the sensor to unit xform
285  if (fgets(line, sizeof(line), config_file) == NULL) break;
286  if (sscanf(line, "%f%f%f", &f[0], &f[1], &f[2]) != 3) break;
287  if (fgets(line, sizeof(line), config_file) == NULL) break;
288  if (sscanf(line, "%f%f%f%f", &f[3], &f[4], &f[5], &f[6]) != 4)
289  break;
290  for (j = 0; j < 3; j++) {
291  unit2sensor[which_sensor][j] = f[j];
292  }
293  for (j = 0; j < 4; j++) {
294  unit2sensor_quat[which_sensor][j] = f[j + 3];
295  }
296  }
297  num_sensors = num_sens;
298  return 0; // success
299  }
300  }
301  fprintf(stderr, "Error reading or %s not found in config file\n",
302  tracker_name);
303  return -1;
304 }
305 
307 {
308  printf("----------------------------------------------------\n");
309  printf("Sensor :%d\n", d_sensor);
310  printf("Timestamp :%ld:%ld\n", timestamp.tv_sec,
311  static_cast<long>(timestamp.tv_usec));
312  printf("Framecount:%d\n", frame_count);
313  printf("Pos :%lf, %lf, %lf\n", pos[0], pos[1], pos[2]);
314  printf("Quat :%lf, %lf, %lf, %lf\n", d_quat[0], d_quat[1], d_quat[2],
315  d_quat[3]);
316 }
317 
319 {
320  if (d_connection) {
322  this, d_sender_id)) {
323  fprintf(stderr, "vrpn_Tracker:can't register t2r handler\n");
324  return -1;
325  }
327  this, d_sender_id)) {
328  fprintf(stderr, "vrpn_Tracker:can't register u2s handler\n");
329  return -1;
330  }
333  d_sender_id)) {
334  fprintf(stderr, "vrpn_Tracker: "
335  "Can't register workspace handler\n");
336  return -1;
337  }
338  }
339  else {
340  return -1;
341  }
342  return 0;
343 }
344 
345 // put copies of vector and quat into arrays passed in
346 void vrpn_Tracker::get_local_t2r(vrpn_float64 *vec, vrpn_float64 *quat)
347 {
348  int i;
349  for (i = 0; i < 3; i++)
350  vec[i] = tracker2room[i];
351  for (i = 0; i < 4; i++)
352  quat[i] = tracker2room_quat[i];
353 }
354 
355 // put copies of vector and quat into arrays passed in
356 void vrpn_Tracker::get_local_u2s(vrpn_int32 sensor, vrpn_float64 *vec,
357  vrpn_float64 *quat)
358 {
359  ensure_enough_unit2sensors(sensor + 1);
360  int i;
361  for (i = 0; i < 3; i++)
362  vec[i] = unit2sensor[sensor][i];
363  for (i = 0; i < 4; i++)
364  quat[i] = unit2sensor_quat[sensor][i];
365 }
366 
368 {
369  struct timeval current_time;
370  char msgbuf[1000];
371  vrpn_int32 len;
372  vrpn_Tracker *me = (vrpn_Tracker *)userdata; // == this normally
373 
374  p = p; // Keep the compiler from complaining
375 
376  vrpn_gettimeofday(&current_time, NULL);
377  me->timestamp.tv_sec = current_time.tv_sec;
378  me->timestamp.tv_usec = current_time.tv_usec;
379 
380  // our t2r transform was read in by the constructor
381 
382  // send t2r transform
383  if (me->d_connection) {
384  len = me->encode_tracker2room_to(msgbuf);
385  if (me->d_connection->pack_message(
386  len, me->timestamp, me->tracker2room_m_id, me->d_sender_id,
387  msgbuf, vrpn_CONNECTION_RELIABLE)) {
388  fprintf(stderr, "vrpn_Tracker: cannot write t2r message\n");
389  }
390  }
391  return 0;
392 }
393 
395 {
396  struct timeval current_time;
397  char msgbuf[1000];
398  vrpn_int32 len;
399  vrpn_int32 i;
401 
402  p = p; // Keep the compiler from complaining
403 
404  vrpn_gettimeofday(&current_time, NULL);
405  me->timestamp.tv_sec = current_time.tv_sec;
406  me->timestamp.tv_usec = current_time.tv_usec;
407 
408  // our u2s transforms were read in by the constructor
409 
410  if (me->d_connection) {
412  for (i = 0; i < me->num_sensors; i++) {
413  me->d_sensor = i;
414  // send u2s transform
415  len = me->encode_unit2sensor_to(msgbuf);
416  if (me->d_connection->pack_message(
417  len, me->timestamp, me->unit2sensor_m_id, me->d_sender_id,
418  msgbuf, vrpn_CONNECTION_RELIABLE)) {
419  fprintf(stderr, "vrpn_Tracker: cannot write u2s message\n");
420  }
421  }
422  }
423  return 0;
424 }
425 
427 {
428  struct timeval current_time;
429  char msgbuf[1000];
430  vrpn_int32 len;
432 
433  p = p; // Keep the compiler from complaining
434 
435  vrpn_gettimeofday(&current_time, NULL);
436  me->timestamp.tv_sec = current_time.tv_sec;
437  me->timestamp.tv_usec = current_time.tv_usec;
438 
439  // our workspace was read in by the constructor
440 
441  if (me->d_connection) {
442  len = me->encode_workspace_to(msgbuf);
443  if (me->d_connection->pack_message(len, me->timestamp,
444  me->workspace_m_id, me->d_sender_id,
445  msgbuf, vrpn_CONNECTION_RELIABLE)) {
446  fprintf(stderr, "vrpn_Tracker: cannot write workspace message\n");
447  }
448  }
449  return 0;
450 }
451 
459 {
460  char *bufptr = buf;
461  int buflen = 1000;
462  int i;
463 
464  // Encode the position part of the transformation.
465  for (i = 0; i < 3; i++) {
466  vrpn_buffer(&bufptr, &buflen, tracker2room[i]);
467  }
468 
469  // Encode the quaternion part of the transformation.
470  for (i = 0; i < 4; i++) {
471  vrpn_buffer(&bufptr, &buflen, tracker2room_quat[i]);
472  }
473 
474  // Return the number of characters sent.
475  return 1000 - buflen;
476 }
477 
488 {
489  char *bufptr = buf;
490  int buflen = 1000;
491  int i;
492 
493  // Encode the sensor number, then put a filler in32 to re-align
494  // to the 64-bit boundary.
495  vrpn_buffer(&bufptr, &buflen, d_sensor);
496  vrpn_buffer(&bufptr, &buflen, (vrpn_int32)(0));
497 
498  // Encode the position part of the transformation.
499  for (i = 0; i < 3; i++) {
500  vrpn_buffer(&bufptr, &buflen, unit2sensor[d_sensor][i]);
501  }
502 
503  // Encode the quaternion part of the transformation.
504  for (i = 0; i < 4; i++) {
505  vrpn_buffer(&bufptr, &buflen, unit2sensor_quat[d_sensor][i]);
506  }
507 
508  // Return the number of characters sent.
509  return 1000 - buflen;
510 }
511 
513 {
514  char *bufptr = buf;
515  int buflen = 1000;
516 
517  vrpn_buffer(&bufptr, &buflen, workspace_min[0]);
518  vrpn_buffer(&bufptr, &buflen, workspace_min[1]);
519  vrpn_buffer(&bufptr, &buflen, workspace_min[2]);
520 
521  vrpn_buffer(&bufptr, &buflen, workspace_max[0]);
522  vrpn_buffer(&bufptr, &buflen, workspace_max[1]);
523  vrpn_buffer(&bufptr, &buflen, workspace_max[2]);
524 
525  return 1000 - buflen;
526 }
527 
528 // NOTE: you need to be sure that if you are sending vrpn_float64 then
529 // the entire array needs to remain aligned to 8 byte boundaries
530 // (malloced data and static arrays are automatically alloced in
531 // this way). Assumes that there is enough room to store the
532 // entire message. Returns the number of characters sent.
534 {
535  char *bufptr = buf;
536  int buflen = 1000;
537 
538  // Message includes: long sensor, long scrap, vrpn_float64 pos[3],
539  // vrpn_float64 quat[4]
540  // Byte order of each needs to be reversed to match network standard
541 
542  vrpn_buffer(&bufptr, &buflen, d_sensor);
543  vrpn_buffer(&bufptr, &buflen,
544  d_sensor); // This is just to take up space to align
545 
546  vrpn_buffer(&bufptr, &buflen, pos[0]);
547  vrpn_buffer(&bufptr, &buflen, pos[1]);
548  vrpn_buffer(&bufptr, &buflen, pos[2]);
549 
550  vrpn_buffer(&bufptr, &buflen, d_quat[0]);
551  vrpn_buffer(&bufptr, &buflen, d_quat[1]);
552  vrpn_buffer(&bufptr, &buflen, d_quat[2]);
553  vrpn_buffer(&bufptr, &buflen, d_quat[3]);
554 
555  return 1000 - buflen;
556 }
557 
559 {
560  char *bufptr = buf;
561  int buflen = 1000;
562 
563  // Message includes: long unitNum, vrpn_float64 vel[3], vrpn_float64
564  // vel_quat[4]
565  // Byte order of each needs to be reversed to match network standard
566 
567  vrpn_buffer(&bufptr, &buflen, d_sensor);
568  vrpn_buffer(&bufptr, &buflen,
569  d_sensor); // This is just to take up space to align
570 
571  vrpn_buffer(&bufptr, &buflen, vel[0]);
572  vrpn_buffer(&bufptr, &buflen, vel[1]);
573  vrpn_buffer(&bufptr, &buflen, vel[2]);
574 
575  vrpn_buffer(&bufptr, &buflen, vel_quat[0]);
576  vrpn_buffer(&bufptr, &buflen, vel_quat[1]);
577  vrpn_buffer(&bufptr, &buflen, vel_quat[2]);
578  vrpn_buffer(&bufptr, &buflen, vel_quat[3]);
579 
580  vrpn_buffer(&bufptr, &buflen, vel_quat_dt);
581 
582  return 1000 - buflen;
583 }
584 
586 {
587  char *bufptr = buf;
588  int buflen = 1000;
589 
590  // Message includes: long unitNum, vrpn_float64 acc[3], vrpn_float64
591  // acc_quat[4]
592  // Byte order of each needs to be reversed to match network standard
593 
594  vrpn_buffer(&bufptr, &buflen, d_sensor);
595  vrpn_buffer(&bufptr, &buflen,
596  d_sensor); // This is just to take up space to align
597 
598  vrpn_buffer(&bufptr, &buflen, acc[0]);
599  vrpn_buffer(&bufptr, &buflen, acc[1]);
600  vrpn_buffer(&bufptr, &buflen, acc[2]);
601 
602  vrpn_buffer(&bufptr, &buflen, acc_quat[0]);
603  vrpn_buffer(&bufptr, &buflen, acc_quat[1]);
604  vrpn_buffer(&bufptr, &buflen, acc_quat[2]);
605  vrpn_buffer(&bufptr, &buflen, acc_quat[3]);
606 
607  vrpn_buffer(&bufptr, &buflen, acc_quat_dt);
608 
609  return 1000 - buflen;
610 }
611 
613  vrpn_int32 sensors, vrpn_float64 Hz)
614  : vrpn_Tracker(name, c)
615  , update_rate(Hz)
616  , d_redundancy(NULL)
617 {
618  num_sensors = sensors;
620  // Nothing left to do
621 }
622 
624 {
625  struct timeval current_time;
626  char msgbuf[1000];
627  vrpn_int32 i, len;
628 
629  // Call the generic server mainloop routine, since this is a server
630  server_mainloop();
631 
632  // See if its time to generate a new report
633  vrpn_gettimeofday(&current_time, NULL);
634  if (vrpn_TimevalDuration(current_time, timestamp) >=
635  1000000.0 / update_rate) {
636 
637  // Update the time
638  timestamp.tv_sec = current_time.tv_sec;
639  timestamp.tv_usec = current_time.tv_usec;
640 
641  // Send messages for all sensors if we have a connection
642  if (d_redundancy) {
643  for (i = 0; i < num_sensors; i++) {
644  d_sensor = i;
645 
646  // Pack position report
647  len = encode_to(msgbuf);
649  d_sender_id, msgbuf,
651  fprintf(stderr,
652  "NULL tracker: can't write message: tossing\n");
653  }
654 
655  // Pack velocity report
656  len = encode_vel_to(msgbuf);
658  d_sender_id, msgbuf,
660  fprintf(stderr,
661  "NULL tracker: can't write message: tossing\n");
662  }
663 
664  // Pack acceleration report
665  len = encode_acc_to(msgbuf);
667  d_sender_id, msgbuf,
669  fprintf(stderr,
670  "NULL tracker: can't write message: tossing\n");
671  }
672  }
673  }
674  else if (d_connection) {
675  for (i = 0; i < num_sensors; i++) {
676  d_sensor = i;
677 
678  // Pack position report
679  len = encode_to(msgbuf);
681  d_sender_id, msgbuf,
683  fprintf(stderr,
684  "NULL tracker: can't write message: tossing\n");
685  }
686 
687  // Pack velocity report
688  len = encode_vel_to(msgbuf);
690  d_sender_id, msgbuf,
692  fprintf(stderr,
693  "NULL tracker: can't write message: tossing\n");
694  }
695 
696  // Pack acceleration report
697  len = encode_acc_to(msgbuf);
699  d_sender_id, msgbuf,
701  fprintf(stderr,
702  "NULL tracker: can't write message: tossing\n");
703  }
704  }
705  }
706  }
707 }
708 
710 {
711  d_redundancy = t;
712 }
713 
715  vrpn_int32 sensors)
716  : vrpn_Tracker(name, c)
717 {
718  num_sensors = sensors;
720  // Nothing left to do
721 }
722 
724 {
725  // Call the generic server mainloop routine, since this is a server
726  server_mainloop();
727 }
728 
729 int vrpn_Tracker_Server::report_pose(const int sensor, const struct timeval t,
730  const vrpn_float64 position[3],
731  const vrpn_float64 quaternion[4],
732  const vrpn_uint32 class_of_service)
733 {
734  char msgbuf[1000];
735  vrpn_int32 len;
736 
737  // Update the time
738  timestamp.tv_sec = t.tv_sec;
739  timestamp.tv_usec = t.tv_usec;
740 
741  // Send messages for all sensors if we have a connection
742  if (sensor >= num_sensors) {
743  send_text_message("Sensor number too high", timestamp, vrpn_TEXT_ERROR);
744  return -1;
745  }
746  else if (!d_connection) {
747  send_text_message("No connection", timestamp, vrpn_TEXT_ERROR);
748  return -1;
749  }
750  else {
751  d_sensor = sensor;
752 
753  // Pack position report
754  memcpy(pos, position, sizeof(pos));
755  memcpy(d_quat, quaternion, sizeof(d_quat));
756  len = encode_to(msgbuf);
758  d_sender_id, msgbuf, class_of_service)) {
759  fprintf(stderr,
760  "vrpn_Tracker_Server: can't write message: tossing\n");
761  return -1;
762  }
763  }
764  return 0;
765 }
766 
768  const int sensor, const struct timeval t, const vrpn_float64 position[3],
769  const vrpn_float64 quaternion[4], const vrpn_float64 interval,
770  const vrpn_uint32 class_of_service)
771 {
772  char msgbuf[1000];
773  vrpn_int32 len;
774 
775  // Update the time
776  timestamp.tv_sec = t.tv_sec;
777  timestamp.tv_usec = t.tv_usec;
778 
779  // Send messages for all sensors if we have a connection
780  if (sensor >= num_sensors) {
781  send_text_message("Sensor number too high", timestamp, vrpn_TEXT_ERROR);
782  return -1;
783  }
784  else if (!d_connection) {
785  send_text_message("No connection", timestamp, vrpn_TEXT_ERROR);
786  return -1;
787  }
788  else {
789  d_sensor = sensor;
790 
791  // Pack velocity report
792  memcpy(vel, position, sizeof(pos));
793  memcpy(vel_quat, quaternion, sizeof(d_quat));
794  vel_quat_dt = interval;
795  len = encode_vel_to(msgbuf);
797  d_sender_id, msgbuf, class_of_service)) {
798  fprintf(stderr,
799  "vrpn_Tracker_Server: can't write message: tossing\n");
800  return -1;
801  }
802  }
803 
804  return 0;
805 }
806 
808  const int sensor, const struct timeval t, const vrpn_float64 position[3],
809  const vrpn_float64 quaternion[4], const vrpn_float64 interval,
810  const vrpn_uint32 class_of_service)
811 {
812  char msgbuf[1000];
813  vrpn_int32 len;
814 
815  // Update the time
816  timestamp.tv_sec = t.tv_sec;
817  timestamp.tv_usec = t.tv_usec;
818 
819  // Send messages for all sensors if we have a connection
820  if (sensor >= num_sensors) {
821  send_text_message("Sensor number too high", timestamp, vrpn_TEXT_ERROR);
822  return -1;
823  }
824  else if (!d_connection) {
825  send_text_message("No connection", timestamp, vrpn_TEXT_ERROR);
826  return -1;
827  }
828  else {
829  d_sensor = sensor;
830 
831  // Pack acceleration report
832  memcpy(acc, position, sizeof(pos));
833  memcpy(acc_quat, quaternion, sizeof(d_quat));
834  acc_quat_dt = interval;
835  len = encode_acc_to(msgbuf);
837  msgbuf, class_of_service)) {
838  fprintf(stderr,
839  "vrpn_Tracker_Server: can't write message: tossing\n");
840  return -1;
841  }
842  }
843 
844  return 0;
845 }
846 
847 #ifndef VRPN_CLIENT_ONLY
849  const char *port, long baud)
850  : vrpn_Tracker(name, c)
851  , serial_fd(-1)
852 {
854  // Find out the port name and baud rate
855  if (port == NULL) {
856  fprintf(stderr, "vrpn_Tracker_Serial: NULL port name\n");
858  return;
859  }
860  else {
861  strncpy(portname, port, sizeof(portname));
862  portname[sizeof(portname) - 1] = '\0';
863  }
864  baudrate = baud;
865 
866  // Open the serial port we're going to use
867  if ((serial_fd = vrpn_open_commport(portname, baudrate)) == -1) {
868  fprintf(stderr, "vrpn_Tracker_Serial: Cannot Open serial port\n");
870  }
871 
872  // Reset the tracker and find out what time it is
875 }
876 
878 {
879  if (serial_fd >= 0) {
881  serial_fd = -1;
882  }
883 }
884 
886 {
887  // Send the message on the connection
888  if (d_connection) {
889  char msgbuf[1000];
890  int len = encode_to(msgbuf);
892  d_sender_id, msgbuf,
894  fprintf(stderr, "Tracker: cannot write message: tossing\n");
895  }
896  }
897  else {
898  fprintf(stderr, "Tracker: No valid connection\n");
899  }
900 }
901 
908 {
909  server_mainloop();
910 
911  switch (status) {
914  case vrpn_TRACKER_PARTIAL: {
915  // It turns out to be important to get the report before checking
916  // to see if it has been too long since the last report. This is
917  // because there is the possibility that some other device running
918  // in the same server may have taken a long time on its last pass
919  // through mainloop(). Trackers that are resetting do this. When
920  // this happens, you can get an infinite loop -- where one tracker
921  // resets and causes the other to timeout, and then it returns the
922  // favor. By checking for the report here, we reset the timestamp
923  // if there is a report ready (ie, if THIS device is still operating).
924 
925  while (get_report()) { // While we get reports, continue to send them.
926  send_report();
927  };
928 
929  struct timeval current_time;
930  vrpn_gettimeofday(&current_time, NULL);
931  int time_lapsed; /* The time since the last report */
932 
933  // Watchdog timestamp is implemented by Polhemus Liberty driver.
934  // XXX All trackers should be modified to use this, or it to not.
935  // If the watchdog timestamp is zero, use the last timestamp to check.
936  if (watchdog_timestamp.tv_sec == 0) {
937  time_lapsed = vrpn_TimevalDuration(current_time, timestamp);
938  }
939  else { // The watchdog_timestamp is being used
940  time_lapsed =
942  }
943 
944  if (time_lapsed > vrpn_ser_tkr_MAX_TIME_INTERVAL) {
945  char errmsg[1024];
946  sprintf(errmsg, "Tracker failed to read... current_time=%ld:%ld, "
947  "timestamp=%ld:%ld\n",
948  current_time.tv_sec,
949  static_cast<long>(current_time.tv_usec), timestamp.tv_sec,
950  static_cast<long>(timestamp.tv_usec));
951  send_text_message(errmsg, current_time, vrpn_TEXT_ERROR);
953  }
954  } break;
955 
957  reset();
958  break;
959 
960  case vrpn_TRACKER_FAIL:
961  send_text_message("Tracker failed, trying to reset (Try power cycle if "
962  "more than 4 attempts made)",
964  if (serial_fd >= 0) {
966  serial_fd = -1;
967  }
968  if ((serial_fd = vrpn_open_commport(portname, baudrate)) == -1) {
969  fprintf(
970  stderr,
971  "vrpn_Tracker_Serial::mainloop(): Cannot Open serial port\n");
973  }
975  break;
976  }
977 }
978 
979 #if defined(VRPN_USE_LIBUSB_1_0)
980 
982  vrpn_uint16 vendor, vrpn_uint16 product,
983  long baud)
984  : vrpn_Tracker(name, c)
985  , _vendor(vendor)
986  , _product(product)
987  , _baudrate(baud)
988  , _device_handle(NULL)
989 {
990  // Register handlers
992 
993  // Initialize libusb
994  if (libusb_init(&_context) != 0) {
995  fprintf(stderr, "vrpn_Tracker_USB: can't init LibUSB\n");
997  return;
998  }
999  // libusb_set_debug (_context, 3);
1000 
1001  // Open and claim an usb device with the expected vendor and product ID.
1002  if ((_device_handle = libusb_open_device_with_vid_pid(_context, _vendor,
1003  _product)) == NULL) {
1004  fprintf(stderr, "vrpn_Tracker_USB: can't find any Polhemus High Speed "
1005  "Liberty Latus devices\n");
1006  fprintf(stderr,
1007  " (Did you remember to run as root?)\n");
1009  return;
1010  }
1011 
1012  if (libusb_claim_interface(_device_handle, 0) != 0) {
1013  fprintf(stderr,
1014  "vrpn_Tracker_USB: can't claim interface for this device\n");
1015  fprintf(stderr,
1016  " (Did you remember to run as root?)\n");
1017  libusb_close(_device_handle);
1018  _device_handle = NULL;
1019  libusb_exit(_context);
1020  _context = NULL;
1022  return;
1023  }
1024 
1025  // Reset the tracker and find out what time it is
1027  vrpn_gettimeofday(&timestamp, NULL);
1028 }
1029 
1031 {
1032  if (_device_handle) {
1033  libusb_close(_device_handle);
1034  _device_handle = NULL;
1035  }
1036  if (_context) {
1037  libusb_exit(_context);
1038  _context = NULL;
1039  }
1040 }
1041 
1043 {
1044  // Send the message on the connection
1045  if (d_connection) {
1046  char msgbuf[1000];
1047  int len = encode_to(msgbuf);
1049  d_sender_id, msgbuf,
1051  fprintf(stderr, "Tracker: cannot write message: tossing\n");
1052  }
1053  }
1054  else {
1055  fprintf(stderr, "Tracker: No valid connection\n");
1056  }
1057 }
1058 
1065 {
1066  server_mainloop();
1067 
1068  switch (status) {
1069  case vrpn_TRACKER_SYNCING:
1070  case vrpn_TRACKER_PARTIAL: {
1071  // It turns out to be important to get the report before checking
1072  // to see if it has been too long since the last report. This is
1073  // because there is the possibility that some other device running
1074  // in the same server may have taken a long time on its last pass
1075  // through mainloop(). Trackers that are resetting do this. When
1076  // this happens, you can get an infinite loop -- where one tracker
1077  // resets and causes the other to timeout, and then it returns the
1078  // favor. By checking for the report here, we reset the timestamp
1079  // if there is a report ready (ie, if THIS device is still operating).
1080 
1081  get_report();
1082 
1083  // Ready for another report
1085 
1086  // Save reception time
1087  struct timeval current_time;
1088  vrpn_gettimeofday(&current_time, NULL);
1089  int time_lapsed; // The time since the last report
1090 
1091  // Watchdog timestamp is implemented by Polhemus Liberty driver.
1092  // XXX All trackers should be modified to use this, or it to not.
1093  // If the watchdog timestamp is zero, use the last timestamp to check.
1094  if (watchdog_timestamp.tv_sec == 0) {
1095  time_lapsed = vrpn_TimevalDuration(current_time, timestamp);
1096  }
1097  else { // The watchdog_timestamp is being used
1098  time_lapsed =
1100  }
1101 
1102  if (time_lapsed > vrpn_ser_tkr_MAX_TIME_INTERVAL) {
1103  char errmsg[1024];
1104  sprintf(errmsg, "Tracker failed to read... current_time=%ld:%ld, "
1105  "timestamp=%ld:%ld\n",
1106  current_time.tv_sec,
1107  static_cast<long>(current_time.tv_usec), timestamp.tv_sec,
1108  static_cast<long>(timestamp.tv_usec));
1109  send_text_message(errmsg, current_time, vrpn_TEXT_ERROR);
1111  }
1112 
1113  } break;
1114 
1116  reset();
1117  break;
1118 
1119  case vrpn_TRACKER_FAIL:
1120  send_text_message("Tracker failed, trying to reset (Try power cycle if "
1121  "more than 4 attempts made)",
1123  // Reset the device handle and then attempt to connect to a device.
1124  if (_device_handle) {
1125  libusb_close(_device_handle);
1126  _device_handle = NULL;
1127  }
1128  if ((_device_handle = libusb_open_device_with_vid_pid(
1129  _context, _vendor, _product)) == NULL) {
1130  fprintf(stderr, "vrpn_Tracker_USB::mainloop(): can't find any "
1131  "Polhemus High Speed Liberty Latus devices\n");
1133  break;
1134  }
1135 
1136  if (libusb_claim_interface(_device_handle, 0) != 0) {
1137  fprintf(stderr, "vrpn_Tracker_USB::mainloop(): can't claim "
1138  "interface for this device\n");
1139  libusb_close(_device_handle);
1140  _device_handle = NULL;
1142  break;
1143  }
1144 
1146  break;
1147  }
1148 }
1149 
1150 // End of LIBUSB
1151 #endif
1152 
1153 #endif // VRPN_CLIENT_ONLY
1154 
1156  : vrpn_Tracker(name, cn)
1157  , sensor_callbacks(NULL)
1158  , num_sensor_callbacks(0)
1159 {
1160  // Make sure that we have a valid connection
1161  if (d_connection == NULL) {
1162  fprintf(stderr, "vrpn_Tracker_Remote: No connection\n");
1163  return;
1164  }
1165 
1166  // Register a handler for the position change callback from this device.
1168  d_sender_id)) {
1169  fprintf(stderr,
1170  "vrpn_Tracker_Remote: can't register position handler\n");
1171  d_connection = NULL;
1172  }
1173 
1174  // Register a handler for the velocity change callback from this device.
1176  this, d_sender_id)) {
1177  fprintf(stderr,
1178  "vrpn_Tracker_Remote: can't register velocity handler\n");
1179  d_connection = NULL;
1180  }
1181 
1182  // Register a handler for the acceleration change callback.
1184  this, d_sender_id)) {
1185  fprintf(stderr,
1186  "vrpn_Tracker_Remote: can't register acceleration handler\n");
1187  d_connection = NULL;
1188  }
1189 
1190  // Register a handler for the room to tracker xform change callback
1193  d_sender_id)) {
1194  fprintf(stderr,
1195  "vrpn_Tracker_Remote: can't register tracker2room handler\n");
1196  d_connection = NULL;
1197  }
1198 
1199  // Register a handler for the sensor to unit xform change callback
1202  d_sender_id)) {
1203  fprintf(stderr,
1204  "vrpn_Tracker_Remote: can't register unit2sensor handler\n");
1205  d_connection = NULL;
1206  }
1207 
1208  // Register a handler for the workspace change callback
1211  d_sender_id)) {
1212  fprintf(stderr,
1213  "vrpn_Tracker_Remote: can't register workspace handler\n");
1214  d_connection = NULL;
1215  }
1216 
1217  // Find out what time it is and put this into the timestamp
1218  vrpn_gettimeofday(&timestamp, NULL);
1219 }
1220 
1221 // The remote tracker has to un-register its handlers when it
1222 // is destroyed to avoid seg faults (this is taken care of by
1223 // using autodeleted handlers above). It should also remove all
1224 // remaining user-registered callbacks to free up memory.
1225 
1227 {
1228  if (sensor_callbacks != NULL) {
1229  delete[] sensor_callbacks;
1230  }
1232 }
1233 
1234 // Make sure we have enough sensor_callback elements in the array.
1235 // Returns false if we run out of memory, true otherwise.
1237 {
1238  unsigned i;
1239  ++num; // Just to make sure we don't fall prey to off-by-one indexing
1240  // errors.
1241  if (num > num_sensor_callbacks) {
1242  // Make sure we allocate in large chunks, rather than one at a time.
1243  if (num < 2 * num_sensor_callbacks) {
1244  num = 2 * num_sensor_callbacks;
1245  }
1246 
1247  // Allocate new space for the list.
1250  if (newlist == NULL) {
1251  return false;
1252  }
1253 
1254  // Copy all of the existing elements.
1255  for (i = 0; i < num_sensor_callbacks; i++) {
1256  newlist[i] = sensor_callbacks[i];
1257  }
1258 
1259  // The new elements will be empty by default, nothing to do here.
1260 
1261  // Switch the new list in for the old, and delete the old.
1262  if (sensor_callbacks != NULL) {
1263  delete[] sensor_callbacks;
1264  }
1265  sensor_callbacks = newlist;
1266  num_sensor_callbacks = num;
1267  }
1268  return true;
1269 }
1270 
1272 {
1273  char *msgbuf = NULL;
1274  vrpn_int32 len = 0; // no payload
1275  struct timeval current_time;
1276 
1277  vrpn_gettimeofday(&current_time, NULL);
1278  timestamp.tv_sec = current_time.tv_sec;
1279  timestamp.tv_usec = current_time.tv_usec;
1280 
1281  if (d_connection) {
1283  d_sender_id, msgbuf,
1285  fprintf(stderr, "vrpn_Tracker_Remote: cannot request t2r xform\n");
1286  return -1;
1287  }
1288  }
1289  return 0;
1290 }
1291 
1293 {
1294  char *msgbuf = NULL;
1295  vrpn_int32 len = 0; // no payload
1296  struct timeval current_time;
1297 
1298  vrpn_gettimeofday(&current_time, NULL);
1299  timestamp.tv_sec = current_time.tv_sec;
1300  timestamp.tv_usec = current_time.tv_usec;
1301 
1302  if (d_connection) {
1304  d_sender_id, msgbuf,
1306  fprintf(stderr, "vrpn_Tracker_Remote: cannot request u2s xform\n");
1307  return -1;
1308  }
1309  }
1310  return 0;
1311 }
1312 
1314 {
1315  char *msgbuf = NULL;
1316  vrpn_int32 len = 0; // no payload
1317  struct timeval current_time;
1318 
1319  vrpn_gettimeofday(&current_time, NULL);
1320  timestamp.tv_sec = current_time.tv_sec;
1321  timestamp.tv_usec = current_time.tv_usec;
1322 
1323  if (d_connection) {
1325  d_sender_id, msgbuf,
1327  fprintf(stderr, "vrpn_Tracker_Remote: cannot request workspace\n");
1328  return -1;
1329  }
1330  }
1331 
1332  return 0;
1333 }
1334 
1335 int vrpn_Tracker_Remote::set_update_rate(vrpn_float64 samplesPerSecond)
1336 {
1337  char *msgbuf;
1338  vrpn_int32 len;
1339  struct timeval now;
1340 
1341  len = sizeof(vrpn_float64);
1342  msgbuf = new char[len];
1343  if (!msgbuf) {
1344  fprintf(stderr, "vrpn_Tracker_Remote::set_update_rate: "
1345  "Out of memory!\n");
1346  return -1;
1347  }
1348 
1349  vrpn_int32 buflen = len;
1350  vrpn_buffer(&msgbuf, &buflen, samplesPerSecond);
1351 
1352  vrpn_gettimeofday(&now, NULL);
1353  timestamp.tv_sec = now.tv_sec;
1354  timestamp.tv_usec = now.tv_usec;
1355 
1356  if (d_connection) {
1358  d_sender_id, msgbuf,
1360  fprintf(stderr, "vrpn_Tracker_Remote::set_update_rate: "
1361  "Cannot send message.\n");
1362  return -1;
1363  }
1364  }
1365  return 0;
1366 }
1367 
1369 {
1370  struct timeval current_time;
1371  vrpn_gettimeofday(&current_time, NULL);
1372  timestamp.tv_sec = current_time.tv_sec;
1373  timestamp.tv_usec = current_time.tv_usec;
1374 
1375  if (d_connection) {
1377  d_sender_id, NULL,
1379  fprintf(stderr,
1380  "vrpn_Tracker_Remote: cannot write message: tossing\n");
1381  }
1382  }
1383  return 0;
1384 }
1385 
1387 {
1388  if (d_connection) {
1390  }
1391  client_mainloop();
1392 }
1393 
1395  void *userdata, vrpn_TRACKERCHANGEHANDLER handler, vrpn_int32 whichSensor)
1396 {
1397  if (whichSensor < vrpn_ALL_SENSORS) {
1398  fprintf(
1399  stderr,
1400  "vrpn_Tracker_Remote::register_change_handler: bad sensor index\n");
1401  return -1;
1402  }
1403  // Ensure that the handler is non-NULL
1404  if (handler == NULL) {
1405  fprintf(stderr,
1406  "vrpn_Tracker_Remote::register_change_handler: NULL handler\n");
1407  return -1;
1408  }
1409 
1410  // If this is the ALL_SENSORS value, put it on the all list; otherwise,
1411  // put it into the normal list.
1412  if (whichSensor == vrpn_ALL_SENSORS) {
1414  handler);
1415  }
1416  else if (ensure_enough_sensor_callbacks(whichSensor)) {
1417  return sensor_callbacks[whichSensor].d_change.register_handler(userdata,
1418  handler);
1419  }
1420  else {
1421  fprintf(
1422  stderr,
1423  "vrpn_Tracker_Remote::register_change_handler: Out of memory\n");
1424  return -1;
1425  }
1426 }
1427 
1429  void *userdata, vrpn_TRACKERVELCHANGEHANDLER handler,
1430  vrpn_int32 whichSensor)
1431 {
1432  if (whichSensor < vrpn_ALL_SENSORS) {
1433  fprintf(
1434  stderr,
1435  "vrpn_Tracker_Remote::register_change_handler: bad sensor index\n");
1436  return -1;
1437  }
1438  // Ensure that the handler is non-NULL
1439  if (handler == NULL) {
1440  fprintf(stderr,
1441  "vrpn_Tracker_Remote::register_change_handler: NULL handler\n");
1442  return -1;
1443  }
1444 
1445  // If this is the ALL_SENSORS value, put it on the all list; otherwise,
1446  // put it into the normal list.
1447  if (whichSensor == vrpn_ALL_SENSORS) {
1449  handler);
1450  }
1451  else if (ensure_enough_sensor_callbacks(whichSensor)) {
1452  return sensor_callbacks[whichSensor].d_velchange.register_handler(
1453  userdata, handler);
1454  }
1455  else {
1456  fprintf(
1457  stderr,
1458  "vrpn_Tracker_Remote::register_change_handler: Out of memory\n");
1459  return -1;
1460  }
1461 }
1462 
1464  void *userdata, vrpn_TRACKERACCCHANGEHANDLER handler,
1465  vrpn_int32 whichSensor)
1466 {
1467  if (whichSensor < vrpn_ALL_SENSORS) {
1468  fprintf(
1469  stderr,
1470  "vrpn_Tracker_Remote::register_change_handler: bad sensor index\n");
1471  return -1;
1472  }
1473 
1474  // Ensure that the handler is non-NULL
1475  if (handler == NULL) {
1476  fprintf(stderr,
1477  "vrpn_Tracker_Remote::register_change_handler: NULL handler\n");
1478  return -1;
1479  }
1480 
1481  // If this is the ALL_SENSORS value, put it on the all list; otherwise,
1482  // put it into the normal list.
1483  if (whichSensor == vrpn_ALL_SENSORS) {
1485  handler);
1486  }
1487  else if (ensure_enough_sensor_callbacks(whichSensor)) {
1488  return sensor_callbacks[whichSensor].d_accchange.register_handler(
1489  userdata, handler);
1490  }
1491  else {
1492  fprintf(
1493  stderr,
1494  "vrpn_Tracker_Remote::register_change_handler: Out of memory\n");
1495  return -1;
1496  }
1497 }
1498 
1500  void *userdata, vrpn_TRACKERUNIT2SENSORCHANGEHANDLER handler,
1501  vrpn_int32 whichSensor)
1502 {
1503  if (whichSensor < vrpn_ALL_SENSORS) {
1504  fprintf(
1505  stderr,
1506  "vrpn_Tracker_Remote::register_change_handler: bad sensor index\n");
1507  return -1;
1508  }
1509 
1510  // Ensure that the handler is non-NULL
1511  if (handler == NULL) {
1512  fprintf(stderr, "%s%s", "vrpn_Tracker_Remote:",
1513  ":register_change_handler: NULL handler\n");
1514  return -1;
1515  }
1516 
1517  // If this is the ALL_SENSORS value, put it on the all list; otherwise,
1518  // put it into the normal list.
1519  if (whichSensor == vrpn_ALL_SENSORS) {
1521  userdata, handler);
1522  }
1523  else if (ensure_enough_sensor_callbacks(whichSensor)) {
1524  return sensor_callbacks[whichSensor]
1526  }
1527  else {
1528  fprintf(
1529  stderr,
1530  "vrpn_Tracker_Remote::register_change_handler: Out of memory\n");
1531  return -1;
1532  }
1533 }
1534 
1536  void *userdata, vrpn_TRACKERCHANGEHANDLER handler, vrpn_int32 whichSensor)
1537 {
1538  if (whichSensor < vrpn_ALL_SENSORS) {
1539  fprintf(stderr, "vrpn_Tracker_Remote::unregister_change_handler: bad "
1540  "sensor index\n");
1541  return -1;
1542  }
1543 
1544  if (whichSensor == vrpn_ALL_SENSORS) {
1546  handler);
1547  }
1548  else if (ensure_enough_sensor_callbacks(whichSensor)) {
1549  return sensor_callbacks[whichSensor].d_change.unregister_handler(
1550  userdata, handler);
1551  }
1552  else {
1553  fprintf(
1554  stderr,
1555  "vrpn_Tracker_Remote::unregister_change_handler: Out of memory\n");
1556  return -1;
1557  }
1558 }
1559 
1561  void *userdata, vrpn_TRACKERVELCHANGEHANDLER handler,
1562  vrpn_int32 whichSensor)
1563 {
1564  if (whichSensor < vrpn_ALL_SENSORS) {
1565  fprintf(stderr, "vrpn_Tracker_Remote::unregister_change_handler: bad "
1566  "sensor index\n");
1567  return -1;
1568  }
1569 
1570  if (whichSensor == vrpn_ALL_SENSORS) {
1572  handler);
1573  }
1574  else if (ensure_enough_sensor_callbacks(whichSensor)) {
1575  return sensor_callbacks[whichSensor].d_velchange.unregister_handler(
1576  userdata, handler);
1577  }
1578  else {
1579  fprintf(
1580  stderr,
1581  "vrpn_Tracker_Remote::unregister_change_handler: Out of memory\n");
1582  return -1;
1583  }
1584 }
1585 
1587  void *userdata, vrpn_TRACKERACCCHANGEHANDLER handler,
1588  vrpn_int32 whichSensor)
1589 {
1590  if (whichSensor < vrpn_ALL_SENSORS) {
1591  fprintf(stderr, "vrpn_Tracker_Remote::unregister_change_handler: bad "
1592  "sensor index\n");
1593  return -1;
1594  }
1595 
1596  if (whichSensor == vrpn_ALL_SENSORS) {
1598  handler);
1599  }
1600  else if (ensure_enough_sensor_callbacks(whichSensor)) {
1601  return sensor_callbacks[whichSensor].d_accchange.unregister_handler(
1602  userdata, handler);
1603  }
1604  else {
1605  fprintf(
1606  stderr,
1607  "vrpn_Tracker_Remote::unregister_change_handler: Out of memory\n");
1608  return -1;
1609  }
1610 }
1611 
1613  void *userdata, vrpn_TRACKERUNIT2SENSORCHANGEHANDLER handler,
1614  vrpn_int32 whichSensor)
1615 {
1616  if (whichSensor < vrpn_ALL_SENSORS) {
1617  fprintf(stderr, "vrpn_Tracker_Remote::unregister_change_handler: bad "
1618  "sensor index\n");
1619  return -1;
1620  }
1621 
1622  if (whichSensor == vrpn_ALL_SENSORS) {
1624  userdata, handler);
1625  }
1626  else if (ensure_enough_sensor_callbacks(whichSensor)) {
1627  return sensor_callbacks[whichSensor]
1629  }
1630  else {
1631  fprintf(
1632  stderr,
1633  "vrpn_Tracker_Remote::unregister_change_handler: Out of memory\n");
1634  return -1;
1635  }
1636 }
1637 
1640 {
1642  const char *params = (p.buffer);
1643  vrpn_int32 padding;
1644  vrpn_TRACKERCB tp;
1645  int i;
1646 
1647  // Fill in the parameters to the tracker from the message
1648  if (p.payload_len != (8 * sizeof(vrpn_float64))) {
1649  fprintf(stderr, "vrpn_Tracker: change message payload error\n");
1650  fprintf(stderr, " (got %d, expected %lud)\n", p.payload_len,
1651  static_cast<unsigned long>(8 * sizeof(vrpn_float64)));
1652  return -1;
1653  }
1654  tp.msg_time = p.msg_time;
1655  vrpn_unbuffer(&params, &tp.sensor);
1656  vrpn_unbuffer(&params, &padding);
1657 
1658  for (i = 0; i < 3; i++) {
1659  vrpn_unbuffer(&params, &tp.pos[i]);
1660  }
1661  for (i = 0; i < 4; i++) {
1662  vrpn_unbuffer(&params, &tp.quat[i]);
1663  }
1664 
1665  // Go down the list of callbacks that have been registered.
1666  // Fill in the parameter and call each.
1668 
1669  // Go down the list of callbacks that have been registered for this
1670  // particular sensor
1671  if (tp.sensor < 0) {
1672  fprintf(stderr, "vrpn_Tracker_Rem:pos sensor index is negative!\n");
1673  return -1;
1674  }
1675  else if (me->ensure_enough_sensor_callbacks(tp.sensor)) {
1677  }
1678  else {
1679  fprintf(stderr, "vrpn_Tracker_Rem:pos sensor index too large\n");
1680  return -1;
1681  }
1682  return 0;
1683 }
1684 
1687 {
1689  const char *params = p.buffer;
1690  vrpn_int32 padding;
1691  vrpn_TRACKERVELCB tp;
1692  int i;
1693 
1694  // Fill in the parameters to the tracker from the message
1695  if (p.payload_len != (9 * sizeof(vrpn_float64))) {
1696  fprintf(stderr, "vrpn_Tracker: vel message payload error\n");
1697  fprintf(stderr, " (got %d, expected %lud)\n", p.payload_len,
1698  static_cast<unsigned long>(9 * sizeof(vrpn_float64)));
1699  return -1;
1700  }
1701  tp.msg_time = p.msg_time;
1702  vrpn_unbuffer(&params, &tp.sensor);
1703  vrpn_unbuffer(&params, &padding);
1704 
1705  for (i = 0; i < 3; i++) {
1706  vrpn_unbuffer(&params, &tp.vel[i]);
1707  }
1708  for (i = 0; i < 4; i++) {
1709  vrpn_unbuffer(&params, &tp.vel_quat[i]);
1710  }
1711 
1712  vrpn_unbuffer(&params, &tp.vel_quat_dt);
1713 
1714  // Go down the list of callbacks that have been registered.
1715  // Fill in the parameter and call each.
1717 
1718  // Go down the list of callbacks that have been registered for this
1719  // particular sensor
1720  if (me->ensure_enough_sensor_callbacks(tp.sensor)) {
1722  }
1723  else {
1724  fprintf(stderr, "vrpn_Tracker_Rem:vel sensor index too large\n");
1725  return -1;
1726  }
1727  return 0;
1728 }
1729 
1732 {
1734  const char *params = p.buffer;
1735  vrpn_int32 padding;
1736  vrpn_TRACKERACCCB tp;
1737  int i;
1738 
1739  // Fill in the parameters to the tracker from the message
1740  if (p.payload_len != (9 * sizeof(vrpn_float64))) {
1741  fprintf(stderr, "vrpn_Tracker: acc message payload error\n");
1742  fprintf(stderr, "(got %d, expected %lud)\n", p.payload_len,
1743  static_cast<unsigned long>(9 * sizeof(vrpn_float64)));
1744  return -1;
1745  }
1746  tp.msg_time = p.msg_time;
1747  vrpn_unbuffer(&params, &tp.sensor);
1748  vrpn_unbuffer(&params, &padding);
1749 
1750  for (i = 0; i < 3; i++) {
1751  vrpn_unbuffer(&params, &tp.acc[i]);
1752  }
1753  for (i = 0; i < 4; i++) {
1754  vrpn_unbuffer(&params, &tp.acc_quat[i]);
1755  }
1756 
1757  vrpn_unbuffer(&params, &tp.acc_quat_dt);
1758 
1759  // Go down the list of callbacks that have been registered.
1760  // Fill in the parameter and call each.
1762 
1763  // Go down the list of callbacks that have been registered for this
1764  // particular sensor
1765  if (me->ensure_enough_sensor_callbacks(tp.sensor)) {
1767  }
1768  else {
1769  fprintf(stderr, "vrpn_Tracker_Rem:acc sensor index too large\n");
1770  return -1;
1771  }
1772  return 0;
1773 }
1774 
1777 {
1779  const char *params = p.buffer;
1780  vrpn_int32 padding;
1782  int i;
1783 
1784  // Fill in the parameters to the tracker from the message
1785  if (p.payload_len != (8 * sizeof(vrpn_float64))) {
1786  fprintf(stderr, "vrpn_Tracker: unit2sensor message payload");
1787  fprintf(stderr, " error\n(got %d, expected %lud)\n", p.payload_len,
1788  static_cast<unsigned long>(8 * sizeof(vrpn_float64)));
1789  return -1;
1790  }
1791  tp.msg_time = p.msg_time;
1792  vrpn_unbuffer(&params, &tp.sensor);
1793  vrpn_unbuffer(&params, &padding);
1794 
1795  // Typecasting used to get the byte order correct on the vrpn_float64
1796  // that are coming from the other side.
1797  for (i = 0; i < 3; i++) {
1798  vrpn_unbuffer(&params, &tp.unit2sensor[i]);
1799  }
1800  for (i = 0; i < 4; i++) {
1801  vrpn_unbuffer(&params, &tp.unit2sensor_quat[i]);
1802  }
1803 
1804  // Go down the list of callbacks that have been registered.
1805  // Fill in the parameter and call each.
1807 
1808  // Go down the list of callbacks that have been registered for this
1809  // particular sensor
1810  if (me->ensure_enough_sensor_callbacks(tp.sensor)) {
1812  }
1813  else {
1814  fprintf(stderr, "vrpn_Tracker_Rem:u2s sensor index too large\n");
1815  return -1;
1816  }
1817 
1818  return 0;
1819 }
1820 
1823 {
1825  const char *params = p.buffer;
1827  int i;
1828 
1829  // Fill in the parameters to the tracker from the message
1830  if (p.payload_len != (7 * sizeof(vrpn_float64))) {
1831  fprintf(stderr, "vrpn_Tracker: tracker2room message payload");
1832  fprintf(stderr, " error\n(got %d, expected %lud)\n", p.payload_len,
1833  static_cast<unsigned long>(7 * sizeof(vrpn_float64)));
1834  return -1;
1835  }
1836  tp.msg_time = p.msg_time;
1837 
1838  for (i = 0; i < 3; i++) {
1839  vrpn_unbuffer(&params, &tp.tracker2room[i]);
1840  }
1841  for (i = 0; i < 4; i++) {
1842  vrpn_unbuffer(&params, &tp.tracker2room_quat[i]);
1843  }
1844 
1845  // Go down the list of callbacks that have been registered.
1846  // Fill in the parameter and call each.
1848 
1849  return 0;
1850 }
1851 
1854 {
1856  const char *params = p.buffer;
1858  int i;
1859 
1860  // Fill in the parameters to the tracker from the message
1861  if (p.payload_len != (6 * sizeof(vrpn_float64))) {
1862  fprintf(stderr, "vrpn_Tracker: tracker2room message payload");
1863  fprintf(stderr, " error\n(got %d, expected %lud)\n", p.payload_len,
1864  static_cast<unsigned long>(6 * sizeof(vrpn_float64)));
1865  return -1;
1866  }
1867  tp.msg_time = p.msg_time;
1868 
1869  for (i = 0; i < 3; i++) {
1870  vrpn_unbuffer(&params, &tp.workspace_min[i]);
1871  }
1872  for (i = 0; i < 3; i++) {
1873  vrpn_unbuffer(&params, &tp.workspace_max[i]);
1874  }
1875 
1876  // Go down the list of callbacks that have been registered.
1877  // Fill in the parameter and call each.
1879 
1880  return 0;
1881 }
vrpn_Tracker::acc
vrpn_float64 acc[3]
Definition: vrpn_Tracker.h:98
vrpn_Tracker::workspace_max
vrpn_float64 workspace_max[3]
Definition: vrpn_Tracker.h:127
vrpn_Tracker_Serial::portname
char portname[VRPN_TRACKER_BUF_SIZE]
Definition: vrpn_Tracker.h:151
vrpn_BaseClassUnique::register_autodeleted_handler
int register_autodeleted_handler(vrpn_int32 type, vrpn_MESSAGEHANDLER handler, void *userdata, vrpn_int32 sender=vrpn_ANY_SENDER)
Registers a handler with the connection, and remembers to delete at destruction.
Definition: vrpn_BaseClass.C:503
vrpn_Tracker.h
vrpn_Connection::pack_message
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...
Definition: vrpn_Connection.C:4632
vrpn_Tracker::read_config_file
int read_config_file(FILE *config_file, const char *tracker_name)
Definition: vrpn_Tracker.C:232
vrpn_Tracker::vel
vrpn_float64 vel[3]
Definition: vrpn_Tracker.h:96
vrpn_Tracker_USB::send_report
virtual void send_report(void)
Definition: vrpn_Tracker.C:1042
vrpn_TRACKERWORKSPACECB::workspace_max
vrpn_float64 workspace_max[3]
Definition: vrpn_Tracker.h:346
vrpn_Tracker_Sensor_Callbacks::d_change
vrpn_Callback_List< vrpn_TRACKERCB > d_change
Definition: vrpn_Tracker.h:355
vrpn_Tracker_Server::report_pose_acceleration
virtual int report_pose_acceleration(const int sensor, const struct timeval t, const vrpn_float64 position[3], const vrpn_float64 quaternion[4], const vrpn_float64 interval, const vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
Definition: vrpn_Tracker.C:807
vrpn_Tracker
Definition: vrpn_Tracker.h:49
vrpn_BaseClassUnique::client_mainloop
void client_mainloop(void)
Handles functions that all clients should provide in their mainloop() (warning of no server,...
Definition: vrpn_BaseClass.C:637
vrpn_TRACKERCB::sensor
vrpn_int32 sensor
Definition: vrpn_Tracker.h:286
vrpn_Tracker_Sensor_Callbacks::d_unit2sensorchange
vrpn_Callback_List< vrpn_TRACKERUNIT2SENSORCB > d_unit2sensorchange
Definition: vrpn_Tracker.h:358
vrpn_Tracker_Remote::register_change_handler
virtual int register_change_handler(void *userdata, vrpn_TRACKERCHANGEHANDLER handler, vrpn_int32 sensor=vrpn_ALL_SENSORS)
Definition: vrpn_Tracker.C:1394
vrpn_Tracker_USB::~vrpn_Tracker_USB
virtual ~vrpn_Tracker_USB()
Definition: vrpn_Tracker.C:1030
vrpn_TimevalDuration
unsigned long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
Return number of microseconds between startT and endT.
Definition: vrpn_Shared.C:129
vrpn_Tracker::encode_to
virtual int encode_to(char *buf)
Definition: vrpn_Tracker.C:533
vrpn_Tracker_Pos
vrpn_float64 vrpn_Tracker_Pos[3]
Definition: vrpn_Tracker.h:46
vrpn_TRACKERTRACKER2ROOMCB::msg_time
struct timeval msg_time
Definition: vrpn_Tracker.h:327
vrpn_Tracker::d_sensor
vrpn_int32 d_sensor
Definition: vrpn_Tracker.h:94
vrpn_Tracker_Remote::sensor_callbacks
vrpn_Tracker_Sensor_Callbacks * sensor_callbacks
Definition: vrpn_Tracker.h:473
vrpn_TRACKERTRACKER2ROOMCB::tracker2room_quat
vrpn_float64 tracker2room_quat[4]
Definition: vrpn_Tracker.h:329
vrpn_Tracker_Server::report_pose_velocity
virtual int report_pose_velocity(const int sensor, const struct timeval t, const vrpn_float64 position[3], const vrpn_float64 quaternion[4], const vrpn_float64 interval, const vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
Definition: vrpn_Tracker.C:767
vrpn_TRACKERUNIT2SENSORCB
Definition: vrpn_Tracker.h:334
vrpn_Tracker::ensure_enough_unit2sensors
bool ensure_enough_unit2sensors(unsigned num)
Definition: vrpn_Tracker.C:182
vrpn_Tracker_Serial::serial_fd
int serial_fd
Definition: vrpn_Tracker.h:153
vrpn_Tracker::handle_workspace_request
static int VRPN_CALLBACK handle_workspace_request(void *userdata, vrpn_HANDLERPARAM p)
Definition: vrpn_Tracker.C:426
vrpn_BaseClassUnique::userdata
void * userdata
Definition: vrpn_BaseClass.h:287
vrpn_Tracker_USB::vrpn_Tracker_USB
vrpn_Tracker_USB(const char *name, vrpn_Connection *c, vrpn_uint16 vendor, vrpn_uint16 product, long baud=115200)
Definition: vrpn_Tracker.C:981
vrpn_Tracker_Server::report_pose
virtual int report_pose(const int sensor, const struct timeval t, const vrpn_float64 position[3], const vrpn_float64 quaternion[4], const vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
These functions should be called to report changes in state, once per sensor.
Definition: vrpn_Tracker.C:729
vrpn_Tracker::d_quat
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
vrpn_Tracker_Remote::num_sensor_callbacks
unsigned num_sensor_callbacks
Definition: vrpn_Tracker.h:474
vrpn_Tracker::velocity_m_id
vrpn_int32 velocity_m_id
Definition: vrpn_Tracker.h:81
vrpn_Tracker_Serial::vrpn_Tracker_Serial
vrpn_Tracker_Serial(const char *name, vrpn_Connection *c, const char *port="/dev/ttyS1", long baud=38400)
Definition: vrpn_Tracker.C:848
vrpn_Tracker::encode_vel_to
virtual int encode_vel_to(char *buf)
Definition: vrpn_Tracker.C:558
vrpn_Tracker::update_rate_id
vrpn_int32 update_rate_id
Definition: vrpn_Tracker.h:89
vrpn_Tracker::timestamp
struct timeval timestamp
Definition: vrpn_Tracker.h:100
vrpn_Tracker::tracker2room_quat
vrpn_float64 tracker2room_quat[4]
Definition: vrpn_Tracker.h:113
vrpn_Tracker_Remote::request_t2r_xform
int request_t2r_xform(void)
Definition: vrpn_Tracker.C:1271
vrpn_TRACKERVELCB::msg_time
struct timeval msg_time
Definition: vrpn_Tracker.h:298
vrpn_Tracker::encode_unit2sensor_to
virtual int encode_unit2sensor_to(char *buf)
Encodes the "Unit to Sensor" transformation into the buffer specified.
Definition: vrpn_Tracker.C:487
vrpn_HANDLERPARAM::payload_len
vrpn_int32 payload_len
Definition: vrpn_Connection.h:48
vrpn_Tracker_Remote::request_u2s_xform
int request_u2s_xform(void)
Definition: vrpn_Tracker.C:1292
vrpn_TRACKERCHANGEHANDLER
void(VRPN_CALLBACK * vrpn_TRACKERCHANGEHANDLER)(void *userdata, const vrpn_TRACKERCB info)
Definition: vrpn_Tracker.h:290
vrpn_Serial.h
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
vrpn_Tracker::handle_t2r_request
static int VRPN_CALLBACK handle_t2r_request(void *userdata, vrpn_HANDLERPARAM p)
Definition: vrpn_Tracker.C:367
vrpn_Tracker_NULL::update_rate
vrpn_float64 update_rate
Definition: vrpn_Tracker.h:234
vrpn_Tracker::register_types
virtual int register_types(void)
Register the types of messages this device sends/receives. Return 0 on success, -1 on fail.
Definition: vrpn_Tracker.C:137
vrpn_Tracker::encode_tracker2room_to
virtual int encode_tracker2room_to(char *buf)
Encodes the "Tracker to Room" transformation into the buffer specified.
Definition: vrpn_Tracker.C:458
vrpn_TRACKERVELCB::vel_quat
vrpn_float64 vel_quat[4]
Definition: vrpn_Tracker.h:301
vrpn_Tracker_Remote::reset_origin
int reset_origin(void)
Definition: vrpn_Tracker.C:1368
vrpn_Callback_List::register_handler
int register_handler(void *userdata, HANDLER_TYPE handler)
Call this to add a handler to the list.
Definition: vrpn_BaseClass.h:391
vrpn_CONNECTION_LOW_LATENCY
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
Definition: vrpn_Connection.h:122
vrpn_Tracker_NULL::vrpn_Tracker_NULL
vrpn_Tracker_NULL(const char *name, vrpn_Connection *c, vrpn_int32 sensors=1, vrpn_float64 Hz=1.0)
Definition: vrpn_Tracker.C:612
vrpn_Tracker_USB::get_report
virtual int get_report(void)=0
Gets reports if some are available, returns 0 if not, 1 if complete report(s).
vrpn_unbuffer
VRPN_API int vrpn_unbuffer(const char **buffer, timeval *t)
Utility routine for taking a struct timeval from a buffer that was sent as a message.
Definition: vrpn_Shared.C:312
vrpn_TRACKERCB::msg_time
struct timeval msg_time
Definition: vrpn_Tracker.h:285
vrpn_TRACKERVELCB::sensor
vrpn_int32 sensor
Definition: vrpn_Tracker.h:299
vrpn_TRACKERUNIT2SENSORCB::unit2sensor_quat
vrpn_float64 unit2sensor_quat[4]
Definition: vrpn_Tracker.h:338
vrpn_Tracker::acc_quat
vrpn_float64 acc_quat[4]
Definition: vrpn_Tracker.h:98
vrpn_Tracker_Remote::~vrpn_Tracker_Remote
virtual ~vrpn_Tracker_Remote(void)
Definition: vrpn_Tracker.C:1226
vrpn_TRACKERCB::quat
vrpn_float64 quat[4]
Definition: vrpn_Tracker.h:288
vrpn_Tracker::accel_m_id
vrpn_int32 accel_m_id
Definition: vrpn_Tracker.h:82
vrpn_Tracker::workspace_m_id
vrpn_int32 workspace_m_id
Definition: vrpn_Tracker.h:88
vrpn_BaseClassUnique::d_connection
vrpn_Connection * d_connection
Connection that this object talks to.
Definition: vrpn_BaseClass.h:224
vrpn_HANDLERPARAM::buffer
const char * buffer
Definition: vrpn_Connection.h:49
vrpn_Connection::register_message_type
virtual vrpn_int32 register_message_type(const char *name)
Definition: vrpn_Connection.C:5074
vrpn_Tracker::encode_workspace_to
virtual int encode_workspace_to(char *buf)
Definition: vrpn_Tracker.C:512
vrpn_TRACKERCB
Definition: vrpn_Tracker.h:284
vrpn_TEXT_ERROR
@ vrpn_TEXT_ERROR
Definition: vrpn_BaseClass.h:103
vrpn_TRACKERACCCHANGEHANDLER
void(VRPN_CALLBACK * vrpn_TRACKERACCCHANGEHANDLER)(void *userdata, const vrpn_TRACKERACCCB info)
Definition: vrpn_Tracker.h:319
vrpn_CONNECTION_RELIABLE
const vrpn_uint32 vrpn_CONNECTION_RELIABLE
Classes of service for messages, specify multiple by ORing them together Priority of satisfying these...
Definition: vrpn_Connection.h:120
vrpn_HANDLERPARAM
This structure is what is passed to a vrpn_Connection message callback.
Definition: vrpn_Connection.h:44
vrpn_Shared.h
vrpn_TRACKER_PARTIAL
const int vrpn_TRACKER_PARTIAL
Definition: vrpn_Tracker.h:38
vrpn_Tracker_NULL::d_redundancy
vrpn_RedundantTransmission * d_redundancy
Definition: vrpn_Tracker.h:236
vrpn_Tracker::unit2sensor
vrpn_Tracker_Pos * unit2sensor
Definition: vrpn_Tracker.h:118
vrpn_Tracker_Serial::send_report
virtual void send_report(void)
Definition: vrpn_Tracker.C:885
vrpn_Tracker_Serial::~vrpn_Tracker_Serial
virtual ~vrpn_Tracker_Serial()
Definition: vrpn_Tracker.C:877
vrpn_Tracker_Remote::handle_unit2sensor_change_message
static int VRPN_CALLBACK handle_unit2sensor_change_message(void *userdata, vrpn_HANDLERPARAM p)
Definition: vrpn_Tracker.C:1775
vrpn_TRACKERUNIT2SENSORCB::msg_time
struct timeval msg_time
Definition: vrpn_Tracker.h:335
vrpn_BaseClassUnique::d_sender_id
vrpn_int32 d_sender_id
Sender ID registered with the connection.
Definition: vrpn_BaseClass.h:228
vrpn_Tracker_Remote::set_update_rate
int set_update_rate(vrpn_float64 samplesPerSecond)
Definition: vrpn_Tracker.C:1335
vrpn_Tracker_Remote::handle_tracker2room_change_message
static int VRPN_CALLBACK handle_tracker2room_change_message(void *userdata, vrpn_HANDLERPARAM p)
Definition: vrpn_Tracker.C:1821
vrpn_TRACKERUNIT2SENSORCHANGEHANDLER
void(VRPN_CALLBACK * vrpn_TRACKERUNIT2SENSORCHANGEHANDLER)(void *userdata, const vrpn_TRACKERUNIT2SENSORCB info)
Definition: vrpn_Tracker.h:340
vrpn_Tracker_Remote
Definition: vrpn_Tracker.h:374
vrpn_TRACKERCB::pos
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:287
vrpn_Tracker::request_workspace_m_id
vrpn_int32 request_workspace_m_id
Definition: vrpn_Tracker.h:87
vrpn_Tracker_Server::mainloop
virtual void mainloop()
This function should be called each time through app mainloop.
Definition: vrpn_Tracker.C:723
vrpn_Tracker::unit2sensor_quat
vrpn_Tracker_Quat * unit2sensor_quat
Definition: vrpn_Tracker.h:119
vrpn_Tracker_Serial::mainloop
virtual void mainloop()
Uses the get_report, send_report, and reset routines to implement a server.
Definition: vrpn_Tracker.C:907
vrpn_Tracker_Quat
vrpn_float64 vrpn_Tracker_Quat[4]
Definition: vrpn_Tracker.h:47
vrpn_TRACKERACCCB::acc_quat_dt
vrpn_float64 acc_quat_dt
Definition: vrpn_Tracker.h:316
vrpn_Tracker::acc_quat_dt
vrpn_float64 acc_quat_dt
Definition: vrpn_Tracker.h:99
vrpn_Connection::mainloop
virtual int mainloop(const struct timeval *timeout=NULL)=0
Call each time through program main loop to handle receiving any incoming messages and sending any pa...
vrpn_HANDLERPARAM::msg_time
struct timeval msg_time
Definition: vrpn_Connection.h:47
vrpn_Tracker::encode_acc_to
virtual int encode_acc_to(char *buf)
Definition: vrpn_Tracker.C:585
vrpn_Tracker::position_m_id
vrpn_int32 position_m_id
Definition: vrpn_Tracker.h:80
vrpn_Connection
Generic connection class not specific to the transport mechanism.
Definition: vrpn_Connection.h:510
vrpn_RedundantTransmission.h
vrpn_Tracker::get_local_u2s
void get_local_u2s(vrpn_int32 sensor, vrpn_float64 *vec, vrpn_float64 *quat)
Definition: vrpn_Tracker.C:356
vrpn_TRACKER_FAIL
const int vrpn_TRACKER_FAIL
Definition: vrpn_Tracker.h:40
vrpn_TRACKERUNIT2SENSORCB::sensor
vrpn_int32 sensor
Definition: vrpn_Tracker.h:336
vrpn_RedundantTransmission::pack_message
virtual int pack_message(vrpn_uint32 len, timeval time, vrpn_uint32 type, vrpn_uint32 sender, const char *buffer, vrpn_uint32 class_of_service, vrpn_int32 numRetransmissions=-1, timeval *transmissionInterval=NULL)
If !isEnabled(), does a normal pack_message(), but if isEnabled() ignores class_of_service and sends ...
Definition: vrpn_RedundantTransmission.C:124
vrpn_TRACKER_RESETTING
const int vrpn_TRACKER_RESETTING
Definition: vrpn_Tracker.h:39
vrpn_Tracker::request_u2s_m_id
vrpn_int32 request_u2s_m_id
Definition: vrpn_Tracker.h:86
vrpn_TRACKERVELCB::vel_quat_dt
vrpn_float64 vel_quat_dt
Definition: vrpn_Tracker.h:302
vrpn_Tracker_Remote::all_sensor_callbacks
vrpn_Tracker_Sensor_Callbacks all_sensor_callbacks
Definition: vrpn_Tracker.h:468
vrpn_gettimeofday
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
vrpn_Tracker::watchdog_timestamp
struct timeval watchdog_timestamp
Definition: vrpn_Tracker.h:111
vrpn_Tracker::status
int status
Definition: vrpn_Tracker.h:129
vrpn_Tracker::register_server_handlers
int register_server_handlers(void)
Definition: vrpn_Tracker.C:318
vrpn_Tracker_Remote::mainloop
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
Definition: vrpn_Tracker.C:1386
vrpn_Tracker_Remote::d_tracker2roomchange_list
vrpn_Callback_List< vrpn_TRACKERTRACKER2ROOMCB > d_tracker2roomchange_list
Definition: vrpn_Tracker.h:478
vrpn_Tracker_USB::_vendor
vrpn_uint16 _vendor
Definition: vrpn_Tracker.h:190
vrpn_Tracker_Sensor_Callbacks::d_accchange
vrpn_Callback_List< vrpn_TRACKERACCCB > d_accchange
Definition: vrpn_Tracker.h:357
vrpn_Tracker_Sensor_Callbacks
Definition: vrpn_Tracker.h:353
vrpn_Tracker::workspace_min
vrpn_float64 workspace_min[3]
Definition: vrpn_Tracker.h:127
vrpn_TRACKERACCCB::acc
vrpn_float64 acc[3]
Definition: vrpn_Tracker.h:314
vrpn_Tracker::~vrpn_Tracker
virtual ~vrpn_Tracker(void)
Definition: vrpn_Tracker.C:169
vrpn_close_commport
int vrpn_close_commport(int comm)
Definition: vrpn_Serial.C:345
vrpn_TRACKERWORKSPACECB
Definition: vrpn_Tracker.h:343
vrpn_Tracker::vel_quat
vrpn_float64 vel_quat[4]
Definition: vrpn_Tracker.h:96
vrpn_Tracker_Remote::handle_vel_change_message
static int VRPN_CALLBACK handle_vel_change_message(void *userdata, vrpn_HANDLERPARAM p)
Definition: vrpn_Tracker.C:1685
vrpn_ser_tkr_MAX_TIME_INTERVAL
#define vrpn_ser_tkr_MAX_TIME_INTERVAL
Definition: vrpn_Tracker.C:38
vrpn_TRACKERACCCB
Definition: vrpn_Tracker.h:311
vrpn_Tracker_USB::_device_handle
struct libusb_device_handle * _device_handle
Definition: vrpn_Tracker.h:188
vrpn_TRACKER_SYNCING
const int vrpn_TRACKER_SYNCING
Definition: vrpn_Tracker.h:35
vrpn_Tracker::frame_count
vrpn_int32 frame_count
Definition: vrpn_Tracker.h:101
vrpn_Tracker_Remote::handle_acc_change_message
static int VRPN_CALLBACK handle_acc_change_message(void *userdata, vrpn_HANDLERPARAM p)
Definition: vrpn_Tracker.C:1730
vrpn_Tracker_USB::reset
virtual void reset(void)=0
Reset the tracker.
vrpn_Tracker_Remote::d_workspacechange_list
vrpn_Callback_List< vrpn_TRACKERWORKSPACECB > d_workspacechange_list
Definition: vrpn_Tracker.h:479
vrpn_Tracker_NULL::mainloop
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
Definition: vrpn_Tracker.C:623
vrpn_TRACKERUNIT2SENSORCB::unit2sensor
vrpn_float64 unit2sensor[3]
Definition: vrpn_Tracker.h:337
vrpn_Tracker_Serial::baudrate
long baudrate
Definition: vrpn_Tracker.h:152
vrpn_Tracker::handle_u2s_request
static int VRPN_CALLBACK handle_u2s_request(void *userdata, vrpn_HANDLERPARAM p)
Definition: vrpn_Tracker.C:394
vrpn_Tracker_USB::_context
struct libusb_context * _context
Definition: vrpn_Tracker.h:189
vrpn_TRACKERACCCB::msg_time
struct timeval msg_time
Definition: vrpn_Tracker.h:312
vrpn_TRACKERWORKSPACECB::msg_time
struct timeval msg_time
Definition: vrpn_Tracker.h:344
vrpn_TRACKERACCCB::sensor
vrpn_int32 sensor
Definition: vrpn_Tracker.h:313
vrpn_Tracker_Remote::handle_change_message
static int VRPN_CALLBACK handle_change_message(void *userdata, vrpn_HANDLERPARAM p)
Definition: vrpn_Tracker.C:1638
vrpn_Tracker_Remote::unregister_change_handler
virtual int unregister_change_handler(void *userdata, vrpn_TRACKERCHANGEHANDLER handler, vrpn_int32 sensor=vrpn_ALL_SENSORS)
Definition: vrpn_Tracker.C:1535
vrpn_BaseClass::init
virtual int init(void)
Initialize things that the constructor can't. Returns 0 on success, -1 on failure.
Definition: vrpn_BaseClass.C:363
vrpn_Tracker::num_unit2sensors
unsigned num_unit2sensors
Definition: vrpn_Tracker.h:120
vrpn_TRACKER_AWAITING_STATION
const int vrpn_TRACKER_AWAITING_STATION
Definition: vrpn_Tracker.h:36
vrpn_Tracker_USB::_product
vrpn_uint16 _product
Definition: vrpn_Tracker.h:191
vrpn_buffer
VRPN_API int vrpn_buffer(char **insertPt, vrpn_int32 *buflen, const timeval t)
Utility routine for placing a timeval struct into a buffer that is to be sent as a message.
Definition: vrpn_Shared.C:241
vrpn_Tracker::tracker2room
vrpn_float64 tracker2room[3]
Definition: vrpn_Tracker.h:113
vrpn_Tracker_Server::vrpn_Tracker_Server
vrpn_Tracker_Server(const char *name, vrpn_Connection *c, vrpn_int32 sensors=1)
Definition: vrpn_Tracker.C:714
vrpn_open_commport
int vrpn_open_commport(const char *portname, long baud, int charsize, vrpn_SER_PARITY parity, bool rts_flow)
Open a serial port, given its name and baud rate.
Definition: vrpn_Serial.C:54
vrpn_TRACKERVELCB::vel
vrpn_float64 vel[3]
Definition: vrpn_Tracker.h:300
vrpn_Tracker::request_t2r_m_id
vrpn_int32 request_t2r_m_id
Definition: vrpn_Tracker.h:85
vrpn_BaseClassUnique::send_text_message
int send_text_message(const char *msg, struct timeval timestamp, vrpn_TEXT_SEVERITY type=vrpn_TEXT_NORMAL, vrpn_uint32 level=0)
Sends a NULL-terminated text message from the device d_sender_id.
Definition: vrpn_BaseClass.C:568
vrpn_Tracker::reset_origin_m_id
vrpn_int32 reset_origin_m_id
Definition: vrpn_Tracker.h:91
vrpn_Tracker_USB::mainloop
virtual void mainloop()
Uses the get_report, send_report, and reset routines to implement a server.
Definition: vrpn_Tracker.C:1064
vrpn_TRACKERVELCHANGEHANDLER
void(VRPN_CALLBACK * vrpn_TRACKERVELCHANGEHANDLER)(void *userdata, const vrpn_TRACKERVELCB info)
Definition: vrpn_Tracker.h:304
vrpn_TRACKERACCCB::acc_quat
vrpn_float64 acc_quat[4]
Definition: vrpn_Tracker.h:315
vrpn_Tracker_Remote::ensure_enough_sensor_callbacks
bool ensure_enough_sensor_callbacks(unsigned num)
Definition: vrpn_Tracker.C:1236
vrpn_Callback_List::call_handlers
void call_handlers(const CALLBACK_STRUCT &info)
This will pass the referenced parameter as a const to all the callbacks.
Definition: vrpn_BaseClass.h:451
vrpn_Tracker::pos
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
vrpn_Tracker::num_sensors
vrpn_int32 num_sensors
Definition: vrpn_Tracker.h:114
vrpn_Callback_List::unregister_handler
int unregister_handler(void *userdata, HANDLER_TYPE handler)
Call this to remove a handler from the list (if it exists)
Definition: vrpn_BaseClass.h:420
vrpn_TRACKERTRACKER2ROOMCB::tracker2room
vrpn_float64 tracker2room[3]
Definition: vrpn_Tracker.h:328
vrpn_Tracker::print_latest_report
void print_latest_report(void)
Definition: vrpn_Tracker.C:306
vrpn_Tracker::unit2sensor_m_id
vrpn_int32 unit2sensor_m_id
Definition: vrpn_Tracker.h:84
vrpn_Tracker::vel_quat_dt
vrpn_float64 vel_quat_dt
Definition: vrpn_Tracker.h:97
vrpn_Tracker::vrpn_Tracker
vrpn_Tracker(const char *name, vrpn_Connection *c=NULL, const char *tracker_cfg_file_name=NULL)
Definition: vrpn_Tracker.C:44
vrpn_Tracker::get_local_t2r
void get_local_t2r(vrpn_float64 *vec, vrpn_float64 *quat)
Definition: vrpn_Tracker.C:346
vrpn_Tracker_Remote::vrpn_Tracker_Remote
vrpn_Tracker_Remote(const char *name, vrpn_Connection *c=NULL)
Definition: vrpn_Tracker.C:1155
vrpn_Tracker_NULL::setRedundantTransmission
void setRedundantTransmission(vrpn_RedundantTransmission *)
Definition: vrpn_Tracker.C:709
vrpn_Tracker::tracker2room_m_id
vrpn_int32 tracker2room_m_id
Definition: vrpn_Tracker.h:83
vrpn_BaseClassUnique::server_mainloop
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
Definition: vrpn_BaseClass.C:603
vrpn_BaseClass
Class from which all user-level (and other) classes that communicate with vrpn_Connections should der...
Definition: vrpn_BaseClass.h:313
vrpn_RedundantTransmission
Definition: vrpn_RedundantTransmission.h:20
vrpn_Tracker_Sensor_Callbacks::d_velchange
vrpn_Callback_List< vrpn_TRACKERVELCB > d_velchange
Definition: vrpn_Tracker.h:356
vrpn_TRACKERWORKSPACECB::workspace_min
vrpn_float64 workspace_min[3]
Definition: vrpn_Tracker.h:345
vrpn_ALL_SENSORS
const int vrpn_ALL_SENSORS
Definition: vrpn_Tracker.h:44
vrpn_Tracker_Serial::get_report
virtual int get_report(void)=0
Gets a report if one is available, returns 0 if not, 1 if complete report.
vrpn_Tracker_Serial::reset
virtual void reset(void)=0
Reset the tracker.
vrpn_BaseClassUnique::handler
vrpn_MESSAGEHANDLER handler
Definition: vrpn_BaseClass.h:284
vrpn_TRACKERVELCB
Definition: vrpn_Tracker.h:297
vrpn_TRACKERTRACKER2ROOMCB
Definition: vrpn_Tracker.h:326
vrpn_Tracker_Remote::handle_workspace_change_message
static int VRPN_CALLBACK handle_workspace_change_message(void *userdata, vrpn_HANDLERPARAM p)
Definition: vrpn_Tracker.C:1852
vrpn_Tracker_Remote::request_workspace
int request_workspace(void)
Definition: vrpn_Tracker.C:1313