//* Drive towards with advanced use of sensors *

#include "../00_nav_library/nav_aria_iface.h" #include "../00_nav_library/nav_geometry.h" #include "../00_nav_library/nav_robot2lines.h" #include <iostream> #include <iomanip> #include <conio.h> using namespace std; /* * Action that drives the robot forward, but goes towards a detected obstacle * and stops in front of it. Uses sonar for obstacle detection */ class ActionToWallAdvanced : public ArAction { public:

//* May be customized, and more methods added *

// constructor, sets myXXXXParameters ActionToWallAdvanced(double maxSpeed=1000, double stopDistance=600, double stopAccuracyDistance=25, double stopAccuracyAngle=10); // altering and accessing certain aspects of the action internal state bool haveAchievedGoal() const { return(myHaveAchievedGoal); }

//* Following need to be always implemented *

// destructor. does not need to do anything virtual ~ActionToWallAdvanced(void) {}; // called by the action resolver to obtain this action's requested behavior virtual ArActionDesired *fire(ArActionDesired currentDesired); // store the robot pointer, and it's ArSonarDevice object, or deactivate this action if there is no sonar. virtual void setRobot(ArRobot *robot); protected: // the sonar device object obtained from the robot by setRobot() ArRangeDevice *mySonar; /* Our current desired action: fire() modifies this object and returns to the action resolver a pointer to this object. This object is kept as a class member so that it persists after fire() returns (otherwise fire() would have to create a new object each invocation, but would never be able to delete that object). */ ArActionDesired myDesired;

//* Following are your parameters initialized in constructors and used among your functions *

double myMaxSpeed; double myStopDistance; double myStopAccuracyDistance; double myStopAccuracyAngle; bool myHaveAchievedGoal; }; /* Note the use of constructor chaining with ArAction(actionName). Also note how it uses setNextArgument, which makes it so that other parts of the program could find out what parameters this action has, and possibly modify them. */ ActionToWallAdvanced::ActionToWallAdvanced(double maxSpeed, double stopDistance, double stopAccuracyDistance, double stopAccuracyAngle) : ArAction("ToWall") { mySonar = NULL; myMaxSpeed = maxSpeed; myStopDistance = stopDistance; myStopAccuracyDistance = stopAccuracyDistance; myStopAccuracyAngle = stopAccuracyAngle; myHaveAchievedGoal = false; setNextArgument(ArArg("maximum speed", &myMaxSpeed, "Maximum linear speed to go.")); setNextArgument(ArArg("stop distance", &myStopDistance, "Distance to wall at which to stop.")); setNextArgument(ArArg("stop accuracy distance", &myStopAccuracyDistance, "Distance accuracy.")); setNextArgument(ArArg("stop accuracy angle", &myStopAccuracyAngle, "Angle accuracy.")); } /* Override ArAction::setRobot() to get the sonar device from the robot, or deactivate this action if it is missing. You must also call ArAction::setRobot() to properly store the ArRobot pointer in the ArAction base class. */ void ActionToWallAdvanced::setRobot(ArRobot *robot) { ArAction::setRobot(robot); mySonar = robot->findRangeDevice("sonar"); if (robot == NULL) { ArLog::log(ArLog::Terse, "actionExample: ActionToWallAdvanced: Warning: I found no sonar, deactivating."); deactivate(); } } /* This fire is the whole point of the action. currentDesired is the combined desired action from other actions previously processed by the action resolver. In this case, we're not interested in that, we will set our desired forward velocity in the myDesired member, and return it. Note that myDesired must be a class member, since this method will return a pointer to myDesired to the caller. If we had declared the desired action as a local variable in this method, the pointer we returned would be invalid after this method returned. */ ArActionDesired *ActionToWallAdvanced::fire(ArActionDesired currentDesired) { // reset the actionDesired (must be done), to clear its previous values. myDesired.reset(); // if the sonar is null we can't do anything, so deactivate if (mySonar == NULL) { deactivate(); return NULL; } /* Proportional Controller: [v(t), w(t)]T = K * [error]T = [Kro * errorDistance, Ka * errorHeading + Kb * errorFinalHeading]T where: errorDistance = sqrt(errorX*errorX + errorY*errorY) errorHeading = -R.Theta() + atan2(errorY, errorX) errorFinalHeading = -R.Theta() - errorHeading Strong stability: Kro>0, Kb<0, Ka-Kb>0, Ka+1.5*Kb-2*Kro/Pi>0 */ static const double Kro = .25; static const double Ka = 1.00; static const double Kb = -.375; myHaveAchievedGoal = false; double v = myMaxSpeed; // if no wall then proceed forward at maximum speed double w = 0; NAV_Line line_ahead; // NAV_FindCurrentLines(myRobot->getPose(), *mySonar, NULL, NULL, &line_ahead /*, NULL, NULL, NULL */); // -90 to 90 degree ahead NAV_FindCurrentLine(myRobot->getPose(), *mySonar, -60, 60, line_ahead); // -45 to +45 degree limited ahead if ( line_ahead.isValid() ) { double dist_to_wall = line_ahead.distance(myRobot->getPose()); double angle_to_wall= ArMath::atan2(line_ahead.getB(), line_ahead.getA()); double robot_angle = myRobot->getTh(); double ro = dist_to_wall - myStopDistance; // distance to travel double a = angle_to_wall - robot_angle; // the "directins" of line perpendicular to wall and robot axes may be reversed, fix that by rotating 180 degrees! if (a<-90) a+=180; else if (a>90) a-=180; double b = 0; // assuem that we arrive at 90 degree to the wall, need more pts to compute // the control law equations v = Kro*ro; w = Ka*a + Kb*b; if (v>myMaxSpeed) { // cannot drive too fast, limit speed v=myMaxSpeed; } else if (v<-myMaxSpeed) { v=-myMaxSpeed; } #ifdef _DEBUG cerr << " ro="<<setw(8)<<ro <<" a="<<setw(8)<<a <<" b="<<setw(8)<<b <<" v="<<setw(8)<<v <<" w="<<setw(8)<<w <<endl; #endif // if reached within desired accuracy? if (fabs(ro)<=myStopAccuracyDistance && fabs(a)<=myStopAccuracyAngle && fabs(b)<=myStopAccuracyAngle) { // goal acheived within accuracy myHaveAchievedGoal = true; } } if (!myHaveAchievedGoal) { // the control laws translated into the robot control input myDesired.setVel(v); myDesired.setRotVel(w); } // return a pointer to the actionDesired to the resolver to make our request return &myDesired; } 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; ArActionAvoidSide avoidside("Avoid Side", 100); ArActionStop stop; // stop if nothing else to do ActionToWallAdvanced go(2200, 550); int state = 0; // 0 - approach, 1 - find new goal R.lock(); R.addAction(&recover, 100); R.addAction(&bumpers, 95); R.addAction(&avoidside, 80); R.addAction(&go, 50); R.addAction(&stop, 10); // stop if nothing else to do R.unlock(); #ifdef _DEBUG cerr << showpoint << fixed << setprecision(2); // for inspection purpose only #endif while( R.is_connected()) { // keyboard exit if (_kbhit()) /* <conio.h> */ { char c=tolower(_getch()); if (c=='q') break; } // state machine goes here R.lock(); if (state==0) { if (go.haveAchievedGoal()) { state=1; // remove all actions that might be triggered and interfere before // taking over with driect robot control // R.remAction(..); R.remAction(&go); if (R.getDistanceL10()<R.getDistanceR10()) R.doRotateAngle(-170); // direct motion overrides all actions else R.doRotateAngle(170); // direct motion overrides all actions // make sure that the robot finished the driect control before resuming actions #ifdef _DEBUG cerr << "Wall reached! Turning" << endl; #endif } else { // continue approaching the wall } } else if (state==1) { if (R.isCommandDone()) { R.clearDirectMotion(); // enables actions again as they were disabled by the issed direct motion command state=0; // R.remAction(..); // R.remAction(..); R.addAction(&go, 50); #ifdef _DEBUG cerr << "Turning done! Searching for a new wall" << endl; #endif } else { // let it finish the turn } } else { cerr << "Error: Invalid state!" << endl; // invalid state } #ifdef false _DEBUG cerr <<" X="<<setw(6)<< R.getX() <<" Y="<<setw(6)<< R.getY() <<" T="<<setw(8)<< R.getTh() <<" D10L="<<setw(8)<< R.getDistanceL10() <<" D10R="<<setw(8)<< R.getDistanceR10() << endl; #endif R.unlock(); R.Sleep(100); } if (R.is_connected()) R.disconnect(); else cerr << "Lost connection, aborting!" << endl; return(0); }