void manualControlHandler(){ if(firstCall){ firstCall=false; showMenu(); } if(keyPressedBefore && !driver->estaEjecutando()){ keyPressedBefore=false; showMenu(); } if(mrpt::system::os::kbhit() ){ char c = mrpt::system::os::getch(); keyPressedBefore=true; switch(c){ case '\033': c=mrpt::system::os::getch(); // skip the [ cout << "Siguiente caracter" << (int)c << endl; double v; switch(mrpt::system::os::getch()) { // the real value case 'A': // code for arrow up v=robot->getVel(); robot->setVel(v+50); break; case 'B': // code for arrow down v=robot->getVel(); robot->setVel(v-50); break; case 'C': // code for arrow right v=robot->getRotVel(); robot->setRotVel(v-5); break; case 'D': // code for arrow left v=robot->getRotVel(); robot->setRotVel(v+5); break; } break; case ' ': robot->stop(); break; case 'x': //Aria::shutdown(); driver->stopRunning(); robot->stopRunning(); break; case 'c': guardarContinuo(); break; case 'p': start(); break; case 'g': guardar(); break; case 'w': guardarTrayectoria(); break; case 't': testParado(); break; case 's': stop(); break; } } }
int main(int argc, char **argv) { Aria::init(); ArRobot robot; ArArgumentParser parser(&argc, argv); parser.loadDefaultArguments(); ArLog::log(ArLog::Terse, "WARNING: this program does no sensing or avoiding of obstacles, the robot WILL collide with any objects in the way! Make sure the robot has approximately 3 meters of free space on all sides."); // ArRobotConnector connects to the robot, get some initial data from it such as type and name, // and then loads parameter files for this robot. ArRobotConnector robotConnector(&parser, &robot); if(!robotConnector.connectRobot()) { ArLog::log(ArLog::Terse, "simpleMotionCommands: Could not connect to the robot."); if(parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(1); return 1; } } if (!Aria::parseArgs()) { Aria::logOptions(); Aria::exit(1); return 1; } ArLog::log(ArLog::Normal, "simpleMotionCommands: Connected."); // Start the robot processing cycle running in the background. // True parameter means that if the connection is lost, then the // run loop ends. robot.runAsync(true); // Print out some data from the SIP. // We must "lock" the ArRobot object // before calling its methods, and "unlock" when done, to prevent conflicts // with the background thread started by the call to robot.runAsync() above. // See the section on threading in the manual for more about this. // Make sure you unlock before any sleep() call or any other code that will // take some time; if the robot remains locked during that time, then // ArRobot's background thread will be blocked and unable to communicate with // the robot, call tasks, etc. robot.lock(); ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV", robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getRotVel(), robot.getBatteryVoltage()); robot.unlock(); // Sleep for 3 seconds. ArLog::log(ArLog::Normal, "simpleMotionCommands: Will start driving in 3 seconds..."); ArUtil::sleep(3000); // Set forward velocity to 50 mm/s ArLog::log(ArLog::Normal, "simpleMotionCommands: Driving forward at 250 mm/s for 5 sec..."); robot.lock(); robot.enableMotors(); robot.setVel(250); robot.unlock(); ArUtil::sleep(5000); ArLog::log(ArLog::Normal, "simpleMotionCommands: Stopping."); robot.lock(); robot.stop(); robot.unlock(); ArUtil::sleep(1000); ArLog::log(ArLog::Normal, "simpleMotionCommands: Rotating at 10 deg/s for 5 sec..."); robot.lock(); robot.setRotVel(10); robot.unlock(); ArUtil::sleep(5000); ArLog::log(ArLog::Normal, "simpleMotionCommands: Rotating at -10 deg/s for 10 sec..."); robot.lock(); robot.setRotVel(-10); robot.unlock(); ArUtil::sleep(10000); ArLog::log(ArLog::Normal, "simpleMotionCommands: Driving forward at 150 mm/s for 5 sec..."); robot.lock(); robot.setRotVel(0); robot.setVel(150); robot.unlock(); ArUtil::sleep(5000); ArLog::log(ArLog::Normal, "simpleMotionCommands: Stopping."); robot.lock(); robot.stop(); robot.unlock(); ArUtil::sleep(1000); // Other motion command functions include move(), setHeading(), // setDeltaHeading(). You can also adjust acceleration and deceleration // values used by the robot with setAccel(), setDecel(), setRotAccel(), // setRotDecel(). See the ArRobot class documentation for more. robot.lock(); ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV", robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getRotVel(), robot.getBatteryVoltage()); robot.unlock(); ArLog::log(ArLog::Normal, "simpleMotionCommands: Ending robot thread..."); robot.stopRunning(); // wait for the thread to stop robot.waitForRunExit(); // exit ArLog::log(ArLog::Normal, "simpleMotionCommands: Exiting."); Aria::exit(0); return 0; }
int main ( int argc, char *argv[] ){ //cout << "running....\n"; try{ // Create the socket ServerSocket server ( 30000 ); Aria::init(); Arnl::init(); ArRobot robot; ArArgumentParser parser(&argc, argv); parser.loadDefaultArguments(); ArSonarDevice sonar; ArSimpleConnector simpleConnector(&parser); // Our server for mobile eyes ArServerBase moServer; // Set up our simpleOpener ArServerSimpleOpener simpleOpener(&parser); parser.loadDefaultArguments(); if (!Aria::parseArgs () || !parser.checkHelpAndWarnUnparsed()){ Aria::logOptions (); Aria::exit (1); } //Add the sonar to the robot robot.addRangeDevice(&sonar); // Look for map in the current directory ArMap arMap; // set it up to ignore empty file names (otherwise the parseFile // on the config will fail) arMap.setIgnoreEmptyFileName (true); // First open the server if (!simpleOpener.open(&moServer)){ if (simpleOpener.wasUserFileBad()) ArLog::log(ArLog::Normal, "Bad user file"); else ArLog::log(ArLog::Normal, "Could not open server port"); exit(2); } // Connect to the robot if (!simpleConnector.connectRobot (&robot)){ ArLog::log (ArLog::Normal, "Could not connect to robot... exiting"); Aria::exit (3); } // Create the localization task (it will start its own thread here) ArSonarLocalizationTask locTask(&robot, &sonar, &arMap); ArLocalizationManager locManager(&robot, &arMap); ArLog::log(ArLog::Normal, "Creating sonar localization task"); locManager.addLocalizationTask(&locTask); // Set the initial pose to the robot's "Home" position from the map, or // (0,0,0) if none, then let the localization thread take over. locTask.localizeRobotAtHomeNonBlocking(); //Create the path planning task ArPathPlanningTask pathTask(&robot,&sonar,&arMap); ArLog::log(ArLog::Normal, "Robot Server: Connected."); robot.enableMotors(); robot.clearDirectMotion(); // Start the robot processing cycle running in the background. // True parameter means that if the connection is lost, then the // run loop ends. robot.runAsync(true); // Read in parameter files. Aria::getConfig ()->useArgumentParser (&parser); if (!Aria::getConfig ()->parseFile (Arnl::getTypicalParamFileName ())){ ArLog::log (ArLog::Normal, "Trouble loading configuration file, exiting"); Aria::exit (5); } //Create the three states robot.lock(); Follow follow = Follow(&robot,&sonar); GoTo goTo(&robot,&pathTask,&arMap); Search s(&robot,&sonar); // Bumpers. ArBumpers bumpers; robot.addRangeDevice(&bumpers); pathTask.addRangeDevice(&bumpers, ArPathPlanningTask::CURRENT); // Forbidden regions from the map ArForbiddenRangeDevice forbidden(&arMap); robot.addRangeDevice(&forbidden); pathTask.addRangeDevice(&forbidden, ArPathPlanningTask::CURRENT); // Mode To stop and remain stopped: ArServerModeStop modeStop(&moServer, &robot); // Action to slow down robot when localization score drops but not lost. ArActionSlowDownWhenNotCertain actionSlowDown(&locTask); pathTask.getPathPlanActionGroup()->addAction(&actionSlowDown, 140); // Action to stop the robot when localization is "lost" (score too low) ArActionLost actionLostPath(&locTask, &pathTask); pathTask.getPathPlanActionGroup()->addAction(&actionLostPath, 150); // These provide various kinds of information to the client: ArServerInfoRobot serverInfoRobot(&moServer, &robot); ArServerInfoSensor serverInfoSensor(&moServer, &robot); ArServerInfoPath serverInfoPath(&moServer, &robot, &pathTask); // Provide the map to the client (and related controls): // This uses both lines and points now, since everything except // sonar localization uses both (path planning with sonar still uses both) ArServerHandlerMap serverMap(&moServer, &arMap); // Provides localization info and allows the client (MobileEyes) to relocalize at a given // pose: ArServerInfoLocalization serverInfoLocalization(&moServer, &robot, &locTask); ArServerHandlerLocalization serverLocHandler(&moServer, &robot, &locTask); robot.unlock(); moServer.runAsync(); //Main loop while (true){ //The socket to accept connection ServerSocket new_sock; server.accept ( new_sock ); int state = 1; //1 = Follow, 2 = Search, 3 = GoTo int lastPos[2]; //Storing last position of BB to search the target int data[2]; //matrix with X,Y of BB try{ while ( true ){ //receive data from tld new_sock >> data; //cout << data[0] << "," << data[1] << endl; if(data[0] != -1) lastPos[0] = data[0]; //cout << state <<endl; //for debugging //Main logic switch(state){ case 1: cout << "Following target\n"; state = follow.run(data); break; case 2: cout << "Searching for target\n"; state = s.seek(lastPos, data); break; case 3: cout << "Going to ...\n"; state = goTo.run(data); break; default: cout << "Not a case for state\n"; break; } std::cout << "Loc score: " << locTask.getLocalizationScore() << std::endl; } } catch ( SocketException& ) { cout << "Lost Connection" << endl; robot.lock(); robot.stop(); robot.unlock(); } } } catch ( SocketException& e ){ std::cout << "Exception was caught:" << e.description() << "\nExiting.\n"; } ArLog::log(ArLog::Normal, "RobotServer: Exiting."); return 0; }
int main(int argc, char **argv) { Aria::init(); ArArgumentParser argParser(&argc, argv); ArSimpleConnector con(&argParser); ArRobot robot; // the connection handler from above ConnHandler ch(&robot); if(!Aria::parseArgs()) { Aria::logOptions(); Aria::shutdown(); return 1; } if(!con.connectRobot(&robot)) { ArLog::log(ArLog::Normal, "directMotionExample: Could not connect to the robot. Exiting."); return 1; } ArLog::log(ArLog::Normal, "directMotionExample: Connected."); // Run the robot processing cycle in its own thread. Note that after starting this // thread, we must lock and unlock the ArRobot object before and after // accessing it. robot.runAsync(false); // Send the robot a series of motion commands directly, sleeping for a // few seconds afterwards to give the robot time to execute them. printf("directMotionExample: Setting rot velocity to 100 deg/sec then sleeping 3 seconds\n"); robot.lock(); robot.setRotVel(100); robot.unlock(); ArUtil::sleep(3*1000); printf("Stopping\n"); robot.lock(); robot.setRotVel(0); robot.unlock(); ArUtil::sleep(200); printf("directMotionExample: Telling the robot to go 300 mm on left wheel and 100 mm on right wheel for 5 seconds\n"); robot.lock(); robot.setVel2(300, 100); robot.unlock(); ArTime start; start.setToNow(); while (1) { robot.lock(); if (start.mSecSince() > 5000) { robot.unlock(); break; } robot.unlock(); ArUtil::sleep(50); } printf("directMotionExample: Telling the robot to move forwards one meter, then sleeping 5 seconds\n"); robot.lock(); robot.move(1000); robot.unlock(); start.setToNow(); while (1) { robot.lock(); if (robot.isMoveDone()) { printf("directMotionExample: Finished distance\n"); robot.unlock(); break; } if (start.mSecSince() > 5000) { printf("directMotionExample: Distance timed out\n"); robot.unlock(); break; } robot.unlock(); ArUtil::sleep(50); } printf("directMotionExample: Telling the robot to move backwards one meter, then sleeping 5 seconds\n"); robot.lock(); robot.move(-1000); robot.unlock(); start.setToNow(); while (1) { robot.lock(); if (robot.isMoveDone()) { printf("directMotionExample: Finished distance\n"); robot.unlock(); break; } if (start.mSecSince() > 10000) { printf("directMotionExample: Distance timed out\n"); robot.unlock(); break; } robot.unlock(); ArUtil::sleep(50); } printf("directMotionExample: Telling the robot to turn to 180, then sleeping 4 seconds\n"); robot.lock(); robot.setHeading(180); robot.unlock(); start.setToNow(); while (1) { robot.lock(); if (robot.isHeadingDone(5)) { printf("directMotionExample: Finished turn\n"); robot.unlock(); break; } if (start.mSecSince() > 5000) { printf("directMotionExample: Turn timed out\n"); robot.unlock(); break; } robot.unlock(); ArUtil::sleep(100); } printf("directMotionExample: Telling the robot to turn to 90, then sleeping 2 seconds\n"); robot.lock(); robot.setHeading(90); robot.unlock(); start.setToNow(); while (1) { robot.lock(); if (robot.isHeadingDone(5)) { printf("directMotionExample: Finished turn\n"); robot.unlock(); break; } if (start.mSecSince() > 5000) { printf("directMotionExample: turn timed out\n"); robot.unlock(); break; } robot.unlock(); ArUtil::sleep(100); } printf("directMotionExample: Setting vel2 to 200 mm/sec on both wheels, then sleeping 3 seconds\n"); robot.lock(); robot.setVel2(200, 200); robot.unlock(); ArUtil::sleep(3000); printf("directMotionExample: Stopping the robot, then sleeping for 2 seconds\n"); robot.lock(); robot.stop(); robot.unlock(); ArUtil::sleep(2000); printf("directMotionExample: Setting velocity to 200 mm/sec then sleeping 3 seconds\n"); robot.lock(); robot.setVel(200); robot.unlock(); ArUtil::sleep(3000); printf("directMotionExample: Stopping the robot, then sleeping for 2 seconds\n"); robot.lock(); robot.stop(); robot.unlock(); ArUtil::sleep(2000); printf("directMotionExample: Setting vel2 with 0 on left wheel, 200 mm/sec on right, then sleeping 5 seconds\n"); robot.lock(); robot.setVel2(0, 200); robot.unlock(); ArUtil::sleep(5000); printf("directMotionExample: Telling the robot to rotate at 50 deg/sec then sleeping 5 seconds\n"); robot.lock(); robot.setRotVel(50); robot.unlock(); ArUtil::sleep(5000); printf("directMotionExample: Telling the robot to rotate at -50 deg/sec then sleeping 5 seconds\n"); robot.lock(); robot.setRotVel(-50); robot.unlock(); ArUtil::sleep(5000); printf("directMotionExample: Setting vel2 with 0 on both wheels, then sleeping 3 seconds\n"); robot.lock(); robot.setVel2(0, 0); robot.unlock(); ArUtil::sleep(3000); printf("directMotionExample: Now having the robot change heading by -125 degrees, then sleeping for 6 seconds\n"); robot.lock(); robot.setDeltaHeading(-125); robot.unlock(); ArUtil::sleep(6000); printf("directMotionExample: Now having the robot change heading by 45 degrees, then sleeping for 6 seconds\n"); robot.lock(); robot.setDeltaHeading(45); robot.unlock(); ArUtil::sleep(6000); printf("directMotionExample: Setting vel2 with 200 on left wheel, 0 on right wheel, then sleeping 5 seconds\n"); robot.lock(); robot.setVel2(200, 0); robot.unlock(); ArUtil::sleep(5000); printf("directMotionExample: Done, shutting down Aria and exiting.\n"); Aria::shutdown(); return 0; }