int main(int argc, char **argv) { int ret; std::string str; // the serial connection (robot) ArSerialConnection serConn; // tcp connection (sim) ArTcpConnection tcpConn; // the robot ArRobot robot; // the laser ArSick sick; // the laser connection ArSerialConnection laserCon; bool useSimForLaser = false; std::string hostname = "prod.local.net"; // timeouts in minutes int wanderTime = 0; int restTime = 0; // check arguments if (argc == 3 || argc == 4) { wanderTime = atoi(argv[1]); restTime = atoi(argv[2]); if (argc == 4) hostname = argv[3]; } else { printf("\nUsage:\n\tpeoplebotTest <wanderTime> <restTime> <hostname>\n\n"); printf("Times are in minutes. Hostname is the machine to pipe the ACTS display to\n\n"); wanderTime = 15; restTime = 45; } printf("Wander time - %d minutes\nRest time - %d minutes\n", wanderTime, restTime); printf("Sending display to %s.\n\n", hostname.c_str()); // sonar, must be added to the robot ArSonarDevice sonar; // the actions we'll use to wander ArActionStallRecover recover; ArActionBumpers bumpers; ArActionAvoidFront avoidFrontNear("Avoid Front Near", 225, 0); ArActionAvoidFront avoidFrontFar; // Make a key handler, so that escape will shut down the program // cleanly ArKeyHandler keyHandler; // mandatory init Aria::init(); // Add the key handler to Aria so other things can find it Aria::setKeyHandler(&keyHandler); // Attach the key handler to a robot now, so that it actually gets // some processing time so it can work, this will also make escape // exit robot.attachKeyHandler(&keyHandler); // First we see if we can open the tcp connection, if we can we'll // assume we're connecting to the sim, and just go on... if we // can't open the tcp it means the sim isn't there, so just try the // robot // modify this next line if you're not using default tcp connection tcpConn.setPort(); // see if we can get to the simulator (true is success) if (tcpConn.openSimple()) { // we could get to the sim, so set the robots device connection to the sim printf("Connecting to simulator through tcp.\n"); robot.setDeviceConnection(&tcpConn); } else { // we couldn't get to the sim, so set the port on the serial // connection and then set the serial connection as the robots // device // modify the next line if you're not using the first serial port // to talk to your robot serConn.setPort(); printf( "Could not connect to simulator, connecting to robot through serial.\n"); robot.setDeviceConnection(&serConn); } // add the sonar to the robot robot.addRangeDevice(&sonar); // add the laser robot.addRangeDevice(&sick); // try to connect, if we fail 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::ENABLE, 1); robot.comInt(ArCommands::SOUNDTOG, 0); // turn off the sonar to start with robot.comInt(ArCommands::SONAR, 0); // add the actions robot.addAction(&recover, 100); robot.addAction(&bumpers, 75); robot.addAction(&avoidFrontNear, 50); robot.addAction(&avoidFrontFar, 49); // start the robot running, true so that if we lose connection the run stops robot.runAsync(true); if (!useSimForLaser) { sick.setDeviceConnection(&laserCon); if ((ret = laserCon.open("/dev/ttyS2")) != 0) { str = tcpConn.getOpenMessage(ret); printf("Open failed: %s\n", str.c_str()); Aria::shutdown(); return 1; } sick.configureShort(false); } else { sick.configureShort(true); } sick.runAsync(); if (!sick.blockingConnect()) { printf("Could not connect to SICK laser... exiting\n"); Aria::shutdown(); return 1; } robot.lock(); robot.comInt(ArCommands::ENABLE, 1); robot.unlock(); // add the peoplebot test PeoplebotTest pbTest(&robot, wanderTime, restTime, hostname); robot.waitForRunExit(); // now exit Aria::shutdown(); return 0; }
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); // Always try to connect to the first laser: argParser.addDefaultArgument("-connectLaser"); 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); } } // Trigger argument parsing if (!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(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 ArActionStallRecover recover; ArActionBumpers bumpers; ArActionAvoidFront avoidFrontNear("Avoid Front Near", 225, 0); ArActionAvoidFront avoidFrontFar; ArActionConstantVelocity constantVelocity("Constant Velocity", 400); robot.addAction(&recover, 100); robot.addAction(&bumpers, 75); robot.addAction(&avoidFrontNear, 50); robot.addAction(&avoidFrontFar, 49); robot.addAction(&constantVelocity, 25); // wait for robot task loop to end before exiting the program robot.waitForRunExit(); Aria::exit(0); }
int main(int argc, char **argv) { Aria::init(); ArArgumentParser argParser(&argc, argv); argParser.loadDefaultArguments(); ArRobot robot; ArSick laser; std::ofstream stream; // for loggin time_t timer; char buffer[120]; //for loggin ArRobotConnector robotConnector(&argParser, &robot); ArLaserConnector laserConnector(&argParser, &robot, &robotConnector); int32_t count = 0; readyLog(stream); 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.addRangeDevice(&laser); 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."); } double sampleAngle, dist; auto sampleRange = laser.currentReadingPolar(-20, 20, &sampleAngle); auto degreeStr = laser.getDegreesChoicesString(); std::cout << "auto sampleRange = laser.currentReadingPolar(-20, 20, &sampleAngle);" << sampleRange << std::endl; std::cout << "auto degreeStr = laser.getDegreesChoicesString(); : " << degreeStr << std::endl; // turn on the motors, turn off amigobot sounds robot.enableMotors(); //robot.getLaserMap() robot.comInt(ArCommands::SOUNDTOG, 0); // add a set of actions that combine together to effect the wander behavior ArActionStallRecover recover; ArActionBumpers bumpers; ArActionAvoidFront avoidFrontNear("Avoid Front Near", 100, 0); ArActionAvoidFront avoidFrontFar; ArActionConstantVelocity constantVelocity("Constant Velocity", 400); robot.addAction(&recover, 100); robot.addAction(&bumpers, 75); robot.addAction(&avoidFrontNear, 50); robot.addAction(&avoidFrontFar, 49); robot.addAction(&constantVelocity, 25); stream << "IMPORTANT !! == robot.getRobotRadius() : " << robot.getRobotRadius << std::endl; std::string timestamp; while (1) { // // time(&timer); strftime(buffer, 80, " - timestamp : %I:%M:%S", localtime(&timer)); timestamp = buffer; dist = robot.checkRangeDevicesCurrentPolar(-70, 70, &sampleAngle) - robot.getRobotRadius(); stream << count << timestamp << "checkRangeDevicesCurrentPolar(-70, 70, &angle) : " << dist << std::endl; Sleep(500); count++; } // wwill add processor here // wait for robot task loop to end before exiting the program robot.waitForRunExit(); Aria::exit(0); return 0; }