void PhasespaceCore::readAndPublish() {
// queries the server for new markers data and returns the number of current active markers (0 means old data)
int num_markers = owlGetMarkers(markers_, init_marker_count_);
if (owlGetError() != OWL_NO_ERROR) {
ROS_FATAL_STREAM("[PhaseSpace] Error while reading markers' positions: " << owlGetError());
throw excp_;
}
// waits for available data
if (num_markers == 0) {
return;
}
ros::Time current_time = ros::Time::now();
// please, make sure that a node does not use both this function and 'gloveMessageCallback(&msg)' one
num_data_retrieved_++;
partial_num_data_retrieved_++;
// counts how many markers are visible at the moment
int* markers_count = markers_count_->data();
int num_visible_markers = 0;
for (int i=0; i<num_markers; i++) {
if (markers_[i].cond > 0) {
markers_count[i]++;
num_visible_markers++;
}
}
publishMessage(current_time, num_visible_markers, num_markers);
}
int vrpn_Tracker_PhaseSpace::read_frame(void)
{
int ret = 0;
char buffer[1024];
OWLEvent e = owlGetEvent();
switch(e.type) {
case 0:
break;
case OWL_DONE:
owlRunning = false;
send_text_message("owl stopped\n", timestamp, vrpn_TEXT_ERROR);
return 0;
case OWL_FREQUENCY:
owlGetFloatv(OWL_FREQUENCY, &frequency);
fprintf(stdout, "frequency: %d\n", frequency);
break;
case OWL_BUTTONS: break;
case OWL_MARKERS:
markers.resize(VRPN_PHASESPACE_MAXMARKERS);
ret = owlGetMarkers(&markers.front(), markers.size());
if(ret > 0) markers.resize(ret);
break;
case OWL_RIGIDS:
rigids.resize(VRPN_PHASESPACE_MAXRIGIDS);
ret = owlGetRigids(&rigids.front(), rigids.size());
if(ret > 0) rigids.resize(ret);
break;
case OWL_COMMDATA:
owlGetString(OWL_COMMDATA, buffer);
break;
case OWL_TIMESTAMP:
owlGetIntegerv(OWL_TIMESTAMP, &ret);
break;
case OWL_PLANES:
planes.resize(VRPN_PHASESPACE_MAXPLANES);
ret = owlGetPlanes(&planes.front(), planes.size());
if(ret > 0) planes.resize(ret);
break;
case OWL_DETECTORS:
detectors.resize(VRPN_PHASESPACE_MAXDETECTORS);
ret = owlGetDetectors(&detectors.front(), detectors.size());
if(ret > 0) detectors.resize(ret);
break;
case OWL_PEAKS:
peaks.resize(VRPN_PHASESPACE_MAXPEAKS);
ret = owlGetPeaks(&peaks.front(), peaks.size());
if(ret > 0) peaks.resize(ret);
break;
case OWL_IMAGES:
images.resize(VRPN_PHASESPACE_MAXIMAGES);
ret = owlGetImages(&images.front(), images.size());
if(ret > 0) images.resize(ret);
break;
case OWL_CAMERAS:
cameras.resize(VRPN_PHASESPACE_MAXCAMERAS);
ret = owlGetCameras(&cameras.front(), cameras.size());
if(ret > 0) cameras.resize(ret);
break;
case OWL_STATUS_STRING: break;
case OWL_CUSTOM_STRING: break;
case OWL_FRAME_NUMBER:
if(owlGetIntegerv(OWL_FRAME_NUMBER, &ret))
frame = ret;
break;
default:
fprintf(stderr, "unsupported OWLEvent type: 0x%x\n", e.type);
break;
}
return e.type;
}
