257 lines
7.2 KiB
C++
257 lines
7.2 KiB
C++
/*
|
|
Adept MobileRobots Robotics Interface for Applications (ARIA)
|
|
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
|
|
Copyright (C) 2006, 2007, 2008, 2009, 2010 MobileRobots Inc.
|
|
Copyright (C) 2011, 2012, 2013 Adept Technology
|
|
|
|
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
|
|
Adept MobileRobots for information about a commercial version of ARIA at
|
|
robots@mobilerobots.com or
|
|
Adept MobileRobots, 10 Columbia Drive, Amherst, NH 03031; +1-603-881-7960
|
|
*/
|
|
#ifndef ARLMS1XX_H
|
|
#define ARLMS1XX_H
|
|
|
|
#include "ariaTypedefs.h"
|
|
#include "ArRobotPacket.h"
|
|
#include "ArLaser.h"
|
|
#include "ArFunctor.h"
|
|
|
|
/** @internal */
|
|
class ArLMS1XXPacket : public ArBasePacket
|
|
{
|
|
public:
|
|
/// Constructor
|
|
AREXPORT ArLMS1XXPacket();
|
|
/// Destructor
|
|
AREXPORT virtual ~ArLMS1XXPacket();
|
|
|
|
/// Gets the command type
|
|
AREXPORT const char *getCommandType(void);
|
|
/// Gets the command name
|
|
AREXPORT const char *getCommandName(void);
|
|
|
|
// only call finalizePacket before a send
|
|
AREXPORT virtual void finalizePacket(void);
|
|
AREXPORT virtual void resetRead(void);
|
|
|
|
/// Gets the time the packet was received at
|
|
AREXPORT ArTime getTimeReceived(void);
|
|
/// Sets the time the packet was received at
|
|
AREXPORT void setTimeReceived(ArTime timeReceived);
|
|
|
|
AREXPORT virtual void duplicatePacket(ArLMS1XXPacket *packet);
|
|
AREXPORT virtual void empty(void);
|
|
|
|
AREXPORT virtual void byteToBuf(ArTypes::Byte val);
|
|
AREXPORT virtual void byte2ToBuf(ArTypes::Byte2 val);
|
|
AREXPORT virtual void byte4ToBuf(ArTypes::Byte4 val);
|
|
AREXPORT virtual void uByteToBuf(ArTypes::UByte val);
|
|
AREXPORT virtual void uByte2ToBuf(ArTypes::UByte2 val);
|
|
AREXPORT virtual void uByte4ToBuf(ArTypes::UByte4 val);
|
|
AREXPORT virtual void strToBuf(const char *str);
|
|
|
|
AREXPORT virtual ArTypes::Byte bufToByte(void);
|
|
AREXPORT virtual ArTypes::Byte2 bufToByte2(void);
|
|
AREXPORT virtual ArTypes::Byte4 bufToByte4(void);
|
|
AREXPORT virtual ArTypes::UByte bufToUByte(void);
|
|
AREXPORT virtual ArTypes::UByte2 bufToUByte2(void);
|
|
AREXPORT virtual ArTypes::UByte4 bufToUByte4(void);
|
|
AREXPORT virtual void bufToStr(char *buf, int len);
|
|
|
|
// adds a raw char to the buf
|
|
AREXPORT virtual void rawCharToBuf(unsigned char c);
|
|
protected:
|
|
int deascii(char c);
|
|
|
|
ArTime myTimeReceived;
|
|
bool myFirstAdd;
|
|
|
|
char myCommandType[1024];
|
|
char myCommandName[1024];
|
|
};
|
|
|
|
|
|
/// Given a device connection it receives packets from the sick through it
|
|
/// @internal
|
|
class ArLMS1XXPacketReceiver
|
|
{
|
|
public:
|
|
/// Constructor with assignment of a device connection
|
|
AREXPORT ArLMS1XXPacketReceiver();
|
|
/// Destructor
|
|
AREXPORT virtual ~ArLMS1XXPacketReceiver();
|
|
|
|
/// Receives a packet from the robot if there is one available
|
|
AREXPORT ArLMS1XXPacket *receivePacket(unsigned int msWait = 0,
|
|
bool shortcut = false,
|
|
bool ignoreRemainders = false);
|
|
|
|
AREXPORT ArLMS1XXPacket *receiveTiMPacket(unsigned int msWait = 0,
|
|
bool shortcut = false,
|
|
bool ignoreRemainders = false);
|
|
|
|
/// Sets the device this instance receives packets from
|
|
AREXPORT void setDeviceConnection(ArDeviceConnection *conn);
|
|
/// Gets the device this instance receives packets from
|
|
AREXPORT ArDeviceConnection *getDeviceConnection(void);
|
|
|
|
// PS - added to pass info to this class
|
|
AREXPORT void setmyInfoLogLevel(ArLog::LogLevel infoLogLevel)
|
|
{ myInfoLogLevel = infoLogLevel; }
|
|
AREXPORT void setLaserModel(int laserModel)
|
|
{ myLaserModel = laserModel; }
|
|
AREXPORT void setmyName(const char *name )
|
|
{ strcpy(myName, name); }
|
|
AREXPORT void setReadTimeout(int timeout )
|
|
{ myReadTimeout = timeout; }
|
|
|
|
|
|
protected:
|
|
ArDeviceConnection *myConn;
|
|
ArLMS1XXPacket myPacket;
|
|
|
|
enum State
|
|
{
|
|
STARTING, ///< Looking for beginning
|
|
DATA, ///< Read the data in a big whack
|
|
REMAINDER ///< Have extra data from reading in data
|
|
};
|
|
State myState;
|
|
char myName[1024];
|
|
unsigned int myNameLength;
|
|
char myReadBuf[100000];
|
|
int myReadCount;
|
|
int myReadTimeout;
|
|
|
|
int myLaserModel;
|
|
|
|
ArLog::LogLevel myInfoLogLevel;
|
|
};
|
|
|
|
/**
|
|
@since Aria 2.7.2
|
|
@see ArLaserConnector
|
|
Use ArLaserConnector to connect to a laser, determining type based on robot and program configuration parameters.
|
|
|
|
This is the ArLaser implementation for SICK LMS1xx, LMS5xx, and TiM3xx lasers. To use these lasers with ArLaserConnector, specify
|
|
the appropriate type in program configuration (lms1xx, lms6xx, or tim3xx).
|
|
*/
|
|
class ArLMS1XX : public ArLaser
|
|
{
|
|
public:
|
|
|
|
enum LaserModel
|
|
{
|
|
LMS1XX,
|
|
LMS5XX,
|
|
TiM3XX
|
|
};
|
|
|
|
/// Constructor
|
|
AREXPORT ArLMS1XX(int laserNumber,
|
|
const char *name,
|
|
LaserModel laserModel);
|
|
|
|
/// Destructor
|
|
AREXPORT ~ArLMS1XX();
|
|
AREXPORT virtual bool blockingConnect(void);
|
|
|
|
// specific init routine per laser
|
|
AREXPORT virtual bool lms5xxConnect(void);
|
|
AREXPORT virtual bool lms1xxConnect(void);
|
|
AREXPORT virtual bool tim3xxConnect(void);
|
|
|
|
AREXPORT virtual bool asyncConnect(void);
|
|
AREXPORT virtual bool disconnect(void);
|
|
AREXPORT virtual bool isConnected(void) { return myIsConnected; }
|
|
AREXPORT virtual bool isTryingToConnect(void)
|
|
{
|
|
if (myStartConnect)
|
|
return true;
|
|
else if (myTryingToConnect)
|
|
return true;
|
|
else
|
|
return false;
|
|
}
|
|
|
|
/// Logs the information about the sensor
|
|
AREXPORT void log(void);
|
|
|
|
|
|
protected:
|
|
AREXPORT virtual void laserSetName(const char *name);
|
|
AREXPORT virtual void * runThread(void *arg);
|
|
AREXPORT virtual void setRobot(ArRobot *robot);
|
|
AREXPORT ArLMS1XXPacket *sendAndRecv(
|
|
ArTime timeout, ArLMS1XXPacket *sendPacket, const char *recvName);
|
|
void sensorInterp(void);
|
|
void failedToConnect(void);
|
|
void clear(void);
|
|
bool validateCheckSum(ArLMS1XXPacket *packet);
|
|
|
|
LaserModel myLaserModel;
|
|
//bool myIsLMS5XX;
|
|
|
|
bool myIsConnected;
|
|
bool myTryingToConnect;
|
|
bool myStartConnect;
|
|
int myScanFreq;
|
|
|
|
int myVersionNumber;
|
|
int myDeviceNumber;
|
|
int mySerialNumber;
|
|
int myDeviceStatus1;
|
|
int myDeviceStatus2;
|
|
int myMessageCounter;
|
|
int myScanCounter;
|
|
int myPowerUpDuration;
|
|
int myTransmissionDuration;
|
|
int myInputStatus1;
|
|
int myInputStatus2;
|
|
int myOutputStatus1;
|
|
int myOutputStatus2;
|
|
int myReserved;
|
|
int myScanningFreq;
|
|
int myMeasurementFreq;
|
|
int myNumberEncoders;
|
|
int myNumChans16Bit;
|
|
int myNumChans8Bit;
|
|
int myFirstReadings;
|
|
int myYear;
|
|
int myMonth;
|
|
int myMonthDay;
|
|
int myHour;
|
|
int myMinute;
|
|
int mySecond;
|
|
int myUSec;
|
|
|
|
ArLog::LogLevel myLogLevel;
|
|
|
|
ArLMS1XXPacketReceiver myReceiver;
|
|
|
|
ArMutex myPacketsMutex;
|
|
ArMutex myDataMutex;
|
|
|
|
std::list<ArLMS1XXPacket *> myPackets;
|
|
|
|
ArFunctorC<ArLMS1XX> mySensorInterpTask;
|
|
ArRetFunctorC<bool, ArLMS1XX> myAriaExitCB;
|
|
};
|
|
|
|
#endif
|