3 #define MM_TO_METERS (0.001)
5 #ifdef VRPN_INCLUDE_PHASESPACE
9 inline void frame_to_time(
int frame,
float frequency,
struct timeval &time) {
10 float t = frame / frequency;
12 time.tv_usec = int((t - time.tv_sec)*1E6);
20 printf(
"%s %s %s %f %d\n", __PRETTY_FUNCTION__, name, device,
frequency, readflag);
26 fprintf(stderr,
"vrpn_Tracker: Can't register workspace handler\n");
38 if(
slave) owlflag |= OWL_SLAVE;
40 int ret = owlInit(device,owlflag);
42 fprintf(stderr,
"owlInit error: 0x%x\n", ret);
50 if(owlGetString(OWL_VERSION,msg)) {
51 printf(
"OWL version: %s\n",msg);
53 printf(
"Unable to query OWL version.\n");
58 owlTrackeri(0, OWL_CREATE, OWL_POINT_TRACKER);
60 fprintf(stderr,
"Error: Unable to create main point tracker.\n");
64 printf(
"Ignoring tracker creation in slave mode.\n");
75 printf(
"%s\n", __PRETTY_FUNCTION__);
99 printf(
"%s %d %d\n", __PRETTY_FUNCTION__, sensor, led_id);
103 if(
slave)
return false;
110 owlMarkeri(MARKER(0,sensor),OWL_SET_LED,led_id);
125 printf(
"%s %d %d %f %f %f\n", __PRETTY_FUNCTION__, sensor, led_id, x, y, z);
129 if(
slave)
return false;
132 fprintf(stderr,
"Error: Attempting to add rigid body marker with no rigid body defined.");
145 owlMarkeri(MARKER(
numRigids,sensor),OWL_SET_LED,led_id);
146 owlMarkerfv(MARKER(
numRigids,sensor),OWL_SET_POSITION,&xyz[0]);
164 printf(
"%s %d\n", __PRETTY_FUNCTION__, sensor);
168 if(
slave)
return false;
175 owlTrackeri(++
numRigids, OWL_CREATE, OWL_RIGID_TRACKER);
177 if(!owlGetStatus()) {
195 printf(
"%s %d\n", __PRETTY_FUNCTION__, enable);
205 int option = enable ? OWL_ENABLE : OWL_DISABLE;
206 owlTracker(0,option);
212 owlTracker(i,option);
220 owlSetInteger(OWL_EVENTS, OWL_ENABLE);
221 owlSetInteger(OWL_STREAMING,OWL_ENABLE);
224 printf(
"Streaming enabled.\n");
226 owlSetInteger(OWL_EVENTS, OWL_DISABLE);
227 owlSetInteger(OWL_STREAMING,OWL_DISABLE);
230 printf(
"Streaming disabled.\n");
242 OWLEvent e = owlGetEvent();
252 fprintf(stdout,
"frequency: %d\n",
frequency);
254 case OWL_BUTTONS:
break;
258 if(ret > 0)
markers.resize(ret);
263 if(ret > 0)
rigids.resize(ret);
266 owlGetString(OWL_COMMDATA, buffer);
269 owlGetIntegerv(OWL_TIMESTAMP, &ret);
274 if(ret > 0)
planes.resize(ret);
283 ret = owlGetPeaks(&
peaks.front(),
peaks.size());
284 if(ret > 0)
peaks.resize(ret);
289 if(ret > 0)
images.resize(ret);
294 if(ret > 0)
cameras.resize(ret);
296 case OWL_STATUS_STRING:
break;
297 case OWL_CUSTOM_STRING:
break;
298 case OWL_FRAME_NUMBER:
299 if(owlGetIntegerv(OWL_FRAME_NUMBER, &ret))
303 fprintf(stderr,
"unsupported OWLEvent type: 0x%x\n", e.type);
318 int oldframe =
frame;
319 for(
int i = 0; i < maxiter && ret; i++) {
321 while(ret && cframe ==
frame) {
326 if(oldframe ==
frame)
331 owlGetString(OWL_FRAME_BUFFER_SIZE, buffer);
332 printf(
"%s\n", buffer);
335 for(
int i = 0; i <
markers.size(); i++)
337 if(
markers[i].cond <= 0)
continue;
359 for(
int j = 0; j <
rigids.size(); j++)
361 if(
rigids[j].cond <= 0)
continue;
402 fprintf(stderr,
"PhaseSpace: cannot write message: tossing\n");
411 printf(
"%s %f\n", __PRETTY_FUNCTION__, freq);
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;
423 owlGetFloatv(OWL_FREQUENCY, &tmp);
424 if(!owlGetStatus() || tmp == -1)
425 fprintf(stderr,
"Error: unable to set frequency\n");
434 printf(
"%s\n", __PRETTY_FUNCTION__);
437 vrpn_float64 update_rate = 0;