ArActionTriangleDriveTo.h

/*
ActivMedia Robotics Interface for Applications (ARIA)
Copyright (C) 2004,2005 ActivMedia Robotics, LLC


     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation; either version 2 of the License, or
     (at your option) any later version.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program; if not, write to the Free Software
     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

If you wish to redistribute ARIA under different terms, contact 
ActivMedia Robotics for information about a commercial version of ARIA at 
robots@activmedia.com or 
ActivMedia Robotics, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481

*/

#ifndef ARACTIONTRIANGLEDRIVETO
#define ARACTIONTRIANGLEDRIVETO

#include "ariaTypedefs.h"
#include "ArAction.h"
#include "ArLineFinder.h"


class ArActionTriangleDriveTo : public ArAction
{
public:
  ArActionTriangleDriveTo(const char *name = "triangleDriveTo", 
                                   double finalDistFromVertex = 400,
                                   double approachDistFromVertex = 1000,
                                   double speed = 200, 
                                   double closeDist = 100,
                                   double acquireTurnSpeed = 30);
  virtual ~ArActionTriangleDriveTo();
  void setAcquire(bool acquire = false) { myAcquire = acquire; }
  bool getAcquire(void) { return myAcquire; }
  void setFinalDistFromVertex(double dist) { myFinalDistFromVertex = dist; }
  double getFinalDistFromVertex(void) { return myFinalDistFromVertex; }
  void setTriangleParams(double line1Length = 254, 
                         double angleBetween = 135,
                         double line2Length = 254)
    { myLine1Length = line1Length; myAngleBetween = angleBetween; myLine2Length = line2Length; }
  void setParameters(double finalDistFromVertex = 400,
                     double approachDistFromVertex = 1000,
                     double speed = 200, 
                     double closeDist = 100,
                     double acquireTurnSpeed = 30);

  bool getAdjustVertex(void) { return myAdjustVertex; }
  void setAdjustVertex(bool adjustVertex) { myAdjustVertex = adjustVertex; }
  bool getGotoVertex(void) { return myGotoVertex; }
  void setGotoVertex(bool gotoVertex) 
    { myGotoVertex = gotoVertex; }
  /*
  void setIgnoreTriangleDist(double dist = 250) 
    { myIgnoreTriangleDist = dist; }
  double getIgnoreTriangleDist(void) { return myIgnoreTriangleDist; }
  */
  void setVertexUnseenStopMSecs(int vertexUnseenStopMSecs = 4000)
    { myVertexUnseenStopMSecs = vertexUnseenStopMSecs; }
  int getVertexUnseenStopMSecs(void) { return myVertexUnseenStopMSecs; }
  enum State { 
    STATE_INACTIVE, 
    STATE_ACQUIRE, 
    STATE_SEARCHING, 
    STATE_GOTO_APPROACH, 
    STATE_ALIGN_APPROACH, 
    STATE_GOTO_VERTEX, 
    STATE_GOTO_FINAL, 
    STATE_ALIGN_FINAL, 
    STATE_SUCCEEDED, 
    STATE_FAILED 
  };
  State getState(void) { return myState; }
  void setLineFinder(ArLineFinder *lineFinder);
  ArLineFinder *getLineFinder(void) { return myLineFinder; }
  
  void setLogging(bool logging) { myPrinting = logging; }
  bool setLogging(void) { return myPrinting; }
  virtual void activate(void);
  virtual void deactivate(void);
  virtual void setRobot(ArRobot *robot);
  virtual ArActionDesired *fire(ArActionDesired currentDesired);
  virtual ArActionDesired *getDesired(void) { return &myDesired; }
  class Data 
  {
    public:
    Data() 
    { myFinalSet = false; myApproachSet = false; myLinesAndVertexSet = false; }
    ~Data() {}
    void setLinesAndVertex(std::map<int, ArLineFinderSegment *> *lines, 
                           ArPose vertex)
    {
      myLines.clear();
      std::map<int, ArLineFinderSegment *>::iterator it;
      for (it = lines->begin(); it != lines->end(); it++)
      {
        myLines.push_front(*(*it).second);
      }
      myVertex = vertex;
      myLinesAndVertexSet = true;
    }
    void setFinal(ArPose final) { myFinalSet = true; myFinal = final; }
    void setApproach(ArPose approach) 
    { myApproachSet = true; myApproach = approach; }
    bool myLinesAndVertexSet;
    std::list<ArLineSegment> myLines;
    ArPose myVertex;
    bool myFinalSet;
    ArPose myFinal;
    bool myApproachSet;
    ArPose myApproach;
  };
  void setSaveData(bool saveData) { mySaveData = saveData; }
  bool getSaveData(void) { return mySaveData; }
#ifndef SWIG

  Data *getData(void);
#endif // SWIG
protected:
  // Find our triangle, Internal function
  void findTriangle(bool initial, bool goStraight = false);
  // Finds a pose this dist from the vertex along the line from the vertex
  ArPose findPoseFromVertex(double distFromVertex);
  // where the vertex was in encoder coords
  ArPose myVertex;

  ArRangeDevice *myLaser;
  ArMutex myDataMutex;
  bool mySaveData;
  Data *myData;
  std::map<int, ArLineFinderSegment *> *myLines;
  unsigned int myGotLinesCounter;
  bool myVertexSeen;
  bool myPrinting;
  ArTime myVertexSeenLast;
  State myState;
  bool myAcquire;
  double myFinalDistFromVertex;
  double myApproachDistFromVertex;
  double mySpeed;
  double myCloseDist;
  double myAcquireTurnSpeed;
  bool myTwoStageApproach;
  double myLine1Length;
  double myAngleBetween;
  double myLine2Length;
  int myVertexUnseenStopMSecs;
  ArActionDesired myDesired;
  ArLineFinder *myLineFinder;
  bool myOwnLineFinder;
  bool myAdjustVertex;
  bool myGotoVertex;
  //double myIgnoreTriangleDist;
};

#endif // ARACTIONTRIANGLEDRIVETO

---