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;
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
virtual void send_report(void)
VRPN_API int vrpn_unbuffer(const char **buffer, timeval *t)
Utility routine for taking a struct timeval from a buffer that was sent as a message.
vrpn_int32 update_rate_id
bool addRigidMarker(int sensor, int led_id, float x, float y, float z)
std::vector< OWLPeak > peaks
#define VRPN_PHASESPACE_MAXDETECTORS
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
std::vector< OWLCamera > cameras
static int VRPN_CALLBACK handle_update_rate_request(void *userdata, vrpn_HANDLERPARAM p)
std::vector< OWLDetectors > detectors
Generic connection class not specific to the transport mechanism.
bool startNewRigidBody(int sensor)
#define VRPN_PHASESPACE_MAXCAMERAS
#define VRPN_PHASESPACE_MSGBUFSIZE
#define VRPN_PHASESPACE_MAXRIGIDS
int register_autodeleted_handler(vrpn_int32 type, vrpn_MESSAGEHANDLER handler, void *userdata, vrpn_int32 sender=vrpn_ANY_SENDER)
Registers a handler with the connection, and remembers to delete at destruction.
virtual int get_report(void)
vrpn_Connection * d_connection
Connection that this object talks to.
This structure is what is passed to a vrpn_Connection message callback.
virtual int pack_message(vrpn_uint32 len, struct timeval time, vrpn_int32 type, vrpn_int32 sender, const char *buffer, vrpn_uint32 class_of_service)
Pack a message that will be sent the next time mainloop() is called. Turn off the RELIABLE flag if yo...
int send_text_message(const char *msg, struct timeval timestamp, vrpn_TEXT_SEVERITY type=vrpn_TEXT_NORMAL, vrpn_uint32 level=0)
Sends a NULL-terminated text message from the device d_sender_id.
std::vector< OWLMarker > markers
virtual int encode_to(char *buf)
void setFrequency(float freq)
std::vector< OWLRigid > rigids
#define VRPN_PHASESPACE_MAXPLANES
bool addMarker(int sensor, int led_id)
std::vector< OWLImage > images
bool enableTracker(bool enable)
#define VRPN_PHASESPACE_MAXIMAGES
void frame_to_time(int frame, float frequency, struct timeval &time)
vrpn_int32 d_sender_id
Sender ID registered with the connection.
~vrpn_Tracker_PhaseSpace()
#define VRPN_PHASESPACE_MAXPEAKS
#define VRPN_PHASESPACE_MAXMARKERS
vrpn_Tracker_PhaseSpace(const char *name, vrpn_Connection *c, const char *device, float frequency, int readflag, int slaveflag=0)
std::vector< OWLPlane > planes