vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Tracker_PhaseSpace.C
Go to the documentation of this file.
2 
3 #define MM_TO_METERS (0.001)
4 
5 #ifdef VRPN_INCLUDE_PHASESPACE
6 //#define DEBUG
7 
8 //
9 inline void frame_to_time(int frame, float frequency, struct timeval &time) {
10  float t = frame / frequency;
11  time.tv_sec = int(t);
12  time.tv_usec = int((t - time.tv_sec)*1E6);
13 }
14 
15 //
16 vrpn_Tracker_PhaseSpace::vrpn_Tracker_PhaseSpace(const char *name, vrpn_Connection *c, const char* device, float frequency,int readflag, int slave)
17  : vrpn_Tracker(name,c)
18 {
19 #ifdef DEBUG
20  printf("%s %s %s %f %d\n", __PRETTY_FUNCTION__, name, device, frequency, readflag);
21 #endif
22 
23  if(d_connection) {
24  // Register a handler for the update change callback
26  fprintf(stderr,"vrpn_Tracker: Can't register workspace handler\n");
27  }
28 
29  this->slave = slave;
30  this->frequency = frequency;
31 
32  numRigids = 0;
33  numMarkers = 0;
36 
37  int owlflag = 0;
38  if(slave) owlflag |= OWL_SLAVE;
39 
40  int ret = owlInit(device,owlflag);
41  if(ret != owlflag) {
42  fprintf(stderr, "owlInit error: 0x%x\n", ret);
43  owlRunning = false;
44  return;
45  } else {
46  owlRunning = true;
47  }
48 
49  char msg[512];
50  if(owlGetString(OWL_VERSION,msg)) {
51  printf("OWL version: %s\n",msg);
52  } else {
53  printf("Unable to query OWL version.\n");
54  }
55 
56  if(!slave) {
57  //The master point tracker is index 0. So all rigid trackers will start from 1.
58  owlTrackeri(0, OWL_CREATE, OWL_POINT_TRACKER);
59  if(!owlGetStatus()) {
60  fprintf(stderr,"Error: Unable to create main point tracker.\n");
61  return;
62  }
63  } else {
64  printf("Ignoring tracker creation in slave mode.\n");
65  }
66 
67  readMostRecent = readflag ? true : false;
68  frame = 0;
69 }
70 
71 //
73 {
74 #ifdef DEBUG
75  printf("%s\n", __PRETTY_FUNCTION__);
76 #endif
77 
78  if(owlRunning)
79  owlDone();
80 }
81 
82 
83 // This function should be called each time through the main loop
84 // of the server code. It polls for data from the OWL server and
85 // sends them if available.
87 {
88  get_report();
89 
90  // Call the generic server mainloop, since we are a server
92  return;
93 }
94 
95 //
96 bool vrpn_Tracker_PhaseSpace::addMarker(int sensor,int led_id)
97 {
98 #ifdef DEBUG
99  printf("%s %d %d\n", __PRETTY_FUNCTION__, sensor, led_id);
100 #endif
101 
102  if(!owlRunning) return false;
103  if(slave) return false;
104 
106  fprintf(stderr, "Error: Maximum markers (%d) exceeded.\n", VRPN_PHASESPACE_MAXMARKERS);
107  return false;
108  }
109 
110  owlMarkeri(MARKER(0,sensor),OWL_SET_LED,led_id);
111 
112  if(!owlGetStatus())
113  return false;
114 
115  numMarkers++;
116  return true;
117 }
118 
119 /*
120 This function must only be called after startNewRigidBody has been called.
121 */
122 bool vrpn_Tracker_PhaseSpace::addRigidMarker(int sensor, int led_id, float x, float y, float z)
123 {
124 #ifdef DEBUG
125  printf("%s %d %d %f %f %f\n", __PRETTY_FUNCTION__, sensor, led_id, x, y, z);
126 #endif
127 
128  if(!owlRunning) return false;
129  if(slave) return false;
130 
131  if(numRigids == 0) {
132  fprintf(stderr, "Error: Attempting to add rigid body marker with no rigid body defined.");
133  return false;
134  }
136  fprintf(stderr, "Error: Maximum markers (%d) exceeded.\n", VRPN_PHASESPACE_MAXMARKERS);
137  return false;
138  }
139 
140  float xyz[3];
141  xyz[0] = x;
142  xyz[1] = y;
143  xyz[2] = z;
144 
145  owlMarkeri(MARKER(numRigids,sensor),OWL_SET_LED,led_id);
146  owlMarkerfv(MARKER(numRigids,sensor),OWL_SET_POSITION,&xyz[0]);
147 
148  if(!owlGetStatus())
149  return false;
150 
151  numMarkers++;
152  return true;
153 }
154 
155 /*
156 Starts a new rigid body definition and creates a tracker for it on
157 the server. Use addRigidMarker() to add markers to the tracker.
158 Trackers must be disabled (using enableTracker(false)) before being
159 modified or created.
160 */
162 {
163 #ifdef DEBUG
164  printf("%s %d\n", __PRETTY_FUNCTION__, sensor);
165 #endif
166 
167  if(!owlRunning) return false;
168  if(slave) return false;
169 
171  fprintf(stderr, "error: maximum rigid bodies (%d) exceeded\n", VRPN_PHASESPACE_MAXRIGIDS);
172  return false;
173  }
174 
175  owlTrackeri(++numRigids, OWL_CREATE, OWL_RIGID_TRACKER);
176 
177  if(!owlGetStatus()) {
178  numRigids--;
179  return false;
180  }
181 
182  //remember the sensor that this rigid tracker is mapped to.
183  r2s_map[numRigids] = sensor;
184  return true;
185 }
186 
187 /*
188 Enables all the trackers and sets the streaming frequency.
189 Note: Trackers according to VRPN and Trackers as defined by OWL
190 are two entirely different things.
191 */
193 {
194 #ifdef DEBUG
195  printf("%s %d\n", __PRETTY_FUNCTION__, enable);
196 #endif
197 
198  if(!owlRunning) return false;
199 
200  // Scale the reports for this tracker to be in meters rather than
201  // in MM, to match the VRPN standard.
202  owlScale(MM_TO_METERS);
203 
204  if(!slave) {
205  int option = enable ? OWL_ENABLE : OWL_DISABLE;
206  owlTracker(0,option); //enables/disables the point tracker
207  if(!owlGetStatus())
208  return false;
209 
210  //enable/disable the rigid trackers
211  for(int i = 1; i <= numRigids; i++) {
212  owlTracker(i,option);
213  if(!owlGetStatus())
214  return false;
215  }
216  }
217 
218  if(enable) {
220  owlSetInteger(OWL_EVENTS, OWL_ENABLE);
221  owlSetInteger(OWL_STREAMING,OWL_ENABLE);
222  if(!owlGetStatus())
223  return false;
224  printf("Streaming enabled.\n");
225  } else {
226  owlSetInteger(OWL_EVENTS, OWL_DISABLE);
227  owlSetInteger(OWL_STREAMING,OWL_DISABLE);
228  if(!owlGetStatus())
229  return false;
230  printf("Streaming disabled.\n");
231  }
232  return true;
233 }
234 
235 int debugcounter = 0;
236 
237 //
239 {
240  int ret = 0;
241  char buffer[1024];
242  OWLEvent e = owlGetEvent();
243  switch(e.type) {
244  case 0:
245  break;
246  case OWL_DONE:
247  owlRunning = false;
248  send_text_message("owl stopped\n", timestamp, vrpn_TEXT_ERROR);
249  return 0;
250  case OWL_FREQUENCY:
251  owlGetFloatv(OWL_FREQUENCY, &frequency);
252  fprintf(stdout, "frequency: %d\n", frequency);
253  break;
254  case OWL_BUTTONS: break;
255  case OWL_MARKERS:
257  ret = owlGetMarkers(&markers.front(), markers.size());
258  if(ret > 0) markers.resize(ret);
259  break;
260  case OWL_RIGIDS:
262  ret = owlGetRigids(&rigids.front(), rigids.size());
263  if(ret > 0) rigids.resize(ret);
264  break;
265  case OWL_COMMDATA:
266  owlGetString(OWL_COMMDATA, buffer);
267  break;
268  case OWL_TIMESTAMP:
269  owlGetIntegerv(OWL_TIMESTAMP, &ret);
270  break;
271  case OWL_PLANES:
273  ret = owlGetPlanes(&planes.front(), planes.size());
274  if(ret > 0) planes.resize(ret);
275  break;
276  case OWL_DETECTORS:
278  ret = owlGetDetectors(&detectors.front(), detectors.size());
279  if(ret > 0) detectors.resize(ret);
280  break;
281  case OWL_PEAKS:
283  ret = owlGetPeaks(&peaks.front(), peaks.size());
284  if(ret > 0) peaks.resize(ret);
285  break;
286  case OWL_IMAGES:
288  ret = owlGetImages(&images.front(), images.size());
289  if(ret > 0) images.resize(ret);
290  break;
291  case OWL_CAMERAS:
293  ret = owlGetCameras(&cameras.front(), cameras.size());
294  if(ret > 0) cameras.resize(ret);
295  break;
296  case OWL_STATUS_STRING: break;
297  case OWL_CUSTOM_STRING: break;
298  case OWL_FRAME_NUMBER:
299  if(owlGetIntegerv(OWL_FRAME_NUMBER, &ret))
300  frame = ret;
301  break;
302  default:
303  fprintf(stderr, "unsupported OWLEvent type: 0x%x\n", e.type);
304  break;
305  }
306  return e.type;
307 }
308 
309 //
311 {
312  if(!owlRunning) return 0;
313 
314  int maxiter = 1;
315  if(readMostRecent) maxiter = 1024; // not technically most recent, but if the client is slow, avoids infinite loop.
316 
317  int ret = 1;
318  int oldframe = frame;
319  for(int i = 0; i < maxiter && ret; i++) {
320  int cframe = frame;
321  while(ret && cframe == frame) {
322  ret = read_frame();
323  }
324  }
325  // no new data? abort.
326  if(oldframe == frame)
327  return 0;
328 
329 #ifdef DEBUG
330  char buffer[1024];
331  owlGetString(OWL_FRAME_BUFFER_SIZE, buffer);
332  printf("%s\n", buffer);
333 #endif
334 
335  for(int i = 0; i < markers.size(); i++)
336  {
337  if(markers[i].cond <= 0) continue;
338 
339  //set the sensor
340  d_sensor = markers[i].id;
341 
342  pos[0] = markers[i].x;
343  pos[1] = markers[i].y;
344  pos[2] = markers[i].z;
345 
346  //raw positions have no rotation
347  d_quat[0] = 0;
348  d_quat[1] = 0;
349  d_quat[2] = 0;
350  d_quat[3] = 1;
351 
352  // send time out in OWL time
354  else memset(&timestamp, 0, sizeof(timestamp));
355 
356  //send the report
357  send_report();
358  }
359  for(int j = 0; j < rigids.size(); j++)
360  {
361  if(rigids[j].cond <= 0) continue;
362 
363  //set the sensor
364  d_sensor = r2s_map[rigids[j].id];
365  if(slave && d_sensor == 0) {
366  // rigid bodies aren't allowed to be sensor zero in slave mode
367  r2s_map[rigids[j].id] = rigids[j].id;
368  }
369 
370  //set the position
371  pos[0] = rigids[j].pose[0];
372  pos[1] = rigids[j].pose[1];
373  pos[2] = rigids[j].pose[2];
374 
375  //set the orientation quaternion
376  //OWL has the scale factor first, whereas VRPN has it last.
377  d_quat[0] = rigids[j].pose[4];
378  d_quat[1] = rigids[j].pose[5];;
379  d_quat[2] = rigids[j].pose[6];;
380  d_quat[3] = rigids[j].pose[3];;
381 
382  // send time out in OWL time
384  else memset(&timestamp, 0, sizeof(timestamp));
385 
386  //send the report
387  send_report();
388  }
389 
390  return markers.size() || rigids.size() > 0 ? 1 : 0;
391 }
392 
393 //
395 {
396  if(d_connection)
397  {
398  char msgbuf[VRPN_PHASESPACE_MSGBUFSIZE];
399  int len = encode_to(msgbuf);
402  fprintf(stderr,"PhaseSpace: cannot write message: tossing\n");
403  }
404  }
405 }
406 
407 //
409 {
410 #ifdef DEBUG
411  printf("%s %f\n", __PRETTY_FUNCTION__, freq);
412 #endif
413  if(freq < 0 || freq > OWL_MAX_FREQUENCY) {
414  fprintf(stderr,"Error: Invalid frequency requested, defaulting to %f hz\n", OWL_MAX_FREQUENCY);
415  freq = OWL_MAX_FREQUENCY;
416  }
417 
418  frequency = freq;
419 
420  if(owlRunning) {
421  float tmp = -1;
422  owlSetFloat(OWL_FREQUENCY, frequency);
423  owlGetFloatv(OWL_FREQUENCY, &tmp);
424  if(!owlGetStatus() || tmp == -1)
425  fprintf(stderr,"Error: unable to set frequency\n");
426  }
427  return;
428 }
429 
430 //
432 {
433 #ifdef DEBUG
434  printf("%s\n", __PRETTY_FUNCTION__);
435 #endif
437  vrpn_float64 update_rate = 0;
438  vrpn_unbuffer(&p.buffer,&update_rate);
439  thistracker->setFrequency((float) update_rate);
440  return 0;
441 }
442 
443 #endif
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_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
Definition: vrpn_Tracker.h:49
VRPN_PHASESPACE_MAXMARKERS
#define VRPN_PHASESPACE_MAXMARKERS
Definition: vrpn_Tracker_PhaseSpace.h:18
vrpn_Tracker_PhaseSpace::peaks
std::vector< OWLPeak > peaks
Definition: vrpn_Tracker_PhaseSpace.h:67
vrpn_Tracker::encode_to
virtual int encode_to(char *buf)
Definition: vrpn_Tracker.C:533
vrpn_Tracker_PhaseSpace::slave
bool slave
Definition: vrpn_Tracker_PhaseSpace.h:58
vrpn_Tracker::d_sensor
vrpn_int32 d_sensor
Definition: vrpn_Tracker.h:94
vrpn_Tracker_PhaseSpace::r2s_map
RigidToSensorMap r2s_map
Definition: vrpn_Tracker_PhaseSpace.h:62
vrpn_BaseClassUnique::userdata
void * userdata
Definition: vrpn_BaseClass.h:287
vrpn_Tracker::d_quat
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
VRPN_PHASESPACE_MAXDETECTORS
#define VRPN_PHASESPACE_MAXDETECTORS
Definition: vrpn_Tracker_PhaseSpace.h:21
vrpn_Tracker_PhaseSpace::mainloop
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
Definition: vrpn_Tracker_PhaseSpace.C:86
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_PhaseSpace::startNewRigidBody
bool startNewRigidBody(int sensor)
Definition: vrpn_Tracker_PhaseSpace.C:161
vrpn_CONNECTION_LOW_LATENCY
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
Definition: vrpn_Connection.h:122
vrpn_Tracker_PhaseSpace::read_frame
int read_frame(void)
Definition: vrpn_Tracker_PhaseSpace.C:238
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_Tracker_PhaseSpace::frame
int frame
Definition: vrpn_Tracker_PhaseSpace.h:59
debugcounter
int debugcounter
Definition: vrpn_Tracker_PhaseSpace.C:235
vrpn_Tracker_PhaseSpace::handle_update_rate_request
static int VRPN_CALLBACK handle_update_rate_request(void *userdata, vrpn_HANDLERPARAM p)
Definition: vrpn_Tracker_PhaseSpace.C:431
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_TEXT_ERROR
Definition: vrpn_BaseClass.h:103
vrpn_HANDLERPARAM
This structure is what is passed to a vrpn_Connection message callback.
Definition: vrpn_Connection.h:44
VRPN_PHASESPACE_MAXCAMERAS
#define VRPN_PHASESPACE_MAXCAMERAS
Definition: vrpn_Tracker_PhaseSpace.h:20
vrpn_Tracker_PhaseSpace::frequency
float frequency
Definition: vrpn_Tracker_PhaseSpace.h:56
vrpn_Tracker_PhaseSpace::numRigids
int numRigids
Definition: vrpn_Tracker_PhaseSpace.h:53
vrpn_Tracker_PhaseSpace::vrpn_Tracker_PhaseSpace
vrpn_Tracker_PhaseSpace(const char *name, vrpn_Connection *c, const char *device, float frequency, int readflag, int slaveflag=0)
Definition: vrpn_Tracker_PhaseSpace.C:16
VRPN_PHASESPACE_MAXRIGIDS
#define VRPN_PHASESPACE_MAXRIGIDS
Definition: vrpn_Tracker_PhaseSpace.h:19
vrpn_Tracker_PhaseSpace::enableTracker
bool enableTracker(bool enable)
Definition: vrpn_Tracker_PhaseSpace.C:192
vrpn_BaseClassUnique::d_sender_id
vrpn_int32 d_sender_id
Sender ID registered with the connection.
Definition: vrpn_BaseClass.h:228
vrpn_Tracker_PhaseSpace::numMarkers
int numMarkers
Definition: vrpn_Tracker_PhaseSpace.h:54
VRPN_PHASESPACE_MSGBUFSIZE
#define VRPN_PHASESPACE_MSGBUFSIZE
Definition: vrpn_Tracker_PhaseSpace.h:25
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_Tracker_PhaseSpace::addMarker
bool addMarker(int sensor, int led_id)
Definition: vrpn_Tracker_PhaseSpace.C:96
vrpn_Tracker_PhaseSpace::detectors
std::vector< OWLDetectors > detectors
Definition: vrpn_Tracker_PhaseSpace.h:69
vrpn_Tracker_PhaseSpace::get_report
virtual int get_report(void)
Definition: vrpn_Tracker_PhaseSpace.C:310
VRPN_PHASESPACE_MAXIMAGES
#define VRPN_PHASESPACE_MAXIMAGES
Definition: vrpn_Tracker_PhaseSpace.h:24
vrpn_Tracker_PhaseSpace::readMostRecent
bool readMostRecent
Definition: vrpn_Tracker_PhaseSpace.h:57
vrpn_Tracker_PhaseSpace::~vrpn_Tracker_PhaseSpace
~vrpn_Tracker_PhaseSpace()
Definition: vrpn_Tracker_PhaseSpace.C:72
vrpn_Tracker_PhaseSpace::cameras
std::vector< OWLCamera > cameras
Definition: vrpn_Tracker_PhaseSpace.h:65
VRPN_PHASESPACE_MAXPLANES
#define VRPN_PHASESPACE_MAXPLANES
Definition: vrpn_Tracker_PhaseSpace.h:22
frame_to_time
void frame_to_time(int frame, float frequency, struct timeval &time)
Definition: vrpn_Tracker_PhaseSpace.C:9
vrpn_Tracker_PhaseSpace::send_report
virtual void send_report(void)
Definition: vrpn_Tracker_PhaseSpace.C:394
vrpn_Tracker_PhaseSpace.h
vrpn_Tracker_PhaseSpace::markers
std::vector< OWLMarker > markers
Definition: vrpn_Tracker_PhaseSpace.h:63
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_PhaseSpace::images
std::vector< OWLImage > images
Definition: vrpn_Tracker_PhaseSpace.h:68
MM_TO_METERS
#define MM_TO_METERS
Definition: vrpn_Tracker_PhaseSpace.C:3
vrpn_Tracker_PhaseSpace::setFrequency
void setFrequency(float freq)
Definition: vrpn_Tracker_PhaseSpace.C:408
vrpn_Tracker::pos
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
vrpn_Tracker_PhaseSpace::addRigidMarker
bool addRigidMarker(int sensor, int led_id, float x, float y, float z)
Definition: vrpn_Tracker_PhaseSpace.C:122
VRPN_PHASESPACE_MAXPEAKS
#define VRPN_PHASESPACE_MAXPEAKS
Definition: vrpn_Tracker_PhaseSpace.h:23
vrpn_Tracker_PhaseSpace::rigids
std::vector< OWLRigid > rigids
Definition: vrpn_Tracker_PhaseSpace.h:64
vrpn_Tracker_PhaseSpace
Definition: vrpn_Tracker_PhaseSpace.h:27
vrpn_Tracker_PhaseSpace::planes
std::vector< OWLPlane > planes
Definition: vrpn_Tracker_PhaseSpace.h:66
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_Tracker_PhaseSpace::owlRunning
bool owlRunning
Definition: vrpn_Tracker_PhaseSpace.h:55