// return false on any error.
bool processOneObservation(CObservationPtr &o)
{
if (!IS_CLASS(o, CObservationOdometry ) )
return true;
CObservationOdometry* obs = CObservationOdometryPtr(o).pointer();
if (!obs->hasEncodersInfo)
throw std::runtime_error("CObservationOdometry entry does not contain encoder info, cannot recalculate odometry!");
CActionRobotMovement2D auxOdoIncr;
auxOdoIncr.hasEncodersInfo = true;
auxOdoIncr.encoderLeftTicks = obs->encoderLeftTicks;
auxOdoIncr.encoderRightTicks = obs->encoderRightTicks;
auxOdoIncr.computeFromEncoders( KL,KR, D );
if (!m_odo_accum_valid)
{
m_odo_accum_valid=true;
m_odo_accum = obs->odometry;
// and don't modify this odo val.
}
else
{
obs->odometry = m_odo_accum + auxOdoIncr.rawOdometryIncrementReading;
m_odo_accum = obs->odometry;
}
m_entriesSaved++;
return true; // All ok
}
