Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/SelfLocator/OdometrySelfLocator.cpp

Go to the documentation of this file.

    00001 /**
    00002  * @file Modules/SelfLocator/OdometrySelfLocator.cpp
    00003  *
    00004  * This file contains a class for self-localization based on odometry only.
    00005  * @author <a href="mailto:juengel@informatik.hu-berlin.de">Matthias Juengel</a>
    00006  */
    00007
    00008 #include "OdometrySelfLocator.h"
    00009 #include "Tools/FieldDimensions.h"
    00010 #include "Platform/SystemCall.h"
    00011
    00012
    00013 OdometrySelfLocator::OdometrySelfLocator(const SelfLocatorInterfaces& interfaces)
    00014 : SelfLocator(interfaces)
    00015 {
    00016   referencePose.translation.x = 0;
    00017   referencePose.translation.y = 0;
    00018   referencePose.rotation = 0;
    00019
    00020   timeOfLastExecute = 0;
    00021 }
    00022
    00023 void OdometrySelfLocator::execute()
    00024 {
    00025   DEBUG_RESPONSE("no wlan:set robot pose angle with robot buttons",
    00026     {
    00027       if(robotState.getButtonPressed( BodyPercept::backFront))
    00028       {
    00029         referencePose.translation.x = 0;
    00030         referencePose.translation.y = 0;
    00031         referencePose.rotation = fromDegrees( 60);
    00032         referenceOdometry = odometryData;
    00033       }
    00034       else if(robotState.getButtonPressed( BodyPercept::backMiddle))
    00035       {
    00036         referencePose.translation.x = 0;
    00037         referencePose.translation.y = 0;
    00038         referencePose.rotation = fromDegrees( 90);
    00039         referenceOdometry = odometryData;
    00040       }
    00041       else if(robotState.getButtonPressed( BodyPercept::backBack))
    00042       {
    00043         referencePose.translation.x = 0;
    00044         referencePose.translation.y = 0;
    00045         referencePose.rotation = fromDegrees( -90);
    00046         referenceOdometry = odometryData;
    00047       }
    00048     }
    00049   );
    00050
    00051   robotPose.setFrameNumber(psdPercept.percepts[psdPercept.numOfPercepts-1].frameNumber);
    00052    robotPose.setTimestamp(SystemCall::getCurrentSystemTime());
    00053
    00054   unsigned long currentTime = SystemCall::getCurrentSystemTime();
    00055   robotPose.validity = 1.0;
    00056   if( (currentTime - timeOfLastExecute) > 2000 )
    00057   {
    00058     referencePose = robotPose;
    00059     referenceOdometry = odometryData;
    00060   }
    00061   else
    00062   {
    00063     DEBUG_RESPONSE("SelfLocator:reset odometry and robotPose",
    00064       referencePose.translation.x = 0;
    00065       referencePose.translation.y = 0;
    00066       referencePose.rotation = 0;
    00067       referenceOdometry = odometryData;
    00068       );
    00069     Pose2D odometry = odometryData - referenceOdometry;
    00070     robotPose = referencePose + odometry;
    00071   }
    00072   timeOfLastExecute = currentTime;
    00073
    00074   selfLocatorSamples.link(NULL, 0,0);
    00075 }