/*! * Shuts down the system. * */ void Advanced::shutDown(void) { Aria::exit(0); // // Stop the path planning thread. // if(myPathPlanningTask){ myPathPlanningTask->stopRunning(); // delete myPathPlanningTask; printf("Stopped Path Planning Thread\n"); } // // Stop the localization thread. // if(myLocaTask){ myLocaTask->stopRunning(); delete myLocaTask; printf("Stopped Localization Thread\n"); } // // Stop the laser thread. // if(mySick) { mySick->lockDevice(); mySick->disconnect(); mySick->unlockDevice(); printf("Stopped Laser Thread\n"); } // // Stop the robot thread. // myRobot->lock(); myRobot->stopRunning(); myRobot->unlock(); printf("Stopped Robot Thread\n"); // // Exit Aria // Aria::shutdown(); printf("Aria Shutdown\n"); }
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); ArTime startTime; // move a couple meters robot.move(distToTravel); robot.unlock(); startTime.setToNow(); do { ArUtil::sleep(100); robot.lock(); robot.setHeading(0); done = robot.isMoveDone(60); robot.unlock(); } while (!done); /* // rotate a few times robot.lock(); robot.setVel(0); robot.setRotVel(60); robot.unlock(); ArUtil::sleep(12000); */ robot.lock(); robot.setHeading(180); robot.unlock(); do { ArUtil::sleep(100); robot.lock(); robot.setHeading(180); done = robot.isHeadingDone(); robot.unlock(); } while (!done); // move a couple meters robot.lock(); robot.move(distToTravel); robot.unlock(); startTime.setToNow(); do { ArUtil::sleep(100); robot.lock(); robot.setHeading(180); done = robot.isMoveDone(60); robot.unlock(); } while (!done); robot.lock(); robot.setHeading(0); robot.setVel(0); robot.unlock(); startTime.setToNow(); do { ArUtil::sleep(100); robot.lock(); robot.setHeading(0); done = robot.isHeadingDone(); robot.unlock(); } while (!done); sick.lockDevice(); sick.disconnect(); sick.unlockDevice(); robot.lock(); robot.disconnect(); robot.unlock(); // now exit Aria::shutdown(); return 0; }
int main(int argc, char **argv) { double speed = 1000; double squareSide = 2000; // 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 // robot robot = new ArRobot; // the laser ArSick sick; // set up our simpleConnector ArSimpleConnector simpleConnector(&argc, argv); // set up a key handler so escape exits and attach to the robot ArKeyHandler keyHandler; robot->attachKeyHandler(&keyHandler); // parse its arguments if (simpleConnector.parseArgs()) { simpleConnector.logOptions(); keyHandler.restore(); exit(1); } // if there are more arguments left then it means we didn't // understand an option /* if (argc > 1) { simpleConnector.logOptions(); keyHandler.restore(); exit(1); } */ // 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); // set up the robot for connecting if (!simpleConnector.connectRobot(robot)) { printf("Could not connect to robot->.. exiting\n"); Aria::shutdown(); return 1; } robot->setRotVelMax(300); robot->setRotAccel(300); robot->setRotDecel(300); robot->setAbsoluteMaxTransVel(2000); robot->setTransVelMax(2000); robot->setTransAccel(500); robot->setTransDecel(500); /* robot->comInt(82, 30); // rotkp robot->comInt(83, 200); // rotkv robot->comInt(84, 0); // rotki robot->comInt(85, 15); // transkp robot->comInt(86, 450); // transkv robot->comInt(87, 4); // transki */ 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 // run the robot, true here so that the run will exit if connection lost robot->runAsync(true); // set up the laser before handing it to the laser mode simpleConnector.setupLaser(&sick); // 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; } robot->lock(); robot->addUserTask("printer", 50, new ArGlobalFunctor(&printer)); robot->unlock(); #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 printf("Starting moving\n"); robot->lock(); // enable the motors, disable amigobot sounds robot->comInt(ArCommands::ENABLE, 1); robot->setHeading(0); robot->setVel(1000); robot->unlock(); ArUtil::sleep(speed / 500.0 * 1000.0); printf("Should be up to speed, moving on first side\n"); ArUtil::sleep(squareSide / speed * 1000); printf("Turning to second side\n"); robot->lock(); robot->setHeading(90); robot->setVel(speed); robot->unlock(); ArUtil::sleep(squareSide / speed * 1000); printf("Turning to third side\n"); robot->lock(); robot->setHeading(180); robot->setVel(speed); robot->unlock(); ArUtil::sleep(squareSide / speed * 1000); printf("Turning to last side\n"); robot->lock(); robot->setHeading(-90); robot->setVel(speed); robot->unlock(); ArUtil::sleep(squareSide / speed * 1000); printf("Pointing back original direction and stopping\n"); robot->lock(); robot->setHeading(0); robot->setVel(0); robot->unlock(); ArUtil::sleep(300); printf("Stopped\n"); sick.lockDevice(); sick.disconnect(); sick.unlockDevice(); robot->lock(); robot->disconnect(); robot->unlock(); // now exit Aria::shutdown(); return 0; }