Modules/SelfLocator/OdometrySelfLocator.cpp

Go to the documentation of this file.

/**
 * @file Modules/SelfLocator/OdometrySelfLocator.cpp
 * 
 * This file contains a class for self-localization based on odometry only.
 * @author <a href="mailto:juengel@informatik.hu-berlin.de">Matthias Juengel</a>
 */

#include "OdometrySelfLocator.h"
#include "Tools/FieldDimensions.h"
#include "Platform/SystemCall.h"


OdometrySelfLocator::OdometrySelfLocator(const SelfLocatorInterfaces& interfaces)
: SelfLocator(interfaces)
{
  referencePose.translation.x = 0;
  referencePose.translation.y = 0;
  referencePose.rotation = 0;
  
  timeOfLastExecute = 0;
}

void OdometrySelfLocator::execute()
{
   robotPose.setFrameNumber(psdPercept.percepts[psdPercept.numOfPercepts-1].frameNumber);

  unsigned long currentTime = SystemCall::getCurrentSystemTime();
  if( (currentTime - timeOfLastExecute) > 2000 )
  {
    referencePose = robotPose;
    referenceOdometry = odometryData;
  }
  else
  {
    Pose2D odometry = odometryData - referenceOdometry;
    robotPose = referencePose + odometry;
  }
  timeOfLastExecute = currentTime;

  selfLocatorSamples.link(NULL, 0,0);
}

/*
 * Change log :
 * 
 * $Log: OdometrySelfLocator.cpp,v $
 * Revision 1.2  2004/05/24 16:27:11  goehring
 * frameNumber bug removed
 *
 * Revision 1.1.1.1  2004/05/22 17:20:48  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.4  2004/03/08 02:11:48  roefer
 * Interfaces should be const
 *
 * Revision 1.3  2003/11/14 19:02:26  goehring
 * frameNumber added
 *
 * Revision 1.2  2003/11/13 16:47:38  goehring
 * frameNumber added
 *
 * Revision 1.1  2003/10/06 14:10:15  cvsadm
 * Created GT2004 (M.J.)
 *
 * Revision 1.1.1.1  2003/07/02 09:40:24  cvsadm
 * created new repository for the competitions in Padova from the 
 * tamara CVS (Tuesday 2:00 pm)
 *
 * removed unused solutions
 *
 * Revision 1.4  2003/05/21 13:36:20  brunn
 * set selfLocatorSamples to NULL as these locators do not have any samples
 *
 * Revision 1.3  2003/01/22 14:59:29  dueffert
 * doxygen docu corrected
 *
 * Revision 1.2  2003/01/12 15:46:57  juengel
 * Comments corrected.
 *
 * Revision 1.1  2003/01/12 15:24:47  juengel
 * Added OdometrySelfLocator
 *
 */

---