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()
{
  DEBUG_RESPONSE("no wlan:set robot pose angle with robot buttons",
    {
      if(robotState.getButtonPressed( BodyPercept::backFront))
      {
        referencePose.translation.x = 0;
        referencePose.translation.y = 0;
        referencePose.rotation = fromDegrees( 60);
        referenceOdometry = odometryData;
      }
      else if(robotState.getButtonPressed( BodyPercept::backMiddle))
      {
        referencePose.translation.x = 0;
        referencePose.translation.y = 0;
        referencePose.rotation = fromDegrees( 90);
        referenceOdometry = odometryData;
      }
      else if(robotState.getButtonPressed( BodyPercept::backBack))
      {
        referencePose.translation.x = 0;
        referencePose.translation.y = 0;
        referencePose.rotation = fromDegrees( -90);
        referenceOdometry = odometryData;
      }
    }
  );
  
  robotPose.setFrameNumber(psdPercept.percepts[psdPercept.numOfPercepts-1].frameNumber);
   robotPose.setTimestamp(SystemCall::getCurrentSystemTime());

  unsigned long currentTime = SystemCall::getCurrentSystemTime();
  robotPose.validity = 1.0;
  if( (currentTime - timeOfLastExecute) > 2000 )
  {
    referencePose = robotPose;
    referenceOdometry = odometryData;
  }
  else
  {
    DEBUG_RESPONSE("SelfLocator:reset odometry and robotPose",
      referencePose.translation.x = 0;
      referencePose.translation.y = 0;
      referencePose.rotation = 0;
      referenceOdometry = odometryData;
      );
    Pose2D odometry = odometryData - referenceOdometry;
    robotPose = referencePose + odometry;
  }
  timeOfLastExecute = currentTime;

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

---