ArSick.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 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 
  };
  ArSick(size_t currentBufferSize = 361, 
                  size_t cumulativeBufferSize = 0,
                  const char *name = "laser",
                  bool addAriaExitCB = true);
  virtual ~ArSick();
  void configure(bool useSim = false, bool powerControl = true, 
                          bool laserFlipped = false, 
                          BaudRate baud = BAUD38400,
                          Degrees deg = DEGREES180, 
                          Increment incr = INCREMENT_ONE);
  void configureShort(bool useSim = false, 
                               BaudRate baud = BAUD38400,
                               Degrees deg = DEGREES180, 
                               Increment incr = INCREMENT_ONE);
  void setSensorPosition(double x, double y, double th);
  void setSensorPosition(ArPose pose);  
  ArPose getSensorPosition();
  double getSensorPositionX();
  double getSensorPositionY();
  double getSensorPositionTh();
  bool blockingConnect(void);
  bool asyncConnect(void);
  bool disconnect(bool doNotLockRobotForSim = false);
  void setDeviceConnection(ArDeviceConnection *conn);
  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; }

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

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

  bool runOnRobot(void);


  int getSickPacCount();

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

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

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

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

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

  ArTime getLastReadingTime(void);


  bool isUsingSim(void);
  bool isControllingPower(void);
  bool isLaserFlipped(void);
  Degrees getDegrees(void);
  Increment getIncrement(void);


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

  bool simPacketHandler(ArRobotPacket * packet);
  void sensorInterpCallback(void);
  bool internalConnectSim(void);
  int internalConnectHandler(void);
  virtual void * runThread(void *arg);
  void processPacket(ArSickPacket *packet, ArPose pose, 
                              ArPose encoderPose, unsigned int counter,
                              bool deinterlace, ArPose deinterlaceDelta);
  void runOnce(bool lockRobot);
  virtual void setRobot(ArRobot *robot);
  void dropConnection(void);
  void failedConnect(void);
  void madeConnection(void);
  void robotConnectCallback(void);
  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);
  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;
  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

---