//* Creating custom actions, switching among actions *

#include "../00_nav_library/nav_aria_iface.h" #include <iostream> #include <iomanip> #include <conio.h> using namespace std; class ActionTurnDeltaHeading : public ArAction { public: ActionTurnDeltaHeading(const char *name="ActionTurnDeltaHeading"); virtual ~ActionTurnDeltaHeading(void) {} virtual ArActionDesired *fire(ArActionDesired currentDesired); virtual void setRobot(ArRobot *robot); void setSpeed(double speed=30); void setDeceleration(double deceleration=0.5); void setDeltaHeading(double turnAmount); bool haveAchievedTurn(); protected: ArActionDesired myDesired; double myDesiredHeading; int myTurning; }; ActionTurnDeltaHeading::ActionTurnDeltaHeading(const char *name) : ArAction(name, "Turn a given angle") { myDesiredHeading = 0; myTurning = 0; } void ActionTurnDeltaHeading::setRobot(ArRobot *robot) { ArAction::setRobot(robot); } ArActionDesired *ActionTurnDeltaHeading::fire(ArActionDesired currentDesired) { myDesired.reset(); /* Available: velocity -- ArActionDesired::setVel(..), heading -- ArActionDesired::setDeltaHeading(..) heading -- ArActionDesired::setHeading(..) for absolute heading maximum forward translational velocity -- ArActionDesired::setMaxVel(..) maximum reverse translational velocity -- ArActionDesired::setMaxNegVel(..) and maximum rotational velocity -- ArActionDesired::setMaxRotVel(..) */ if (myTurning==1) { double angleToGo = myDesiredHeading-myRobot->getTh(); if (ArMath::fabs(angleToGo)< myRobot->getHeadingDoneDiff() ) { myDesired.setRotVel(0); myTurning = 0; } else { myDesired.setHeading(myDesiredHeading); } } return &myDesired; } void ActionTurnDeltaHeading::setDeltaHeading(double turnAmount) { myDesiredHeading = ArMath::fixAngle( myRobot->getTh() + turnAmount ); myTurning = 1; } bool ActionTurnDeltaHeading::haveAchievedTurn() { return myTurning==0; } int main(){ // NAV_Robot R("gdansk.bradley.edu", 8101); NAV_Robot R("localhost", 8101); // NAV_Robot R("com1"); // NAV_Robot R("com4"); if (!R.is_connected()) { cout << "Cannot connect to the robot or robot simulator, exiting" << endl; return(0); } // Create instances of the actions defined above, plus ArActionStallRecover, // a predefined action from Aria. ArActionStallRecover recover; ArActionBumpers bumpers; ArActionAvoidFront avoidfront; ArActionAvoidSide avoidside; ArActionStop stop; // stop if nothing else to do ArActionDriveDistance go; ActionTurnDeltaHeading turn; R.lock(); R.addAction(&recover, 100); R.addAction(&bumpers, 95); R.addAction(&avoidfront,85); R.addAction(&avoidside, 80); R.addAction(&stop, 10); // stop if nothing else to do R.unlock(); #ifdef _DEBUG cerr << showpoint << fixed << setprecision(2); // for inspection purpose only #endif R.lock(); R.addAction(&go, 50); go.setDistance(1000); // note: must add to robot before setting go parameter R.unlock(); int state = 0; // robot state machine while( R.is_connected()) { // keyboard exit if (_kbhit()) /* <conio.h> */ { char c=tolower(_getch()); if (c=='q') break; } // state machine decision engine goes here R.lock(); // lock for working with actions and their parameters // do not perform any significant loops inside an active lock! if (state==0) { if (go.haveAchievedDistance()) { state=1; R.remAction(&go); R.addAction(&turn, 50); turn.setDeltaHeading(90); } else { // continue wandering in state 0 } } else if (state==1) { if (turn.haveAchievedTurn()) { state=0; R.remAction(&turn); R.addAction(&go, 50); go.setDistance(2000); } else { // continue turning in state 1 } } else { cerr << "Error: Invalid state!" << endl; // invalid state } R.unlock(); // done working with actions and their parameters #ifdef _DEBUG R.lock(); cerr <<"S:"<<setw(1)<<state <<" X="<<setw(6)<< R.getX() <<" Y="<<setw(6)<< R.getY() <<" T="<<setw(8)<< R.getTh() << endl; R.unlock(); #endif R.Sleep(50); // give some time for the system to talk to the robot firmware and update the state reflection } R.lock(); if (R.is_connected()) R.disconnect(); else cerr << "Lost connection, aborting!" << endl; R.unlock(); return(0); }