ArActionTriangleDriveTo.h

/*
MobileRobots Advanced Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007 MobileRobots Inc.

     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 
MobileRobots for information about a commercial version of ARIA at 
robots@mobilerobots.com or 
MobileRobots Inc, 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:
  AREXPORT ArActionTriangleDriveTo(const char *name = "triangleDriveTo", 
                   double finalDistFromVertex = 400,
                   double approachDistFromVertex = 1000,
                   double speed = 200, 
                   double closeDist = 100,
                   double acquireTurnSpeed = 30);
  AREXPORT 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; }
  AREXPORT 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; }
  void setVertexOffset(int localXOffset, int localYOffset, double thOffset) 
    { myLocalXOffset = localXOffset; myLocalYOffset = localYOffset; 
    myThOffset = thOffset; } 

  bool getGotoVertex(void) { return myGotoVertex; }
  void setGotoVertex(bool gotoVertex) 
    { myGotoVertex = gotoVertex; }
  void setIgnoreTriangleDist(double dist = 250, 
                 bool useIgnoreInGotoVertexMode = false) 
    { myIgnoreTriangleDist = dist; 
    myUseIgnoreInGoto = useIgnoreInGotoVertexMode; }
  double getIgnoreTriangleDist(void) { return myIgnoreTriangleDist; }
  bool getUseIgnoreInGOtoVertexMode(void) { return myUseIgnoreInGoto; }
  void setVertexUnseenStopMSecs(int vertexUnseenStopMSecs = 4000)
    { myVertexUnseenStopMSecs = vertexUnseenStopMSecs; }
  int getVertexUnseenStopMSecs(void) { return myVertexUnseenStopMSecs; }
  void setMaxDistBetweenLinePoints(int maxDistBetweenLinePoints = 0) 
    { myMaxDistBetweenLinePoints = maxDistBetweenLinePoints; }
  void setMaxLateralDist(int maxLateralDist = 0)
    { myMaxLateralDist = maxLateralDist; }
  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; }
  AREXPORT void setLineFinder(ArLineFinder *lineFinder);
  AREXPORT ArLineFinder *getLineFinder(void) { return myLineFinder; }
  
  void setLogging(bool logging) { myPrinting = logging; }
  bool setLogging(void) { return myPrinting; }
  AREXPORT virtual void activate(void);
  AREXPORT virtual void deactivate(void);
  AREXPORT virtual void setRobot(ArRobot *robot);
  AREXPORT virtual ArActionDesired *fire(ArActionDesired currentDesired);
  AREXPORT virtual ArActionDesired *getDesired(void) { return &myDesired; }
#ifndef SWIG
  AREXPORT virtual const ArActionDesired *getDesired(void) const 
                                                        { return &myDesired; }
#endif

  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
  AREXPORT void findTriangle(bool initial, bool goStraight = false);
  // Finds a pose this dist from the vertex along the line from the vertex
  AREXPORT 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;
  int myLocalXOffset;
  int myLocalYOffset;
  double myThOffset;
  double myIgnoreTriangleDist;
  bool myUseIgnoreInGoto;
  int myMaxDistBetweenLinePoints;
  int myMaxLateralDist;
};

#endif // ARACTIONTRIANGLEDRIVETO

---