vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
vrpn_Tracker_NDI_Polaris.C
Go to the documentation of this file.
1
2#include <stdio.h> // for fprintf, printf, sprintf, etc
3#include <string.h> // for strncmp, strlen, strncpy
4
5#include "vrpn_Connection.h" // for vrpn_CONNECTION_LOW_LATENCY, etc
6#include "vrpn_Serial.h" // for vrpn_close_commport, etc
7#include "vrpn_Shared.h" // for vrpn_SleepMsecs, etc
8#include "vrpn_Tracker.h" // for vrpn_Tracker
10#include "vrpn_Types.h" // for vrpn_float64
11
14 const char *port,
15 int numOfRigidBodies,
16 const char** rigidBodyNDIRomFileNames) : vrpn_Tracker(name,c)
17{
19 //STEP 1: open com port at NDI's default speed
21
23 if (serialFd==-1){
24 fprintf(stderr,"vrpn_Tracker_NDI_Polaris: Can't open serial port: %s\n",port);
25 } else {
26
27 printf("connected to NDI Polaris at default 9600 baud on device:%s.\n",port);
28
29 //send a reset
30#ifdef DEBUG
31 printf("DEBUG: Reseting com port");
32#endif
34 vrpn_SleepMsecs(100);
36 printf("done\n");
37
38 vrpn_SleepMsecs(100); //if the NDI was previously running at the higher rate, it will need some time to reset
39 vrpn_flush_input_buffer(serialFd); //get rid of any reset message the NDI might have sent
40
42 //STEP 2: switch to a higher baud rate
45
47 //STEP 3: INIT tracker
49
50
51 sendCommand("INIT ");
53#ifdef DEBUG
54 printf("DEBUG:Init response: >%s<\n",latestResponseStr);
55#endif
56
57 //0 = 20hz(default), 1= 30Hz, 2=60Hz
58 sendCommand("IRATE 0"); //set the illumination to the default
60#ifdef DEBUG
61 printf("DEBUG: IRATE response: >%s<\n",latestResponseStr);
62#endif
63
64
66 //STEP 4: SETUP EACH TOOL (i.e. rigid body)
68
69 this->numOfRigidBodies=numOfRigidBodies;
70
71 //declare an array of filenames, one for each tool
72 for (int t=0; t<numOfRigidBodies; t++) {
73 if (setupOneTool(rigidBodyNDIRomFileNames[t])<1) {
74 fprintf(stderr,"vrpn_Tracker_NDI_Polaris: tool %s didn't init properly!\n",rigidBodyNDIRomFileNames[t]);
75 }
76 }
77
79 //STEP 5: GO TO TRACKING MODE
81
82 sendCommand("TSTART 80"); //80 resets the frame counter to zero
84#ifdef DEBUG
85 printf("DEBUG: Tstart response: >%s<\n",latestResponseStr);
86#endif
87 }
88
89}
90
95
97{
98 get_report();
99
100 // Call the generic server mainloop, since we are a server
102
103 return;
104}
105
107// returns 0 on fail
108{
110
111 sendCommand("TX ");
112 readResponse();
113 //printf("DEBUG: TX response: >%s %i<\n",latestResponseStr);
114
115 //int numOfHandles=parse2CharIntFromNDIResponse(latestResponseStr);
116 int TXResponseStrIndex=2;
117 bool gotAtLeastOneReport=false;
118
119 for (int t=0; t<numOfRigidBodies; t++) {
120 //int handleNum=parse2CharIntFromNDIResponse(latestResponseStr,&TXResponseStrIndex);
121 // check if the tool is present. Parse and print out the transform data if and only if
122 // it's not missing
123 if (strncmp("MISSING",(char *) &latestResponseStr[TXResponseStrIndex],7)!=0) {
124 gotAtLeastOneReport=true;
125 d_sensor = t;
126
127 // NDI gives qw first(qw,qx,qy,qz), whereas VRPN wants it last (qx,qy,qz,qw)
128 d_quat[3]=parse6CharFloatFromNDIResponse(latestResponseStr,&TXResponseStrIndex); //qw
129 d_quat[0]=parse6CharFloatFromNDIResponse(latestResponseStr,&TXResponseStrIndex); //qx
130 d_quat[1]=parse6CharFloatFromNDIResponse(latestResponseStr,&TXResponseStrIndex); //qy
131 d_quat[2]=parse6CharFloatFromNDIResponse(latestResponseStr,&TXResponseStrIndex); //qz
132
136
137 send_report(); //call this once per sensor
138 }
139
140 //seek to the new line character (since each rigid body is on its own line)
141 while (latestResponseStr[TXResponseStrIndex]!='\n') {
142 TXResponseStrIndex++;
143 }
144 TXResponseStrIndex++; // advance one more char as to be on the next line of text
145
146 }// for
147
148 return(gotAtLeastOneReport); //return 1 if something was sent, 0 otherwise
149}
150
151
152void vrpn_Tracker_NDI_Polaris::send_report(void) // called from get_report()
153{
154 if (d_connection)
155 {
156 char msgbuf[VRPN_MSGBUFSIZE];
157 int len = encode_to(msgbuf);
160 fprintf(stderr,"vrpn_Tracker_NDI_Polaris: cannot write message: tossing\n");
161 }
162 }
163}
164
165
166// Send a command to the NDI tracker over the serial port.
167// This assumes the serial port has already been opened.
168// Some NDI commands require a white-space char at the end of the command, the
169// call must be sure to add it.
170// NDI commands end with a Carriage-return (\r) char. This function automatically adds it, and
171// flushes the output buffer.
172// commandString MUST be terminated with \0, since we don't know the string
173// length otherwise.
174// The NDI trackers have two different command syntaxes - this only supports the syntax
175// WITHOUT CRC checksums
176
177void vrpn_Tracker_NDI_Polaris::sendCommand(const char* commandString ){
178 vrpn_write_characters(serialFd,(const unsigned char* )commandString,strlen(commandString));
179 vrpn_write_characters(serialFd,(const unsigned char* )"\r",1); //send the CR
181}
182
183// Read a fully formed responses from the NDI tracker, (including the 4-byte CRC at the end)
184// and copies it to latestResponseString.
185// Returns the number of characters in the response (not including the CR),
186// or -1 in case of an error
187// NDI responses are all terminated with a CR, which this function replace with a end-of-string char.
188//
189// This function blocks until the CR has been received.
190// FIXME: add a timeout parameter, and timeout if it's been too long
192 //read in characters one-at-a-time until we come across a CR
193 int charIndex=0;
194 bool foundCR=false;
195 unsigned char c;
196 while (!foundCR) {
198 latestResponseStr[charIndex]=c;
199 if (c=='\r') { //found CR
200 latestResponseStr[charIndex]='\0';
201 foundCR=true;
202 } else {
203 charIndex++;
204 }
205 }
206 }
207 return (charIndex);
208}
209
210// Given a filename of a binary .rom file, this reads the file and returns
211// a string with the contents of the file as ascii encoded hex: For each byte of
212// the file, this returns two ascii characters, each of which are ascii representation
213// of a HEX digit (0-9 & a-f). Hex letters are returned as lower case.
214// The string is padded with zeros to make it's length a multiple of 128
215// characters( which is 64 bytes of the original binary file).
216// asciiEncodedHexStr must be allocated before calling, be
217// MAX_NDI_FROM_FILE_SIZE_IN_BYTES * 2 characters long.
218//
219// RETURNS the number of bytes represented in the string (which is half the number of ASCII characters)
220// , or -1 on failure
221int vrpn_Tracker_NDI_Polaris::convertBinaryFileToAsciiEncodedHex(const char* filename, char *asciiEncodedHexStr)
222{
223 FILE* fptr=fopen(filename,"rb");
224 if (fptr==NULL) {
225 fprintf(stderr,"vrpn_Tracker_NDI_Polaris: can't open NDI .rom file %s\n",filename);
226 return (-1);
227 }
228
229 // obtain file size:
230 fseek(fptr , 0 , SEEK_END);
231 long int fileSizeInBytes = ftell(fptr);
232 rewind(fptr);
233
234 if (fileSizeInBytes>MAX_NDI_ROM_FILE_SIZE_IN_BYTES) {
235 fprintf(stderr,"vrpn_Tracker_NDI_Polaris: file is %ld bytes long - which is larger than expected NDI ROM file size of %d bytes.\n",
236 fileSizeInBytes,MAX_NDI_ROM_FILE_SIZE_IN_BYTES);
237 fclose(fptr);
238 return (-1);
239 }
240
241 // allocate memory to contain the whole file:
242 unsigned char* rawBytesFromRomFile;
243 try { rawBytesFromRomFile = new unsigned char[fileSizeInBytes]; }
244 catch (...) {
245 fprintf(stderr, "vrpn_Tracker_NDI_Polaris: Out of memory!\n");
246 fclose(fptr);
247 return(-1);
248 }
249
250 // copy the file into the buffer:
251 size_t result = fread (rawBytesFromRomFile,1,fileSizeInBytes,fptr);
252 if (result != (unsigned int) fileSizeInBytes) {
253 fprintf(stderr,"vrpn_Tracker_NDI_Polaris: error while reading .rom file!\n");
254 try {
255 delete [] rawBytesFromRomFile;
256 } catch (...) {
257 fprintf(stderr, "vrpn_Tracker_NDI_Polaris::convertBinaryFileToAsciiEncodedHex(): delete failed\n");
258 return -1;
259 }
260 fclose(fptr);
261 return(-1);
262 }
263 fclose(fptr);
264
265 // init array with "_" for debugging
266 for (int i=0; i<MAX_NDI_ROM_FILE_SIZE_IN_BYTES; i++) {
267 asciiEncodedHexStr[i]='_';
268 }
269 int byteIndex;
270 for (byteIndex=0; byteIndex<fileSizeInBytes; byteIndex++) {
271 sprintf(&(asciiEncodedHexStr[byteIndex*2]),"%02x ",rawBytesFromRomFile[byteIndex]);
272 }
273
274 // pad the length to make it a multiple of 64
275 int numOfBytesToPadRemaining= NDI_ROMFILE_CHUNK_SIZE-(fileSizeInBytes% NDI_ROMFILE_CHUNK_SIZE);
276 if (numOfBytesToPadRemaining== NDI_ROMFILE_CHUNK_SIZE) {numOfBytesToPadRemaining=0;}
277
278 int paddedFileSizeInBytes=fileSizeInBytes+numOfBytesToPadRemaining;
279
280 while (numOfBytesToPadRemaining>0) {
281 asciiEncodedHexStr[byteIndex*2]='0';
282 asciiEncodedHexStr[byteIndex*2+1]='0';
283 byteIndex++;
284 numOfBytesToPadRemaining--;
285 }
286
287 asciiEncodedHexStr[byteIndex*2]='\0'; //end of string marker, which is used just for debugging
288
289 //printf("DEBUG: >>%s<<\n",asciiEncodedHexStr);
290 try {
291 delete [] rawBytesFromRomFile;
292 } catch (...) {
293 fprintf(stderr, "vrpn_Tracker_NDI_Polaris::convertBinaryFileToAsciiEncodedHex(): delete failed\n");
294 return -1;
295 }
296
297 return paddedFileSizeInBytes;
298}
299
300// NDI response strings often encode ints as a two ascii's WITHOUT any separator behind it
301// this returns the value as an int.
302// The caller passes in the string, and a pointer to an (int) index into that string (which will be advanced
303// to the end of the value we just parsed.
304// The caller must make sure the string is at least two characters long
305unsigned int vrpn_Tracker_NDI_Polaris::parse2CharIntFromNDIResponse(unsigned char* str, int* strIndexPtr) {
306 unsigned intVal;
307 if (strIndexPtr==NULL){ //if no index was passed in, start at the beginning of the string, and don't
308 //advance the index
309 sscanf((char*) str,"%2u",&intVal);
310 } else {
311 sscanf((char*) &(str[*strIndexPtr]),"%2u",&intVal);
312 *strIndexPtr+=2;
313 }
314 return (intVal);
315}
316
317// NDI TX response strings often encode floats as a size ascii's WITHOUT any separator behind it
318// this returns the value as an float. The last 4 digits are implicitly to the right of the decimal point
319// (the decimal point itself is not in the string)
320// The caller passes in the string, and a pointer to an (int) index into that string (which will be advanced
321// to the end of the value we just parsed.
322// The caller must make sure the string is at least six characters long
323float vrpn_Tracker_NDI_Polaris::parse6CharFloatFromNDIResponse(unsigned char* str, int* strIndexPtr) {
324 int intVal;
325 sscanf((char*) &(str[*strIndexPtr]),"%6d",&intVal);
326 *strIndexPtr+=6;
327 return (intVal/10000.0f);
328}
329
330// NDI TX response strings often encode floats as a size ascii's WITHOUT any separator behind it
331// this returns the value as an float. The last 2 digits are implicitly to the right of the decimal point
332// (the decimal point itself is not in the string)
333// The caller passes in the string, and a pointer to an (int) index into that string (which will be advanced
334// to the end of the value we just parsed.
335// The caller must make sure the string is at least seven characters long
336float vrpn_Tracker_NDI_Polaris::parse7CharFloatFromNDIResponse(unsigned char* str, int* strIndexPtr) {
337 int intVal;
338 sscanf((char*) &(str[*strIndexPtr]),"%7d",&intVal);
339 *strIndexPtr+=7;
340 return (intVal/100.0f);
341}
342
343int vrpn_Tracker_NDI_Polaris::setupOneTool(const char* NDIToolRomFilename)
344{
345 // For each tool (i.e. rigid body) that we want to track, we must first need to initialize a port handle (on the NDI machine)
346 // for that tool. This function does that. the caller must call it for each tool.
347 // This returns -1 on failure, or the tool number (i.e. port handle), which is 1 or greater (VRPN
348 // sensors start at 0, whereas NDI port handles start at 1).
349
350 // We use the command
351 // sequence PHRQ -> PVWR -> PINIT -> PENA; we must do this for each tool we want tracked.
352
353 // sending the PHRQ command first; this will assign a port handler number to the tracking device. the
354 // first 8 digits are the hardware device ID, but one can just use asterisks as wild cards. the 9th digit
355 // is system type, 0 for polaris or just *. the 10th digit needs to be a 1 with the passive tracking
356 // balls that we are using in this setup (0 for an active wired LED setup). 11th & 12th digits are to
357 // request a specific port number (we may as well do that here), and the last two are "reserved".
358 char commandStr[MAX_NDI_RESPONSE_LENGTH];
359
360 sendCommand("PHRQ *********1****");
361 readResponse();
362#ifdef DEBUG
363 printf("DEBUG: PHRQ response: >%s<\n",latestResponseStr);
364#endif
365 if (strncmp("ERROR",(char *) latestResponseStr,5)==0) return (-1);
366
367 unsigned int portHandleNum = parse2CharIntFromNDIResponse(latestResponseStr);
368
369 //convert to ASCII-encoded hex, write to array of chars
370 char asciiEncodedHex[MAX_NDI_ROM_FILE_SIZE_IN_BYTES*2]; //each byte needs two digits, so we multiply by 2
371 int numOfFileBytes=convertBinaryFileToAsciiEncodedHex(NDIToolRomFilename,asciiEncodedHex);
372
373 //feed that array of chars to NDI, but we must first divide it up into 64 byte (128 ascii char)
374 //"chunks", and send on chunk at a time.
375 int numOfChunks=numOfFileBytes/NDI_ROMFILE_CHUNK_SIZE;
376 for (int chunkIndex=0; chunkIndex<numOfChunks; chunkIndex++) {
377 char chunk[129]; //64*2 +1 for the end of string
378 vrpn_strcpy(chunk,&(asciiEncodedHex[chunkIndex*NDI_ROMFILE_CHUNK_SIZE*2]));
379
380 int NDIAddress=chunkIndex*NDI_ROMFILE_CHUNK_SIZE; //the memory offset (in the NDI machine, not this PC)
381 // where this chunk will start
382 sprintf(commandStr,"PVWR %02u%04x%s",portHandleNum, NDIAddress,chunk);
383 //printf("DEBUG: >>%s<<\n",commandStr);
384 sendCommand(commandStr);
385 readResponse();
386#ifdef DEBUG
387 printf("DEBUG: PVWR response: >%s<\n",latestResponseStr);
388#endif
389 }
390 sprintf(commandStr,"PINIT %02u",portHandleNum); // initializes the port handle
391 sendCommand(commandStr);
392 readResponse();
393#ifdef DEBUG
394 printf("DEBUG: PINIT response: >%s<\n",latestResponseStr); //this will be an error if PVWR hasn't been sent
395#endif
396 if (strncmp("ERROR",(char *) latestResponseStr,5)==0) return (-1);
397
398 sprintf(commandStr,"PENA %02uD",portHandleNum); //enables the port handle as a dynamic tool (expect motion)
399 sendCommand(commandStr);
400 readResponse();
401#ifdef DEBUG
402 printf("DEBUG: PENA response: >%s<\n",latestResponseStr); //this will be an error if PINIT hasn't been sent
403#endif
404 //FIXME: PENA returns warnings if the tool geometry has problems, Parse these warnings and return them
405 return(portHandleNum);
406}
407
409 printf("vrpn_Tracker_NDI_Polaris: Switching NDI to higher baud rate, and then reopening com port at higher rate...");
410 sendCommand("COMM 70000"); // 1.2Mbit (1228739 baud), which requires the NDI USB 2.0 adapter
411 readResponse();
412#ifdef DEBUG
413 printf("DEBUG: COMM response: >%s<\n",latestResponseStr);
414#endif
415 //if the response is "RESET", try again
416 if (strncmp("RESET",(char *) latestResponseStr,5)==0) {
417 //we got a reset, which means the track reset itself (without being commanded too?)
418 sendCommand("COMM 70000"); // 1.2Mbit, which requires the NDI USB 2.0 adapter
419 readResponse();
420 //printf("DEBUG: COMM response: >%s<\n",latestResponseStr);
421 }
422 vrpn_SleepMsecs(100); //we should wait 100 msec after getting the OKAY from the NDI before changing the PC's comm rate
423
425#ifdef _WIN32
426 serialFd=vrpn_open_commport(port,19200); //19.2k is aliased to 1.2Mbit in the Window's version of the NDI USB virtual com port driver
427#else
428 serialFd=vrpn_open_commport(port,1228739); //should work on linux (UNTESTED!)
429#endif
430 printf("done\n");
431}
432
433
vrpn_Connection * d_connection
Connection that this object talks to.
vrpn_int32 d_sender_id
Sender ID registered with the connection.
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
Generic connection class not specific to the transport mechanism.
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...
void switchToHigherBaudRate(const char *port)
int convertBinaryFileToAsciiEncodedHex(const char *filename, char *asciiEncodedHexStr)
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
int setupOneTool(const char *NDIToolRomFilename)
unsigned int parse2CharIntFromNDIResponse(unsigned char *str, int *strIndexPtr=NULL)
void sendCommand(const char *commandString)
float parse6CharFloatFromNDIResponse(unsigned char *str, int *strIndexPtr)
unsigned char latestResponseStr[MAX_NDI_RESPONSE_LENGTH]
vrpn_Tracker_NDI_Polaris(const char *name, vrpn_Connection *c, const char *port, int numOfRigidBodies, const char **rigidBodyNDIRomFileNames)
The constructor is given the name of the tracker (the name of the sender it should use),...
float parse7CharFloatFromNDIResponse(unsigned char *str, int *strIndexPtr)
virtual int encode_to(char *buf)
vrpn_float64 d_quat[4]
vrpn_int32 d_sensor
vrpn_float64 pos[3]
struct timeval timestamp
vrpn_int32 position_m_id
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
int vrpn_set_rts(int comm)
int vrpn_close_commport(int comm)
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
int vrpn_flush_output_buffer(int comm)
Throw out any characters (do not send) within the output buffer.
int vrpn_clear_rts(int comm)
int vrpn_open_commport(const char *portname, long baud, int charsize, vrpn_SER_PARITY parity, bool rts_flow)
Open a serial port, given its name and baud rate.
Definition vrpn_Serial.C:54
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
void vrpn_SleepMsecs(double dMilliSecs)
void vrpn_strcpy(char(&to)[charCount], const char *pSrc)
Null-terminated-string copy function that both guarantees not to overrun the buffer and guarantees th...
#define vrpn_gettimeofday
Definition vrpn_Shared.h:99