---

TurnNeckToPoint.cc

Go to the documentation of this file.

/* -*- Mode: C++ -*-
 * $Id: TurnNeckToPoint.cc,v 1.7 2006/09/06 19:08:59 frehe Exp $
 *
 *
 * COPYRIGHT INFORMATION
 *
 * This file is part of RoboSoc created by Fredrik Heintzfrehe <@ida.liu.se>
 * Copyright (C) 1999, 2000 Fredrik Heintz, Linköping University, Sweden
 *
 * You are allowed to modify and use this code as long as you retain this
 * notice. If you make any changes or have any comments I would appreciate
 * it if you send me a message. For more information, please see
 * http://www.ida.liu.se/~frehe/RoboCup/RoboSoc/
 *
 *
 * IDENTIFICATION
 *
 * Filename: TurnNeckToPoint.cc
 * Unitname: Framework/skill
 * $Revision: 1.7 $
 * Created by: Fredrik Heintz 2003-09-02
 * Last modified by $Author: frehe $ $Date: 2006/09/06 19:08:59 $
 *
 *
 * HISTORY
 *
 * $Log: TurnNeckToPoint.cc,v $
 * Revision 1.7  2006/09/06 19:08:59  frehe
 * Fixed buggy implementations.
 *
 * Revision 1.6  2005/09/08 08:58:36  frehe
 * Implemented the reset method.
 *
 * Revision 1.5  2004/08/26 17:10:28  frehe
 * Fixed bug (generatePlan didn't return anything).
 *
 * Revision 1.4  2004/08/13 15:39:29  petan
 * Moved implementation to generateStep
 *
 * Revision 1.3  2004/08/13 12:00:39  petan
 * Removed couts and fixed bug with turning head outside limits
 *
 * Revision 1.2  2003/11/07 15:19:29  frehe
 * Make sure the neck angles are valid.
 *
 * Revision 1.1  2003/09/02 17:59:17  frehe
 * Added TurnNeckToPoint and TurnNeckToBall.
 *
 */

#include "TurnNeckToPoint.h"
#include "CommandStrategy.h"
#include "NewCycleStrategy.h"
#include "WorldModelInterface.h"
#include "TurnNeckCommand.h"


RS_BEGIN_NAMESPACE


// --- CONSTRUCTORS ---

TurnNeckToPoint::TurnNeckToPoint(const Point& pos, ServerCommand* cmd)
  : SkillWithPositionAndCommand(pos, cmd)
{
}


// --- DESTRUCTOR ---

TurnNeckToPoint::~TurnNeckToPoint()
{
}


// --- METHODS ---

void
TurnNeckToPoint::reset()
{
}

Plan
TurnNeckToPoint::generatePlan()
{
  return Plan(generateStep());
}


AgentStep
TurnNeckToPoint::generateStep()
{
  const CommandStrategy* cmd_strategy
    = WorldModelInterface::instance()->getCommandStrategy();
  const NewCycleStrategy* new_cycle_strategy
    = WorldModelInterface::instance()->getNewCycleStrategy();

  WorldModel predicted_wm(WorldModelInterface::instance()->getWorld(NOW));

  // Predict the state of the world after executing the command
  if ( command != 0 ) {
    cmd_strategy->updateAfterCommand(predicted_wm, command);
  }
  new_cycle_strategy->updateBeforeNewCycle(predicted_wm);

  // Now predicted_wm contains the predicted world
  AgentObject& agent = predicted_wm.getAgent();
  AngleDegUE turn_dir = agent.getRelativeNeckDirectionToPoint(position);

  AgentStep step;
  if ( turn_dir.isKnown() ) {
    TurnNeckCommand *cmd;
    cmd = new TurnNeckCommand(minMax(agent.getMinNeckDirection().getDeg(),
             turn_dir.getDeg(),
             agent.getMaxNeckDirection().getDeg()));
    step.setNeckCommand(cmd);
  }
  return step;
}


FuzzyBool
TurnNeckToPoint::persistent()
{
  // Never persistent.
  return FuzzyBool(0.0);
}


FuzzyBool
TurnNeckToPoint::applicable()
{
  WorldModel wm(WorldModelInterface::instance()->getWorld(NOW));
  AgentObject& agent = wm.getAgent();

  // Only check body and neck direction since we can't be sure whether
  // the ball will be known after prediction even if it's known now.
  return agent.getBodyDirection().isKnown()
    and agent.getNeckDirection().isKnown();
}


FuzzyBool
TurnNeckToPoint::succeed()
{
  // The skill has not succeeded yet.
  return FuzzyBool(0.0);
}


FuzzyBool
TurnNeckToPoint::failed()
{
  // The skill has not failed yet.
  return FuzzyBool(0.0);
}



RS_END_NAMESPACE

---