//* NAV Course simplified Pioneer robot interface - header file - 08/23/2011 11:45AM *

// PROJECT properties/linker/input/Additional Dependences: // MS Visual Studio 2013: // ../Aria/AriaVC12.lib wsock32.lib winmm.lib - for "Release" version, requires AriaVC12.DLL // ../Aria/AriaDebugVC12.lib wsock32.lib winmm.lib - for "Debug" version, requires AriaDebugVC12.DLL // MS Visual Studio 2012: // ../Aria/AriaVC11.lib wsock32.lib winmm.lib - for "Release" version, requires AriaVC11.DLL // ../Aria/AriaDebugVC11.lib wsock32.lib winmm.lib - for "Debug" version, requires AriaDebugVC11.DLL #ifndef _NAV_ARIA_IFACE #define _NAV_ARIA_IFACE #include <list> #include <vector> #include <algorithm> #include "../Aria/include/Aria.h" //#include "C:/Program Files/Mobile Robots/MobileRobots/Aria/include/Aria.h" //#include <Aria.h> #include "nav_aria_chandler.h" class NAV_Robot { public: NAV_Robot(const char* serial); NAV_Robot(const char* server, unsigned short port); ~NAV_Robot(); private: NAV_Robot(const NAV_Robot&); // Note: Highlander said: "There can be only one" // copy constructor blocked - no passing by value - not needed as we can have only one copy of a particular robot handle void operator= (const NAV_Robot&); // Note: Highlander said: "There can be only one" // operator= is blocked - not needed as we can have only one copy of a particular robot handle

//* utility *

public: void lock() { robot->lock(); } void unlock() { robot->unlock(); } void Sleep(unsigned int ms) const { ArUtil::sleep(ms); } // DO NOT KEEP ROBOT LOCKED while sleeping! ArRobot& getRobot() { return(*robot); } const ArRobot& getRobot() const { return(*robot); }

//* set up and shut down *

public: void connect(); /* WARNING! connect() uses lock/unlock internally */ void disconnect(); /* WARNING! disconnect() uses lock/unlock internally */ bool is_connected() const { return(robot->isConnected() && robot->isRunning()); } void enableMotors() { robot->enableMotors(); } void disableMotors() { robot->disableMotors(); } bool isMotorsOn() const { return(robot->areMotorsEnabled()); } void enableSonar() { robot->enableSonar(); } void disableSonar() { robot->disableSonar(); } bool isSonarOn() const { return(robot->areSonarsEnabled()); }

//* Direct Motion API - differential drive with castor *

// all movement in mm, spped in mm/sec, rotation in degrees, omega in degrees/sec // Note that speed commands relate to continuous movement until you stop each of them with spped(0) or stop() // Note that dirve/rotate commands are executed with internal (ramp) speed settings until done ot stop() public: void Stop() { robot->stop(); } void doClearDirectMotion() { robot->clearDirectMotion(); } void doDriveSpeed(int linvel) { robot->setVel(linvel); } void doDriveDistance(int dist) { robot->move(dist); } void doRotateSpeed(int angvel) { robot->setRotVel(angvel); } void doRotateAngle(int angle) { robot->setDeltaHeading(angle); } void doRotateToAngle(int angle) { robot->setHeading(angle); } void doDriveDirect(int Vleft, int Vright){ robot->setVel2(Vleft, Vright); } public: bool isHeadingDone(double err=0) const { return(robot->isHeadingDone(err)); } // err=0 means use value set by setHeadingDoneDiff(.) // bool isHeadingDone(double err=0) const { return(robot->isHeadingDone(err)); } // err=0 means use value set by setHeadingDoneDiff(.) bool isTryingToMove() const { return(robot->isTryingToMove()); } bool isMoveDone(double err=0) const { return(robot->isMoveDone(err)); } // err=0 means use value set by setMoveDoneDiff(.) bool isStalled() const { return(robot->isLeftMotorStalled()||robot->isRightMotorStalled()); } bool isCommandDone() const { return((! robot->isTryingToMove()) && robot->isMoveDone() && robot->isHeadingDone()); } void setHeadingDoneDiff(double err) { robot->setHeadingDoneDiff(err); } // accuracy used by waitUntilDone() void setMoveDoneDist(double err) { robot->setMoveDoneDist(err); } // accuracy used by waitUntilDone() double getHeadingDoneDiff() const { return(robot->getHeadingDoneDiff()); } // accuracy used by waitUntilDone() double getMoveDoneDist() const { return(robot->getMoveDoneDist()); } // accuracy used by waitUntilDone() public: double getVel() const { return(robot->getVel()); } double getRotVel() const { return(robot->getRotVel()); } double getBattery() const { return(robot->getRealBatteryVoltage()); } // 12V is 1.00 ArRobot::ChargeState getChargeState() { return(robot->getChargeState()); }

//* Robot location API *

// Note: this is internal state counter that is affected by cumulative wheel slippage error, // cannot be relied on in long run without adjusting against a map public: double getX() const { return(robot->getX()); } double getY() const { return(robot->getY()); } double getTh() const { return(robot->getTh()); } ArPose getPose() const { return(robot->getPose()); } void moveCoords( const ArPose &to, const ArPose &from) { robot->moveTo(to, from);} void moveCoords( const ArPose &to) { robot->moveTo(to); } // note moveCoords will recalculate all positions of senosr reading stored on the robot sensor interface classes // !!! this will not move the actual robot, only what we call its global position origin point !!! // Note: this is robot dimensions from robot configuration file public: double getRobotRadius() const { return(robot->getRobotRadius()); } double getRobotLengthFront() const { return(robot->getRobotLengthFront()); } double getRobotLengthRear() const { return(robot->getRobotLengthRear()); } double getRobotLength() const { return(robot->getRobotLength()); } double getRobotWidth() const { return(robot->getRobotWidth()); }

//* Odometer API *

public: double resetTripOdometer() { robot->resetTripOdometer(); } double getTripOdometerDistance() const { return(robot->getTripOdometerDistance()); } double getTripOdometerDegrees() const { return(robot->getTripOdometerDegrees()); } double getTripOdometerTime() const { return(robot->getTripOdometerTime()); }

//* Action API *

public: bool addAction (ArAction *action, int priority) { return(robot->addAction(action, priority)); } bool remAction (ArAction *action) { return(robot->remAction(action)); } bool isDirectMotion() const { return(robot->isDirectMotion()); } void deactivateActions() { robot->deactivateActions(); } // cancel or actions void clearDirectMotion() { robot->clearDirectMotion(); } // allow actions to work after issuing a direct command

//* Navigation API - sonar senosors *

//* *** Lock/unlock the sonar with SM(Un)lock(), not the Robot! *** *

public: void SMlock() { sonar->lockDevice(); } // lock before processing exported list void SMunlock() { sonar->unlockDevice(); } // unlock immeidately after you are done ArSonarDevice& getSonar() { return(*sonar); } const ArSonarDevice& getSonar() const { return(*sonar); } public: unsigned int getSonarDistanceTooFar() const { return(sonar->getMaxRange()); } // typically 5000. int getSonarDistance(int anglefrom, int angleto) const { return(robot->getClosestSonarRange(anglefrom, angleto)); } double getSonarClosestEcho(int anglefrom, int angleto, double *pangle) const { return(sonar->currentReadingPolar(anglefrom, angleto, pangle)); }

//* Robot support for trajectory and mapping *

public: bool getPoseAt(const ArTime timeStamp, ArPose &position) // returns true if valid interpolation obtained { return(robot->getPoseInterpPosition(timeStamp, &position) >=0 ); } size_t getPoseInterpNumReadings (void) { return(robot->getPoseInterpNumReadings()); } void setPoseInterpNumReadings (size_t n) const { robot->setPoseInterpNumReadings(n); }

//* Mapping API Sonar *

// direct reading from a particular sonar sensor public: int getSonarCount() const { return(robot->getNumSonar()); } // how many sonars are there int getSonarRange(int snr) const { return(robot->getSonarRange(snr)); } // raw data for snr-th sonar (from the sonar plate) bool isSonarNew (int snr) const { return(robot->isSonarNew(snr)); } // is there a recent reading for the snr-th sonar

//* Data structures for processing containers holding sensor readings *

public: typedef std::list<ArPoseWithTime*>* pListpArPWT; typedef std::list<ArPoseWithTime*>::iterator itListpArPWT; typedef std::vector<ArPoseWithTime>* pVectArPWT; typedef std::vector<ArPoseWithTime>::iterator itVectArPWT;

//* Mapping API using Sonar *

// List of sensor readings - must lock the sonar sensor, not the robot, before accessing public: // primary buffer used for obstacle avoidance const pListpArPWT SMgetCurrentBuffer() const { return(sonar->getCurrentBuffer()); } const pVectArPWT SMgetCurrentBufferAsVector() const { return(sonar->getCurrentBufferAsVector()); } // secondary buffer void SMsetMaxSecondsToKeepCumulative(int s=0) { sonar->setMaxSecondsToKeepCumulative(s); } // <0 keep forever void SMsetMaxDistToKeepCumulative(int d=0) { sonar->setMaxDistToKeepCumulative(d); } // <0 keep forever void SMclearCumulativeReadings () { sonar->clearCumulativeReadings(); } const pListpArPWT SMgetCumulativeBuffer() const { return(sonar->getCumulativeBuffer()); } // returns the list of pointers to detected obstacle points each wrapped in ArPoseWithTime class const pVectArPWT SMgetCumulativeBufferAsVector()const { return(sonar->getCumulativeBufferAsVector()); } // returns the frozen vector of points thagt were available at the time of function call

//* Mapping API using Sick Laser *

// List of sensor readings - must lock the laser sensor, not the robot, before accessing public: void LMlock() { laser->lockDevice(); } // lock before processing exported list void LMunlock() { laser->unlockDevice(); } // unlock immeidately after you are done ArSick& getLaser() { return(*laser); } const ArSick& getLaser() const { return(*laser); } // primary buffer used for obstacle avoidance const pListpArPWT LMgetCurrentBuffer() const { return(laser->getCurrentBuffer()); } const pVectArPWT LMgetCurrentBufferAsVector() const { return(laser->getCurrentBufferAsVector()); } // secondary buffer void LMsetMaxSecondsToKeepCumulative(int s) { laser->setMaxSecondsToKeepCumulative(s); } // <0 keep forever void LMsetMaxDistToKeepCumulative(int d) { laser->setMaxDistToKeepCumulative(d); } // <0 keep forever void LMclearCumulativeReadings () { laser->clearCumulativeReadings(); } const pListpArPWT LMgetCumulativeBuffer() const { return(laser->getCumulativeBuffer()); } const pVectArPWT LMgetCumulativeBufferAsVector()const { return(laser->getCumulativeBufferAsVector()); }

//* Useful functions for simple navigation *

// Note: distance is from the center of the robot, subtract at least getRobotRadius() if clearance needed public: int getDistanceAhead() const { return(getSonarDistance(-30,30)); } // P2 and P3 have: two sensors at +15 and -15 degrees int getDistanceLeft() const { return(getSonarDistance(70,110)); } // P2 and P3 have: one actual sensor at -90 int getDistanceFrontLeft() const { return(getSonarDistance(10,50)); } int getDistanceFrontRight() const { return(getSonarDistance(-50,-10)); } int getDistanceRight() const { return(getSonarDistance(-110,-70));} // P2 and P3 have: one actual sensor at -90 int getDistanceTooFar() const { return(sonar->getMaxRange()); } // typically 5000, kept for backward compatibility

//* Private *

protected: ArRobot *robot; // the robot ConnHandler *chandler; // the connection handler from above: ch(&robot) ArSonarDevice *sonar; // sonar device access ArAnalogGyro *gyro; // gyroscope if present and enabled ArDeviceConnection *con; // connection to the robot via Serial (ArSerialConnection) or TCP/IP (ArTcpConnection) ArSick *laser; // laser device access - WARNING! not present on real robots in our lab }; #endif