//----------------------------------------------------------------------------- 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(); } }