void vrpn_Tracker_PhaseSpace::setFrequency(float freq)
{
#ifdef DEBUG
printf("%s %f\n", __PRETTY_FUNCTION__, freq);
#endif
if(freq < 0 || freq > OWL_MAX_FREQUENCY) {
fprintf(stderr,"Error: Invalid frequency requested, defaulting to %f hz\n", OWL_MAX_FREQUENCY);
freq = OWL_MAX_FREQUENCY;
}
frequency = freq;
if(owlRunning) {
float tmp = -1;
owlSetFloat(OWL_FREQUENCY, frequency);
owlGetFloatv(OWL_FREQUENCY, &tmp);
if(!owlGetStatus() || tmp == -1)
fprintf(stderr,"Error: unable to set frequency\n");
}
return;
}
void PhasespaceCore::initializeCommunication() {
ROS_INFO_STREAM("[PhaseSpace] Initalizing the PhaseSpace...");
// opens a socket and configures the communication channels to pass data between the OWL server and client; this
// function will block until there is a connection or an error; returns the passed flags if OK
if (owlInit(server_ip_.c_str(), init_flags_) < 0) {
ROS_FATAL_STREAM("[PhaseSpace] Can't initialize the communication with OWL server: " << owlGetError());
throw excp_;
}
// initializes a point tracker:
// - OWL_CREATE: tells the system to create a tracker
// - OWL_POINT_TRACKER: specifies the creation of a point tracker
owlTrackeri(tracker_, OWL_CREATE, OWL_POINT_TRACKER);
// creates the marker structures, each with the proper id:
// - MARKER Macro: builds a marker id out of a tracker id and marker index
// - OWL_SET_LED: the following 'i' is an integer representing the LED ID of the marker
for (int i=0; i<init_marker_count_; i++) {
owlMarkeri(MARKER(tracker_, i), OWL_SET_LED, i);
}
// enables the tracker (it has to be disabled when the markers have to be added)
owlTracker(tracker_, OWL_ENABLE);
// checking the status will block until all commands are processed and any errors are sent to the client
if (!owlGetStatus()) {
ROS_FATAL_STREAM("[PhaseSpace] Initialization generic error: " << owlGetError());
throw excp_;
}
// sets frequency with default maximum value (OWL_MAX_FREQUENCY = 480 Hz):
// - OWL_FREQUENCY: specifies the rate at which the server streams data
owlSetFloat(OWL_FREQUENCY, OWL_MAX_FREQUENCY);
// enables the streaming of data
owlSetInteger(OWL_STREAMING, OWL_ENABLE);
}
void
phasespace_update_frequency()
{
owlSetFloat(OWL_FREQUENCY, system_frequency());
}
