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();
}
InterSense::InterSense(VRDevice::Factory* sFactory,VRDeviceManager* sDeviceManager,Misc::ConfigurationFile& configFile)
:VRDevice(sFactory,sDeviceManager,configFile),
serialPort(configFile.retrieveString("./serialPort").c_str()),
stations(0),
timers(0),notFirstMeasurements(0),oldPositionOrientations(0)
{
/* Set device port parameters: */
int deviceBaudRate=configFile.retrieveValue<int>("./deviceBaudRate");
serialPort.setSerialSettings(deviceBaudRate,8,Comm::SerialPort::PARITY_NONE,1,false);
serialPort.setRawMode(1,0);
if(configFile.retrieveValue<bool>("./resetDevice",false))
{
/* Reset device: */
#ifdef VERBOSE
printf("InterSense: Resetting device\n");
fflush(stdout);
#endif
serialPort.writeByte('\31');
delay(15.0);
}
else
{
/* Stop continuous mode (in case it's still active): */
#ifdef VERBOSE
printf("InterSense: Disabling continuous mode\n");
fflush(stdout);
#endif
serialPort.writeByte('c');
}
/* Request status record to check if device is okey-dokey: */
#ifdef VERBOSE
printf("InterSense: Requesting status record\n");
fflush(stdout);
#endif
serialPort.writeByte('S');
if(!readStatusReply())
{
/* Try resetting the device, seeing if that helps: */
#ifdef VERBOSE
printf("InterSense: Resetting device\n");
fflush(stdout);
#endif
serialPort.writeByte('\31');
delay(15.0);
/* Request another status record: */
#ifdef VERBOSE
printf("InterSense: Re-requesting status record\n");
fflush(stdout);
#endif
serialPort.writeByte('S');
if(!readStatusReply())
Misc::throwStdErr("InterSense: Device not responding");
}
/* Get the array of station enable flags: */
#ifdef VERBOSE
printf("InterSense: Detecting enabled stations\n");
fflush(stdout);
#endif
serialPort.writeString("l*\r\n");
delay(0.1);
char buffer[256];
readLine(sizeof(buffer),buffer,Misc::Time(1.0));
if(strncmp(buffer,"21l",3)!=0)
Misc::throwStdErr("InterSense: Unable to detect enabled stations");
/* Disable all stations: */
for(int i=0;i<32;++i)
{
stationIdToIndex[i]=-1;
if(buffer[i+3]=='1')
{
char command[20];
snprintf(command,sizeof(command),"l%d,0\r\n",i+1);
serialPort.writeString(command);
delay(0.1);
}
}
/* Probe the constellation configuration if asked: */
if(configFile.retrieveValue<bool>("./probeConstellation",false))
{
/* Check if the device has a valid constellation configuration: */
#ifdef VERBOSE
printf("InterSense: Probing constellation configuration\n");
fflush(stdout);
#endif
serialPort.writeString("MCF\r\n");
delay(0.1);
int numTransmitters=0;
while(true)
{
/* Read the next transmitter line: */
readLine(sizeof(buffer),buffer,Misc::Time(1.0));
/* Check if it's a valid transmitter line: */
int transmitterNum,transmitterId;
double posX,posY,posZ,dirX,dirY,dirZ;
if(sscanf(buffer,"31F %d %lf %lf %lf %lf %lf %lf %d",&transmitterNum,&posX,&posY,&posZ,&dirX,&dirY,&dirZ,&transmitterId)!=8)
break;
/* Increase the number of transmitters: */
++numTransmitters;
}
#ifdef VERBOSE
printf("InterSense: Detected %d configured transmitters\n",numTransmitters);
fflush(stdout);
#endif
}
/* Upload a constellation configuration if asked: */
std::string constellationName=configFile.retrieveString("./uploadConstellationConfiguration","");
if(constellationName!="")
{
#ifdef VERBOSE
printf("InterSense: Uploading constellation configuration %s\n",constellationName.c_str());
fflush(stdout);
#endif
/* Go to constellation configuration's section: */
configFile.setCurrentSection(constellationName.c_str());
Transmitter* transmitters=0;
try
{
/* Get the number of configured transmitters and the base number for internal IDs: */
int numTransmitters=configFile.retrieveValue<int>("./numTransmitters");
int transmitterIdBase=configFile.retrieveValue<int>("./transmitterIdBase",5001);
/* Get the conversion factor from configuration units to meters: */
float unitSize=configFile.retrieveValue<float>("./unitSize",1.0f);
/* Read all transmitter configurations to check for validity first: */
transmitters=new Transmitter[numTransmitters];
for(int i=0;i<numTransmitters;++i)
{
/* Read the transmitter's configuration: */
char transmitterName[20];
snprintf(transmitterName,sizeof(transmitterName),"./MCF%d",i+1);
transmitters[i]=configFile.retrieveValue<Transmitter>(transmitterName);
/* Convert the transmitter's position to meters: */
for(int j=0;j<3;++j)
transmitters[i].pos[j]*=unitSize;
/* Normalize the transmitter's direction: */
transmitters[i].dir.normalize();
}
/* Upload constellation configuration to device: */
serialPort.writeString("MCC\r\n");
delay(0.1);
for(int i=0;i<numTransmitters;++i)
{
const Transmitter& t=transmitters[i];
char transmitterLine[256];
snprintf(transmitterLine,sizeof(transmitterLine),
"MCF%d, %8.4f, %8.4f, %8.4f, %6.3f, %6.3f, %6.3f, %d\r\n",
i+1,t.pos[0],t.pos[1],t.pos[2],t.dir[0],t.dir[1],t.dir[2],i+transmitterIdBase);
serialPort.writeString(transmitterLine);
delay(0.1);
}
serialPort.writeString("MCe\r\n");
delay(0.1);
}
catch(std::runtime_error err)
{
printf("InterSense: Ignoring constellation configuration %s due to exception %s\n",constellationName.c_str(),err.what());
}
/* Clean up: */
delete[] transmitters;
/* Go back to device's section: */
configFile.setCurrentSection("..");
}
/* Retrieve list of station names: */
typedef std::vector<std::string> StringList;
StringList stationNames=configFile.retrieveValue<StringList>("./stationNames");
setNumTrackers(stationNames.size(),configFile);
stations=new Station[numTrackers];
int totalNumButtons=0;
int totalNumValuators=0;
/* Initialize all tracked stations: */
#ifdef VERBOSE
printf("InterSense: Initializing tracked stations\n");
fflush(stdout);
#endif
for(int i=0;i<numTrackers;++i)
{
char command[80];
/* Go to station's section: */
configFile.setCurrentSection(stationNames[i].c_str());
/* Read station's configuration: */
stations[i].id=configFile.retrieveValue<int>("./id",i+1);
stationIdToIndex[stations[i].id]=i;
stations[i].numButtons=configFile.retrieveValue<int>("./numButtons",0);
stations[i].firstButtonIndex=totalNumButtons;
totalNumButtons+=stations[i].numButtons;
stations[i].joystick=configFile.retrieveValue<bool>("./joystick",false);
stations[i].firstValuatorIndex=totalNumValuators;
if(stations[i].joystick)
totalNumValuators+=2;
/* Enable station: */
snprintf(command,sizeof(command),"l%d,1\r\n",stations[i].id);
serialPort.writeString(command);
delay(0.1);
/* Reset station's alignment frame: */
snprintf(command,sizeof(command),"R%d\r\n",stations[i].id);
serialPort.writeString(command);
delay(0.1);
/* Disable boresight mode: */
snprintf(command,sizeof(command),"b%d\r\n",stations[i].id);
serialPort.writeString(command);
delay(0.1);
/* Reset station's tip offset: */
snprintf(command,sizeof(command),"N%d,%8.4f,%8.4f,%8.4f\r\n",stations[i].id,0.0f,0.0f,0.0f);
serialPort.writeString(command);
delay(0.1);
/* Set station's output format: */
snprintf(command,sizeof(command),"O%d,2,4,22,23,1\r\n",stations[i].id);
serialPort.writeString(command);
delay(0.1);
/* Set stations' motion prediction: */
int predictionTime=configFile.retrieveValue<int>("./predictionTime",0);
snprintf(command,sizeof(command),"Mp%d,%d\r\n",stations[i].id,predictionTime);
serialPort.writeString(command);
delay(0.1);
/* Set stations' perceptual enhancement level: */
int perceptualEnhancement=configFile.retrieveValue<int>("./perceptualEnhancement",2);
snprintf(command,sizeof(command),"MF%d,%d\r\n",stations[i].id,perceptualEnhancement);
serialPort.writeString(command);
delay(0.1);
/* Set station's rotational sensitivity: */
int rotationalSensitivity=configFile.retrieveValue<int>("./rotationalSensitivity",3);
snprintf(command,sizeof(command),"MQ%d,%d\r\n",stations[i].id,rotationalSensitivity);
serialPort.writeString(command);
delay(0.1);
/* Go back to device's section: */
configFile.setCurrentSection("..");
}
/* Enable/disable sonistrip LEDs: */
if(configFile.retrieveValue<bool>("./enableLEDs",true))
{
/* Enable LEDs: */
#ifdef VERBOSE
printf("InterSense: Enabling sonistrip LEDs\n");
fflush(stdout);
#endif
serialPort.writeString("ML1\r\n");
delay(0.1);
}
else
{
/* Enable LEDs: */
#ifdef VERBOSE
printf("InterSense: Disabling sonistrip LEDs\n");
fflush(stdout);
#endif
serialPort.writeString("ML0\r\n");
delay(0.1);
}
/* Set unit mode to inches: */
#ifdef VERBOSE
printf("InterSense: Setting unit mode\n");
fflush(stdout);
#endif
serialPort.writeByte('U');
delay(0.1);
/* Enable binary mode: */
#ifdef VERBOSE
printf("InterSense: Enabling binary mode\n");
fflush(stdout);
#endif
serialPort.writeByte('f');
/* Set number of buttons and valuators: */
setNumButtons(totalNumButtons,configFile);
setNumValuators(totalNumValuators,configFile);
/* Create free-running timers: */
timers=new Misc::Timer[numTrackers];
notFirstMeasurements=new bool[numTrackers];
oldPositionOrientations=new PositionOrientation[numTrackers];
}
VRDeviceManager::VRDeviceManager(Misc::ConfigurationFile& configFile)
:deviceFactories(configFile.retrieveString("./deviceDirectory",VRDEVICEDAEMON_CONFIG_VRDEVICESDIR),this),
calibratorFactories(configFile.retrieveString("./calibratorDirectory",VRDEVICEDAEMON_CONFIG_VRCALIBRATORSDIR)),
numDevices(0),
devices(0),trackerIndexBases(0),buttonIndexBases(0),valuatorIndexBases(0),
fullTrackerReportMask(0x0),trackerReportMask(0x0),trackerUpdateNotificationEnabled(false),
trackerUpdateCompleteCond(0)
{
/* Allocate device and base index arrays: */
typedef std::vector<std::string> StringList;
StringList deviceNames=configFile.retrieveValue<StringList>("./deviceNames");
numDevices=deviceNames.size();
devices=new VRDevice*[numDevices];
trackerIndexBases=new int[numDevices];
buttonIndexBases=new int[numDevices];
valuatorIndexBases=new int[numDevices];
/* Initialize VR devices: */
for(currentDeviceIndex=0;currentDeviceIndex<numDevices;++currentDeviceIndex)
{
/* Save the device's base indices: */
trackerIndexBases[currentDeviceIndex]=int(trackerNames.size());
buttonIndexBases[currentDeviceIndex]=int(buttonNames.size());
valuatorIndexBases[currentDeviceIndex]=int(valuatorNames.size());
/* Go to device's section: */
configFile.setCurrentSection(deviceNames[currentDeviceIndex].c_str());
/* Retrieve device type: */
std::string deviceType=configFile.retrieveString("./deviceType");
/* Initialize device: */
#ifdef VERBOSE
printf("VRDeviceManager: Loading device %s of type %s\n",deviceNames[currentDeviceIndex].c_str(),deviceType.c_str());
fflush(stdout);
#endif
DeviceFactoryManager::Factory* deviceFactory=deviceFactories.getFactory(deviceType);
devices[currentDeviceIndex]=deviceFactory->createObject(configFile);
if(configFile.hasTag("./trackerNames"))
{
StringList deviceTrackerNames=configFile.retrieveValue<StringList>("./trackerNames");
int trackerIndex=trackerIndexBases[currentDeviceIndex];
int numTrackers=trackerNames.size();
for(StringList::iterator dtnIt=deviceTrackerNames.begin();dtnIt!=deviceTrackerNames.end()&&trackerIndex<numTrackers;++dtnIt,++trackerIndex)
trackerNames[trackerIndex]=*dtnIt;
}
/* Override device's button names: */
if(configFile.hasTag("./buttonNames"))
{
StringList deviceButtonNames=configFile.retrieveValue<StringList>("./buttonNames");
int buttonIndex=buttonIndexBases[currentDeviceIndex];
int numButtons=buttonNames.size();
for(StringList::iterator dtnIt=deviceButtonNames.begin();dtnIt!=deviceButtonNames.end()&&buttonIndex<numButtons;++dtnIt,++buttonIndex)
buttonNames[buttonIndex]=*dtnIt;
}
/* Override device's valuator names: */
if(configFile.hasTag("./valuatorNames"))
{
StringList deviceValuatorNames=configFile.retrieveValue<StringList>("./valuatorNames");
int valuatorIndex=valuatorIndexBases[currentDeviceIndex];
int numValuators=valuatorNames.size();
for(StringList::iterator dtnIt=deviceValuatorNames.begin();dtnIt!=deviceValuatorNames.end()&&valuatorIndex<numValuators;++dtnIt,++valuatorIndex)
valuatorNames[valuatorIndex]=*dtnIt;
}
/* Return to parent section: */
configFile.setCurrentSection("..");
}
#ifdef VERBOSE
printf("VRDeviceManager: Managing %d trackers, %d buttons, %d valuators\n",int(trackerNames.size()),int(buttonNames.size()),int(valuatorNames.size()));
fflush(stdout);
#endif
/* Set server state's layout: */
state.setLayout(trackerNames.size(),buttonNames.size(),valuatorNames.size());
/* Read names of all virtual devices: */
StringList virtualDeviceNames=configFile.retrieveValue<StringList>("./virtualDeviceNames",StringList());
/* Initialize virtual devices: */
for(StringList::iterator vdnIt=virtualDeviceNames.begin();vdnIt!=virtualDeviceNames.end();++vdnIt)
{
/* Create a new virtual device descriptor by reading the virtual device's configuration file section: */
Vrui::VRDeviceDescriptor* vdd=new Vrui::VRDeviceDescriptor;
vdd->load(configFile.getSection(vdnIt->c_str()));
/* Store the virtual device descriptor: */
virtualDevices.push_back(vdd);
}
#ifdef VERBOSE
printf("VRDeviceManager: Managing %d virtual devices\n",int(virtualDevices.size()));
fflush(stdout);
#endif
}
