void SimSetablePosition::setData(const std::vector<PositionData>& posDataVec)
{
mPos.setTime(); // Set the time
addPositionSample(posDataVec);
swapPositionBuffers(); // Swap the buffers
}
bool SerialEncoder::sample()
{
std::vector<gadget::PositionData> positionSample(1);
positionSample[0].setTime();
positionSample[0].setValue(mSerialEncoder->getSample());
addPositionSample(positionSample);
return true;
}
// Sample data.
bool MotionStar::sample()
{
bool retval;
std::vector< gadget::PositionData > cur_samples(mMotionStar.getNumSensors());
retval = false;
if ( isActive() == false )
{
vprDEBUG(vprDBG_ALL, vprDBG_CRITICAL_LVL)
<< clrSetNORM(clrRED) << "MotionStar ("
<< getAddressName() << ") NOT ACTIVE IN SAMPLE\n"
<< clrRESET << vprDEBUG_FLUSH;
}
else
{
try
{
mMotionStar.sample();
// get an initial timestamp for this entire sample. we'll copy it into
// each PositionData for this sample.
if ( ! cur_samples.empty() )
{
cur_samples[0].setTime();
// For each bird
for ( unsigned int i = 0; i < mMotionStar.getNumSensors(); ++i )
{
// Get the index to the current read buffer
cur_samples[i].setTime( cur_samples[0].getTime() );
cur_samples[i].mPosData = mMotionStar.getDeviceData(i);
}
}
// Add the current data as a sample
addPositionSample(cur_samples);
retval = true;
}
catch (...)
{
vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_CRITICAL_LVL)
<< clrOutNORM(clrRED, "gadget::MotionStar::sample() caught unknown exception")
<< std::endl << vprDEBUG_FLUSH;
retval = false;
}
}
return retval;
}
bool Vrpn::sample()
{
if ( mTrackerNumber > 0 )
{
std::vector<PositionData> positions(mTrackerNumber);
vpr::Guard<vpr::Mutex> g(mTrackerMutex);
for ( int i = 0; i < mTrackerNumber; ++i )
{
gmtl::Matrix44f pos;
gmtl::setRot(pos, mQuats[i]);
gmtl::setTrans(pos, mPositions[i]);
positions[i].setValue(pos);
positions[i].setTime();
}
addPositionSample(positions);
}
if ( mButtonNumber > 0 )
{
std::vector<DigitalData> buttons(mButtonNumber);
vpr::Guard<vpr::Mutex> g(mButtonMutex);
for ( int i = 0; i < mButtonNumber; ++i )
{
buttons[i] = mButtons[i];
buttons[i].setTime();
}
addDigitalSample(buttons);
}
if ( mAnalogNumber > 0 )
{
std::vector<AnalogData> analogs(mAnalogNumber);
vpr::Guard<vpr::Mutex> g(mAnalogMutex);
for ( int i = 0; i < mAnalogNumber; ++i )
{
analogs[i] = mAnalogs[i];
analogs[i].setTime();
}
addAnalogSample(analogs);
}
return true;
}
void SimSetablePosition::setData(const PositionData& posData)
{
std::vector<PositionData> pos_data_vec;
pos_data_vec.push_back(posData);
mPos.setTime(); // Set the time
addPositionSample(pos_data_vec);
swapPositionBuffers(); // Swap the buffers
//vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_DETAILED_LVL)
// << "[SimSetablePosition::setData()] "
// << *(const_cast<PositionData*>(&pos_data)->getPosition()) << std::endl
// << vprDEBUG_FLUSH;
}
/**
* Updates to the sampled data.
*
* @pre None.
* @post Most recent value is copied over to temp area
*/
void TrackdSensor::updateData()
{
vprASSERT(mTrackdSensors != NULL && "Make sure that trackd sensors has been initialized");
vprASSERT((unsigned)mTrackdSensors->numSensors() <= mCurSensorValues.size());
for(int i=0;i<mTrackdSensors->numSensors();i++)
{
mCurSensorValues[i].mPosData = mTrackdSensors->getSensorPos(i);
mCurSensorValues[i].setTime();
}
// Update the data buffer
addPositionSample(mCurSensorValues);
swapPositionBuffers();
}
bool P5GloveWrapper::sample()
{
P5GloveStandalone::Record rec;
bool sample_read = mGlove->readRecordsFromHardware(rec);
if ( sample_read )
{
// Furst, we set the flexion of the gloves
mAnalogP5[0] = rec.thumb; // Thumb (0.0 - 1.0)
mAnalogP5[1] = rec.index; // Index
mAnalogP5[2] = rec.middle; // Middle
mAnalogP5[3] = rec.ring; // Ring
mAnalogP5[4] = rec.pinky; // Pinky
addAnalogSample(mAnalogP5);
swapAnalogBuffers();
// Then, we define the buttons of the glove
mDigitalP5[0] = static_cast<DigitalState::State>(rec.buttonA);
mDigitalP5[1] = static_cast<DigitalState::State>(rec.buttonB);
mDigitalP5[2] = static_cast<DigitalState::State>(rec.buttonC);
addDigitalSample(mDigitalP5);
swapDigitalBuffers();
// Finally, we set the position of the glove ...
const gmtl::AxisAnglef rotation(rec.rotationAngle, rec.rotationAxis[0],
rec.rotationAxis[1],
rec.rotationAxis[2]);
const gmtl::Vec3f translation(static_cast<float>(rec.position[0]),
static_cast<float>(rec.position[1]),
static_cast<float>(rec.position[2]));
gmtl::Matrix44f position = gmtl::makeTrans<gmtl::Matrix44f>(translation);
position = position * gmtl::make<gmtl::Matrix44f>(rotation);
mPositionP5[0].setValue(position);
addPositionSample(mPositionP5);
swapPositionBuffers();
}
return true;
}
bool Flock::sample()
{
std::vector<gadget::PositionData> cur_samples(mFlockOfBirds.getNumSensors());
if ( !isActive() )
{
return false;
}
mFlockOfBirds.sample();
// get an initial timestamp for this entire sample. we'll copy it into
// each PositionData for this sample.
if (!cur_samples.empty())
{
cur_samples[0].setTime();
}
vpr::Thread::yield();
// For each bird
for (unsigned i=0; i < mFlockOfBirds.getNumSensors(); ++i)
{
// Transforms between the cord frames
gmtl::Matrix44f transmitter_T_reciever = mFlockOfBirds.getSensorPosition(i);
// Set timestamp & Store the corrected xform back into buffer.
cur_samples[i].mPosData = transmitter_T_reciever;
cur_samples[i].setTime (cur_samples[0].getTime());
}
// Add data sample
addPositionSample(cur_samples);
return true;
}
bool DTrack::sample()
{
bool stat;
int i, j, id;
int nbt, nvt;
int num_body, num_flystick, num_meatool;
if(!thrRunning){
return false;
}
stat = standalone->receive(); // receive data from DTrack (blocking with timeout)
if(!stat){
return false;
}
static int known_num_body = 0;
num_body = standalone->get_num_body();
num_flystick = standalone->get_num_flystick();
num_meatool = standalone->get_num_meatool();
if (known_num_body != num_body) {
vprDEBUG(vprDBG_ALL, vprDBG_CONFIG_LVL)
<< "[DTrack] Change in the number of known bodies - "
<< "was " << known_num_body << ", now is " << num_body
<< std::endl << vprDEBUG_FLUSH;
known_num_body = num_body;
}
resize_curPosition(num_flystick + num_meatool + num_body);
resize_curDigital(num_flystick * BUTTONS_PER_FLYSTICK + num_meatool * BUTTONS_PER_MEATOOL);
resize_curAnalog(num_flystick * VALUATORS_PER_FLYSTICK);
// get 'Flystick' data:
for(i=0; i<num_flystick; i++){
dtrack_flystick_type dat = standalone->get_flystick(i);
if(dat.quality >= 0){ // check if Flystick position is tracked
curPosition[i].setValue(getpos(dat));
curPosition[i].setTime();
} // otherwise keep last valid position
// Flystick buttons:
nbt = dat.num_button;
if(nbt > BUTTONS_PER_FLYSTICK){
nbt = BUTTONS_PER_FLYSTICK;
}
for(j=0; j<nbt; j++){
id = j + i * BUTTONS_PER_FLYSTICK; // VRJuggler id number
curDigital[id] =
static_cast<DigitalState::State>(dat.button[j]);
curDigital[id].setTime();
}
// Flystick valuators ('HAT switch' or 'joystick'):
nvt = dat.num_joystick;
if(nvt > VALUATORS_PER_FLYSTICK){
nvt = VALUATORS_PER_FLYSTICK;
}
for(j=0; j<nvt; j++){
id = j + i * VALUATORS_PER_FLYSTICK; // VRJuggler id number
curAnalog[id] = dat.joystick[j];
curAnalog[id].setTime();
}
}
// get 'measurement tool' data:
for(i=0; i<num_meatool; i++){
dtrack_meatool_type dat = standalone->get_meatool(i);
if(dat.quality >= 0){ // check if position is tracked
id = i + num_flystick; // VRJuggler id number
curPosition[id].setValue(getpos(dat));
curPosition[id].setTime();
} // otherwise keep last valid position
// measurement tool buttons:
nbt = dat.num_button;
if(nbt > BUTTONS_PER_MEATOOL){
nbt = BUTTONS_PER_MEATOOL;
}
for(j=0; j<nbt; j++){
id = j + i * BUTTONS_PER_MEATOOL + num_flystick * BUTTONS_PER_FLYSTICK; // VRJuggler id number
curDigital[id] =
static_cast<DigitalState::State>(dat.button[j]);
curDigital[id].setTime();
}
}
// get 'standard body' data:
for(i=0; i<num_body; i++){
dtrack_body_type dat = standalone->get_body(i);
if(dat.quality >= 0){ // check if position is tracked
id = i + num_flystick + num_meatool; // VRJuggler id number
curPosition[id].setValue(getpos(dat));
curPosition[id].setTime();
} // otherwise keep last valid position
}
// update buffers:
addPositionSample(curPosition);
addDigitalSample(curDigital);
addAnalogSample(curAnalog);
return true;
}
bool DTrack::sample()
{
bool stat;
int i, j, id;
int nbt, nvt;
int num_body, num_flystick, num_meatool;
if(!thrRunning){
return false;
}
stat = standalone->receive(); // receive data from DTrack (blocking with timeout)
if(!stat){
return false;
}
num_body = standalone->get_num_body();
num_flystick = standalone->get_num_flystick();
num_meatool = standalone->get_num_meatool();
resize_curPosition(num_flystick + num_meatool + num_body);
resize_curDigital(num_flystick * BUTTONS_PER_FLYSTICK + num_meatool * BUTTONS_PER_MEATOOL);
resize_curAnalog(num_flystick * VALUATORS_PER_FLYSTICK);
// get 'Flystick' data:
for(i=0; i<num_flystick; i++){
dtrack_flystick_type dat = standalone->get_flystick(i);
if(dat.quality >= 0){ // check if Flystick position is tracked
curPosition[i].mPosData = getpos(dat);
curPosition[i].setTime();
} // otherwise keep last valid position
// Flystick buttons:
nbt = dat.num_button;
if(nbt > BUTTONS_PER_FLYSTICK){
nbt = BUTTONS_PER_FLYSTICK;
}
for(j=0; j<nbt; j++){
id = j + i * BUTTONS_PER_FLYSTICK; // VRJuggler id number
curDigital[id] = dat.button[j];
curDigital[id].setTime();
}
// Flystick valuators ('HAT switch' or 'joystick'):
nvt = dat.num_joystick;
if(nvt > VALUATORS_PER_FLYSTICK){
nvt = VALUATORS_PER_FLYSTICK;
}
for(j=0; j<nvt; j++){
id = j + i * VALUATORS_PER_FLYSTICK; // VRJuggler id number
curAnalog[id] = dat.joystick[j] / 2.0 + 0.5; // normalizing
curAnalog[id].setTime();
}
}
// get 'measurement tool' data:
for(i=0; i<num_meatool; i++){
dtrack_meatool_type dat = standalone->get_meatool(i);
if(dat.quality >= 0){ // check if position is tracked
id = i + num_flystick; // VRJuggler id number
curPosition[id].mPosData = getpos(dat);
curPosition[id].setTime();
} // otherwise keep last valid position
// measurement tool buttons:
nbt = dat.num_button;
if(nbt > BUTTONS_PER_MEATOOL){
nbt = BUTTONS_PER_MEATOOL;
}
for(j=0; j<nbt; j++){
id = j + i * BUTTONS_PER_MEATOOL + num_flystick * BUTTONS_PER_FLYSTICK; // VRJuggler id number
curDigital[id] = dat.button[j];
curDigital[id].setTime();
}
}
// get 'standard body' data:
for(i=0; i<num_body; i++){
dtrack_body_type dat = standalone->get_body(i);
if(dat.quality >= 0){ // check if position is tracked
id = i + num_flystick + num_meatool; // VRJuggler id number
curPosition[id].mPosData = getpos(dat);
curPosition[id].setTime();
} // otherwise keep last valid position
}
// update buffers:
addPositionSample(curPosition);
addDigitalSample(curDigital);
addAnalogSample(curAnalog);
return true;
}
bool IntersenseAPI::sample()
{
// If we are not active, then don't try to sample.
if (!isActive())
{
return false;
}
// Check to see if we have new data to pull
if ( ! mTracker.updateData() )
{
vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_CRITICAL_LVL)
<< clrOutBOLD(clrRED, "[gadget::IntersenseAPI::sample()]")
<< ": Could not read data from InterSense API driver!\n"
<< vprDEBUG_FLUSH;
return false;
}
// This is the some code for the beginnings of trying to eliminate
// the sleep in the control loop. Needs more testing.
/*
bool has_new_data(false);
for ( unsigned int i = 0 ; i < mStations.size() ; ++i )
{
// Make sure station is enabled and tracker has updated data.
if( mStations[i].enabled && mTracker.hasData(mStations[i].stationIndex) )
{
has_new_data = true;
break;
}
}
// If there wasn't any new data then reliquish control to the cpu and return
if( ! has_new_data )
{
vpr::Thread::yield();
vpr::System::msleep(10);
}
*/
// Create the data buffers to put the new data into.
std::vector<gadget::PositionData> cur_pos_samples(mStations.size());
std::vector<gadget::DigitalData> cur_digital_samples;
std::vector<gadget::AnalogData> cur_analog_samples;
// get an initial timestamp for this entire sample. we'll copy it into
// each PositionData for this sample.
if ( ! cur_pos_samples.empty() )
{
cur_pos_samples[0].setTime();
}
for ( unsigned int i = 0 ; i < mStations.size() ; ++i )
{
// Get the station index for the given station.
int stationIndex = mStations[i].stationIndex;
// Set the time of each PositionData to match the first.
cur_pos_samples[i].setTime( cur_pos_samples[0].getTime() );
// Don't process data from disabled stations
if( ! mStations[i].enabled )
{
continue;
}
gmtl::Matrix44f& pos_data(cur_pos_samples[i].editValue());
gmtl::identity(pos_data);
// If the Intersense is returning data in Euler format. Otherwise we
// assume that it is returning data in quaternion format.
if ( mTracker.getAngleFormat(stationIndex) == ISD_EULER )
{
gmtl::EulerAngleZYXf euler(
gmtl::Math::deg2Rad(mTracker.zRot(stationIndex)),
gmtl::Math::deg2Rad(mTracker.yRot(stationIndex)),
gmtl::Math::deg2Rad(mTracker.xRot(stationIndex))
);
gmtl::setRot(pos_data, euler);
}
else
{
gmtl::Quatf quatValue(mTracker.xQuat(stationIndex),
mTracker.yQuat(stationIndex),
mTracker.zQuat(stationIndex),
mTracker.wQuat(stationIndex));
gmtl::setRot(pos_data, quatValue);
}
gmtl::setTrans(pos_data,
gmtl::Vec3f(mTracker.xPos(stationIndex),
mTracker.yPos(stationIndex),
mTracker.zPos(stationIndex)));
// We start at the index of the first digital item (set in the config
// files) and we copy the digital data from this station to the
// InterSense device for range (min -> min+count-1).
if (mStations[i].useDigital)
{
for ( int j = 0; j < mStations[i].dig_num; ++j )
{
DigitalData new_digital(mTracker.buttonState(stationIndex, j));
new_digital.setTime();
cur_digital_samples.push_back(new_digital);
}
}
// Analog works the same as the digital
if (mStations[i].useAnalog)
{
for ( int j = 0; j < mStations[i].ana_num; ++j )
{
AnalogData new_analog(mTracker.analogData(stationIndex, j));
new_analog.setTime();
cur_analog_samples.push_back(new_analog);
}
}
}
// Lock and then swap the buffers.
addAnalogSample(cur_analog_samples);
addDigitalSample(cur_digital_samples);
addPositionSample(cur_pos_samples);
return true;
}
