Modules/SelfLocator/NoOdometrySelfLocator.cpp

Go to the documentation of this file.

/**
* @file Modules/SelfLocator/NoOdometrySelfLocator.cpp
* 
* This file contains a class for self-localization based on SingleLandmarks w/o odometry.
* @author <a href="mailto:spranger@informatik.hu-berlin.de">Michael Spranger</a>
*/

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



NoOdometrySelfLocator::NoOdometrySelfLocator(const SelfLocatorInterfaces& interfaces)
: SelfLocator(interfaces),refGoalvalid(false), debugflags(0), debugframes(10), speed(0)
{ 
}

void NoOdometrySelfLocator::execute()
{
  
  
  robotPose.setFrameNumber(landmarksPercept.frameNumber);
  
  
  if(landmarksPercept.numberOfGoals)
  {
    Goal tempGoal = landmarksPercept.goals[0];

      
    double cameraOffset = 0;
    if(debugflags & 0x10){
      /*tempGoal     */
    }
        
    /*
     * if 2 goals are seen reference the one closest (in anticipation that the distance is mor correct
     */
    if(landmarksPercept.numberOfGoals == 2 && landmarksPercept.goals[0].distance > landmarksPercept.goals[1].distance)
    {
       tempGoal = landmarksPercept.goals[1];
    }

    /*
     * locate self by which goal is seen and distance to it
     */
    if(
      (getPlayer().getTeamColor() == Player::red && tempGoal.color == skyblue) 
      || 
      (getPlayer().getTeamColor() == Player::blue && tempGoal.color == yellow)
      )
    {
      robotPose.setPose( Pose2D(0,(-1 * (xPosOpponentGroundline - (tempGoal.distance + cameraOffset))), 0));
    }
    else
    {
      robotPose.setPose( Pose2D(0,(xPosOpponentGroundline - (tempGoal.distance + cameraOffset)), 0));
    }
    
    /*
     * calculate speed
     */
    if(refGoalvalid)
    {
      speed = 8 * fabs(referenceGoal.distance - tempGoal.distance)/(landmarksPercept.frameNumber - refGoalFramenr);
       
    }

    /*
     * set reference goal to the currentlyProcessed
     */
    referenceGoal = landmarksPercept.goals[0];
    refGoalFramenr = landmarksPercept.frameNumber;
    refGoalvalid = 1;
   
  }
  else
  {
    refGoalvalid=0;
  }


  /*
   * Debugging
   * frames & flags
   * flags is data [0]
   * frames is data[1]
   */
  if(debugframes && landmarksPercept.frameNumber % (int)debugframes == 0)
  {
    if(debugflags & 1)
    {
        OUTPUT(idText, text,"::speed:"<<speed);
    }
    if(2 & debugflags)
    {
        OUTPUT(idText, text,"::x:"<<robotPose.translation.x);
    }
    if(4 & debugflags)
    {
        OUTPUT(idText, text,"::y:"<<robotPose.translation.y);
    }
    if(8 & debugflags)
    {
        OUTPUT(idText, text,"::cOffx:"<<landmarksPercept.cameraOffset.x);
        OUTPUT(idText, text,"::cOffy:"<<landmarksPercept.cameraOffset.y);
        OUTPUT(idText, text,"::cOffz:"<<landmarksPercept.cameraOffset.z);
    }
  }
  /*

        OUTPUT(idText, text,"::distance:"<<landmarksPercept.goals[0].distance);
        OUTPUT(idText, text,"::xPosOpon:"<<xPosOpponentGroundline);
  */
  if(robotPose.translation.y != 0)
  {
        OUTPUT(idText, text, "ERROR!!!! y should not change.");
  }
}

bool NoOdometrySelfLocator::handleMessage(InMessage& message)
{
  bool handled = false;

  switch(message.getMessageID())
  {
  // case id... : ... break;

  case idGenericDebugData:  GenericDebugData d;
                            message.bin >> d;
                            if(d.id == GenericDebugData::noOdometrySelfLocator)
                            {
                              OUTPUT(idText,text,
                                "message handled by module NoOdometrySelfLocator");
                              debugflags = static_cast<int> (d.data[0]);
                              if(d.data[1])
                              {
                                debugframes = static_cast<int>(d.data[1]);
                              }
                            }
                            handled = true;
                            break;
   }
return handled;
}
/*
* Change log :
* 
* $Log: NoOdometrySelfLocator.cpp,v $
* Revision 1.2  2004/05/22 22:52:03  juengel
* Renamed ballP_osition to ballModel.
*
* Revision 1.1.1.1  2004/05/22 17:20:48  cvsadm
* created new repository GT2004_WM
*
* Revision 1.5  2004/03/08 02:11:48  roefer
* Interfaces should be const
*
* Revision 1.4  2004/02/03 13:20:48  spranger
* renamed all references to  class BallP_osition to BallModel (possibly changed include files)
*
* Revision 1.3  2003/12/18 11:48:45  spranger
* added support for seeing 2 goals and some mor debugmessage handling
*
* Revision 1.2  2003/12/03 21:03:45  spranger
* added DebugMessageHandling (handleMessage)
*
* Revision 1.1  2003/12/02 19:18:21  spranger
* added NoOdometrySelfLocator
*
*
*/

---