Modules/BehaviorControl/KickLogger.cpp

Go to the documentation of this file.

/**
* @file KickLogger.cpp
* 
* Implementation of class KickLogger.
*
* @author Martin Lötzsch
*/

#include "KickLogger.h"
#include "Platform/SystemCall.h"
#include "Platform/GTAssert.h"
#include "Tools/Math/Geometry.h"

KickLogger::KickLogger(BehaviorControlInterfaces& interfaces)
: timeOfLastStateChange(SystemCall::getCurrentSystemTime()),
BehaviorControlInterfaces(interfaces), state(initial)
{
}

KickLogger::~KickLogger()
{
}

void KickLogger::execute()
{
  switch (state)
  {
  case initial:
    if (motionRequest.motionType == MotionRequest::specialAction
      && SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen) < 500
      && Geometry::distanceTo(robotPose.getPose(),ballModel.seen) < 350)
    {
      kick.ballXR = getBallX(robotPose);
      kick.ballYR = getBallY(robotPose);
      kick.ballTR = SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen);
      kickID = motionRequest.specialActionRequest.specialActionType;
      odometryDataWhenKickWasStarted = odometryData;
      changeState(kickRequested);
    }
    break;

  case kickRequested:
    if (motionInfo.executedMotionRequest.motionType == MotionRequest::specialAction)
    {
      Pose2D odometryDiff = odometryData- odometryDataWhenKickWasStarted;
      Pose2D relativePoseWhenStarted = robotPose.minusDiff(odometryDiff);

      kick.ballXS = getBallX(relativePoseWhenStarted);
      kick.ballYS = getBallY(relativePoseWhenStarted);
      kick.ballTS = SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen);
      changeState(kickStarted);
    }
    break;

  case kickStarted:
    if (motionInfo.executedMotionRequest.motionType != MotionRequest::specialAction)
    {
      changeState(kickFinished);
    }
    break;

  case kickFinished:
    if (motionInfo.executedMotionRequest.motionType == MotionRequest::specialAction)
    {
      changeState(wait);
    }
    if (SystemCall::getTimeSince(timeOfLastStateChange) > 2000)
    {
      changeState(waitingForBall);
    }
    break;

  case waitingForBall:
    if (motionInfo.executedMotionRequest.motionType == MotionRequest::specialAction)
    {
      changeState(wait);
    }
    if (SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen) < 100)
    {
      Pose2D odometryDiff = odometryData - odometryDataWhenKickWasStarted;
      Pose2D relativePoseWhenStarted = robotPose.minusDiff(odometryDiff);

      kick.ballXA = getBallX(relativePoseWhenStarted);
      kick.ballYA = getBallY(relativePoseWhenStarted);
      kick.tA = SystemCall::getTimeSince(timeOfLastStateChange) + 2000;
      record();
      changeState(wait);
    }
    if (SystemCall::getTimeSince(timeOfLastStateChange) > 4000)
    {
      changeState(wait);
    }
    break;

  case wait:
    if (SystemCall::getTimeSince(timeOfLastStateChange) > 3000
      && (motionInfo.executedMotionRequest.motionType == MotionRequest::walk
      || motionInfo.executedMotionRequest.motionType == MotionRequest::stand))
    {
      changeState(initial);
    }
    break;

  default:
    ASSERT(false);
  }
}

double KickLogger::getBallX(Pose2D from)
{
  return Geometry::distanceTo(from,ballModel.seen) 
    * cos(Geometry::angleTo(from,ballModel.seen));
}

double KickLogger::getBallY(Pose2D from)
{
  return Geometry::distanceTo(from,ballModel.seen) 
    * sin(Geometry::angleTo(from,ballModel.seen));
}

void KickLogger::changeState(State newState)
{
  timeOfLastStateChange = SystemCall::getCurrentSystemTime();
  state = newState;
//  OUTPUT(idText,text,"KickLogger: changed state to " << state << ".");
}

void KickLogger::record()
{
  INFO(sendKickRecords, idKickRecord, bin, SpecialActionRequest::getSpecialActionIDName(kickID) << kick);
}

/*
 * Change log :
 * 
 * $Log: KickLogger.cpp,v $
 * Revision 1.5  2004/06/24 18:32:59  risler
 * bugfix: odometry calculation corrected
 *
 * Revision 1.4  2004/06/23 18:18:27  risler
 * also log failed kicks
 *
 * Revision 1.3  2004/06/20 15:22:35  risler
 * increased kick editor usability:
 * KickLogger now sends kick record via debug key instead of writing to file
 * KickEditor automatically displays sent kick records
 * KickCase moved to KickSelectionTable
 *
 * Revision 1.2  2004/06/02 17:18:24  spranger
 * MotionRequest cleanup
 *
 * Revision 1.1  2004/05/27 16:48:08  loetzsch
 * added the kick logger
 *
 * Revision 1.3  2004/03/20 00:20:18  kerdels
 * removed some errors
 *
 * Revision 1.1  2004/03/16 14:00:17  juengel
 * Integrated Improvments from "Günne"
 * -ATH2004ERS7Behavior
 * -ATHHeadControl
 * -KickSelectionTable
 * -KickEditor
 *
 * Revision 1.1  2004/03/06 12:52:11  loetzsch
 * cloned ATH2004BehaviorControl into ATH2004ERS7BehaviorControl
 *
 * Revision 1.1.1.1  2004/03/05 10:10:11  loetzsch
 * created local cvs for Günne
 *
 * Revision 1.6  2004/03/04 18:30:53  loetzsch
 * bug fix
 *
 * Revision 1.5  2004/02/09 16:45:04  loetzsch
 * bug fix (missing break in switch statement)
 *
 * Revision 1.4  2004/02/04 14:09:56  loetzsch
 * improved
 *
 * Revision 1.3  2004/02/03 23:16:46  loetzsch
 * added debug key sendShootRecords
 *
 * Revision 1.2  2004/01/30 13:25:28  loetzsch
 * using odometry data in shoot logger,
 * first draft of visualization
 *
 * Revision 1.1  2004/01/28 21:16:56  loetzsch
 * added KickLogger
 *
 */

---