/* 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; }
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 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); }