#include "walkto.ih"rf<Behavior::Goal> WalkTo::generateGoal(unsigned step, unsigned slot){  Vector3D pos, straight(1,0,0);  double angleToPos;    rf<Goal> goal = new Goal();  rf<OrNode> dis = goal->addDisjunct();  rf<AndNode> con = dis->addConjunct();        switch (step)  {    case 0:      pos = d_goal->getVector("Pos");      pos.setZ(0);      angleToPos = pos.angle(straight) / M_PI * 180;      if (pos.getY() < 0)        angleToPos = -angleToPos;              _debugLevel4(ident() << " " << pos << " " << angleToPos);            con->addVar("Angle", angleToPos, angleToPos);          return goal;          case 1:      return d_goal;  }    return 0;}