ArSick.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 ARSICK_H
#define ARSICK_H

#include "ariaTypedefs.h"
#include "ArSickPacket.h"
#include "ArSickPacketReceiver.h"
#include "ArRobotPacket.h"
#include "ArRangeDeviceThreaded.h"   
#include "ArFunctor.h"
#include "ArCondition.h"


class ArSick : public ArRangeDeviceThreaded
{
public:
  enum BaudRate {
    BAUD9600, 
    BAUD19200, 
    BAUD38400 
  };
  enum Degrees {
    DEGREES180, 
    DEGREES100 
  };
  enum Increment {
    INCREMENT_ONE, 
    INCREMENT_HALF 
  };
  enum Units {
    UNITS_1MM, 
    UNITS_1CM, 
    UNITS_10CM 
  };
  enum Bits {
    BITS_1REFLECTOR, 
    BITS_2REFLECTOR, 
    BITS_3REFLECTOR 
  };
  AREXPORT ArSick(size_t currentBufferSize = 361, 
          size_t cumulativeBufferSize = 0,
          const char *name = "laser",
          bool addAriaExitCB = true);
  AREXPORT virtual ~ArSick();
  AREXPORT void configure(bool useSim = false, bool powerControl = true, 
              bool laserFlipped = false, 
              BaudRate baud = BAUD38400,
              Degrees deg = DEGREES180, 
              Increment incr = INCREMENT_ONE);
  AREXPORT void configureShort(bool useSim = false, 
                   BaudRate baud = BAUD38400,
                   Degrees deg = DEGREES180, 
                   Increment incr = INCREMENT_ONE);
  AREXPORT void setRangeInformation(Bits bits = BITS_1REFLECTOR,
                    Units units = UNITS_1MM);
  AREXPORT void setSensorPosition(double x, double y, double th);
  AREXPORT void setSensorPosition(ArPose pose);  
  AREXPORT ArPose getSensorPosition();
  AREXPORT double getSensorPositionX();
  AREXPORT double getSensorPositionY();
  AREXPORT double getSensorPositionTh();
  AREXPORT bool blockingConnect(void);
  AREXPORT bool asyncConnect(void);
  AREXPORT bool disconnect(bool doNotLockRobotForSim = false);
  AREXPORT void setDeviceConnection(ArDeviceConnection *conn);
  AREXPORT ArDeviceConnection *getDeviceConnection(void);
  bool isConnected(void) 
    { if (myState == STATE_CONNECTED) return true; else return false; }
  bool tryingToConnect(void) 
    { if (myState != STATE_CONNECTED && myState != STATE_NONE) 
      return true; else return false; }

  AREXPORT unsigned int getMinRange(void) { return myMinRange; }
  AREXPORT void setMinRange(unsigned int minRange) { myMinRange = minRange; }
  AREXPORT void addIgnoreReading(double ignoreReading) 
               { myIgnoreReadings.push_back(ignoreReading); }
  AREXPORT void clearIgnoreReadings(void) { myIgnoreReadings.clear(); }
  AREXPORT const std::list<double> *getIgnoreReadings(void) const
             { return &myIgnoreReadings; }
  AREXPORT void setFilterNearDist(double dist); 
  AREXPORT double getFilterNearDist(void);

  AREXPORT void setFilterCumulativeMaxDist(double dist) 
    { setMaxDistToKeepCumulative(dist); }
  AREXPORT double getFilterCumulativeMaxDist(void)
    { return getMaxDistToKeepCumulative(); }
  AREXPORT void setFilterCumulativeInsertMaxDist(double dist);
  AREXPORT double getFilterCumulativeInsertMaxDist(void);
  AREXPORT void setFilterCumulativeNearDist(double dist);
  AREXPORT double getFilterCumulativeNearDist(void);
  AREXPORT void setFilterCumulativeCleanDist(double dist);
  AREXPORT double getFilterCumulativeCleanDist(void);
  AREXPORT void setFilterCleanCumulativeInterval(int milliSeconds);
  AREXPORT int getFilterCleanCumulativeInterval(void);
  void setFilterCumulativeMaxAge(int seconds) 
    { setMaxSecondsToKeepCumulative(seconds); }
  int getFilterCumulativeMaxAge(void) 
    { return getMaxSecondsToKeepCumulative(); }

  AREXPORT bool runOnRobot(void);


  AREXPORT int getSickPacCount();

  AREXPORT void addConnectCB(ArFunctor *functor,
                 ArListPos::Pos position = ArListPos::LAST);
  AREXPORT void remConnectCB(ArFunctor *functor);

  AREXPORT void addFailedConnectCB(ArFunctor *functor, 
                   ArListPos::Pos position = ArListPos::LAST);
  AREXPORT void remFailedConnectCB(ArFunctor *functor);

  AREXPORT void addDisconnectNormallyCB(ArFunctor *functor, 
                ArListPos::Pos position = ArListPos::LAST);
  AREXPORT void remDisconnectNormallyCB(ArFunctor *functor);
  
  AREXPORT void addDisconnectOnErrorCB(ArFunctor *functor, 
                   ArListPos::Pos position = ArListPos::LAST);
  AREXPORT void remDisconnectOnErrorCB(ArFunctor *functor);

  AREXPORT void addDataCB(ArFunctor *functor,
                   ArListPos::Pos position = ArListPos::LAST);
  AREXPORT void remDataCB(ArFunctor *functor);

  AREXPORT void setConnectionTimeoutTime(int mSecs);
  AREXPORT int getConnectionTimeoutTime(void);

  AREXPORT ArTime getLastReadingTime(void);


  AREXPORT bool isUsingSim(void);
  AREXPORT bool isControllingPower(void);
  AREXPORT bool isLaserFlipped(void);
  AREXPORT Degrees getDegrees(void);
  AREXPORT Increment getIncrement(void);
  AREXPORT Bits getBits(void);
  AREXPORT Units getUnits(void);

  AREXPORT void setIsUsingSim(bool usingSim);
  AREXPORT void setIsControllingPower(bool controlPower);
  AREXPORT void setIsLaserFlipped(bool laserFlipped);

  AREXPORT bool simPacketHandler(ArRobotPacket * packet);
  AREXPORT void sensorInterpCallback(void);
  AREXPORT bool internalConnectSim(void);
  AREXPORT int internalConnectHandler(void);
  AREXPORT virtual void * runThread(void *arg);
  AREXPORT void processPacket(ArSickPacket *packet, ArPose pose, 
                  ArPose encoderPose, unsigned int counter,
                  bool deinterlace, ArPose deinterlaceDelta);
  AREXPORT void runOnce(bool lockRobot);
  AREXPORT virtual void setRobot(ArRobot *robot);
  AREXPORT void dropConnection(void);
  AREXPORT void failedConnect(void);
  AREXPORT void madeConnection(void);
  AREXPORT void robotConnectCallback(void);
  AREXPORT virtual void applyTransform(ArTransform trans,
                                        bool doCumulative = true);
protected:
  enum State {
    STATE_NONE, 
    STATE_INIT, 
    STATE_WAIT_FOR_POWER_ON, 
    STATE_CHANGE_BAUD, 
    STATE_CONFIGURE, 
    STATE_WAIT_FOR_CONFIGURE_ACK, 
    STATE_INSTALL_MODE, 
    STATE_WAIT_FOR_INSTALL_MODE_ACK, 
    STATE_SET_MODE, 
    STATE_WAIT_FOR_SET_MODE_ACK, 
    STATE_START_READINGS, 
    STATE_WAIT_FOR_START_ACK, 
    STATE_CONNECTED 
  };
  void filterReadings();
  void filterAddAndCleanCumulative(double x, double y, bool clean);
  void filterFarAndOldCumulative(void);
  AREXPORT void switchState(State state);
  std::list<double> myIgnoreReadings;
  State myState;
  unsigned int myMinRange;
  ArTime myStateStart;
  ArFunctorC<ArSick> myRobotConnectCB;
  ArRetFunctor1C<bool, ArSick, ArRobotPacket *> mySimPacketHandler;
  ArFunctorC<ArSick> mySensorInterpCB;
  std::list<ArSensorReading *>::iterator myIter;
  bool myStartConnect;
  bool myRunningOnRobot;
  int myTimeoutTime;

  // range buffers to hold current range set and assembling range set
  std::list<ArSensorReading *> *myAssembleReadings;
  std::list<ArSensorReading *> *myCurrentReadings;

  bool myProcessImmediately;
  bool myInterpolation;
  // list of packets, so we can process them from the sensor callback
  std::list<ArSickPacket *> myPackets;

  // a lock for the device (so we are mostly unlocked for people to get data, 
  // but so we don't have people trying to read multiple times from the same 
  // device
  ArMutex myConnLock;

  // whether we did a real configure or a short configure
  bool myRealConfigured;
  
  // these two are just for the sim packets
  unsigned int myWhichReading;
  unsigned int myTotalNumReadings;

  // some variables so we don't have to do a tedios if every time
  double myOffsetAmount;
  double myIncrementAmount;

  // packet stuff
  ArSickPacket myPacket;
  bool myRunInOwnThread;
  bool myUseSim;
  bool myLaserFlipped;
  ArPose mySensorPose;
  bool myPowerControl;
  BaudRate myBaud;
  Degrees myDegrees;
  Increment myIncrement;
  Units myUnits;
  Bits myBits;
  ArTime myLastReading;
  
  // stuff for the sim packet
  ArPose mySimPacketStart;
  ArTransform mySimPacketTrans;
  ArTransform mySimPacketEncoderTrans;
  unsigned int mySimPacketCounter;

  // packet count
  time_t myTimeLastSickPacket;
  int mySickPacCurrentCount;
  int mySickPacCount;

  // connection
  ArSickPacketReceiver mySickPacketReceiver;
  ArDeviceConnection *myConn;
  std::list<ArFunctor *> myConnectCBList;
  std::list<ArFunctor *> myFailedConnectCBList;
  std::list<ArFunctor *> myDisconnectNormallyCBList;
  std::list<ArFunctor *> myDisconnectOnErrorCBList;
  std::list<ArFunctor *> myDataCBList;

  ArTime myLastCleanedCumulative;
  // filtering parameters, see the docs in the acessor
  double myFilterNearDist;  
  double myFilterCumulativeInsertMaxDist; 
  double myFilterSquaredCumulativeInsertMaxDist;
  double myFilterCumulativeNearDist; 
  double myFilterSquaredCumulativeNearDist; 
  double myFilterCumulativeCleanDist; 
  double myFilterSquaredCumulativeCleanDist; 
  int myFilterCleanCumulativeInterval; 

  ArMutex myStateMutex;
  ArRetFunctor1C<bool, ArSick, bool> myAriaExitCB;
};

#endif //ARSICK_H

---