void VRPNClient::start(void)
{
/* Set device manager's states to current states: */
{
Threads::Mutex::Lock stateLock(stateMutex);
for(int i=0;i<getNumTrackers();++i)
setTrackerState(i,trackerStates[i]);
for(int i=0;i<getNumButtons();++i)
setButtonState(i,buttonStates[i]);
for(int i=0;i<getNumValuators();++i)
setValuatorState(i,valuatorStates[i]);
/* Start reporting events to the device manager: */
reportEvents=true;
}
}
void ViconTarsus::start(void)
{
/* Initialize tracker states: */
for(int i=0;i<getNumTrackers();++i)
{
trackerStates[i].positionOrientation=PositionOrientation(Vector::zero,Rotation::identity);
trackerStates[i].linearVelocity=Vrui::VRDeviceState::TrackerState::LinearVelocity::zero;
trackerStates[i].angularVelocity=Vrui::VRDeviceState::TrackerState::AngularVelocity::zero;
}
/* Start device communication thread: */
startDeviceThread();
/* Activate streaming: */
#ifdef VERBOSE
printf("ViconTarsus: Starting continuous update mode\n");
#endif
pipe.write<int>(3);
pipe.write<int>(0);
}
void ArtDTrack::processAsciiData(void)
{
while(true)
{
/* Wait for the next data message from the DTrack daemon: */
char messageBuffer[4096];
size_t messageSize=dataSocket.receiveMessage(messageBuffer,sizeof(messageBuffer)-1);
/* Newline-terminate the message: */
messageBuffer[messageSize]='\n';
/* Parse the received message: */
char* mPtr=messageBuffer;
char* mEnd=messageBuffer+messageSize;
while(mPtr<mEnd)
{
/* Find the end of the current line: */
char* lineEnd;
for(lineEnd=mPtr;*lineEnd!='\n';++lineEnd)
;
/* Skip whitespace: */
while(mPtr<lineEnd&&isspace(*mPtr))
++mPtr;
/* Get the line identifier: */
char* idStart=mPtr;
while(mPtr<lineEnd&&!isspace(*mPtr))
++mPtr;
/* Determine the type of the line: */
int lineType=-1;
if(mPtr-idStart==2&&strncasecmp(idStart,"6d",2)==0)
lineType=0;
else if(mPtr-idStart==3&&strncasecmp(idStart,"6df",3)==0)
lineType=1;
else if(mPtr-idStart==4&&strncasecmp(idStart,"6df2",4)==0)
lineType=2;
else if(mPtr-idStart==4&&strncasecmp(idStart,"6dmt",4)==0)
lineType=3;
else if(mPtr-idStart==2&&strncasecmp(idStart,"3d",2)==0)
lineType=4;
/* Process the line: */
if(lineType>=0)
{
if(lineType==2)
{
/* Skip the number of defined flysticks: */
readInt(mPtr);
}
/* Read the number of reported bodies: */
int numBodies=readInt(mPtr);
/* Parse each body: */
for(int body=0;body<numBodies;++body)
{
/* Find the first opening bracket: */
while(mPtr<lineEnd&&isspace(*mPtr))
++mPtr;
if(*mPtr!='[')
break; // Ignore the rest of the line
++mPtr;
/* Read the body's ID: */
int id=readInt(mPtr);
Device* device=deviceIdToIndex[id]>=0?&devices[deviceIdToIndex[id]]:0;
/* Skip the quality value: */
float(readFloat(mPtr));
if(lineType==1||lineType==3)
{
/* Read the button bit flag: */
unsigned int bits=readUint(mPtr);
if(device!=0)
{
/* Set the button states: */
for(int i=0;i<32&&i<device->numButtons;++i)
{
setButtonState(device->firstButtonIndex+i,(bits&0x1)!=0x0);
bits>>=1;
}
}
}
int numButtons=0;
int numValuators=0;
if(lineType==2)
{
/* Read the number of buttons and valuators: */
numButtons=readInt(mPtr);
numValuators=readInt(mPtr);
}
/* Find the first closing bracket: */
while(mPtr<lineEnd&&isspace(*mPtr))
++mPtr;
if(*mPtr!=']')
break; // Ignore the rest of the line
++mPtr;
/* Find the second opening bracket: */
while(mPtr<lineEnd&&isspace(*mPtr))
++mPtr;
if(*mPtr!='[')
break; // Ignore the rest of the line
++mPtr;
/* Read the body's position: */
Vector pos;
for(int i=0;i<3;++i)
pos[i]=VScalar(readFloat(mPtr));
Rotation orient=Rotation::identity;
if(lineType==0||lineType==1)
{
/* Read the body's orientation angles: */
VScalar angles[3];
for(int i=0;i<3;++i)
angles[i]=VScalar(readFloat(mPtr));
/* Calculate the body's orientation quaternion: */
orient*=Rotation::rotateX(Math::rad(angles[0]));
orient*=Rotation::rotateY(Math::rad(angles[1]));
orient*=Rotation::rotateZ(Math::rad(angles[2]));
}
/* Find the second closing bracket: */
while(mPtr<lineEnd&&isspace(*mPtr))
++mPtr;
if(*mPtr!=']')
break; // Ignore the rest of the line
++mPtr;
if(lineType!=4)
{
/* Find the third opening bracket: */
while(mPtr<lineEnd&&isspace(*mPtr))
++mPtr;
if(*mPtr!='[')
break; // Ignore the rest of the line
++mPtr;
}
if(lineType==2||lineType==3)
{
/* Read the body's orientation matrix (yuck!): */
Geometry::Matrix<VScalar,3,3> matrix;
for(int j=0;j<3;++j)
for(int i=0;i<3;++i)
matrix(i,j)=VScalar(readFloat(mPtr));
/* Calculate the body's orientation quaternion: */
orient=Rotation::fromMatrix(matrix);
/* Find the third closing bracket: */
while(mPtr<lineEnd&&isspace(*mPtr))
++mPtr;
if(*mPtr!=']')
break; // Ignore the rest of the line
++mPtr;
}
else if(lineType!=4)
{
/* Ignore the body's orientation matrix: */
while(mPtr<lineEnd&&*mPtr!=']')
++mPtr;
if(*mPtr!=']') // Ignore the rest of the line
break;
}
if(lineType==2)
{
/* Find the fourth opening bracket: */
while(mPtr<lineEnd&&isspace(*mPtr))
++mPtr;
if(*mPtr!='[')
break; // Ignore the rest of the line
++mPtr;
/* Read the flystick's button bits: */
for(int bitIndex=0;bitIndex<numButtons;bitIndex+=32)
{
/* Read the next button bit mask: */
unsigned int bits=readUint(mPtr);
if(device!=0)
{
/* Set the device's button values: */
for(int i=0;i<32;++i)
{
/* Set the button state if the button is valid: */
if(bitIndex+i<device->numButtons)
setButtonState(device->firstButtonIndex+bitIndex+i,(bits&0x1)!=0x0);
bits>>=1;
}
}
}
/* Read the flystick's valuator values: */
for(int i=0;i<numValuators;++i)
{
/* Read the next valuator value: */
float value=float(readFloat(mPtr));
/* Set the valuator value if the valuator is valid: */
if(device!=0&&i<device->numValuators)
setValuatorState(device->firstValuatorIndex+i,value);
}
/* Find the fourth closing bracket: */
while(mPtr<lineEnd&&isspace(*mPtr))
++mPtr;
if(*mPtr!=']')
break; // Ignore the rest of the line
++mPtr;
}
/* Check if this body has been configured as a device: */
if(device!=0)
{
/* Set the device's tracker state: */
trackerStates[deviceIdToIndex[id]].positionOrientation=PositionOrientation(pos,orient);
}
}
}
/* Go to the start of the next line: */
mPtr=lineEnd+1;
}
/* Update all tracker states (including those that were not updated): */
for(int i=0;i<getNumTrackers();++i)
setTrackerState(i,trackerStates[i]);
}
VRPNClient::VRPNClient(VRDevice::Factory* sFactory,VRDeviceManager* sDeviceManager,Misc::ConfigurationFile& configFile)
:VRDevice(sFactory,sDeviceManager,configFile),
VRPNConnection(configFile.retrieveString("./serverName").c_str(),configFile.retrieveValue<int>("./serverPort",3883)),
reportEvents(false),
trackerStates(0),trackerFlags(0),buttonStates(0),valuatorStates(0)
{
#ifdef VERBOSE
printf("VRPNClient: Initializing senders...");
fflush(stdout);
#endif
/* Check if the z axis if incoming position data needs to be flipped: */
setFlipZAxis(configFile.retrieveValue<bool>("./flipZAxis",false));
/* Retrieve list of sender names: */
typedef std::vector<std::string> StringList;
StringList senderNames=configFile.retrieveValue<StringList>("./senderNames");
/* Process all senders: */
int totalNumTrackers=0;
int totalNumButtons=0;
int totalNumValuators=0;
for(StringList::const_iterator snIt=senderNames.begin();snIt!=senderNames.end();++snIt)
{
/* Go to the sender's section: */
configFile.setCurrentSection(snIt->c_str());
/* Read the number of trackers, buttons, and valuators for this sender: */
int numTrackers=configFile.retrieveValue<int>("./numTrackers",0);
if(numTrackers>0)
{
requestTrackers(snIt->c_str(),totalNumTrackers,numTrackers);
totalNumTrackers+=numTrackers;
}
int numButtons=configFile.retrieveValue<int>("./numButtons",0);
if(numButtons>0)
{
requestButtons(snIt->c_str(),totalNumButtons,numButtons);
totalNumButtons+=numButtons;
}
int numValuators=configFile.retrieveValue<int>("./numValuators",0);
if(numValuators>0)
{
requestValuators(snIt->c_str(),totalNumValuators,numValuators);
totalNumValuators+=numValuators;
}
/* Go back to device's section: */
configFile.setCurrentSection("..");
}
#ifdef VERBOSE
printf(" done\n");
fflush(stdout);
#endif
/* Set number of trackers, buttons, and valuators: */
setNumTrackers(totalNumTrackers,configFile);
setNumButtons(totalNumButtons,configFile);
setNumValuators(totalNumValuators,configFile);
/* Read the initial position/orientation for all trackers: */
PositionOrientation defaultPosition=configFile.retrieveValue<PositionOrientation>("./defaultPosition",PositionOrientation::identity);
/* Initialize the local state arrays: */
trackerStates=new TrackerState[getNumTrackers()];
trackerFlags=new int[getNumTrackers()];
for(int i=0;i<getNumTrackers();++i)
{
trackerStates[i].positionOrientation=defaultPosition;
trackerStates[i].linearVelocity=LinearVelocity::zero;
trackerStates[i].angularVelocity=AngularVelocity::zero;
trackerFlags[i]=0x0;
}
buttonStates=new ButtonState[getNumButtons()];
for(int i=0;i<getNumButtons();++i)
buttonStates[i]=false;
valuatorStates=new ValuatorState[getNumValuators()];
for(int i=0;i<getNumValuators();++i)
valuatorStates[i]=ValuatorState(0);
/* Start device communication thread: */
startDeviceThread();
}
void ViconTarsus::deviceThreadMethod(void)
{
while(true)
{
/* Wait for the next packet from the server: */
int packetKind=pipe.read<int>();
if(pipe.read<int>()==1) // Ignore request packets
{
switch(packetKind)
{
case 0:
case 4:
/* Shut down the thread method: */
return;
case 2:
{
/* Read the data packet: */
int numPacketChannels=pipe.read<int>();
if(numPacketChannels>numChannels)
{
/* Read the relevant channels: */
pipe.read<double>(channelPacketBuffer,numChannels);
/* Ignore all spurious channels: */
for(int i=numChannels;i<numPacketChannels;++i)
pipe.read<double>();
numPacketChannels=numChannels;
}
else
{
/* Read all channels: */
pipe.read<double>(channelPacketBuffer,numPacketChannels);
}
/* Process the data packet: */
for(int trackerIndex=0;trackerIndex<getNumTrackers();++trackerIndex)
{
/* Get the tracker's position: */
Vector translation;
bool valid=true;
for(int i=0;i<3;++i)
{
if(trackerChannelIndices[trackerIndex*6+i]<numPacketChannels)
translation[i]=VScalar(channelPacketBuffer[trackerChannelIndices[trackerIndex*6+i]]);
else
valid=false;
}
if(valid)
{
if(trackerSixDofs[trackerIndex])
{
/* Get the tracker's orientation: */
PositionOrientation::Rotation::Vector rotation;
bool sixDof=true;
for(int i=0;i<3;++i)
{
if(trackerChannelIndices[trackerIndex*6+3+i]<numPacketChannels)
rotation[i]=RScalar(channelPacketBuffer[trackerChannelIndices[trackerIndex*6+3+i]]);
else
sixDof=false;
}
if(sixDof)
{
/* Set the 6-DOF tracker state: */
trackerStates[trackerIndex].positionOrientation=PositionOrientation(translation,Rotation::rotateScaledAxis(rotation));
}
else
{
/* Set the 6-DOF tracker state with the previous orientation: */
trackerStates[trackerIndex].positionOrientation=PositionOrientation(translation,trackerStates[trackerIndex].positionOrientation.getRotation());
}
}
else
{
/* Set the 3-DOF tracker state: */
trackerStates[trackerIndex].positionOrientation=PositionOrientation(translation,Rotation::identity);
}
}
}
/* Update all tracker states (including those that were not updated): */
for(int i=0;i<getNumTrackers();++i)
setTrackerState(i,trackerStates[i]);
break;
}
default:
/* Ignore the packet: */
;
}
}
}
}
ViconTarsus::ViconTarsus(VRDevice::Factory* sFactory,VRDeviceManager* sDeviceManager,Misc::ConfigurationFile& configFile)
:VRDevice(sFactory,sDeviceManager,configFile),
pipe(configFile.retrieveString("./serverName"),configFile.retrieveValue<int>("./serverPort",800),Comm::TCPPipe::LittleEndian),
trackerChannelIndices(0),
trackerSixDofs(0),
trackerMap(0),
channelPacketBuffer(0),
trackerStates(0)
{
/* Read the list of tracked bodies: */
std::vector<std::string> trackedBodies=configFile.retrieveValue<std::vector<std::string> >("./trackedBodies");
/* Set tracker's layout: */
setNumTrackers(trackedBodies.size(),configFile);
/* Initialize the tracker channel index array: */
trackerChannelIndices=new int[getNumTrackers()*6];
for(int i=0;i<getNumTrackers()*6;++i)
trackerChannelIndices[i]=-1;
/* Send info request to server: */
#ifdef VERBOSE
printf("ViconTarsus: Requesting info packet\n");
#endif
pipe.write<int>(1); // Info
pipe.write<int>(0); // Request
/* Wait for the server's reply: */
if(pipe.read<int>()!=1)
Misc::throwStdErr("ViconTarsus: Unable to connect to tracking server at %s",pipe.getPeerHostname().c_str());
if(pipe.read<int>()!=1)
Misc::throwStdErr("ViconTarsus: Unable to connect to tracking server at %s",pipe.getPeerHostname().c_str());
/* Read the info packet's data: */
numChannels=pipe.read<int>();
#ifdef VERBOSE
printf("ViconTarsus: Server reports %d channels\n",numChannels);
#endif
for(int channelIndex=0;channelIndex<numChannels;++channelIndex)
{
/* Read the channel descriptor (fortunately, Vicon's string protocol is compatible to TCPPipe's): */
std::string channelName=pipe.read<std::string>();
#ifdef VERBOSE
printf("ViconTarsus: Server channel %2d: %s\n",channelIndex,channelName.c_str());
#endif
/* Parse the channel name: */
const char* nameStart=channelName.c_str();
const char* nameEnd;
for(nameEnd=nameStart;*nameEnd!='\0'&&*nameEnd!=' '&&*nameEnd!='-'&&*nameEnd!='<'&&*nameEnd!='>';++nameEnd)
;
const char* codeStart;
for(codeStart=nameEnd;*codeStart!='\0'&&*codeStart!='<';++codeStart)
;
const char* codeEnd;
for(codeEnd=codeStart;*codeEnd!='\0'&&*codeEnd!='>';++codeEnd)
;
/* Check if the channel name is well-formed: */
if(*codeStart=='<'&&*codeEnd=='>')
{
std::string code(codeStart+1,codeEnd);
/* Check if the channel encodes a body parameter or a marker parameter: */
if(code[0]=='P'||code[0]=='A'||code[0]=='T')
{
/* Check if the name corresponds to a tracked body: */
std::string name(nameStart,nameEnd);
int bodyIndex;
for(bodyIndex=0;bodyIndex<getNumTrackers();++bodyIndex)
if(trackedBodies[bodyIndex]==name)
break;
if(bodyIndex<getNumTrackers())
{
/* Parse the channel code: */
if(code=="P-X"||code=="T-X")
trackerChannelIndices[bodyIndex*6+0]=channelIndex;
else if(code=="P-Y"||code=="T-Y")
trackerChannelIndices[bodyIndex*6+1]=channelIndex;
else if(code=="P-Z"||code=="T-Z")
trackerChannelIndices[bodyIndex*6+2]=channelIndex;
else if(code=="A-X")
trackerChannelIndices[bodyIndex*6+3]=channelIndex;
else if(code=="A-Y")
trackerChannelIndices[bodyIndex*6+4]=channelIndex;
else if(code=="A-Z")
trackerChannelIndices[bodyIndex*6+5]=channelIndex;
}
}
}
}
/* Check if all requested trackers have channels associated with them: */
trackerSixDofs=new bool[getNumTrackers()];
int numSixDofTrackers=0;
for(int bodyIndex=0;bodyIndex<getNumTrackers();++bodyIndex)
{
/* Check if the tracker has 3-DOF data: */
bool valid=true;
for(int i=0;i<3;++i)
if(trackerChannelIndices[bodyIndex*6+i]==-1)
valid=false;
if(!valid)
Misc::throwStdErr("ViconTarsus: Insufficient data to track body %s",trackedBodies[bodyIndex].c_str());
/* Check if the tracker has 6-DOF data (for later): */
bool sixDof=true;
for(int i=3;i<6;++i)
if(trackerChannelIndices[bodyIndex*6+i]==-1)
sixDof=false;
trackerSixDofs[bodyIndex]=sixDof;
if(sixDof)
++numSixDofTrackers;
}
#ifdef VERBOSE
printf("ViconTarsus: Tracking %d 3-DOF tracker(s) and %d 6-DOF tracker(s)\n",getNumTrackers()-numSixDofTrackers,numSixDofTrackers);
#endif
/* Construct the tracker map: */
trackerMap=new std::pair<int,int>[numChannels];
for(int i=0;i<numChannels;++i)
trackerMap[i]=std::pair<int,int>(-1,-1);
for(int bodyIndex=0;bodyIndex<getNumTrackers();++bodyIndex)
for(int i=0;i<6;++i)
{
if(trackerChannelIndices[bodyIndex*6+i]!=-1)
trackerMap[trackerChannelIndices[bodyIndex*6+i]]=std::pair<int,int>(bodyIndex,i);
}
/* Initialize the channel packet buffer: */
channelPacketBuffer=new double[numChannels];
/* Initialize tracker states: */
trackerStates=new Vrui::VRDeviceState::TrackerState[getNumTrackers()];
}