/*
Starts a new rigid body definition and creates a tracker for it on
the server. Use addRigidMarker() to add markers to the tracker.
Trackers must be disabled (using enableTracker(false)) before being
modified or created.
*/
bool vrpn_Tracker_PhaseSpace::startNewRigidBody(int sensor)
{
#ifdef DEBUG
printf("%s %d\n", __PRETTY_FUNCTION__, sensor);
#endif
if(!owlRunning) return false;
if(slave) return false;
if(numRigids >= VRPN_PHASESPACE_MAXRIGIDS) {
fprintf(stderr, "error: maximum rigid bodies (%d) exceeded\n", VRPN_PHASESPACE_MAXRIGIDS);
return false;
}
owlTrackeri(++numRigids, OWL_CREATE, OWL_RIGID_TRACKER);
if(!owlGetStatus()) {
numRigids--;
return false;
}
//remember the sensor that this rigid tracker is mapped to.
r2s_map[numRigids] = sensor;
return true;
}
/*
This function must only be called after startNewRigidBody has been called.
*/
bool vrpn_Tracker_PhaseSpace::addRigidMarker(int sensor, int led_id, float x, float y, float z)
{
#ifdef DEBUG
printf("%s %d %d %f %f %f\n", __PRETTY_FUNCTION__, sensor, led_id, x, y, z);
#endif
if(!owlRunning) return false;
if(slave) return false;
if(numRigids == 0) {
fprintf(stderr, "Error: Attempting to add rigid body marker with no rigid body defined.");
return false;
}
if(numMarkers >= VRPN_PHASESPACE_MAXMARKERS) {
fprintf(stderr, "Error: Maximum markers (%d) exceeded.\n", VRPN_PHASESPACE_MAXMARKERS);
return false;
}
float xyz[3];
xyz[0] = x;
xyz[1] = y;
xyz[2] = z;
owlMarkeri(MARKER(numRigids,sensor),OWL_SET_LED,led_id);
owlMarkerfv(MARKER(numRigids,sensor),OWL_SET_POSITION,&xyz[0]);
if(!owlGetStatus())
return false;
numMarkers++;
return true;
}
/*
Enables all the trackers and sets the streaming frequency.
Note: Trackers according to VRPN and Trackers as defined by OWL
are two entirely different things.
*/
bool vrpn_Tracker_PhaseSpace::enableTracker(bool enable)
{
#ifdef DEBUG
printf("%s %d\n", __PRETTY_FUNCTION__, enable);
#endif
if(!owlRunning) return false;
// Scale the reports for this tracker to be in meters rather than
// in MM, to match the VRPN standard.
owlScale(MM_TO_METERS);
if(!slave) {
int option = enable ? OWL_ENABLE : OWL_DISABLE;
owlTracker(0,option); //enables/disables the point tracker
if(!owlGetStatus())
return false;
//enable/disable the rigid trackers
for(int i = 1; i <= numRigids; i++) {
owlTracker(i,option);
if(!owlGetStatus())
return false;
}
}
if(enable) {
setFrequency(frequency);
owlSetInteger(OWL_EVENTS, OWL_ENABLE);
owlSetInteger(OWL_STREAMING,OWL_ENABLE);
if(!owlGetStatus())
return false;
printf("Streaming enabled.\n");
} else {
owlSetInteger(OWL_EVENTS, OWL_DISABLE);
owlSetInteger(OWL_STREAMING,OWL_DISABLE);
if(!owlGetStatus())
return false;
printf("Streaming disabled.\n");
}
return true;
}
vrpn_Tracker_PhaseSpace::vrpn_Tracker_PhaseSpace(const char *name, vrpn_Connection *c, const char* device, float frequency,int readflag, int slave)
: vrpn_Tracker(name,c)
{
#ifdef DEBUG
printf("%s %s %s %f %d\n", __PRETTY_FUNCTION__, name, device, frequency, readflag);
#endif
if(d_connection) {
// Register a handler for the update change callback
if (register_autodeleted_handler(update_rate_id, handle_update_rate_request, this, d_sender_id))
fprintf(stderr,"vrpn_Tracker: Can't register workspace handler\n");
}
this->slave = slave;
this->frequency = frequency;
numRigids = 0;
numMarkers = 0;
markers.reserve(VRPN_PHASESPACE_MAXMARKERS);
rigids.reserve(VRPN_PHASESPACE_MAXRIGIDS);
int owlflag = 0;
if(slave) owlflag |= OWL_SLAVE;
int ret = owlInit(device,owlflag);
if(ret != owlflag) {
fprintf(stderr, "owlInit error: 0x%x\n", ret);
owlRunning = false;
return;
} else {
owlRunning = true;
}
char msg[512];
if(owlGetString(OWL_VERSION,msg)) {
printf("OWL version: %s\n",msg);
} else {
printf("Unable to query OWL version.\n");
}
if(!slave) {
//The master point tracker is index 0. So all rigid trackers will start from 1.
owlTrackeri(0, OWL_CREATE, OWL_POINT_TRACKER);
if(!owlGetStatus()) {
fprintf(stderr,"Error: Unable to create main point tracker.\n");
return;
}
} else {
printf("Ignoring tracker creation in slave mode.\n");
}
readMostRecent = readflag ? true : false;
frame = 0;
}
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;
}
bool vrpn_Tracker_PhaseSpace::addMarker(int sensor,int led_id)
{
#ifdef DEBUG
printf("%s %d %d\n", __PRETTY_FUNCTION__, sensor, led_id);
#endif
if(!owlRunning) return false;
if(slave) return false;
if(numMarkers >= VRPN_PHASESPACE_MAXMARKERS) {
fprintf(stderr, "Error: Maximum markers (%d) exceeded.\n", VRPN_PHASESPACE_MAXMARKERS);
return false;
}
owlMarkeri(MARKER(0,sensor),OWL_SET_LED,led_id);
if(!owlGetStatus())
return false;
numMarkers++;
return true;
}
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);
}