//-----------------------------------------------------------------------------
void CStageDrivetrain2dof::updateData( const double dt)
{
float time;
if ( mFgEnabled ) {
applyVelocityLimits();
applyAccelerationLimits( mStgPosition->GetPoseInterval() / 1e6);
mStgPosition->SetXSpeed ( mVelocityLimitedCmd.mXDot );
mStgPosition->SetTurnSpeed ( mVelocityLimitedCmd.mYawDot );
// update odometry
( ( CStageOdometry* ) mOdometry )->updateData(dt);
// stage doesn't seem to allow us access to the sensed speed of the robot
// so we simply assume it is the commanded velocity
mVelocityMeas = mVelocityLimitedCmd;
time = mTimeStamp;
mTimeStamp = mStgPosition->GetWorld()->SimTimeNow() / 1e6;
mFgStalled = mStgPosition->Stalled();
if (mFgStalled)
mStalledTimer += mTimeStamp - time;
else
mStalledTimer = 0.0;
notifyDataUpdateObservers();
}
}
//-----------------------------------------------------------------------------
void CStageLaser::updateData( const double dt )
{
//std::vector<Stg::ModelLaser::Sample>* sample;
//tRangeData* temp;
//uint32_t sampleCount;
if ( mFgEnabled ) {
// get range data from stage, if the simulation is paused at start up
// stage returns NULL, so we store the result in a temporal variable
// and store is only if it is any good
Stg::ModelLaser::Config config = mStgLaser->GetConfig();
uint32_t sampleCount = config.sample_count;
if ( sampleCount != mNumSamples ) {
ERROR2( "Got wrong number of laser readings from Stage, "\
"expected %d but got %d", mNumSamples, sampleCount );
}
else {
const std::vector<Stg::ModelLaser::Sample> sample = mStgLaser->GetSamples();
if ( !sample.empty() ) {
for ( unsigned int i = 0; i < mNumSamples; i ++ ) {
mRangeData[i].range = sample[i].range;
mRangeData[i].reflectance = sample[i].reflectance;
}
}
}
mTimeStamp = mStgLaser->GetWorld()->SimTimeNow() / 1e6;
notifyDataUpdateObservers();
}
}
//-----------------------------------------------------------------------------
void CStagePowerPack::updateData( const double dt)
{
double prevBatteryCapacity;
if ( ( mFgEnabled ) && ( mStgPowerPack ) ) {
prevBatteryCapacity = mBatteryCapacity;
if (mStgPowerPack->GetStored() < 0) {
mBatteryCapacity = INFINITY;
mMaxBatteryCapacity = INFINITY;
}
else {
mBatteryCapacity = JOULES_TO_WATTHOURS ( mStgPowerPack->GetStored() );
mMaxBatteryCapacity = JOULES_TO_WATTHOURS ( mStgPowerPack->GetCapacity() );
}
mTotalEnergyDissipated = JOULES_TO_WATTHOURS (
mStgPowerPack->GetDissipated() );
// take the change in battery capacity (in Ah)
// devide by simulution step duration (converted to hours)
mCurrent = ( prevBatteryCapacity - mBatteryCapacity ) /
SECONDS_TO_HOURS ( mSimInterval ) / mVoltage;
mTimeStamp = mStgModel->GetWorld()->SimTimeNow() / 1e6;
notifyDataUpdateObservers();
}
}
//-----------------------------------------------------------------------------
void CStageLaser::updateData( const double dt )
{
if ( mFgEnabled ) {
// get range data from stage, if the simulation is paused at start up
// stage returns NULL, so we store the result in a temporal variable
// and store is only if it is any good
Stg::ModelRanger::Sensor sensor = mStgLaser->GetSensors()[0];
uint32_t sampleCount = sensor.sample_count;
if ( sampleCount != mNumSamples ) {
ERROR2( "Got wrong number of laser readings from Stage, "\
"expected %d but got %d", mNumSamples, sampleCount );
}
else {
if ( !sensor.ranges.empty() ) { //!sample.empty() ) {
for ( unsigned int i = 0; i < mNumSamples; i ++ ) {
mRangeData[i].range = sensor.ranges[i];
mRangeData[i].reflectance = sensor.intensities[i];
}
}
}
mTimeStamp = mStgLaser->GetWorld()->SimTimeNow() / 1e6;
notifyDataUpdateObservers();
}
}
