int main(int argc, char **argv) { Aria::init(); ArSimpleConnector connector(&argc, argv); ArRobot robot; ArSick sick; if (!Aria::parseArgs() || argc > 1) { Aria::logOptions(); Aria::exit(1); // exit program with error code 1 return 1; } ArKeyHandler keyHandler; Aria::setKeyHandler(&keyHandler); robot.attachKeyHandler(&keyHandler); robot.addRangeDevice(&sick); // Create the ArLineFinder object. Set it to log lots of information about its // processing. ArLineFinder lineFinder(&sick); lineFinder.setVerbose(true); // Add key callbacks that simply call the ArLineFinder::getLinesAndSaveThem() // function, which searches for lines in the current set of laser sensor // readings, and saves them in files with the names 'points' and 'lines'. ArFunctorC<ArLineFinder> findLineCB(&lineFinder, &ArLineFinder::getLinesAndSaveThem); keyHandler.addKeyHandler('f', &findLineCB); keyHandler.addKeyHandler('F', &findLineCB); ArLog::log(ArLog::Normal, "lineFinderExample: connecting to robot..."); if (!connector.connectRobot(&robot)) { printf("Could not connect to robot... exiting\n"); Aria::exit(1); // exit program with error code 1 return 1; } robot.runAsync(true); // now set up the laser ArLog::log(ArLog::Normal, "lineFinderExample: connecting to SICK laser..."); connector.setupLaser(&sick); sick.runAsync(); if (!sick.blockingConnect()) { printf("Could not connect to SICK laser... exiting\n"); Aria::exit(1); return 1; } printf("If you press the 'f' key the points and lines found will be saved\n"); printf("Into the 'points' and 'lines' file in the current working directory\n"); robot.waitForRunExit(); Aria::exit(0); return 0; }
int main(int argc, char **argv) { int ret; //Don't know what this variable is for ArRobot robot;// Robot object ArSick sick; // Laser scanner ArSerialConnection laserCon; // Scanner connection ArSerialConnection con; // Robot connection std::string str; // Standard output // sonar, must be added to the robot ArSonarDevice sonar; // the actions we'll use to wander // recover from stalls ArActionStallRecover recover; // react to bumpers ArActionBumpers bumpers; // limiter for close obstacles ArActionLimiterForwards limiter("speed limiter near", 300, 600, 250, 1.1); // limiter for far away obstacles ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 600, 1.1); // limiter for the table sensors ArActionLimiterTableSensor tableLimiter; // actually move the robot ArActionConstantVelocity constantVelocity("Constant Velocity", 400); // turn the orbot if its slowed down ArActionTurn turn; // mandatory init Aria::init(); // Parse all our args ArSimpleConnector connector(&argc, argv); connector.parseArgs(); if (argc > 1) { connector.logOptions(); exit(1); } // add the sonar to the robot robot.addRangeDevice(&sonar); // add the laser to the robot robot.addRangeDevice(&sick); // NOTE: HARDCODED USB PORT! // Attempt to open hard-coded USB to robot if ((ret = con.open("/dev/ttyUSB2")) != 0){ // If connection fails, exit str = con.getOpenMessage(ret); printf("Open failed: %s\n", str.c_str()); Aria::shutdown(); return 1; } // set the robot to use the given connection robot.setDeviceConnection(&con); // do a blocking connect, if it fails exit if (!robot.blockingConnect()) { printf("Could not connect to robot... exiting\n"); Aria::shutdown(); return 1; } // turn on the motors, turn off amigobot sounds //robot.comInt(ArCommands::SONAR, 0); robot.comInt(ArCommands::SOUNDTOG, 0); // start the robot running, true so that if we lose connection the run stops robot.runAsync(true); // Attempt to connect to SICK using another hard-coded USB connection sick.setDeviceConnection(&laserCon); if((ret=laserCon.open("/dev/ttyUSB3")) !=0) { //If connection fails, shutdown Aria::shutdown(); return 1; } //Configure the SICK sick.configureShort(false,/*not using sim*/ArSick::BAUD38400,ArSick::DEGREES180,ArSick::INCREMENT_HALF); //Run the sick sick.runAsync(); // Presumably test to make sure that the connection is good if(!sick.blockingConnect()){ printf("Could not get sick...exiting\n"); Aria::shutdown(); return 1; } printf("We are connected to the laser!"); /* robot.lock(); robot.comInt(ArCommands::ENABLE, 1); robot.unlock(); */ int range [361] = {0}; int drange [360] = {0}; int i = 0; int obj_range [2]; int old_range [360]={0}; clock_t now, prev; while(1){ range [361] = {0}; drange [360] = {0}; i = 0; obj_range[2]; std::list<ArSensorReading *> *readings; std::list<ArSensorReading *>::iterator it; sick.lockDevice(); readings=(list<ArSensorReading *,allocator<ArSensorReading *> > *)sick.getRawReadings(); if(NULL!=readings){ if ((readings->end() != readings->begin())){ for (it = readings->begin(); it!= readings->end(); it++){ // std::cout << (*it)->getRange()<<" "; range[i] = ((*it)->getRange()); if(i){ drange[i-1] = range[i] - range[i-1]; printf("%f %i %i\r\n", (float)i/2.0, range[i], drange[i-1]); } i++; } int i = 0; //detect the object range while (i < 360) { if (range[i]>Default_Distance + alpha) { ; } else { if (obj_range[0]=0) obj_range[0]=i; else obj_range[1]=i; } } if (!now) prev=now; now=clock(); duration=now-prev; /******moving straight*******/ float speed = avg_speed(obj_range,old_range,range,(float)duration) /*while(i < 360){ int r_edge = 0; int l_edge = 0; float obsticle_degree = 0; if(drange[i] > D_DISTANCE){ r_edge = i; while(drange[i] > -(D_DISTANCE)){ i++; } l_edge = i; obsticle_degree = (r_edge + (l_edge - r_edge)/2.0)/2.0; printf("\r\n object detected at %f\r\n", obsticle_degree); } std::cout<<std::endl; }*/ } else{ std::cout << "(readings->end() == readings -> begin())" << std::endl; } } else{
// when we connect turn off the sonar, turn on the motors, and disable amigobot // sound void ConnHandler::connected(void) { myRobot->comInt(ArCommands::SONAR, 0); myRobot->comInt(ArCommands::ENABLE, 1); myRobot->comInt(ArCommands::SOUNDTOG, 0); }
int main(void) { ArSerialConnection con; ArRobot robot; int ret; std::string str; ArActionLimiterForwards limiter("speed limiter near", 300, 600, 250); ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 400); ArActionLimiterBackwards backwardsLimiter; ArActionConstantVelocity stop("stop", 0); ArActionConstantVelocity backup("backup", -200); ArSonarDevice sonar; ArACTS_1_2 acts; ArSonyPTZ sony(&robot); ArGripper gripper(&robot, ArGripper::GENIO); Acquire acq(&acts, &gripper); DriveTo driveTo(&acts, &gripper, &sony); PickUp pickUp(&acts, &gripper, &sony); TakeBlockToWall takeBlock(&robot, &gripper, &sony, &acq, &driveTo, &pickUp, &backup); Aria::init(); if (!acts.openPort(&robot)) { printf("Could not connect to acts\n"); exit(1); } robot.addRangeDevice(&sonar); //con.setBaud(38400); if ((ret = con.open()) != 0) { str = con.getOpenMessage(ret); printf("Open failed: %s\n", str.c_str()); Aria::shutdown(); return 1; } robot.setDeviceConnection(&con); if (!robot.blockingConnect()) { printf("Could not connect to robot... exiting\n"); Aria::shutdown(); return 1; } sony.init(); ArUtil::sleep(1000); //robot.setAbsoluteMaxTransVel(400); robot.setStateReflectionRefreshTime(250); robot.comInt(ArCommands::ENABLE, 1); robot.comInt(ArCommands::SOUNDTOG, 0); ArUtil::sleep(200); robot.addAction(&limiter, 100); robot.addAction(&limiterFar, 99); robot.addAction(&backwardsLimiter, 98); robot.addAction(&acq, 77); robot.addAction(&driveTo, 76); robot.addAction(&pickUp, 75); robot.addAction(&backup, 50); robot.addAction(&stop, 30); robot.run(true); Aria::shutdown(); return 0; }
int main(int argc, char **argv) { std::string str; int ret; int successes = 0, failures = 0; int action; bool exitOnFailure = true; ArSerialConnection con; ArRobot robot; srand(time(NULL)); // if (!exitOnFailure) // ArLog::init(ArLog::None, ArLog::Terse); //else //ArLog::init(ArLog::None); while (1) { if (con.getStatus() != ArDeviceConnection::STATUS_OPEN && (ret = con.open(ArUtil::COM2)) != 0) { str = con.getOpenMessage(ret); printf("Open failed: %s\n", str.c_str()); ++failures; if (exitOnFailure) { printf("Failed\n"); exit(0); } else { ArUtil::sleep(200); continue; } } robot.setDeviceConnection(&con); ArUtil::sleep((rand() % 5) * 100); if (robot.blockingConnect()) { robot.comInt(ArCommands::SONAR, 0); robot.comInt(ArCommands::SOUNDTOG, 0); robot.comInt(ArCommands::PLAYLIST, 0); robot.comInt(ArCommands::ENCODER, 1); ArUtil::sleep((rand() % 20) * 100); ++successes; // okay, now try to leave it in a messed up state action = rand() % 8; switch (action) { case 0: printf("Discon 0 "); robot.disconnect(); ArUtil::sleep(100); robot.com(0); break; case 1: printf("Discon 1 "); robot.disconnect(); ArUtil::sleep(100); robot.com(0); ArUtil::sleep(100); robot.com(1); break; case 2: printf("Discon 2 "); robot.disconnect(); ArUtil::sleep(100); robot.com(0); ArUtil::sleep(100); robot.com(1); ArUtil::sleep(100); robot.com(2); break; case 3: printf("Discon 10 "); robot.disconnect(); ArUtil::sleep(100); robot.com(10); break; case 4: printf("Discon "); robot.disconnect(); break; default: printf("Leave "); break; } } else { if (exitOnFailure) { printf("Failed after %d tries.\n", successes); exit(0); } printf("Failed "); ++failures; } if ((rand() % 2) == 0) { printf(" ! RadioDisconnect ! "); con.write("|||\15", strlen("!!!\15")); ArUtil::sleep(100); con.write("WMD\15", strlen("WMD\15")); ArUtil::sleep(200); } if ((rand() % 2) == 0) { printf(" ! ClosePort !\n"); con.close(); } else printf("\n"); printf("#### %d successes %d failures, %% %.2f success\n", successes, failures, (float)successes/(float)(successes+failures)*100); ArUtil::sleep((rand() % 2)* 1000); } return 0; }
int main (int argc, char** argv) { Aria::init(); Arnl::init(); ArRobot robot; ArArgumentParser parser(&argc, argv); parser.loadDefaultArguments(); ArRobotConnector robotConnector(&parser, &robot); if (!robotConnector.connectRobot()) { ArLog::log(ArLog::Terse, "Could not connect to the robot."); if(parser.checkHelpAndWarnUnparsed()) { // -help not given, just exit. Aria::logOptions(); Aria::exit(1); return 1; } } ArSonarDevice sonarDev; ArLog::log(ArLog::Normal, "OK"); char fileDir[1024]; ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(), "examples"); ArLog::log(ArLog::Normal, "Installation directory is: %s\nMaps directory is: %s\n", Aria::getDirectory(), fileDir); // strcpy(fileDir, "columbia.map"); ArLog::log(ArLog::Normal, "file Maps directory is: %s\n",fileDir); ArMap map(fileDir); map.readFile("columbia.map"); // set it up to ignore empty file names (otherwise the parseFile // on the config will fail) map.setIgnoreEmptyFileName(true); robot.addRangeDevice(&sonarDev); // set home pose robot.moveTo(ArPose(0,0,0)); ArPathPlanningTask pathPlan(&robot, &sonarDev, &map); ArActionGoto gotoPoseAction("goto"); // gotoPoseAction.activate(); // pathPlan.getPathPlanActionGroup()->addAction(&gotoPoseAction, 50); // pathPlan.getPathPlanAction()->activate(); ArForbiddenRangeDevice forbidden(&map); robot.addRangeDevice(&forbidden); pathPlan.addRangeDevice(&forbidden, ArPathPlanningTask::CURRENT); // pathPlan.planAndSetupAction(ArPose(0, 0, 0)); // pathPlan. /* if (pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true)) ArLog::log(ArLog::Normal, "OK"); else ArLog::log(ArLog::Normal, "FAILED"); */ // create functor ArGlobalFunctor1<ArPose> goalDone(&addGoalDone);// = new ArGlobalFunctor1(&addGoalDone); ArGlobalFunctor1<ArPose> goalFail(&addGoalFailed); ArGlobalFunctor1<ArPose> goalFinished(&addGoalFinished); ArGlobalFunctor1<ArPose> goalInterrupted(&addGoalInterrupted); // add functor pathPlan.addGoalDoneCB(&goalDone); pathPlan.addGoalFailedCB(&goalFail); pathPlan.addGoalFinishedCB(&goalFinished); pathPlan.addGoalInterruptedCB(&goalInterrupted); robot.runAsync(true); robot.enableMotors(); pathPlan.runAsync(); // ArPathPlanningTask::PathPlanningState state = pathPlan.getState(); // while(!pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true)); pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true); ArPathPlanningTask::PathPlanningState state = pathPlan.getState(); char* s = ""; switch(state) { case ArPathPlanningTask::NOT_INITIALIZED: {s = "NOT_INITIALIZED"; break;} case ArPathPlanningTask::PLANNING_PATH: { s = "PLANNING_PATH";break;} case ArPathPlanningTask::MOVING_TO_GOAL:{s = "MOVING_TO_GOAL";break;} case ArPathPlanningTask::REACHED_GOAL: {s = "REACHED_GOAL";break;} case ArPathPlanningTask::FAILED_PLAN: { s = "FAILED_PLAN";break;} case ArPathPlanningTask::FAILED_MOVE: { s="FAILED_MOVE";break;} case ArPathPlanningTask::ABORTED_PATHPLAN:{s = "ABORTED_PATHPLAN";break;} // case ArPathPlanningTask::INVALID: // default: // return "UNKNOWN"; } ArLog::log(ArLog::Normal,s); robot.waitForRunExit(); // pathPlan. // char* text = new char[512]; // pathPlan.getStatusString(text, sizeof(text)); // printf("Planning status: %s.\n", text); // while(pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true)); Aria::shutdown(); // Arnl::s; Aria::exit(0); }
int main(int argc, char **argv) { Aria::init(); ArArgumentParser parser(&argc, argv); parser.loadDefaultArguments(); ArRobot robot; ArRobotConnector robotConnector(&parser, &robot); if(!robotConnector.connectRobot()) { ArLog::log(ArLog::Terse, "teleopActionsExample: Could not connect to the robot."); if(parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(1); } } if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(1); } ArLog::log(ArLog::Normal, "teleopActionsExample: Connected."); // limiter for close obstacles ArActionLimiterForwards limiter("speed limiter near", 300, 600, 250); // limiter for far away obstacles ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 400); // limiter that checks IR sensors (like Peoplebot has) ArActionLimiterTableSensor tableLimiter; // limiter so we don't bump things backwards ArActionLimiterBackwards backwardsLimiter; // the joydrive action ArActionJoydrive joydriveAct; // the keydrive action ArActionKeydrive keydriveAct; // sonar device, used by the limiter actions. ArSonarDevice sonar; printf("This program will allow you to use a joystick or keyboard to control the robot.\nYou can use the arrow keys to drive, and the spacebar to stop.\nFor joystick control press the trigger button and then drive.\nPress escape to exit.\n"); // if we don't have a joystick, let 'em know if (!joydriveAct.joystickInited()) printf("Do not have a joystick, only the arrow keys on the keyboard will work.\n"); // add the sonar to the robot robot.addRangeDevice(&sonar); // set the robots maximum velocity (sonar don't work at all well if you're // going faster) robot.setAbsoluteMaxTransVel(400); // enable the motor robot.enableMotors(); // Add the actions, with the limiters as highest priority, then the teleop. // actions. This will keep the teleop. actions from being able to drive too // fast and hit something robot.addAction(&tableLimiter, 100); robot.addAction(&limiter, 95); robot.addAction(&limiterFar, 90); robot.addAction(&backwardsLimiter, 85); robot.addAction(&joydriveAct, 50); robot.addAction(&keydriveAct, 45); // Configure the joydrive action so it will let the lower priority actions // (i.e. keydriveAct) request motion if the joystick button is // not pressed. joydriveAct.setStopIfNoButtonPressed(false); // run the robot, true means that the run will exit if connection lost robot.run(true); Aria::exit(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; }
int main(int argc, char **argv) { // mandatory init Aria::init(); //ArLog::init(ArLog::StdOut, ArLog::Verbose); // set up our parser ArArgumentParser parser(&argc, argv); // load the default arguments parser.loadDefaultArguments(); // robot ArRobot robot; // set up our simple connector ArRobotConnector robotConnector(&parser, &robot); // add a gyro, it'll see if it should attach to the robot or not ArAnalogGyro gyro(&robot); // set up the robot for connecting if (!robotConnector.connectRobot()) { printf("Could not connect to robot... exiting\n"); Aria::exit(1); } ArDataLogger dataLogger(&robot, "dataLog.txt"); dataLogger.addToConfig(Aria::getConfig()); // our base server object ArServerBase server; ArLaserConnector laserConnector(&parser, &robot, &robotConnector); ArServerSimpleOpener simpleOpener(&parser); ArClientSwitchManager clientSwitchManager(&server, &parser); // parse the command line... fail and print the help if the parsing fails // or if the help was requested if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(1); } // Set up where we'll look for files such as user/password char fileDir[1024]; ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(), "ArNetworking/examples"); // first open the server up if (!simpleOpener.open(&server, fileDir, 240)) { if (simpleOpener.wasUserFileBad()) printf("Bad user/password/permissions file\n"); else printf("Could not open server port\n"); exit(1); } // Range devices: ArSonarDevice sonarDev; robot.addRangeDevice(&sonarDev); ArIRs irs; robot.addRangeDevice(&irs); ArBumpers bumpers; robot.addRangeDevice(&bumpers); // attach services to the server ArServerInfoRobot serverInfoRobot(&server, &robot); ArServerInfoSensor serverInfoSensor(&server, &robot); ArServerInfoDrawings drawings(&server); // modes for controlling robot movement ArServerModeStop modeStop(&server, &robot); ArServerModeRatioDrive modeRatioDrive(&server, &robot); ArServerModeWander modeWander(&server, &robot); modeStop.addAsDefaultMode(); modeStop.activate(); // set up the simple commands ArServerHandlerCommands commands(&server); ArServerSimpleComUC uCCommands(&commands, &robot); // send commands directly to microcontroller ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // control debug logging ArServerSimpleComGyro gyroCommands(&commands, &robot, &gyro); // configure gyro ArServerSimpleComLogRobotConfig configCommands(&commands, &robot); // control more debug logging ArServerSimpleServerCommands serverCommands(&commands, &server); // control ArNetworking debug logging ArServerSimpleLogRobotDebugPackets logRobotDebugPackets(&commands, &robot, "."); // debugging tool // ArServerModeDrive is an older drive mode. ArServerModeRatioDrive is newer and generally performs better, // but you can use this for old clients if neccesary. //ArServerModeDrive modeDrive(&server, &robot); //modeDrive.addControlCommands(&commands); // configure the drive modes (e.g. enable/disable safe drive) ArServerHandlerConfig serverHandlerConfig(&server, Aria::getConfig()); // make a config handler ArLog::addToConfig(Aria::getConfig()); // let people configure logging modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings"); // able to configure teleop settings modeRatioDrive.addControlCommands(&commands); // Forward video if either ACTS or SAV server are running. // You can find out more about SAV and ACTS on our website // http://robots.activmedia.com. ACTS is for color tracking and is // a separate product. SAV just does software A/V transmitting and is // free to all our customers. Just run ACTS or SAV server before you // start this program and this class here will forward video from the // server to the client. ArHybridForwarderVideo videoForwarder(&server, "localhost", 7070); // Control a pan/tilt/zoom camera, if one is installed, and the video // forwarder was enabled above. ArPTZ *camera = NULL; ArServerHandlerCamera *handlerCamera = NULL; ArCameraCollection *cameraCollection = NULL; if (videoForwarder.isForwardingVideo()) { bool invertedCamera = false; camera = new ArVCC4(&robot, invertedCamera, ArVCC4::COMM_UNKNOWN, true, true); camera->init(); cameraCollection = new ArCameraCollection(); cameraCollection->addCamera("Cam1", "VCC4", "Camera", "VCC4"); handlerCamera = new ArServerHandlerCamera("Cam1", &server, &robot, camera, cameraCollection); } // You can use this class to send a set of arbitrary strings // for MobileEyes to display, this is just a small example ArServerInfoStrings stringInfo(&server); Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor()); Aria::getInfoGroup()->addStringInt( "Motor Packet Count", 10, new ArConstRetFunctorC<int, ArRobot>(&robot, &ArRobot::getMotorPacCount)); /* Aria::getInfoGroup()->addStringInt( "Laser Packet Count", 10, new ArRetFunctorC<int, ArSick>(&sick, &ArSick::getSickPacCount)); */ // start the robot running, true means that if we lose connection the run thread stops robot.runAsync(true); // connect the laser(s) if it was requested if (!laserConnector.connectLasers()) { printf("Could not connect to lasers... exiting\n"); Aria::exit(2); } drawings.addRobotsRangeDevices(&robot); // log whatever we wanted to before the runAsync simpleOpener.checkAndLog(); // now let it spin off in its own thread server.runAsync(); printf("Server is now running...\n"); // Add a key handler so that you can exit by pressing // escape. Note that a key handler prevents you from running // a program in the background on Linux, since it expects an // active terminal to read keys from; remove this if you want // to run it in the background. ArKeyHandler *keyHandler; if ((keyHandler = Aria::getKeyHandler()) == NULL) { keyHandler = new ArKeyHandler; Aria::setKeyHandler(keyHandler); robot.lock(); robot.attachKeyHandler(keyHandler); robot.unlock(); printf("To exit, press escape.\n"); } // Read in parameter files. std::string configFile = "serverDemoConfig.txt"; Aria::getConfig()->setBaseDirectory("./"); if (Aria::getConfig()->parseFile(configFile.c_str(), true, true)) { ArLog::log(ArLog::Normal, "Loaded config file %s", configFile.c_str()); } else { if (ArUtil::findFile(configFile.c_str())) { ArLog::log(ArLog::Normal, "Trouble loading configuration file %s, continuing", configFile.c_str()); } else { ArLog::log(ArLog::Normal, "No configuration file %s, will try to create if config used", configFile.c_str()); } } clientSwitchManager.runAsync(); robot.lock(); robot.enableMotors(); robot.unlock(); robot.waitForRunExit(); Aria::exit(0); }
int main(int argc, char **argv) { std::string str; int ret; int dist; ArTime start; ArPose startPose; bool vel2 = false; // connection to the robot ArSerialConnection con; // the robot ArRobot robot; // the connection handler from above ConnHandler ch(&robot); // init area with a dedicated signal handling thread Aria::init(Aria::SIGHANDLE_THREAD); if (argc != 2 || (dist = atoi(argv[1])) == 0) { printf("Usage: %s <distInMM>\n", argv[0]); exit(0); } if (dist < 1000) { printf("You must go at least a meter\n"); exit(0); } // open the connection with the defaults, exit if failed if ((ret = con.open()) != 0) { str = con.getOpenMessage(ret); printf("Open failed: %s\n", str.c_str()); Aria::shutdown(); return 1; } // set the robots connection robot.setDeviceConnection(&con); // try to connect, if we fail, the connection handler should bail if (!robot.blockingConnect()) { // this should have been taken care of by the connection handler // but just in case printf( "asyncConnect failed because robot is not running in its own thread.\n"); Aria::shutdown(); return 1; } // run the robot in its own thread, so it gets and processes packets and such robot.runAsync(false); // just a big long set of printfs, direct motion commands and sleeps, // it should be self-explanatory robot.lock(); /* robot.setAbsoluteMaxTransVel(2000); robot.setTransVelMax(2000); robot.setTransAccel(1000); robot.setTransDecel(1000); robot.comInt(82, 30); // rotkp robot.comInt(83, 200); // rotkv robot.comInt(84, 0); // rotki robot.comInt(85, 30); // transkp robot.comInt(86, 450); // transkv robot.comInt(87, 4); // transki */ printf("Driving %d mm (going full speed for that far minus a meter then stopping)\n", dist); if (vel2) robot.setVel2(2200, 2200); else robot.setVel(2200); robot.unlock(); start.setToNow(); startPose = robot.getPose(); while (1) { robot.lock(); printf("\r vel: %.0f x: %.0f y: %.0f: dist: %.0f heading: %.2f", robot.getVel(), robot.getX(), robot.getY(), startPose.findDistanceTo(robot.getPose()), robot.getTh()); if (startPose.findDistanceTo(robot.getPose()) > abs(dist) - 1000) { printf("\nFinished distance\n"); robot.setVel(0); robot.unlock(); break; } if (start.mSecSince() > 10000) { printf("\nDistance timed out\n"); robot.setVel(0); robot.unlock(); break; } robot.unlock(); ArUtil::sleep(50); } if (vel2) robot.setVel2(0, 0); else robot.setVel(0); start.setToNow(); while (1) { robot.lock(); if (vel2) robot.setVel2(0, 0); else robot.setVel(0); if (fabs(robot.getVel()) < 20) { printf("Stopped\n"); robot.unlock(); break; } if (start.mSecSince() > 2000) { printf("\nStop timed out\n"); robot.unlock(); break; } robot.unlock(); ArUtil::sleep(50); } robot.lock(); robot.disconnect(); robot.unlock(); // shutdown and ge tout Aria::shutdown(); return 0; }
int main(int argc, char **argv) { Aria::init(); ArArgumentParser parser(&argc, argv); parser.loadDefaultArguments(); ArRobot robot; ArRobotConnector robotConnector(&parser, &robot); ArLaserConnector laserConnector(&parser, &robot, &robotConnector); ArAnalogGyro analogGyro(&robot); // Always connect to the laser, and add half-degree increment and 180 degrees as default arguments for // laser parser.addDefaultArgument("-connectLaser -laserDegrees 180 -laserIncrement half"); if(!robotConnector.connectRobot()) { ArLog::log(ArLog::Terse, "sickLagger: Could not connect to the robot."); if(parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(1); } } if(!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(1); } ArKeyHandler keyHandler; Aria::setKeyHandler(&keyHandler); robot.attachKeyHandler(&keyHandler); #ifdef WIN32 printf("Pausing 5 seconds so you can disconnect VNC if you are using it.\n"); ArUtil::sleep(5000); #endif std::string filename = "1scans.2d"; if (argc > 1) filename = argv[1]; printf("Logging to file %s\n", filename.c_str()); ArActionGroupRatioDriveUnsafe group(&robot); group.activateExclusive(); robot.runAsync(true); if(!laserConnector.connectLasers(false, false, true)) { ArLog::log(ArLog::Terse, "sickLogger: Error: Could not connect to laser(s). Use -help to list options."); Aria::exit(3); } // Allow some time for first set of laser reading to arrive ArUtil::sleep(500); // enable the motors, disable amigobot sounds robot.comInt(ArCommands::SONAR, 0); robot.comInt(ArCommands::ENABLE, 1); robot.comInt(ArCommands::SOUND, 32); robot.comInt(ArCommands::SOUNDTOG, 0); // enable the joystick driving from the one connected to the microcontroller robot.comInt(ArCommands::JOYDRIVE, 1); // Create the logging object // This must be created after the robot and laser are connected so it can get // correct parameters from them. // The third argmument is how far the robot must travel before a new laser // scan is logged. // The fourth argument is how many degrees the robot must rotate before a new // laser scan is logged. The sixth boolean argument is whether to place a goal // when the g or G key is pressed (by adding a handler to the global // ArKeyHandler) or when the robots joystick button is // pressed. ArLaser *laser = robot.findLaser(1); if(!laser) { ArLog::log(ArLog::Terse, "sickLogger: Error, not connected to any lasers."); Aria::exit(2); } ArLaserLogger logger(&robot, laser, 300, 25, filename.c_str(), true); // just hang out and wait for the end robot.waitForRunExit(); Aria::exit(0); }
// the important function void KeyPTU::drive(void) { // if the PTU isn't initialized, initialize it here... it has to be // done here instead of above because it needs to be done when the // robot is connected if (!myPTUInited && myRobot->isConnected()) { ArLog::log(ArLog::Normal, "Initializing ArDPPTU..."); myPTU.init(); ArLog::log(ArLog::Normal, "Resetting PTU and performing self-calibration..."); myPTU.resetCalib(); myPTU.awaitExec(); // DPPTU will wait for self-calibration to end before executing the following commands (though they will still be sent) mySlew = myPTU.getPanSlew(); //uses only pan slew rate myPTU.awaitExec(); myPTUInited = true; myInit = false; myAbsolute = true; } if (myInit == true) // User hit initialization key { ArLog::log(ArLog::Normal, "Initializing PTU..."); myPTU.init(); myInit = false; myDesiredPanPos = myPTU.getPan(); myDesiredTiltPos = myPTU.getTilt(); mySlew = myPTU.getPanSlew(); //uses only pan slew rate myReset = false; } if (myReset == true) // User hit reset key { ArLog::log(ArLog::Normal, "Resetting PTU and performing self-calibration..."); myPTU.resetCalib(); myPTU.awaitExec(); myDesiredPanPos = myPTU.getPan(); myDesiredTiltPos = myPTU.getTilt(); myReset = false; } else // User did nothing, or hit a key that changed myDesiredPanPos, myDesiredTiltPos, or mySlew (so request PTU to move if those changed since last request) { // Some PTUs can determine their current position (with encoders, etc) and return that. // canGetRealPanTilt() will return true in this case, and getPan() and // getTilt() will return those received values. Otherwise, getPan() and // getTilt() return the last commanded values. getLastPanRequest() and // getLastTiltRequest() will always return the last commanded values sent by // ArDPPTU (so in the case that canGetRealPanTilt() is false, getPan() and // getTilt() return the same pair of values as getLastPanRequest() and // getLastTiltRequest(). ArDPPTU::canGetRealPanTilt() is initialally false, // but once the first set of pan and tilt positions is read back from the // PTU device, it becomes true. if(myPTU.canGetRealPanTilt()) printf("Position (%.1f deg, %.1f deg) [Incr. %d deg] Press ? for help \r", myPTU.getPan(), myPTU.getTilt(), myPosIncrement); else printf("Requested (%.1f deg, %.1f deg) [Incr. %d deg] Press ? for help \r", myPTU.getPan(), myPTU.getTilt(), myPosIncrement); if (myDesiredPanPos != myPTU.getLastPanRequest()) { if (myAbsolute) myPTU.pan(myDesiredPanPos); else myPTU.panRel(myDesiredPanPos - myPTU.getPan()); } if (myDesiredTiltPos != myPTU.getLastTiltRequest()) { if (myAbsolute) myPTU.tilt(myDesiredTiltPos); else myPTU.tiltRel(myDesiredTiltPos - myPTU.getTilt()); } if (mySlew != myPTU.getPanSlew()) { myPTU.panSlew(mySlew); myPTU.tiltSlew(mySlew); } } }
int main(void) { int ret; std::string str; CBTest cbTest; ArFunctorC<CBTest> connectCB(&cbTest, &CBTest::connected); ArFunctorC<CBTest> failedConnectCB(&cbTest, &CBTest::failedConnect); ArFunctorC<CBTest> disconnectCB(&cbTest, &CBTest::disconnected); ArFunctorC<CBTest> disconnectErrorCB(&cbTest, &CBTest::disconnectedError); ArSerialConnection con; ArRobot robot; printf("If a robot is attached to your port you should see:\n"); printf("Failed connect, Connected, Disconnected Error, Connected, Disconnected\n"); printf("If no robot is attached you should see:\n"); printf("Failed connect, Failed connect, Failed connect\n"); printf("-------------------------------------------------------\n"); ArLog::init(ArLog::None, ArLog::Terse); srand(time(NULL)); robot.setDeviceConnection(&con); robot.addConnectCB(&connectCB, ArListPos::FIRST); robot.addFailedConnectCB(&failedConnectCB, ArListPos::FIRST); robot.addDisconnectNormallyCB(&disconnectCB, ArListPos::FIRST); robot.addDisconnectOnErrorCB(&disconnectErrorCB, ArListPos::FIRST); // this should fail since there isn't an open port yet robot.blockingConnect(); if ((ret = con.open()) != 0) { str = con.getOpenMessage(ret); printf("Open failed: %s\n", str.c_str()); exit(0); } robot.blockingConnect(); con.close(); robot.loopOnce(); if ((ret = con.open()) != 0) { str = con.getOpenMessage(ret); printf("Open failed: %s\n", str.c_str()); exit(0); } robot.blockingConnect(); robot.disconnect(); exit(0); }
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); } } if (!Aria::parseArgs()) { Aria::logOptions(); Aria::shutdown(); 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."); return 0; }
int main(int argc, char **argv) { Aria::init(); ArSimpleConnector con(&argc, argv); ArRobot robot; ArP2Arm arm; if(!Aria::parseArgs()) { Aria::logOptions(); Aria::shutdown(); return 1; } ArLog::log(ArLog::Normal, "armExample: Connecting to the robot."); if(!con.connectRobot(&robot)) { ArLog::log(ArLog::Terse, "armExample: Could not connect to the robot. Exiting."); Aria::shutdown(); return 1; } robot.runAsync(true); // turn off sonar robot.comInt(28, 0); // Set up and initialize the arm arm.setRobot(&robot); if (arm.init() != ArP2Arm::SUCCESS) { ArLog::log(ArLog::Terse, "armExample: Error initializing the P2 Arm!"); return 1; } // Print out some of the settings P2ArmJoint *joint; printf("Current joint info:\nJoint Vel Home Center\n"); for (int i=1; i<=ArP2Arm::NumJoints; i++) { joint = arm.getJoint(i); printf(" %2i: %5i %5i %5i\n", i, joint->myVel, joint->myHome, joint->myCenter); } printf("\n"); // Put the arm to work printf("Powering on (takes a couple seconds to stabilize)\n"); arm.powerOn(); // Request one status packet and print out the arm's status printf("Current arm status:\n"); arm.requestStatus(ArP2Arm::StatusSingle); ArUtil::sleep(200); // Give time to get the packet printf("Arm Status: "); if (arm.getStatus() & ArP2Arm::ArmGood) printf("Good=1 "); else printf("Good=0 "); if (arm.getStatus() & ArP2Arm::ArmInited) printf("Inited=1 "); else printf("Inited=0 "); if (arm.getStatus() & ArP2Arm::ArmPower) printf("Power=1 "); else printf("Power=0 "); if (arm.getStatus() & ArP2Arm::ArmHoming) printf("Homing=1 "); else printf("Homing=0 "); printf("\n\n"); // Move the arm joints to specific positions printf("Moving Arm...\n"); int deploy_offset[] = {0, -100, 10, 40, 80, -55, 20}; for (int i=1; i<=ArP2Arm::NumJoints; i++) { joint = arm.getJoint(i); arm.moveToTicks(i, joint->myCenter + deploy_offset[i]); } // Wait for arm to achieve new position, printing joint positions and M for // moving, NM for not moving. arm.requestStatus(ArP2Arm::StatusContinuous); ArUtil::sleep(300); // wait a moment for arm status packet update with joints moving bool moving = true; while (moving) { moving = false; printf("Joints: "); for (int i=1; i<=ArP2Arm::NumJoints; i++) { printf("[%d] %.0f, ", i, arm.getJointPos(i)); if (arm.getMoving(i)) { printf("M; "); moving = true; } else { printf("NM; "); } } printf("\r"); } printf("\n\n"); // Return arm to park, wait, and disconnect. (Though the arm will automatically park // on client disconnect) printf("Parking arm.\n"); arm.park(); // Wait 5 seconds or until arm shuts off for(int i = 5; (i > 0) && (arm.getStatus() & ArP2Arm::ArmPower); i--) { ArUtil::sleep(1000); } // Disconnect from robot, etc., and exit. printf("Shutting down ARIA and exiting.\n"); Aria::shutdown(); return(0); }
int main(int argc, char** argv) { // Initialize some global data Aria::init(); // If you want ArLog to print "Verbose" level messages uncomment this: //ArLog::init(ArLog::StdOut, ArLog::Verbose); // This object parses program options from the command line ArArgumentParser parser(&argc, argv); // Load some default values for command line arguments from /etc/Aria.args // (Linux) or the ARIAARGS environment variable. parser.loadDefaultArguments(); // Central object that is an interface to the robot and its integrated // devices, and which manages control of the robot by the rest of the program. ArRobot robot; // Object that connects to the robot or simulator using program options ArRobotConnector robotConnector(&parser, &robot); // If the robot has an Analog Gyro, this object will activate it, and // if the robot does not automatically use the gyro to correct heading, // this object reads data from it and corrects the pose in ArRobot ArAnalogGyro gyro(&robot); // Connect to the robot, get some initial data from it such as type and name, // and then load parameter files for this robot. if (!robotConnector.connectRobot()) { // Error connecting: // if the user gave the -help argumentp, then just print out what happened, // and continue so options can be displayed later. if (!parser.checkHelpAndWarnUnparsed()) { ArLog::log(ArLog::Terse, "Could not connect to robot, will not have parameter file so options displayed later may not include everything"); } // otherwise abort else { ArLog::log(ArLog::Terse, "Error, could not connect to robot."); Aria::logOptions(); Aria::exit(1); } } if(!robot.isConnected()) { ArLog::log(ArLog::Terse, "Internal error: robot connector succeeded but ArRobot::isConnected() is false!"); } // Connector for laser rangefinders ArLaserConnector laserConnector(&parser, &robot, &robotConnector); // Connector for compasses ArCompassConnector compassConnector(&parser); // Parse the command line options. Fail and print the help message if the parsing fails // or if the help was requested with the -help option if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(1); return 1; } // Used to access and process sonar range data ArSonarDevice sonarDev; // Used to perform actions when keyboard keys are pressed ArKeyHandler keyHandler; Aria::setKeyHandler(&keyHandler); // ArRobot contains an exit action for the Escape key. It also // stores a pointer to the keyhandler so that other parts of the program can // use the same keyhandler. robot.attachKeyHandler(&keyHandler); printf("You may press escape to exit\n"); // Attach sonarDev to the robot so it gets data from it. robot.addRangeDevice(&sonarDev); // Start the robot task loop running in a new background thread. The 'true' argument means if it loses // connection the task loop stops and the thread exits. robot.runAsync(true); // Connect to the laser(s) if lasers were configured in this robot's parameter // file or on the command line, and run laser processing thread if applicable // for that laser class. For the purposes of this demo, add all // possible lasers to ArRobot's list rather than just the ones that were // connected by this call so when you enter laser mode, you // can then interactively choose which laser to use from that list of all // lasers mentioned in robot parameters and on command line. Normally, // only connected lasers are put in ArRobot's list. if (!laserConnector.connectLasers( false, // continue after connection failures false, // add only connected lasers to ArRobot true // add all lasers to ArRobot )) { printf("Could not connect to lasers... exiting\n"); Aria::exit(2); } /* not needed, robot connector will do it by default if (!sonarConnector.connectSonars( false, // continue after connection failures false, // add only connected lasers to ArRobot true // add all lasers to ArRobot )) { printf("Could not connect to sonars... exiting\n"); Aria::exit(2); } */ // Create and connect to the compass if the robot has one. ArTCM2 *compass = compassConnector.create(&robot); if(compass && !compass->blockingConnect()) { compass = NULL; } // Sleep for a second so some messages from the initial responses // from robots and cameras and such can catch up ArUtil::sleep(1000); // We need to lock the robot since we'll be setting up these modes // while the robot task loop thread is already running, and they // need to access some shared data in ArRobot. robot.lock(); // now add all the modes for this demo // these classes are defined in ArModes.cpp in ARIA's source code. if(robot.getOrigRobotConfig()->getHasGripper()) new ArModeGripper(&robot, "gripper", 'g', 'G'); else ArLog::log(ArLog::Normal, "Robot does not indicate that it has a gripper."); ArModeActs actsMode(&robot, "acts", 'a', 'A'); ArModeTCM2 tcm2(&robot, "tcm2", 'm', 'M', compass); ArModeIO io(&robot, "io", 'i', 'I'); ArModeConfig cfg(&robot, "report robot config", 'o' , 'O'); ArModeCommand command(&robot, "command", 'd', 'D'); ArModeCamera camera(&robot, "camera", 'c', 'C'); ArModePosition position(&robot, "position", 'p', 'P', &gyro); ArModeSonar sonar(&robot, "sonar", 's', 'S'); ArModeBumps bumps(&robot, "bumps", 'b', 'B'); ArModeLaser laser(&robot, "laser", 'l', 'L'); ArModeWander wander(&robot, "wander", 'w', 'W'); ArModeUnguardedTeleop unguardedTeleop(&robot, "unguarded teleop", 'u', 'U'); ArModeTeleop teleop(&robot, "teleop", 't', 'T'); // activate the default mode teleop.activate(); // turn on the motors robot.comInt(ArCommands::ENABLE, 1); robot.unlock(); // Block execution of the main thread here and wait for the robot's task loop // thread to exit (e.g. by robot disconnecting, escape key pressed, or OS // signal) robot.waitForRunExit(); Aria::exit(0); return 0; }
int main(int argc, char **argv) { bool done; double distToTravel = 2300; // whether to use the sim for the laser or not, if you use the sim // for hte laser, you have to use the sim for the robot too bool useSim = false; // the laser ArSick sick; // connection ArDeviceConnection *con; // Laser connection ArSerialConnection laserCon; // robot ArRobot robot; // set a default filename //std::string filename = "c:\\log\\1scans.2d"; std::string filename = "1scans.2d"; // see if we want to use a different filename //if (argc > 1) //Lfilename = argv[1]; printf("Logging to file %s\n", filename.c_str()); // start the logger with good values sick.configureShort(useSim, ArSick::BAUD38400, ArSick::DEGREES180, ArSick::INCREMENT_HALF); ArSickLogger logger(&robot, &sick, 300, 25, filename.c_str()); // mandatory init Aria::init(); // add it to the robot robot.addRangeDevice(&sick); //ArAnalogGyro gyro(&robot); // if we're not using the sim, make a serial connection and set it up if (!useSim) { ArSerialConnection *serCon; serCon = new ArSerialConnection; serCon->setPort(); //serCon->setBaud(38400); con = serCon; } // if we are using the sim, set up a tcp connection else { ArTcpConnection *tcpCon; tcpCon = new ArTcpConnection; tcpCon->setPort(); con = tcpCon; } // set the connection on the robot robot.setDeviceConnection(con); // try to connect, if we fail exit if (!robot.blockingConnect()) { printf("Could not connect to robot... exiting\n"); Aria::shutdown(); return 1; } // set up a key handler so escape exits and attach to the robot ArKeyHandler keyHandler; robot.attachKeyHandler(&keyHandler); // run the robot, true here so that the run will exit if connection lost robot.runAsync(true); // if we're not using the sim, set up the port for the laser if (!useSim) { laserCon.setPort(ArUtil::COM3); sick.setDeviceConnection(&laserCon); } // now that we're connected to the robot, connect to the laser sick.runAsync(); if (!sick.blockingConnect()) { printf("Could not connect to SICK laser... exiting\n"); robot.disconnect(); Aria::shutdown(); return 1; } #ifdef WIN32 // wait until someone pushes the motor button to go while (1) { robot.lock(); if (!robot.isRunning()) exit(0); if (robot.areMotorsEnabled()) { robot.unlock(); break; } robot.unlock(); ArUtil::sleep(100); } #endif // basically from here on down the robot just cruises around a bit robot.lock(); // enable the motors, disable amigobot sounds robot.comInt(ArCommands::ENABLE, 1); // move a couple meters robot.setRotVel(3000); robot.unlock(); ArUtil::sleep(15 * 1000); robot.lock(); robot.disconnect(); robot.unlock(); keyHandler.restore(); exit(1); // now exit return 0; }
int main() { ArSyncTask *task; UserTaskTest test; ArRobot robot; ArFunctorC<UserTaskTest> userTaskOne(&test, &UserTaskTest::userTaskOne); ArFunctorC<UserTaskTest> userTaskTwo(&test, &UserTaskTest::userTaskTwo); ArFunctorC<UserTaskTest> userTaskThree(&test, &UserTaskTest::userTaskThree); ArFunctorC<UserTaskTest> userTaskFour(&test, &UserTaskTest::userTaskFour); printf("Before tasks added:\n"); robot.logUserTasks(); printf("---------------------------------------------------------------\n"); // the order you add tasks doesn't matter, it goes off of the integer // you give the function call to add them // Caveat: if you give the function call the same number it goes off of order robot.addUserTask("procTwo", 20, &userTaskTwo); robot.addUserTask("procFour", 40, &userTaskFour, &test.myTaskFourState); robot.addUserTask("procThree", 30, &userTaskThree); robot.addUserTask("procOne", 10, &userTaskOne); printf("After tasks added:\n"); robot.logUserTasks(); printf("---------------------------------------------------------------\n"); printf("What happens when these are run:\n"); robot.loopOnce(); printf("---------------------------------------------------------------\n"); printf("After tasks removed by name:\n"); printf("---------------------------------------------------------------\n"); robot.remUserTask("procOne"); robot.remUserTask("procTwo"); robot.remUserTask("procThree"); robot.remUserTask("procFour"); robot.logUserTasks(); printf("\nAfter they are added again, procThree is found by name and set to SUSPEND and procFour is found by functor and set to FAILURE:\n"); printf("---------------------------------------------------------------\n"); printf("---------------------------------------------------------------\n"); robot.addUserTask("procTwo", 20, &userTaskTwo); robot.addUserTask("procFour", 40, &userTaskFour, &test.myTaskFourState); robot.addUserTask("procThree", 30, &userTaskThree); robot.addUserTask("procOne", 10, &userTaskOne); task = robot.findUserTask("procThree"); if (task != NULL) task->setState(ArTaskState::SUSPEND); task = robot.findUserTask(&userTaskFour); if (task != NULL) task->setState(ArTaskState::FAILURE); robot.logUserTasks(); task = robot.findUserTask(&userTaskFour); if (task != NULL) { printf("---------------------------------------------------------------\n"); printf("Did the real state on procFourState get set:\n"); printf("getState: %d realState: %d\n", task->getState(), test.myTaskFourState); } printf("---------------------------------------------------------------\n"); printf("What happens when these are run:\n"); robot.loopOnce(); printf("---------------------------------------------------------------\n"); printf("After tasks removed by functor:\n"); printf("---------------------------------------------------------------\n"); robot.remUserTask(&userTaskOne); robot.remUserTask(&userTaskTwo); robot.remUserTask(&userTaskThree); robot.remUserTask(&userTaskFour); robot.logUserTasks(); }
int main(int argc, char *argv[]) { // Initialize location of Aria, Arnl and their args. Aria::init(); Arnl::init(); // The robot object ArRobot robot; #ifndef SONARNL // The laser object ArSick sick(181, 361); #endif // Parse them command line arguments. ArArgumentParser parser(&argc, argv); // Set up our simpleConnector ArSimpleConnector simpleConnector(&parser); // Load default arguments for this computer parser.loadDefaultArguments(); // Parse its arguments for the simple connector. simpleConnector.parseArgs(); // sonar, must be added to the robot, for teleop and wander ArSonarDevice sonarDev; // add the sonar to the robot robot.addRangeDevice(&sonarDev); ArMap arMap; #ifndef SONARNL // Initialize the localization ArLocalizationTask locTask(&robot, &sick, &arMap); // Make the path task ArPathPlanningTask pathTask(&robot, &sick, &sonarDev, &arMap); #else // Initialize the localization ArSonarLocalizationTask locTask(&robot, &sonarDev, &arMap); // Make the path task ArPathPlanningTask pathTask(&robot, NULL, &sonarDev, &arMap); #endif // Stop the robot as soon as localization fails. ArFunctor1C<ArPathPlanningTask, int> locaFailed(&pathTask, &ArPathPlanningTask::trackingFailed); locTask.addFailedLocalizationCB(&locaFailed); // Read in param files. Aria::getConfig()->useArgumentParser(&parser); if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName())) { printf("Trouble loading configuration file, exiting\n"); exit(1); } // Warn about unknown params. if (!parser.checkHelpAndWarnUnparsed()) { printf("\nUsage: %s -map mapfilename\n\n", argv[0]); simpleConnector.logOptions(); exit(2); } // Our server ArServerBase server; // First open the server up if (!server.open(7272)) { printf("Could not open server port\n"); exit(1); } // Connect the robot if (!simpleConnector.connectRobot(&robot)) { printf("Could not connect to robot... exiting\n"); Aria::shutdown(); return 1; } robot.com2Bytes(31, 14, 0); robot.com2Bytes(31, 15, 0); ArUtil::sleep(100); robot.com2Bytes(31, 14, 1); robot.com2Bytes(31, 15, 1); robot.enableMotors(); robot.clearDirectMotion(); #ifndef SONARNL // Set up the laser before handing it to the laser mode simpleConnector.setupLaser(&sick); // Add the laser to the robot robot.addRangeDevice(&sick); #endif // Start the robot thread. robot.runAsync(true); #ifndef SONARNL // Start the laser thread. sick.runAsync(); // Connect the laser if (!sick.blockingConnect()) { printf("Couldn't connect to sick, exiting\n"); Aria::shutdown(); return 1; } #endif ArUtil::sleep(300); // If you want to set the number of samples change the line below locTask.setNumSamples(2000); // Localize the robot to home if(locTask.localizeRobotAtHomeBlocking()) { printf("Successfully localized at home.\n"); } else { printf("WARNING: Unable to localize at home position!\n"); } robot.lock(); // attach stuff to the server ArServerInfoRobot serverInfoRobot(&server, &robot); ArServerInfoSensor serverInfoSensor(&server, &robot); ArServerInfoPath serverInfoPath(&server, &robot, &pathTask); ArServerInfoLocalization serverInfoLocalization(&server, &robot, &locTask); #ifndef SONARNL // Set it up to handle maps. ArServerHandlerMap serverMap(&server, &arMap, ArServerHandlerMap::POINTS); #else ArServerHandlerMap serverMap(&server, &arMap, ArServerHandlerMap::LINES); #endif // Set up a service that allows the client to monitor the communication // between the robot and the client. // ArServerHandlerCommMonitor serverCommMonitor(&server); //ArServerModeGoto modeGoto(&server, &robot, &pathTask); //ArServerModeStop modeStop(&server, &robot); //ArServerModeDrive modeDrive(&server, &robot); SimpleTask simpleTask(&robot, &pathTask); robot.unlock(); // Read in param files. Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()); // Now let it spin off in its own thread server.run(); exit(0); }
int main(int argc, char **argv) { Aria::init(); ArRobot robot; ArArgumentParser parser(&argc, argv); parser.loadDefaultArguments(); ArRobotConnector robotConnector(&parser, &robot); ArLaserConnector laserConnector(&parser, &robot, &robotConnector); // Connect to the robot, get some initial data from it such as type and name, // and then load parameter files for this robot. if(!robotConnector.connectRobot()) { ArLog::log(ArLog::Terse, "lasersExample: Could not connect to the robot."); if(parser.checkHelpAndWarnUnparsed()) { // -help not given Aria::logOptions(); Aria::exit(1); } } if (!Aria::parseArgs()) { Aria::logOptions(); Aria::exit(2); return 2; } ArLog::log(ArLog::Normal, "lasersExample: Connected to robot."); // 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); // Connect to laser(s) as defined in parameter files. // (Some flags are available as arguments to connectLasers() to control error behavior and to control which lasers are put in the list of lasers stored by ArRobot. See docs for details.) if(!laserConnector.connectLasers()) { ArLog::log(ArLog::Terse, "Could not connect to configured lasers. Exiting."); Aria::exit(3); return 3; } // Allow some time to read laser data ArUtil::sleep(500); ArLog::log(ArLog::Normal, "Connected to all lasers."); // Print out some data from each connected laser. while(robot.isConnected()) { int numLasers = 0; // Get a pointer to ArRobot's list of connected lasers. We will lock the robot while using it to prevent changes by tasks in the robot's background task thread or any other threads. Each laser has an index. You can also store the laser's index or name (laser->getName()) and use that to get a reference (pointer) to the laser object using ArRobot::findLaser(). robot.lock(); std::map<int, ArLaser*> *lasers = robot.getLaserMap(); for(std::map<int, ArLaser*>::const_iterator i = lasers->begin(); i != lasers->end(); ++i) { int laserIndex = (*i).first; ArLaser* laser = (*i).second; if(!laser) continue; ++numLasers; laser->lockDevice(); // The current readings are a set of obstacle readings (with X,Y positions as well as other attributes) that are the most recent set from teh laser. std::list<ArPoseWithTime*> *currentReadings = laser->getCurrentBuffer(); // see ArRangeDevice interface doc // The raw readings are just range or other data supplied by the sensor. It may also include some device-specific extra values associated with each reading as well. (e.g. Reflectance for LMS200) const std::list<ArSensorReading*> *rawReadings = laser->getRawReadings(); // There is a utility to find the closest reading wthin a range of degrees around the laser, here we use this laser's full field of view (start to end) // If there are no valid closest readings within the given range, dist will be greater than laser->getMaxRange(). double angle = 0; double dist = laser->currentReadingPolar(laser->getStartDegrees(), laser->getEndDegrees(), &angle); ArLog::log(ArLog::Normal, "Laser #%d (%s): %s.\n\tHave %d 'current' readings.\n\tHave %d 'raw' readings.\n\tClosest reading is at %3.0f degrees and is %2.4f meters away.", laserIndex, laser->getName(), (laser->isConnected() ? "connected" : "NOT CONNECTED"), currentReadings->size(), rawReadings->size(), angle, dist/1000.0); laser->unlockDevice(); } if(numLasers == 0) ArLog::log(ArLog::Normal, "No lasers."); else ArLog::log(ArLog::Normal, ""); // Unlock robot and sleep for 5 seconds before next loop. robot.unlock(); ArUtil::sleep(5000); } ArLog::log(ArLog::Normal, "lasersExample: exiting."); Aria::exit(0); return 0; }
void TakeBlockToWall::handler(void) { Color tempColor; switch (myState) { case STATE_START: setState(STATE_ACQUIRE_BLOCK); myDropWall = COLOR_FIRST_WALL; myLapWall = COLOR_SECOND_WALL; printf("!! Started state handling!\n"); //handler(); return; break; case STATE_ACQUIRE_BLOCK: if (myNewState) { printf("!! Acquire block\n"); myNewState = false; mySony->panTilt(0, -10); myAcquire->activate(); myAcquire->setChannel(COLOR_BLOCK); myPickUp->deactivate(); myDriveTo->deactivate(); myBackup->deactivate(); } if (myGripper->getGripState() == 2 && myGripper->getBreakBeamState() != 0) { printf("###### AcquireBlock: Successful (have cube?)\n"); setState(STATE_ACQUIRE_DROP_WALL); //handler(); return; } else if (myGripper->getBreakBeamState() != 0) { printf("###### AcquireBlock: Successful (cube in gripper?)\n"); setState(STATE_PICKUP_BLOCK); //handler(); return; } if (myAcquire->getState() == Acquire::STATE_FAILED || myStateStart.mSecSince() > 35000) { printf("###### AcqiureBlock: failed\n"); setState(STATE_BACKUP); //handler(); return; } else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED) { printf("###### AcquireBlock: successful\n"); setState(STATE_PICKUP_BLOCK); //handler(); return; } break; case STATE_PICKUP_BLOCK: if (myNewState) { printf("!! Pickup block\n"); myNewState = false; mySony->panTilt(0, -15); myAcquire->deactivate(); myPickUp->activate(); myPickUp->setChannel(COLOR_BLOCK); myDriveTo->deactivate(); myBackup->deactivate(); } if (myPickUp->getState() == PickUp::STATE_FAILED) { printf("###### PickUpBlock: failed\n"); setState(STATE_BACKUP); //handler(); return; } else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED) { printf("###### PickUpBlock: successful\n"); setState(STATE_ACQUIRE_DROP_WALL); //handler(); return; } break; case STATE_BACKUP: if (myNewState) { printf("!! Backup\n"); myAcquire->deactivate(); myPickUp->deactivate(); myDriveTo->deactivate(); myBackup->activate(); myNewState = false; } if (myStateStart.mSecSince() > 2000) { printf("###### Backup: done\n"); setState(STATE_ACQUIRE_BLOCK2); //handler(); return; } break; case STATE_ACQUIRE_BLOCK2: if (myNewState) { printf("!! Acquire block 2\n"); myNewState = false; mySony->panTilt(0, -25); myAcquire->activate(); myAcquire->setChannel(COLOR_BLOCK); myPickUp->deactivate(); myDriveTo->deactivate(); myBackup->deactivate(); } if (myGripper->getGripState() == 2 && myGripper->getBreakBeamState() != 0) { printf("###### AcquireBlock2: Successful (have cube?)\n"); setState(STATE_ACQUIRE_DROP_WALL); //handler(); return; } else if (myGripper->getBreakBeamState() != 0) { printf("###### AcquireBlock2: Successful (cube in gripper?)\n"); setState(STATE_PICKUP_BLOCK2); //handler(); return; } if (myAcquire->getState() == Acquire::STATE_FAILED || myStateStart.mSecSince() > 35000) { printf("###### AcqiureBlock2: failed\n"); setState(STATE_FAILED); //handler(); return; } else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED) { printf("###### AcquireBlock2: successful\n"); setState(STATE_PICKUP_BLOCK2); //handler(); return; } break; case STATE_PICKUP_BLOCK2: if (myNewState) { printf("!! Pickup block 2\n"); myNewState = false; myAcquire->deactivate(); myPickUp->activate(); mySony->panTilt(0, -25); myPickUp->setChannel(COLOR_BLOCK); myDriveTo->deactivate(); myBackup->deactivate(); } if (myPickUp->getState() == PickUp::STATE_FAILED) { printf("###### PickUpBlock2: failed\n"); setState(STATE_FAILED); //handler(); return; } else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED) { printf("###### PickUpBlock2: successful\n"); setState(STATE_ACQUIRE_DROP_WALL); //handler(); return; } break; case STATE_ACQUIRE_DROP_WALL: if (myNewState) { printf("!! Acquire Drop wall, channel %d\n", myDropWall); myNewState = false; mySony->panTilt(0, -5); myAcquire->activate(); myAcquire->setChannel(myDropWall); myPickUp->deactivate(); myDriveTo->deactivate(); myBackup->deactivate(); } if (myGripper->getGripState() != 2 || myGripper->getBreakBeamState() == 0) { printf("###### AcquireDropWall:: failed (lost cube)\n"); setState(STATE_BACKUP); //handler(); return; } if (myAcquire->getState() == Acquire::STATE_FAILED || myStateStart.mSecSince() > 35000) { printf("###### AcquireDropWall:: failed\n"); setState(STATE_FAILED); //handler(); return; } else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED) { printf("###### AcquireDropWall: successful\n"); setState(STATE_DRIVETO_DROP_WALL); //handler(); return; } break; case STATE_DRIVETO_DROP_WALL: if (myNewState) { printf("!! Driveto Drop wall, channel %d\n", myDropWall); myNewState = false; myAcquire->deactivate(); myPickUp->deactivate(); myDriveTo->activate(); myDriveTo->setChannel(myDropWall); myBackup->deactivate(); } if (myGripper->getGripState() != 2 || myGripper->getBreakBeamState() == 0) { printf("###### DriveToDropWall:: failed (lost cube)\n"); setState(STATE_BACKUP); //handler(); return; } if (myDriveTo->getState() == DriveTo::STATE_FAILED) { printf("###### DriveToDropWall: failed\n"); setState(STATE_FAILED); //handler(); return; } else if (myDriveTo->getState() == DriveTo::STATE_SUCCEEDED) { printf("###### DriveToDropWall: succesful\n"); setState(STATE_DROP); //handler(); return; } break; case STATE_DROP: if (myNewState) { printf("!! Drop\n"); printf("@@@@@ Drop lowering lift\n"); myGripper->liftDown(); myNewState = false; myAcquire->deactivate(); myPickUp->deactivate(); myDriveTo->deactivate(); myBackup->deactivate(); myGripOpenSent = false; } myGripper->liftDown(); if ((myStateStart.mSecSince() > 500 && myGripper->isLiftMaxed() && myStateStart.mSecSince() < 5000) || myStateStart.mSecSince() > 5000) { myGripper->gripOpen(); /*if (!myGripOpenSent) { ArUtil::sleep(3); myGripper->gripOpen(); } myGripOpenSent = true; */ } if (myGripper->getGripState() == 1 || myStateStart.mSecSince() > 6000) { printf("###### Drop: success\n"); setState(STATE_DROP_BACKUP); //handler(); return; } break; case STATE_DROP_BACKUP: if (myNewState) { printf("!! Drop backup\n"); myAcquire->deactivate(); myPickUp->deactivate(); myDriveTo->deactivate(); myBackup->activate(); myNewState = false; } if (myStateStart.mSecSince() > 2000) { printf("###### Drop_Backup: done\n"); setState(STATE_ACQUIRE_LAP_WALL); //handler(); return; } break; case STATE_ACQUIRE_LAP_WALL: if (myNewState) { printf("!! Acquire Lap wall, channel %d\n", myLapWall); myNewState = false; mySony->panTilt(0, -5); myAcquire->activate(); myAcquire->setChannel(myLapWall); myPickUp->deactivate(); myDriveTo->deactivate(); myBackup->deactivate(); } if (myAcquire->getState() == Acquire::STATE_FAILED || myStateStart.mSecSince() > 35000) { printf("###### AcquireLapWall:: failed\n"); setState(STATE_SWITCH); //handler(); return; } else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED) { printf("###### AcquireLapWall: successful\n"); setState(STATE_DRIVETO_LAP_WALL); //handler(); return; } break; case STATE_DRIVETO_LAP_WALL: if (myNewState) { printf("!! Driveto Lap wall, channel %d\n", myLapWall); myNewState = false; myAcquire->deactivate(); myPickUp->deactivate(); myDriveTo->activate(); myDriveTo->setChannel(myLapWall); myBackup->deactivate(); } if (myDriveTo->getState() == DriveTo::STATE_FAILED) { printf("###### DriveToLapWall: failed\n"); setState(STATE_SWITCH); //handler(); return; } else if (myDriveTo->getState() == DriveTo::STATE_SUCCEEDED) { printf("###### DriveToLapWall: succesful\n"); setState(STATE_SWITCH); //handler(); return; } break; case STATE_SWITCH: printf("!! Switching walls around.\n"); tempColor = myDropWall; myDropWall = myLapWall; myLapWall = tempColor; setState(STATE_ACQUIRE_BLOCK); //handler(); return; case STATE_FAILED: printf("@@@@@ Failed to complete the task!\n"); myRobot->comInt(ArCommands::SONAR, 0); ArUtil::sleep(50); myRobot->comStr(ArCommands::SAY, "\52\77\37\62\42\70"); ArUtil::sleep(500); Aria::shutdown(); myRobot->disconnect(); myRobot->stopRunning(); return; } }
int main(int argc, char **argv) { Aria::init(); ArRobot robot; ArSonarDevice sonar; ArSick sick; robot.addRangeDevice(&sonar); ArServerBase server; // Argument parser: ArArgumentParser parser(&argc, argv); parser.loadDefaultArguments(); // Connector and server opener: ArRobotConnector robotConnector(&parser, &robot); if(!robotConnector.connectRobot()) { ArLog::log(ArLog::Terse, "popupExample: Could not connect to the robot."); if(parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); } Aria::exit(1); } ArLaserConnector laserConnector(&parser, &robot, &robotConnector); ArServerSimpleOpener simpleOpener(&parser); // Get command-line and other parameters if(!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(1); } robot.runAsync(true); // connect to the laser if(!laserConnector.connectLasers()) { ArLog::log(ArLog::Normal, "popupExample: Warning: Could not connect to lasers."); } // Open the server if(!simpleOpener.open(&server)) { ArLog::log(ArLog::Terse, "popupExample: Error, could not open server."); return 1; } server.runAsync(); ArLog::log(ArLog::Normal, "popupExample: Server running. Press control-C to exit."); ArLog::log(ArLog::Normal, "popupExample: Each time an obstacle is detected near the robot, a new popup message will be created. Connect with MobileEyes to see them."); // Sends robot position etc. ArServerInfoRobot robotInfoServer(&server, &robot); // This service sends drawings e.g. showing range device positions ArServerInfoDrawings drawingsServer(&server); drawingsServer.addRobotsRangeDevices(&robot); // This service can send messages to clients to display as popup dialogs: ArServerHandlerPopup popupServer(&server); // This object contains the robot sensor interpretation task and creates // popups: SensorDetectPopup(&robot, &popupServer); // modes for controlling robot movement ArServerModeStop modeStop(&server, &robot); ArServerModeRatioDrive modeRatioDrive(&server, &robot); ArServerModeWander modeWander(&server, &robot); modeStop.addAsDefaultMode(); modeStop.activate(); // allow configuration of driving and other settings ArServerHandlerConfig serverHandlerConfig(&server, Aria::getConfig()); // make a config handler ArLog::addToConfig(Aria::getConfig()); // let people configure logging modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings"); // able to configure teleop settings robot.enableMotors(); robot.waitForRunExit(); Aria::exit(0); }
int _tmain(int argc, char** argv) { //-------------- M A I N D E L P R O G R A M A D E L R O B O T------------// //----------------------------------------------------------------------------------// //inicializaion de variables Aria::init(); ArArgumentParser parser(&argc, argv); parser.loadDefaultArguments(); ArSimpleConnector simpleConnector(&parser); ArRobot robot; ArSonarDevice sonar; ArAnalogGyro gyro(&robot); robot.addRangeDevice(&sonar); ActionTurns turn(400, 55); robot.addAction(&turn, 49); ActionTurns turn2(400, 55); robot.addAction(&turn2, 49); turn.deactivate(); turn2.deactivate(); // presionar tecla escape para salir del programa ArKeyHandler keyHandler; Aria::setKeyHandler(&keyHandler); robot.attachKeyHandler(&keyHandler); printf("Presionar ESC para salir\n"); // uso de sonares para evitar colisiones con las paredes u // obstaculos grandes, mayores a 8cm de alto ArActionLimiterForwards limiterAction("limitador velocidad cerca", 300, 600, 250); ArActionLimiterForwards limiterFarAction("limitador velocidad lejos", 300, 1100, 400); ArActionLimiterTableSensor tableLimiterAction; robot.addAction(&tableLimiterAction, 100); robot.addAction(&limiterAction, 95); robot.addAction(&limiterFarAction, 90); // Inicializon la funcion de goto ArActionGoto gotoPoseAction("goto"); robot.addAction(&gotoPoseAction, 50); // Finaliza el goto si es que no hace nada ArActionStop stopAction("stop"); robot.addAction(&stopAction, 40); // Parser del CLI if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); exit(1); } // Conexion del robot if (!simpleConnector.connectRobot(&robot)) { printf("Could not connect to robot... exiting\n"); Aria::exit(1); } robot.runAsync(true); // enciende motores, apaga sonidos robot.enableMotors(); robot.comInt(ArCommands::SOUNDTOG, 0); // Imprimo algunos datos del robot como posicion velocidad y bateria robot.lock(); ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV", robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage()); robot.unlock(); const int duration = 100000; //msec ArLog::log(ArLog::Normal, "Completados los puntos en %d segundos", duration/1000); bool first = true; int goalNum = 0; int color = 3; ArTime start; start.setToNow(); while (Aria::getRunning()) { robot.lock(); // inicia el primer punto if (first || gotoPoseAction.haveAchievedGoal()) { first = false; goalNum++; //cambia de 0 a 1 el contador printf("El contador esta en: --> %d <---\n",goalNum); if (goalNum > 20) goalNum = 1; //comienza la secuencia de puntos if (goalNum == 1) { gotoPoseAction.setGoal(ArPose(1150, 0)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); // Imprimo algunos datos del robot como posicion velocidad y bateria robot.lock(); ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV", robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage()); robot.unlock(); } else if (goalNum == 2) { printf("Gira 90 grados izquierda\n"); robot.unlock(); turn.myActivate = 1; turn.myDirection = 1; turn.activate(); ArUtil::sleep(1000); turn.deactivate(); turn.myActivate = 0; turn.myDirection = 0; robot.lock(); } else if (goalNum == 3) { gotoPoseAction.setGoal(ArPose(1150, 2670)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); // Imprimo algunos datos del robot como posicion velocidad y bateria robot.lock(); ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV", robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage()); robot.unlock(); } else if (goalNum == 4) { printf("Gira 90 grados izquierda\n"); robot.unlock(); turn2.myActivate = 1; turn2.myDirection = 1; turn2.activate(); ArUtil::sleep(1000); turn2.deactivate(); turn2.myActivate = 0; turn2.myDirection = 0; robot.lock(); } else if (goalNum == 5) { gotoPoseAction.setGoal(ArPose(650, 2670)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } else if (goalNum == 6) { printf("Gira 90 grados izquierda\n"); robot.unlock(); turn2.myActivate = 1; turn2.myDirection = 1; turn2.activate(); ArUtil::sleep(1000); turn2.deactivate(); turn2.myActivate = 0; turn2.myDirection = 0; robot.lock(); } else if (goalNum == 7) { gotoPoseAction.setGoal(ArPose(650, 0)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } else if (goalNum == 8) { gotoPoseAction.setGoal(ArPose(1800,1199)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } else if (goalNum == 9) { gotoPoseAction.setGoal(ArPose(2600, 1199)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } else if (goalNum == 10) { if (color == 1) { gotoPoseAction.setGoal(ArPose(2800, 850)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 2) { gotoPoseAction.setGoal(ArPose(3500, 1199)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 3) { gotoPoseAction.setGoal(ArPose(2800, 1550)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 11) { if (color == 1) { gotoPoseAction.setGoal(ArPose(2800, 613)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 2) { printf("Gira 180 grados derecha\n"); robot.unlock(); turn2.myActivate = 1; turn2.myDirection = 2; turn2.activate(); ArUtil::sleep(2000); turn2.deactivate(); turn2.myActivate = 0; turn2.myDirection = 0; robot.lock(); goalNum = 19; } if (color == 3) { gotoPoseAction.setGoal(ArPose(2800, 1785)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 12) { if (color == 1) { gotoPoseAction.setGoal(ArPose(3300, 413)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 3) { gotoPoseAction.setGoal(ArPose(3300, 1985)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 13) { if (color == 1) { gotoPoseAction.setGoal(ArPose(3500, 413)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 3) { gotoPoseAction.setGoal(ArPose(3500, 1985)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 14) { //secuencia de drop de la figura } else if (goalNum == 15) { printf("Gira 180 grados derecha\n"); robot.unlock(); turn2.myActivate = 1; turn2.myDirection = 2; turn2.activate(); ArUtil::sleep(2000); turn2.deactivate(); turn2.myActivate = 0; turn2.myDirection = 0; robot.lock(); } else if (goalNum == 16) { if (color == 1) { gotoPoseAction.setGoal(ArPose(3300, 413)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 3) { gotoPoseAction.setGoal(ArPose(3300, 1985)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 17) { if (color == 1) { gotoPoseAction.setGoal(ArPose(2800, 603)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 3) { gotoPoseAction.setGoal(ArPose(2800, 1795)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 18) { if (color == 1) { gotoPoseAction.setGoal(ArPose(2800, 860)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 3) { gotoPoseAction.setGoal(ArPose(2800, 1540)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 19) { gotoPoseAction.setGoal(ArPose(2600, 1199)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } else if (goalNum == 20) { gotoPoseAction.setGoal(ArPose(1800, 1199)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } if(start.mSecSince() >= duration) { ArLog::log(ArLog::Normal, "No puede llegar al punto, y la aplicacion saldra en %d", duration/1000); gotoPoseAction.cancelGoal(); robot.unlock(); ArUtil::sleep(3000); break; } robot.unlock(); ArUtil::sleep(10); } // Robot desconectado al terminal el sleep Aria::shutdown(); //----------------------------------------------------------------------------------// return 0; }
/* * dynamic reconfigure call back */ void RosAriaNode::dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level) { // // Odometry Settings // robot->lock(); if(TicksMM != config.TicksMM and config.TicksMM > 0) { ROS_INFO("Setting TicksMM from Dynamic Reconfigure: %d -> %d ", TicksMM, config.TicksMM); TicksMM = config.TicksMM; robot->comInt(93, TicksMM); } if(DriftFactor != config.DriftFactor) { ROS_INFO("Setting DriftFactor from Dynamic Reconfigure: %d -> %d ", DriftFactor, config.DriftFactor); DriftFactor = config.DriftFactor; robot->comInt(89, DriftFactor); } if(RevCount != config.RevCount and config.RevCount > 0) { ROS_INFO("Setting RevCount from Dynamic Reconfigure: %d -> %d ", RevCount, config.RevCount); RevCount = config.RevCount; robot->comInt(88, RevCount); } // // Acceleration Parameters // int value; value = config.trans_accel * 1000; if(value != robot->getTransAccel() and value > 0) { ROS_INFO("Setting TransAccel from Dynamic Reconfigure: %d", value); robot->setTransAccel(value); } value = config.trans_decel * 1000; if(value != robot->getTransDecel() and value > 0) { ROS_INFO("Setting TransDecel from Dynamic Reconfigure: %d", value); robot->setTransDecel(value); } value = config.lat_accel * 1000; if(value != robot->getLatAccel() and value > 0) { ROS_INFO("Setting LatAccel from Dynamic Reconfigure: %d", value); if (robot->getAbsoluteMaxLatAccel() > 0 ) robot->setLatAccel(value); } value = config.lat_decel * 1000; if(value != robot->getLatDecel() and value > 0) { ROS_INFO("Setting LatDecel from Dynamic Reconfigure: %d", value); if (robot->getAbsoluteMaxLatDecel() > 0 ) robot->setLatDecel(value); } value = config.rot_accel * 180/M_PI; if(value != robot->getRotAccel() and value > 0) { ROS_INFO("Setting RotAccel from Dynamic Reconfigure: %d", value); robot->setRotAccel(value); } value = config.rot_decel * 180/M_PI; if(value != robot->getRotDecel() and value > 0) { ROS_INFO("Setting RotDecel from Dynamic Reconfigure: %d", value); robot->setRotDecel(value); } robot->unlock(); }
int main(int argc, char **argv) { Aria::init(); ArRobot robot; ArArgumentParser argParser(&argc, argv); ArSimpleConnector connector(&argParser); ArGripper gripper(&robot); ArSonarDevice sonar; robot.addRangeDevice(&sonar); argParser.loadDefaultArguments(); if (!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::shutdown(); return 1; } if (!connector.connectRobot(&robot)) { ArLog::log(ArLog::Terse, "gripperExample: Could not connect to robot... exiting"); Aria::shutdown(); return 2; } ArLog::log(ArLog::Normal, "gripperExample: Connected to robot."); ArLog::log(ArLog::Normal, "gripperExample: GripperType=%d", gripper.getType()); gripper.logState(); if(gripper.getType() == ArGripper::NOGRIPPER) { ArLog::log(ArLog::Terse, "gripperExample: Error: Robot does not have a gripper. Exiting."); Aria::shutdown(); return -1; } // Teleoperation actions with obstacle-collision avoidance ArActionLimiterTableSensor tableLimit; robot.addAction(&tableLimit, 110); ArActionLimiterForwards limitNearAction("near", 300, 600, 250); robot.addAction(&limitNearAction, 100); ArActionLimiterForwards limitFarAction("far", 300, 1100, 400); robot.addAction(&limitFarAction, 90); ArActionLimiterBackwards limitBackAction; robot.addAction(&limitBackAction, 50); ArActionJoydrive joydriveAction("joydrive", 400, 15); robot.addAction(&joydriveAction, 40); joydriveAction.setStopIfNoButtonPressed(false); ArActionKeydrive keydriveAction; robot.addAction(&keydriveAction, 30); // Handlers to control the gripper and print out info (classes defined above) PrintGripStatus printStatus(&gripper); GripperControlHandler gripControl(&gripper); printStatus.addRobotTask(&robot); gripControl.addKeyHandlers(&robot); // enable motors and run (if we lose connection to the robot, exit) ArLog::log(ArLog::Normal, "You may now operate the robot with arrow keys or joystick. Operate the gripper with the u, d, o, c, and page up/page down keys."); robot.enableMotors(); robot.run(true); Aria::shutdown(); return 0; }
int RosAriaNode::Setup() { // Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be // called once per instance, and these objects need to persist until the process terminates. robot = new ArRobot(); ArArgumentBuilder *args = new ArArgumentBuilder(); // never freed ArArgumentParser *argparser = new ArArgumentParser(args); // Warning never freed argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args. Useful on robots with unusual serial port or baud rate (e.g. pioneer lx) // Now add any parameters given via ros params (see RosAriaNode constructor): // if serial port parameter contains a ':' character, then interpret it as hostname:tcpport // for wireless serial connection. Otherwise, interpret it as a serial port name. size_t colon_pos = serial_port.find(":"); if (colon_pos != std::string::npos) { args->add("-remoteHost"); // pass robot's hostname/IP address to Aria args->add(serial_port.substr(0, colon_pos).c_str()); //ROS_INFO( "RosAria: using IP: [%s]", serial_port.substr(0, colon_pos).c_str() ); args->add("-remoteRobotTcpPort"); // pass robot's TCP port to Aria args->add(serial_port.substr(colon_pos+1).c_str()); //ROS_INFO( "RosAria: using port: [%s]", serial_port.substr(colon_pos+1).c_str() ); } else { args->add("-robotPort"); // pass robot's serial port to Aria args->add(serial_port.c_str()); } // if a baud rate was specified in baud parameter if(serial_baud != 0) { args->add("-robotBaud"); char tmp[100]; snprintf(tmp, 100, "%d", serial_baud); args->add(tmp); } if( debug_aria ) { // turn on all ARIA debugging args->add("-robotLogPacketsReceived"); // log received packets args->add("-robotLogPacketsSent"); // log sent packets args->add("-robotLogVelocitiesReceived"); // log received velocities args->add("-robotLogMovementSent"); args->add("-robotLogMovementReceived"); ArLog::init(ArLog::File, ArLog::Verbose, aria_log_filename.c_str(), true); } // Connect to the robot conn = new ArRobotConnector(argparser, robot); // warning never freed if (!conn->connectRobot()) { ROS_ERROR("RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device.)"); return 1; } // causes ARIA to load various robot-specific hardware parameters from the robot parameter file in /usr/local/Aria/params if(!Aria::parseArgs()) { ROS_ERROR("RosAria: ARIA error parsing ARIA startup parameters!"); return 1; } readParameters(); // Start dynamic_reconfigure server dynamic_reconfigure_server = new dynamic_reconfigure::Server<rosaria::RosAriaConfig>; /* * 横向速度和纵向速度单位为米 */ /* * 初始化参数的最小值 其中TickMM=10 DriftFactor=-200 Revount=-32760 */ // Setup Parameter Minimums rosaria::RosAriaConfig dynConf_min; dynConf_min.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000; dynConf_min.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000; // TODO: Fix rqt dynamic_reconfigure gui to handle empty intervals // Until then, set unit length interval. dynConf_min.lat_accel = ((robot->getAbsoluteMaxLatAccel() > 0.0) ? robot->getAbsoluteMaxLatAccel() : 0.1) / 1000; dynConf_min.lat_decel = ((robot->getAbsoluteMaxLatDecel() > 0.0) ? robot->getAbsoluteMaxLatDecel() : 0.1) / 1000; dynConf_min.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180; dynConf_min.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180; // I'm setting these upper bounds relitivly arbitrarily, feel free to increase them. dynConf_min.TicksMM = 10; dynConf_min.DriftFactor = -200; dynConf_min.RevCount = -32760; dynamic_reconfigure_server->setConfigMin(dynConf_min); // 初始化参数的最大值 其中TickMM=200 DriftFactor=200 Revount=32760 rosaria::RosAriaConfig dynConf_max; dynConf_max.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000; dynConf_max.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000; // TODO: Fix rqt dynamic_reconfigure gui to handle empty intervals // Until then, set unit length interval. dynConf_max.lat_accel = ((robot->getAbsoluteMaxLatAccel() > 0.0) ? robot->getAbsoluteMaxLatAccel() : 0.1) / 1000; dynConf_max.lat_decel = ((robot->getAbsoluteMaxLatDecel() > 0.0) ? robot->getAbsoluteMaxLatDecel() : 0.1) / 1000; dynConf_max.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180; dynConf_max.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180; // I'm setting these upper bounds relitivly arbitrarily, feel free to increase them. dynConf_max.TicksMM = 200; dynConf_max.DriftFactor = 200; dynConf_max.RevCount = 32760; dynamic_reconfigure_server->setConfigMax(dynConf_max); // 初始化参数的默认值 rosaria::RosAriaConfig dynConf_default; dynConf_default.trans_accel = robot->getTransAccel() / 1000; dynConf_default.trans_decel = robot->getTransDecel() / 1000; dynConf_default.lat_accel = robot->getLatAccel() / 1000; dynConf_default.lat_decel = robot->getLatDecel() / 1000; dynConf_default.rot_accel = robot->getRotAccel() * M_PI/180; dynConf_default.rot_decel = robot->getRotDecel() * M_PI/180; dynConf_default.TicksMM = TicksMM; dynConf_default.DriftFactor = DriftFactor; dynConf_default.RevCount = RevCount; dynamic_reconfigure_server->setConfigDefault(dynConf_max); dynamic_reconfigure_server->setCallback(boost::bind(&RosAriaNode::dynamic_reconfigureCB, this, _1, _2)); // Enable the motors robot->enableMotors(); // disable sonars on startup robot->disableSonar(); // callback will be called by ArRobot background processing thread for every SIP data packet received from robot robot->addSensorInterpTask("ROSPublishingTask", 100, &myPublishCB); // Initialize bumpers with robot number of bumpers bumpers.front_bumpers.resize(robot->getNumFrontBumpers()); bumpers.rear_bumpers.resize(robot->getNumRearBumpers()); // Run ArRobot background processing thread robot->runAsync(true); return 0; }
int main(int argc, char **argv) { Aria::init(); ArRobot robot; ArArgumentParser parser(&argc, argv); ArSimpleConnector simpleConnector(&parser); // The base server object, manages all connections to clients. ArServerBase server; // This object simplifies configuration and opening of the ArServerBase // object. ArServerSimpleOpener simpleOpener(&parser); // parse the command line. fail and print the help if the parsing fails // or if the help was requested with -help parser.loadDefaultArguments(); if (!simpleConnector.parseArgs() || !simpleOpener.parseArgs() || !parser.checkHelpAndWarnUnparsed()) { simpleConnector.logOptions(); simpleOpener.logOptions(); exit(1); } // Use the ArSimpleOpener to open the server port if (!simpleOpener.open(&server)) { ArLog::log(ArLog::Terse, "Error: Could not open server on port %d", simpleOpener.getPort()); exit(1); } // // Create services attached to the base server: // // Robot position etc.: ArServerInfoRobot serverInfoRobot(&server, &robot); // Robot control modes (only one mode can be active at once): ArServerModeStop modeStop(&server, &robot); // old ArServerModeDrive modeDrive(&server, &robot); ArServerModeRatioDrive modeRatioDrive(&server, &robot); ArServerModeWander modeWander(&server, &robot); modeStop.addAsDefaultMode(); modeStop.activate(); // This provides a simple way to add new commands. ArServerHandlerCommands commands(&server); // Add our custom command. ArServerHandlerCommands also has other methods // for adding commands taht take different kinds of arguments, or no // arguments. ArGlobalFunctor1<ArArgumentBuilder*> customCommandFunctor(&customCommandHandler); commands.addStringCommand("ExampleCustomCommand", "Example of a custom command. simpleServerExample will print out the text sent with the command.", &customCommandFunctor); // These objects provide various debugging and diagnostic custom commands: ArServerSimpleComUC uCCommands(&commands, &robot); // Get information about the robot ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // Control logging modeRatioDrive.addControlCommands(&commands); // Drive mode diagnostics // This provides the client (e.g. MobileEyes) with a simple table of string values // (called an InfoGroup). An InfoGroup is kept globally by Aria. // The values in the table sent to clients are retrieved periodically by calling a // functor. ArServerInfoStrings stringInfo(&server); Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor()); // Here are some example entries in the InfoGroup: Aria::getInfoGroup()->addStringInt( "Motor Packet Count", 10, new ArConstRetFunctorC<int, ArRobot>(&robot, &ArRobot::getMotorPacCount)); // // Connect to the robot: // if (!simpleConnector.connectRobot(&robot)) { printf("Error: Could not connect to robot... exiting\n"); Aria::shutdown(); return 1; } robot.enableMotors(); robot.runAsync(true); // The simple opener might have information to display right before starting // the server thread: simpleOpener.checkAndLog(); // now let the server base run in a new thread, accepting client connections. server.runAsync(); ArLog::log(ArLog::Normal, "Server is now running... Press Ctrl-C to exit."); robot.waitForRunExit(); Aria::shutdown(); exit(0); }
void RosAriaNode::publish() { // Note, this is called via SensorInterpTask callback (myPublishCB, named "ROSPublishingTask"). ArRobot object 'robot' sholud not be locked or unlocked. pos = robot->getPose(); tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180), tf::Vector3(pos.getX()/1000, pos.getY()/1000, 0)), position.pose.pose); //Aria returns pose in mm. position.twist.twist.linear.x = robot->getVel()/1000; //Aria returns velocity in mm/s. position.twist.twist.linear.y = robot->getLatVel()/1000.0; position.twist.twist.angular.z = robot->getRotVel()*M_PI/180; position.header.frame_id = frame_id_odom; position.child_frame_id = frame_id_base_link; position.header.stamp = ros::Time::now(); pose_pub.publish(position); ROS_DEBUG("RosAria: publish: (time %f) pose x: %f, y: %f, angle: %f; linear vel x: %f, y: %f; angular vel z: %f", position.header.stamp.toSec(), (double)position.pose.pose.position.x, (double)position.pose.pose.position.y, (double)position.pose.pose.orientation.w, (double) position.twist.twist.linear.x, (double) position.twist.twist.linear.y, (double) position.twist.twist.angular.z ); // publishing transform odom->base_link odom_trans.header.stamp = ros::Time::now(); odom_trans.header.frame_id = frame_id_odom; odom_trans.child_frame_id = frame_id_base_link; odom_trans.transform.translation.x = pos.getX()/1000; odom_trans.transform.translation.y = pos.getY()/1000; odom_trans.transform.translation.z = 0.0; odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pos.getTh()*M_PI/180); odom_broadcaster.sendTransform(odom_trans); // getStallValue returns 2 bytes with stall bit and bumper bits, packed as (00 00 FrontBumpers RearBumpers) int stall = robot->getStallValue(); unsigned char front_bumpers = (unsigned char)(stall >> 8); unsigned char rear_bumpers = (unsigned char)(stall); bumpers.header.frame_id = frame_id_bumper; bumpers.header.stamp = ros::Time::now(); std::stringstream bumper_info(std::stringstream::out); // Bit 0 is for stall, next bits are for bumpers (leftmost is LSB) for (unsigned int i=0; i<robot->getNumFrontBumpers(); i++) { bumpers.front_bumpers[i] = (front_bumpers & (1 << (i+1))) == 0 ? 0 : 1; bumper_info << " " << (front_bumpers & (1 << (i+1))); } ROS_DEBUG("RosAria: Front bumpers:%s", bumper_info.str().c_str()); bumper_info.str(""); // Rear bumpers have reverse order (rightmost is LSB) unsigned int numRearBumpers = robot->getNumRearBumpers(); for (unsigned int i=0; i<numRearBumpers; i++) { bumpers.rear_bumpers[i] = (rear_bumpers & (1 << (numRearBumpers-i))) == 0 ? 0 : 1; bumper_info << " " << (rear_bumpers & (1 << (numRearBumpers-i))); } ROS_DEBUG("RosAria: Rear bumpers:%s", bumper_info.str().c_str()); bumpers_pub.publish(bumpers); //Publish battery information // TODO: Decide if BatteryVoltageNow (normalized to (0,12)V) is a better option std_msgs::Float64 batteryVoltage; batteryVoltage.data = robot->getRealBatteryVoltageNow(); voltage_pub.publish(batteryVoltage); if(robot->haveStateOfCharge()) { std_msgs::Float32 soc; soc.data = robot->getStateOfCharge()/100.0; state_of_charge_pub.publish(soc); } // publish recharge state if changed char s = robot->getChargeState(); if(s != recharge_state.data) { ROS_INFO("RosAria: publishing new recharge state %d.", s); recharge_state.data = s; recharge_state_pub.publish(recharge_state); } // publish motors state if changed bool e = robot->areMotorsEnabled(); if(e != motors_state.data || !published_motors_state) { ROS_INFO("RosAria: publishing new motors state %d.", e); motors_state.data = e; motors_state_pub.publish(motors_state); published_motors_state = true; } // Publish sonar information, if enabled. if (use_sonar) { sensor_msgs::PointCloud cloud; //sonar readings. cloud.header.stamp = position.header.stamp; //copy time. // sonar sensors relative to base_link cloud.header.frame_id = frame_id_sonar; // Log debugging info std::stringstream sonar_debug_info; sonar_debug_info << "Sonar readings: "; for (int i = 0; i < robot->getNumSonar(); i++) { ArSensorReading* reading = NULL; reading = robot->getSonarReading(i); if(!reading) { ROS_WARN("RosAria: Did not receive a sonar reading."); continue; } // getRange() will return an integer between 0 and 5000 (5m) sonar_debug_info << reading->getRange() << " "; // local (x,y). Appears to be from the centre of the robot, since values may // exceed 5000. This is good, since it means we only need 1 transform. // x & y seem to be swapped though, i.e. if the robot is driving north // x is north/south and y is east/west. // //ArPose sensor = reading->getSensorPosition(); //position of sensor. // sonar_debug_info << "(" << reading->getLocalX() // << ", " << reading->getLocalY() // << ") from (" << sensor.getX() << ", " // << sensor.getY() << ") ;; " ; //add sonar readings (robot-local coordinate frame) to cloud geometry_msgs::Point32 p; p.x = reading->getLocalX() / 1000.0; p.y = reading->getLocalY() / 1000.0; p.z = 0.0; cloud.points.push_back(p); } ROS_DEBUG_STREAM(sonar_debug_info.str()); sonar_pub.publish(cloud); } }
// this is the function called in the new thread void *Joydrive::runThread(void *arg) { threadStarted(); int trans, rot; // only run while running, ie play nice and pay attention to the thread //being shutdown while (myRunning) { // lock the robot before touching it myRobot->lock(); if (!myRobot->isConnected()) { myRobot->unlock(); break; } // print out some information about the robot printf("\rx %6.1f y %6.1f tth %6.1f vel %7.1f mpacs %3d ", myRobot->getX(), myRobot->getY(), myRobot->getTh(), myRobot->getVel(), myRobot->getMotorPacCount()); fflush(stdout); // if one of the joystick buttons is pushed, drive the robot if (myJoyHandler.haveJoystick() && (myJoyHandler.getButton(1) || myJoyHandler.getButton(2))) { // get out the values from the joystick myJoyHandler.getAdjusted(&rot, &trans); // drive the robot myRobot->setVel(trans); myRobot->setRotVel(-rot); } // if no buttons are pushed stop the robot else { myRobot->setVel(0); myRobot->setRotVel(0); } // unlock the robot, so everything else can run myRobot->unlock(); // now take a little nap ArUtil::sleep(50); } // return out here, means the thread is done return NULL; }
int main(int argc, char **argv) { Aria::init(); ArArgumentParser argParser(&argc, argv); argParser.loadDefaultArguments(); ArRobot robot; ArRobotConnector robotConnector(&argParser, &robot); ArLaserConnector laserConnector(&argParser, &robot, &robotConnector); if(!robotConnector.connectRobot()) { ArLog::log(ArLog::Terse, "Could not connect to the robot."); if(argParser.checkHelpAndWarnUnparsed()) { // -help not given, just exit. Aria::logOptions(); Aria::exit(1); return 1; } } // Trigger argument parsing if (!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(1); return 1; } ArKeyHandler keyHandler; Aria::setKeyHandler(&keyHandler); robot.attachKeyHandler(&keyHandler); puts("This program will make the robot wander around. It uses some avoidance\n" "actions if obstacles are detected, otherwise it just has a\n" "constant forward velocity.\n\nPress CTRL-C or Escape to exit."); ArSonarDevice sonar; robot.addRangeDevice(&sonar); robot.runAsync(true); // try to connect to laser. if fail, warn but continue, using sonar only if(!laserConnector.connectLasers()) { ArLog::log(ArLog::Normal, "Warning: unable to connect to requested lasers, will wander using robot sonar only."); } // turn on the motors, turn off amigobot sounds robot.enableMotors(); robot.comInt(ArCommands::SOUNDTOG, 0); // add a set of actions that combine together to effect the wander behavior //działa //general structure: robot.addAction (new Name_of_action (parameters), priority); //to find out more about Actions go to ~/catkin_ws/src/rosaria/local/Aria/docs/index.html // if we're stalled we want to back up and recover robot.addAction(new ArActionStallRecover("stall recover", 100,50,100,30), 100); //basic values 225, 50, 150, 45 //when_stop, cycles_to_move, speed, turn' //slow down when near obstacle robot.addAction(new ArActionLimiterForwards("speed limiter near", 200, 200, 200), 95); // basic values 300,600,250 //stop_distance, slow_distance, slow_speed // react to bumpers robot.addAction(new ArActionLimiterBackwards, 75); // turn avoid very close robot.addAction(new ArActionAvoidFront("speed limiter far", 150,450,20), 47); //basic values 450, 200,15 //when_turn, speed, turn' // turn avoid things near robot.addAction(new ArActionAvoidFront("speed limiter far", 300,450,10), 46); //basic values 450, 200,15 //when_turn, speed, turn' // turn avoid things further away robot.addAction(new ArActionAvoidFront("speed limiter far", 800,450,5), 45); //basic values 450, 200,15 //when_turn, speed, turn' //move forward robot.addAction(new ArActionConstantVelocity("Constant Velocity", 500), 25); // wait for robot task loop to end before exiting the program robot.waitForRunExit(); Aria::exit(0); return 0; }