robotskill.h

一个简单使用的控制器

#ifndef ROBOT_SKILL_H
#define ROBOT_SKILL_H
#include <vector>
#include "Action.h"
#include "RobotAction.h"
#include "VFHState.h"
#include "msg_types.h"
#include "RFIDAction.h"

using namespace std;



class RobotSkill : public Action
{
   public:
      struct Point {
         double x, z;
      };

      struct Pos {
         struct timeval timeStamp;  /* timestamp */
         double x, z, th;           /* position */
         double tVel;               /* translational velocity */
         double rVel;               /* rotational velocity */
      };

      enum TAG_DEPLOY_STATE {
         INIT_TAG_DEPLOY,
         DEPLOY_TAG,
         WAIT_FOR_ACK,
         END_TAG_DEPLOY
      };

      RobotAction *robot;
      RFIDAction *rfidA;

   public:
      void update(rescue_path_message &msg);
      void updateVictimRFID(bool ackState);
      void updateRangeScanRanges(rescue_rangescan_ranges_message &msg);

      RobotSkill(bool useKalmanPoses, bool simulationMode);
      ~RobotSkill() {};
      bool ActionStop();
      bool ActionJoystick();
      bool ActionFollowPath(bool init, VFHState* vfh);
      bool ActionReachGoal(bool init, Pos goal, VFHState* vfh,bool angle=false);
      bool ActionReachGoalWithObstacles(bool init, Pos goal, VFHState* vfh);
      bool ActionBestAngleFreeFromStall(bool init, VFHState* vfhState);
      bool ActionRandomFreeFromStall(bool init, VFHState* vfhState);
      bool ActionWaitForVictimAck(bool init);
      bool ActionFreeGradient(RobotSkill::Pos goal);

      int getBaffsEstimate();
      bool VictimRFIDWaitRequired();
      void setVictimWait();
	bool robotInStall(int bumps);
      void setReachedDist(double d){REACHED_DIST=d;}
   protected:
      double distanceRobotToPoint(double x, double z);
      double angleRobotToPoint(double x, double z);

   private:
       int getBestDir(Pos goal, VFHState* vfhState);
      vector<Pos> _path;
      vector<long> _lastRFIDTags;
      void adjustPathToRobotPos();
      struct timeval _activePercetionTimeout;
      struct timeval _co2StartTime;
      struct timeval _tagDeployStartTime;
      Pos _lastRFIDPos;
      bool _lastRFIDPosValid;
      int _lastActivePerception;
      double _angleBeforeTurn;
      bool _turnAroundSecondHalf;
     bool _activePerceptionDone;
      bool _initActivePerception;
      bool _victimAck;
      Pos _goal;
      Pos _beginPos;
      struct timeval _startedToStopRobot;
      int _tagDeployState;
      bool _simulationMode;
      int _angleHyst;
      rescue_path_message path_message;
      rescue_active_perception_req_message _actPerceptionReqMessage;
      bool _automaticallyDeployTags;
	double REACHED_DIST;
	//Stall recognition
	double _rotCmdSpeed;
	double _lastRotCmdSpeed;
	double _lastInu;
	struct timeval _lastInuTime;
	int _countStall;
	int _countSmallStall;
};
#endif

/*********************************************************************
 * (C) Copyright 2006 Albert Ludwigs University Freiburg
 *     Institute of Computer Science
 *
 * All rights reserved. Use of this software is permitted for
 * non-commercial research purposes, and it may be copied only
 * for that use. All copies must include this copyright message.
 * This software is made available AS IS, and neither the authors
 * nor the Albert Ludwigs University Freiburg make any warranty
 * about the software or its performance.
 *********************************************************************/