void SetupRobot(void)
{
puts("attempting to connect to robot");
RobotConnectoin.setPort("COM8");
RobotConnectoin.setBaud(9600);
robot.setDeviceConnection(&RobotConnectoin);
if(!robot.blockingConnect()){puts("not connected to robot");Aria::shutdown();}
robot.addRangeDevice(&sonarDev);
robot.addRangeDevice(&bumpers);
robot.enableMotors();
robot.enableSonar();
robot.requestEncoderPackets();
robot.setCycleChained(false);
// robot.setRotVelMax(robot.getRotVelMax());
}
int main(int argc, char** argv)
{
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobot robot;
ArSonarDevice sonar;
// Connect to the robot, get some initial data from it such as type and name,
// and then load parameter files for this robot.
ArRobotConnector robotConnector(&parser, &robot);
if(!robotConnector.connectRobot())
{
ArLog::log(ArLog::Terse, "actionExample: Could not connect to the robot.");
if(parser.checkHelpAndWarnUnparsed())
{
// -help not given
Aria::logOptions();
Aria::exit(1);
}
}
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
}
ArLog::log(ArLog::Normal, "actionExample: Connected to robot.");
// Create instances of the actions defined above, plus ArActionStallRecover,
// a predefined action from Aria.
ActionGo go(500, 350);
ActionTurn turn(400, 10);
ArActionStallRecover recover;
// Add the range device to the robot. You should add all the range
// devices and such before you add actions
robot.addRangeDevice(&sonar);
// Add our actions in order. The second argument is the priority,
// with higher priority actions going first, and possibly pre-empting lower
// priority actions.
robot.addAction(&recover, 100);
robot.addAction(&go, 50);
robot.addAction(&turn, 49);
// Enable the motors, disable amigobot sounds
robot.enableMotors();
// Run the robot processing cycle.
// 'true' means to return if it loses connection,
// after which we exit the program.
robot.run(true);
Aria::exit(0);
}
int main (int argc, char** argv) {
Aria::init();
Arnl::init();
ArRobot robot;
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobotConnector robotConnector(&parser, &robot);
//ArAnalogGyro gyro = new
ArAnalogGyro gyro(&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;
robot.addRangeDevice(&sonarDev);
robot.runAsync(true);
ArMap map("office.map");
// set it up to ignore empty file names (otherwise if a configuration omits
// the map file, the whole configuration change will fail)
map.setIgnoreEmptyFileName(true);
// ignore the case, so that if someone is using MobileEyes or
// MobilePlanner from Windows and changes the case on a map name,
// it will still work.
map.setIgnoreCase(true);
ArSonarLocalizationTask locTask(&robot, &sonarDev, &map);
locTask.localizeRobotAtHomeBlocking();
robot.runAsync(true);
robot.enableMotors();
//robot.lock();
robot.setVel(200);
//robot.unlock();
ArPose pose;
locTask.forceUpdatePose(pose);
while(true) {
//while (robot.blockingConnect()){
//robot.lock();
//ArPose pose = robot.getPose();
//pose.setX(100);
//robot.moveTo(pose);
//t = robot.getLastOdometryTime();
//int a = interp.getPose(t, &pose);
ArLog::log(ArLog::Normal, "x = %f \t y = %f\n", pose.getX(), pose.getY());
//robot.unlock();
}
}
int main(int argc, char** argv)
{
Aria::init();
ArSimpleConnector conn(&argc, argv);
ArRobot robot;
ArSonarDevice sonar;
// Create instances of the actions defined above, plus ArActionStallRecover,
// a predefined action from Aria.
ActionGo go(500, 350);
ActionTurn turn(400, 10);
ArActionStallRecover recover;
// Parse all command-line arguments
if(!Aria::parseArgs())
{
Aria::logOptions();
return 1;
}
// Connect to the robot
if(!conn.connectRobot(&robot))
{
ArLog::log(ArLog::Terse, "actionExample: Could not connect to robot! Exiting.");
return 2;
}
// Add the range device to the robot. You should add all the range
// devices and such before you add actions
robot.addRangeDevice(&sonar);
// Add our actions in order. The second argument is the priority,
// with higher priority actions going first, and possibly pre-empting lower
// priority actions.
robot.addAction(&recover, 100);
robot.addAction(&go, 50);
robot.addAction(&turn, 49);
// Enable the motors, disable amigobot sounds
robot.enableMotors();
// Run the robot processing cycle.
// 'true' means to return if it loses connection,
// after which we exit the program.
robot.run(true);
Aria::shutdown();
return 0;
}
int main(int argc, char **argv){
Aria::init();
ArRobot robot;
ArArgumentParser parser(&argc, argv);
ArSimpleConnector connector(& parser);
parser.loadDefaultArguments();
Aria::logOptions();
if (!connector.parseArgs()){
cout << "Unknown settings\n";
Aria::exit(0);
exit(1);
}
if (!connector.connectRobot(&robot)){
cout << "Unable to connect\n";
Aria::exit(0);
exit(1);
}
robot.runAsync(true);
robot.lock();
robot.comInt(ArCommands::ENABLE, 1);
robot.unlock();
ArSonarDevice sonar;
robot.addRangeDevice(&sonar);
G_id = 0;
G_SONAR_FD = fopen("../sensors/sonars","w");
G_pose_fd = fopen("../sensors/pose","w");
int numSonar = robot.getNumSonar();
while(1){
readPosition(robot);
readSonars(robot, 8);
setMotors(robot);
usleep(20000);
}
fclose(G_SONAR_FD);
fclose(G_pose_fd);
Aria::exit(0);
}
int main (int argc, char** argv) {
Aria::init();
Arnl::init();
ArRobot robot;
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobotConnector robotConnector(&parser, &robot);
//ArAnalogGyro gyro = new
ArAnalogGyro gyro(&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;
// robot.runAsync(true);
// ArMap map("office.map");
// set it up to ignore empty file names (otherwise if a configuration omits
// the map file, the whole configuration change will fail)
// map.setIgnoreEmptyFileName(true);
// ignore the case, so that if someone is using MobileEyes or
// MobilePlanner from Windows and changes the case on a map name,
// it will still work.
// map.setIgnoreCase(true);
robot.runAsync(true);
robot.enableMotors();
robot.moveTo(ArPose(0,0,0));
//robot.lock();
robot.comInt(ArCommands::ENABLE, 1);
robot.addRangeDevice(&sonarDev);
//robot.unlock();
// ArSonarLocalizationTask locTask(&robot, &sonarDev, &map);
<<<<<<< HEAD
int main(void)
{
ArTcpConnection con;
ArRobot robot;
ArSonarDevice sonar;
int ret;
std::string str;
ArActionStallRecover recover;
ArActionConstantVelocity constantVelocity("Constant Velocity", 400);
Aria::init();
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
robot.addRangeDevice(&sonar);
robot.setDeviceConnection(&con);
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
robot.addAction(&recover, 100);
robot.addAction(&constantVelocity, 25);
robot.run(true);
Aria::shutdown();
return 0;
}
int RosAriaNode::Setup()
{
ArArgumentBuilder *args;
args = new ArArgumentBuilder();
args->add("-rp"); //pass robot's serial port to Aria
args->add(serial_port.c_str());
conn = new ArSimpleConnector(args);
robot = new ArRobot();
robot->setCycleTime(1);
ArLog::init(ArLog::File, ArLog::Verbose, "aria.log", true);
//parse all command-line arguments
if (!Aria::parseArgs())
{
Aria::logOptions();
return 1;
}
// Connect to the robot
if (!conn->connectRobot(robot)) {
ArLog::log(ArLog::Terse, "rotate: Could not connect to robot! Exiting.");
return 1;
}
//Sonar sensor
sonar.setMaxRange(Max_Range);
robot->addRangeDevice(&sonar);
robot->enableSonar();
// Enable the motors
robot->enableMotors();
robot->runAsync(true);
return 0;
}
int main (int argc, char** argv) {
Aria::init();
Arnl::init();
ArRobot robot;
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobotConnector robotConnector(&parser, &robot);
//ArAnalogGyro gyro = new
// ArAnalogGyro gyro(&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;
// robot.runAsync(true);
ArLog::log(ArLog::Normal, "OK");
ArMap map("office.map");
// set it up to ignore empty file names (otherwise if a configuration omits
// the map file, the whole configuration change will fail)
// map.setIgnoreEmptyFileName(true);
// ignore the case, so that if someone is using MobileEyes or
// MobilePlanner from Windows and changes the case on a map name,
// it will still work.
// map.setIgnoreCase(true);
robot.runAsync(true);
// robot.lock();
// robot.comInt(ArCommands::ENABLE, 1);
robot.addRangeDevice(&sonarDev);
// ArSonarLocalizationTask locTask(&robot, &sonarDev, &map);
// ArActionAvoidFront avoidFront("avoid front obstacles");
// ArActionGoto gotoPoseAction("goto");
// robot.addAction(&gotoPoseAction, 50);
// robot.addAction(&avoidFront, 60);
// robot.moveTo(ArPose(0,0,0));
ArPathPlanningTask pathPlan(&robot, &sonarDev, &map);
// pathPlan.getRun
// locTask.localizeRobotAtHomeBlocking();
ArGlobalFunctor1<ArPose> goalDone(&addGoalDone);// = new ArGlobalFunctor1(&addGoalDone);
ArGlobalFunctor1<ArPose> goalFail(&addGoalFailed);
ArGlobalFunctor1<ArPose> goalFinished(&addGoalFinished);
ArGlobalFunctor1<ArPose> goalInterrupted(&addGoalInterrupted);
pathPlan.addGoalDoneCB(&goalDone);
pathPlan.addGoalFailedCB(&goalFail);
pathPlan.addGoalFinishedCB(&goalFinished);
pathPlan.addGoalInterruptedCB(&goalInterrupted);
// ro
// ArActionPlanAndMoveToGoal gotoGoal (200, 10, pathPlan, NULL, &sonarDev);
// pathPlan.runAsync();
robot.enableMotors();
// while(!pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true));
pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true);
// pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true)
pathPlan.startPathPlanToLocalPose(true);
// pathPlan.getPathPlanActionGroup()->activate();
// pathPlan.getCurrentGoal()
// pathPlan.is
// robot.unlock();
/*
gotoPoseAction.setGoal(ArPose(10000, 15000, 0));
while (!gotoPoseAction.haveAchievedGoal()) {
robot.lock();
printf ("x = %.2f, y = %.2f", robot.getX(), robot.getY());
robot.unlock();
}
*/
// robot.lock();
// robot.setVel(200);
// robot.unlock();
while (pathPlan.endPathPlanToLocalPose(true));
//ArUtil::sleep(1000);
// ArPose pose;
// locTask.forceUpdatePose(pose);
/*
while(true) {
//while (robot.blockingConnect()){
//robot.lock();
//ArPose pose = robot.getPose();
//pose.setX(100);
//robot.moveTo(pose);
//t = robot.getLastOdometryTime();
//int a = interp.getPose(t, &pose);
ArLog::log(ArLog::Normal, "x = %f \t y = %f\n", pose.getX(), pose.getY());
//robot.unlock();
}
* */
}
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[])
{
// Initialize location of Aria, Arnl and their args.
Aria::init();
Arnl::init();
// The robot object
ArRobot robot;
// Parse the command line arguments.
ArArgumentParser parser(&argc, argv);
// Read data_index if exists
int data_index;
bool exist_data_index;
parser.checkParameterArgumentInteger("dataIndex",&data_index,&exist_data_index);
// Load default arguments for this computer (from /etc/Aria.args, environment
// variables, and other places)
parser.loadDefaultArguments();
// Object that connects to the robot or simulator using program options
ArRobotConnector robotConnector(&parser, &robot);
// set up a gyro
ArAnalogGyro gyro(&robot);
ArLog::init(ArLog::File,ArLog::Normal,"run.log",false,true,true);
// Parse arguments for the simple connector.
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
ArLog::log(ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]);
Aria::logOptions();
Aria::exit(1);
}
// Collision avoidance actions at higher priority
ArActionAvoidFront avoidAction("avoid",200);
ArActionLimiterForwards limiterAction("speed limiter near", 150, 500, 150);
ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100, 400);
ArActionLimiterTableSensor tableLimiterAction;
//robot.addAction(&tableLimiterAction, 100);
//robot.addAction(&avoidAction,100);
//robot.addAction(&limiterAction, 95);
//robot.addAction(&limiterFarAction, 90);
// Goto action at lower priority
ArActionGoto gotoPoseAction("goto");
//robot.addAction(&gotoPoseAction, 50);
gotoPoseAction.setCloseDist(750);
// Stop action at lower priority, so the robot stops if it has no goal
ArActionStop stopAction("stop");
//robot.addAction(&stopAction, 40);
// Connect the robot
if (!robotConnector.connectRobot())
{
ArLog::log(ArLog::Normal, "Could not connect to robot... exiting");
Aria::exit(3);
}
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);
// Sonar, must be added to the robot, used by teleoperation and wander to
// detect obstacles, and for localization if SONARNL
ArSonarDevice sonarDev;
// Add the sonar to the robot
robot.addRangeDevice(&sonarDev);
// Start the robot thread.
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
true, // add only connected lasers to ArRobot
true // add all lasers to ArRobot
))
{
ArLog::log(ArLog::Normal ,"Could not connect to lasers... exiting\n");
Aria::exit(2);
}
// Puntero a laser
ArSick* sick=(ArSick*)robot.findLaser(1);
// Conectamos el laser
sick->asyncConnect();
//Esperamos a que esté encendido
while(!sick->isConnected())
{
ArUtil::sleep(100);
}
ArLog::log(ArLog::Normal ,"Laser conectado\n");
// Set up things so data can be logged (only do it with the laser
// since it can overrun a 9600 serial connection which the sonar is
// more likely to have)
ArDataLogger dataLogger(&robot);
dataLogger.addToConfig(Aria::getConfig());
// add our logging to the config
//ArLog::addToConfig(Aria::getConfig());
// Set up a class that'll put the movement and gyro parameters into ArConfig
ArRobotConfig robotConfig(&robot);
robotConfig.addAnalogGyro(&gyro);
// Add additional range devices to the robot and path planning task.
// IRs if the robot has them.
robot.lock();
ArIRs irs;
robot.addRangeDevice(&irs);
// Bumpers.
ArBumpers bumpers;
robot.addRangeDevice(&bumpers);
// cause the sonar to turn off automatically
// when the robot is stopped, and turn it back on when commands to move
// are sent. (Note, this should not be done if you need the sonar
// data to localize, or for other purposes while stopped)
ArSonarAutoDisabler sonarAutoDisabler(&robot);
// Read in parameter files.
Aria::getConfig()->useArgumentParser(&parser);
if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
{
ArLog::log(ArLog::Normal, "Trouble loading configuration file, exiting");
Aria::exit(5);
}
//Configuracion del laser
sick->setMinDistBetweenCurrent(0);
robot.enableMotors();
robot.setAbsoluteMaxTransVel(1000);
/* Finally, get ready to run the robot: */
robot.unlock();
Controlador driver(&robot);
if(exist_data_index){
driver.setDataIndex(data_index);
}
driver.runAsync();
ControlHandler handler(&driver,&robot);
// Use manual key handler
//handler.addKeyHandlers(&robot);
robot.addSensorInterpTask("ManualKeyHandler",50,handler.getFunctor());
ArLog::log(ArLog::Normal ,"Añadido manejador teclado\n");
// double x,y,dist,angle;
// ArPose punto;
// ArPose origen=robot.getPose();
// sick->lockDevice();
// for(int i=-90;i<90;i++){
// // Obtengo la medida de distancia y angulo
// dist=sick->currentReadingPolar(i,i+1,&angle);
// // Obtengo coordenadas del punto usando el laser como referencia
// x=dist*ArMath::cos(angle);
// y=dist*ArMath::sin(angle);
// //Roto los puntos
// ArMath::pointRotate(&x,&y,-origen.getTh());
// punto.setX(x);
// punto.setY(y);
// punto=punto + origen;
// printf("Medida: %d\t Angulo:%.2f\t Angulo:%.2f\t Distancia:%0.2f\t X:%0.2f\t Y:%0.2f\n",i,angle,angle+origen.getTh(),dist,punto.getX(),punto.getY());
// }
// printf("Medidas adquiridas\n");
// sick->unlockDevice();
robot.waitForRunExit();
//ArUtil::sleep(10000);
return 0;
}
int main(int argc, char **argv)
{
//----------------------initialized robot server------------------------------------------
Aria::init();
Arnl::init();
ArServerBase server;
//-----------------------------------------------------------------------------------
VCCHandler ptz(&robot); //create keyboard for control vcc50i
G_PTZHandler->reset();
ArUtil::sleep(300);
G_PTZHandler->panSlew(30);
//-----------------------------------------------------------------------------------
argc = 2 ;
argv[0] = "-map";
argv[1] = "map20121111.map";
// Parse the command line arguments.
ArArgumentParser parser(&argc, argv);
// Set up our simpleConnector
ArSimpleConnector simpleConnector(&parser);
// Set up our simpleOpener
ArServerSimpleOpener simpleOpener(&parser);
//*******
// Set up our client for the central server
// ArClientSwitchManager clientSwitch(&server, &parser);
//************
// Load default arguments for this computer (from /etc/Aria.args, environment
// variables, and other places)
// parser.loadDefaultArguments();
// set up a gyro
ArAnalogGyro gyro(&robot);
//gyro.activate();
// Parse arguments for the simple connector.
// if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
// {
// ArLog::log(ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]);
// Aria::logOptions();
// Aria::exit(1);
// }
// The laser object, will be used if we have one
// Add the laser to the robot
robot.addRangeDevice(&sick);
// Sonar, must be added to the robot, used by teleoperation and wander to
// detect obstacles, and for localization if SONARNL
ArSonarDevice sonarDev;
// Add the sonar to the robot
robot.addRangeDevice(&sonarDev);
// Set up where we'll look for files
char fileDir[1024];
ArUtil::addDirectories(fileDir, sizeof(fileDir), "./", "maps");
ArLog::log(ArLog::Normal, "Installation directory is: %s\nMaps directory is: %s\n", Aria::getDirectory(), fileDir);
// Set up the map, this will look for files in the examples
// directory (unless the file name starts with a /, \, or .
// You can take out the 'fileDir' argument to look in the current directory
// instead
ArMap arMap(fileDir);
// set it up to ignore empty file names (otherwise the parseFile
// on the config will fail)
arMap.setIgnoreEmptyFileName(true);
//********************************
//Localization
//********************************
ArLocalizationManager locManager(&robot, &arMap);
ArLog::log(ArLog::Normal, "Creating laser localization task");
ArLocalizationTask locTask (&robot, &sick, &arMap);
locManager.addLocalizationTask(&locTask);
//*******************************
//Path planning
//*******************************
// Make the path task planning task
ArPathPlanningTask pathTask(&robot, &sick, &sonarDev, &arMap);
G_PathPlanning = &pathTask;
// Set up things so data can be logged (only do it with the laser
// since it can overrun a 9600 serial connection which the sonar is
// more likely to have)
ArDataLogger dataLogger(&robot);
dataLogger.addToConfig(Aria::getConfig());
// add our logging to the config
// ArLog::addToConfig(Aria::getConfig());
// First open the server
if (!simpleOpener.open(&server, fileDir, 240))
{
if (simpleOpener.wasUserFileBad())
ArLog::log(ArLog::Normal, "Bad user file");
else
ArLog::log(ArLog::Normal, "Could not open server port");
exit(2);
}
// Connect the robot
if (!simpleConnector.connectRobot(&robot))
{
ArLog::log(ArLog::Normal, "Could not connect to robot... exiting");
Aria::exit(3);
}
//-----------------------------------------------
//**************************
// Set up a class that'll put the movement and gyro parameters into ArConfig
ArRobotConfig robotConfig(&robot);
robotConfig.addAnalogGyro(&gyro);
//*****************************
robot.enableMotors();
robot.clearDirectMotion();
// if we are connected to a simulator, reset it to its start position
robot.comInt(ArCommands::RESETSIMTOORIGIN, 1);
robot.moveTo(ArPose(0,0,0));
// Set up laser using connector (command line arguments, etc.)
simpleConnector.setupLaser(&sick);
// Start the robot thread.
robot.runAsync(true);
// Start the laser thread.
sick.runAsync();
// Try to connect the laser
if (!sick.blockingConnect())
ArLog::log(ArLog::Normal, "Warning: Couldn't connect to SICK laser, it won't be used");
else
ArLog::log(ArLog::Normal, "Connected to laser.");
//***************************************
// Add additional range devices to the robot and path planning task.
// IRs if the robot has them.
robot.lock();
// ArIRs irs;
// robot.addRangeDevice(&irs);
// pathTask.addRangeDevice(&irs, ArPathPlanningTask::CURRENT);
//******************************************
// Forbidden regions from the map
ArForbiddenRangeDevice forbidden(&arMap);
robot.addRangeDevice(&forbidden);
// This is the place to add a range device which will hold sensor data
// and delete it appropriately to replan around blocked paths.
ArGlobalReplanningRangeDevice replanDev(&pathTask);
// Create objects that add network services:
// Drawing in the map display:
ArServerInfoDrawings drawings(&server);
drawings.addRobotsRangeDevices(&robot);
drawings.addRangeDevice(&replanDev);
/* If you want to draw the destination put this code back in:
ArServerDrawingDestination destination(
&drawings, &pathTask, "destination",
500, 500,
new ArDrawingData("polyDots",
ArColor(0xff, 0xff, 0x0),
800, // size
49), // just below the robot
*/
/* If you want to see the local path planning area use this
(You can enable this particular drawing from custom commands
which is set up down below in ArServerInfoPath)
ArDrawingData drawingDataP("polyLine", ArColor(200,200,200), 1, 75);
ArFunctor2C<ArPathPlanningTask, ArServerClient *, ArNetPacket *>
drawingFunctorP(pathTask, &ArPathPlanningTask::drawSearchRectangle);
drawings.addDrawing(&drawingDataP, "Local Plan Area", &drawingFunctorP);
*/
/* If you want to see the points making up the local path in addition to the
* main path use this.
ArDrawingData drawingDataP2("polyDots", ArColor(0,128,0), 100, 70);
ArFunctor2C<ArPathPlanningTask, ArServerClient *, ArNetPacket *>
drawingFunctorP2(pathTask, &ArPathPlanningTask::drawPathPoints);
drawings.addDrawing(&drawingDataP2, "Path Points", &drawingFunctorP2);
*/
// Misc. simple commands:
ArServerHandlerCommands commands(&server);
// These provide various kinds of information to the client:
ArServerInfoRobot serverInfoRobot(&server, &robot);
ArServerInfoSensor serverInfoSensor(&server, &robot);
ArServerInfoPath serverInfoPath(&server, &robot, &pathTask);
serverInfoPath.addSearchRectangleDrawing(&drawings);
serverInfoPath.addControlCommands(&commands);
//-------------------------receive commands or events from client---------------------
// ArServerHandlerCommands commands(&server);
server.addData("RobotVideo" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&VideoServerBase::RobotVideoCB) ,"","");
server.addData("turn" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&turn_func) ,"","");
server.addData("RobotMotion" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_RobotMotion) ,"","");
server.addData("CameraMotion" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_CameraMotion) ,"","");
server.addData("RobotTurnLeft" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_RobotTurnLeft) ,"","");
server.addData("RobotTurnRight" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_RobotTurnRight) ,"","");
server.addData("TargetApproach" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_TargetApproach) ,"","");
server.addData("TargetApproachObstacles" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_TargetApproach_Obstacles) ,"","");
server.addData("GlassesCancel" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_GlassesCancel) ,"","");
server.addData("Calibration" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_Calibration) ,"","");
server.addClientRemovedCallback(new ArGlobalFunctor1< ArServerClient * >(&clientCloseCallback));
//-------------------------receive commands or events from client---------------------
//***********************
ArServerInfoLocalization serverInfoLocalization (&server, &robot, &locManager);
ArServerHandlerLocalization serverLocHandler (&server, &robot, &locManager);
// Provide the map to the client (and related controls):
// This uses both lines and points now, since everything except
// sonar localization uses both (path planning with sonar still uses both)
ArServerHandlerMap serverMap(&server, &arMap);
// Add some simple (custom) commands for testing and debugging:
ArServerSimpleComUC uCCommands(&commands, &robot); // Send any command to the microcontroller
ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // configure logging
ArServerSimpleComGyro gyroCommands(&commands, &robot, &gyro); // monitor the gyro
ArServerSimpleComLogRobotConfig configCommands(&commands, &robot); // trigger logging of the robot config parameters
ArServerSimpleServerCommands serverCommands(&commands, &server); // monitor networking behavior (track packets sent etc.)
/* Set up the possible modes for remote control from a client such as
* MobileEyes:
*/
// Mode To go to a goal or other specific point:
ArServerModeGoto modeGoto(&server, &robot, &pathTask, &arMap, ArPose(0,0,0));
// Add a simple (custom) command that allows you to give a list of
// goals to tour, instead of all. Useful for testing and debugging.
modeGoto.addTourGoalsInListSimpleCommand(&commands);
// Mode To stop and remain stopped:
ArServerModeStop modeStop(&server, &robot);
// cause the sonar to turn off automatically
// when the robot is stopped, and turn it back on when commands to move
// are sent. (Note, this should not be done if you need the sonar
// data to localize, or for other purposes while stopped)
ArSonarAutoDisabler sonarAutoDisabler(&robot);
// Teleoperation modes To drive by keyboard, joystick, etc:
ArServerModeRatioDrive modeRatioDrive(&server, &robot); // New, improved mode
ArServerModeDrive modeDrive(&server, &robot); // Older mode for compatability
// Drive mode's configuration and custom (simple) commands:
modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings");
modeDrive.addControlCommands(&commands);
modeRatioDrive.addControlCommands(&commands);
// Wander mode
// ArServerModeWander modeWander(&server, &robot);
//*********************************
// Prevent driving if localization is lost:
ArActionLost actionLostRatioDrive (&locManager, NULL, &modeRatioDrive);
modeRatioDrive.getActionGroup ()->addAction (&actionLostRatioDrive, 110);
// Prevent wandering if lost:
// ArActionLost
// actionLostWander (&locManager, NULL, &modeWander);
// modeWander.getActionGroup ()->addAction (&actionLostWander, 110);
// This provides a small table of interesting information for the client
// to display to the operator:
ArServerInfoStrings stringInfo(&server);
Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor());
Aria::getInfoGroup()->addStringInt(
"Motor Packet Count", 10,
new ArConstRetFunctorC<int, ArRobot>(&robot,
&ArRobot::getMotorPacCount));
// Make Stop mode the default (If current mode deactivates without entering
// a new mode, then Stop Mode will be selected)
modeStop.addAsDefaultMode();
// Create the service that allows the client to monitor the communication
// between the robot and the client.
//
ArServerHandlerCommMonitor handlerCommMonitor(&server);
// Create service that allows client to change configuration parameters in ArConfig
ArServerHandlerConfig handlerConfig(&server, Aria::getConfig(),
Arnl::getTypicalDefaultParamFileName(),
Aria::getDirectory());
// Read in parameter files. read the paras from input
Aria::getConfig()->useArgumentParser(&parser);
if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
{
ArLog::log(ArLog::Normal, "Trouble loading configuration file, exiting");
Aria::exit(5);
}
// Warn about unknown params.
if (!simpleOpener.checkAndLog() || !parser.checkHelpAndWarnUnparsed())
{
ArLog::log(ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]);
simpleConnector.logOptions();
simpleOpener.logOptions();
Aria::exit(6);
}
// Warn if there is no map
if (arMap.getFileName() == NULL || strlen(arMap.getFileName()) <= 0)
{
ArLog::log(ArLog::Normal, "");
ArLog::log(ArLog::Normal, "### Warning, No map file is set up, you can make a map with sickLogger or arnlServer, and Mapper3; More info in docs/Mapping.txt and README.txt. Set the map with the -map command line option, or by changing the config with MobileEyes or by editing the config file.");
ArLog::log(ArLog::Normal, "");
}
// find out where we'll want to put files
ArLog::log(ArLog::Normal, "");
ArLog::log(ArLog::Normal,
"Directory for maps and file serving: %s", fileDir);
ArLog::log(ArLog::Normal, "See the ARNL README.txt for more information");
ArLog::log(ArLog::Normal, "");
// If you want MobileSim to try and load up the same map as you are
// using in guiServer then uncomment out the next line and this object
// will send a command to MobileSim to do so, but make sure you start
// MobileSim from the Arnl/examples directory or use the --cwd option,
// so that the map names used by MobileSim match the map names used
// by guiServer
//ArSimMapSwitcher mapSwitcher(&robot, &arMap);
/******************************************************
* ****************************************************
* Camera
* ****************************************************
* *****************************************************/
robot.unlock();
// Localize robot at home.
locTask.localizeRobotAtHomeBlocking();
locTask.forceUpdatePose(ArPose(0,0,0));
resetMotion();
// robot.enableMotors();
// robot.runAsync(true);
//locTask.localizeRobotAtHomeBlocking();
server.runAsync();
VideoServerBase videoserver;
videoserver.runAsync();
robot.comInt(ArCommands::SOUNDTOG, 0);
//G_PathPlanning->pathPlanToPose(ArPose(1500, -1500 , -32),true,true);
//while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL );
//S_TargetApproach_Obstacles1();
//ArUtil::sleep(5000);
// G_PTZHandler->panRel(60);
// ArUtil::sleep(3000);
//while(1)
//S_TargetApproach1();
cout << "done!!!!!!!---------------------" <<endl;
robot.waitForRunExit();
Aria::exit(0);
}
int main(int argc, char **argv)
{
int t, cnt;
double laser_dist[900];
double laser_angle[900];
std::list<ArSensorReading *> *readings;
std::list<ArSensorReading *>::iterator it;
ArKeyHandler keyHandler;
Aria::init();
// Add the key handler to Aria so other things can find it
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
// add the laser to the robot
robot.addRangeDevice(&sick);
// Parse all our args
ArSimpleConnector connector(&argc, argv);
if (!connector.parseArgs() || argc > 1)
{
connector.logOptions();
exit(1);
}
robot.addRangeDevice(&sick);
// try to connect, if we fail exit
if (!connector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// start the robot running, true so that if we lose connection the run stops
robot.runAsync(true);
// now set up the laser
sick.configureShort(true,ArSick::BAUD38400,ArSick::DEGREES180,ArSick::INCREMENT_ONE);
connector.setupLaser(&sick);
sick.runAsync();
if (!sick.blockingConnect())
{
printf("Could not connect to SICK laser... exiting\n");
Aria::shutdown();
return 1;
}
cnt = 1;
while(cnt<10000){
readings=(list<ArSensorReading *,allocator<ArSensorReading *> > *)sick.getRawReadings();//CurrentBuffer..
while (readings == NULL){
readings = (list<ArSensorReading *, allocator<ArSensorReading *> > *)sick.getRawReadings();
}
t=0;
for(it=readings->begin(); it!=readings->end(); it++){
//cout << "t: " << t << endl;
laser_dist[t]=(*it)->getRange();
laser_angle[t]=-90+t;
//cout << "laser angle: " << laser_angle[t] << " laser dist.: " << laser_dist[t] <<" "<<"\n";
t++;
}
cout << "count: " << cnt << endl; //for some reason this line needs to be here
cnt++;
}
for (t=0; t<181; t++){
cout << "laser angle: " << laser_angle[t] << " laser dist.: " << laser_dist[t] <<" "<<"\n";
}
robot.waitForRunExit();
Aria::shutdown();
return 0;
}
int main(int argc, char **argv)
{
Aria::init();
ArRobot robot;
ArServerBase server;
ArArgumentParser parser(&argc, argv);
ArSimpleConnector simpleConnector(&parser);
ArServerSimpleOpener simpleOpener(&parser);
// parse the command line... fail and print the help if the parsing fails
// or if help was requested
parser.loadDefaultArguments();
if (!simpleConnector.parseArgs() || !simpleOpener.parseArgs() ||
!parser.checkHelpAndWarnUnparsed())
{
simpleConnector.logOptions();
simpleOpener.logOptions();
exit(1);
}
// Set up where we'll look for files such as config file, user/password file,
// etc.
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("Error: Bad user/password/permissions file.\n");
else
printf("Error: Could not open server port. Use -help to see options.\n");
exit(1);
}
// Devices
ArAnalogGyro gyro(&robot);
ArSonarDevice sonarDev;
robot.addRangeDevice(&sonarDev);
ArIRs irs;
robot.addRangeDevice(&irs);
ArBumpers bumpers;
robot.addRangeDevice(&bumpers);
ArSick sick(361, 180);
robot.addRangeDevice(&sick);
ArServerInfoRobot serverInfoRobot(&server, &robot);
ArServerInfoSensor serverInfoSensor(&server, &robot);
// This is the service that provides drawing data to the client.
ArServerInfoDrawings drawings(&server);
// Convenience function that sets up drawings for all the robot's current
// range devices (using default shape and color info)
drawings.addRobotsRangeDevices(&robot);
// Add our custom drawings
drawings.addDrawing(
// shape: color: size: layer:
new ArDrawingData("polyLine", ArColor(255, 0, 0), 2, 49),
"exampleDrawing_Home",
new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleHomeDrawingNetCallback)
);
drawings.addDrawing(
new ArDrawingData("polyDots", ArColor(0, 255, 0), 250, 48),
"exampleDrawing_Dots",
new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleDotsDrawingNetCallback)
);
drawings.addDrawing(
new ArDrawingData("polySegments", ArColor(0, 0, 0), 4, 52),
"exampleDrawing_XMarksTheSpot",
new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleXDrawingNetCallback)
);
drawings.addDrawing(
new ArDrawingData("polyArrows", ArColor(255, 0, 255), 500, 100),
"exampleDrawing_Arrows",
new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleArrowsDrawingNetCallback)
);
// modes for moving the robot
ArServerModeStop modeStop(&server, &robot);
ArServerModeDrive modeDrive(&server, &robot);
ArServerModeRatioDrive modeRatioDrive(&server, &robot);
ArServerModeWander modeWander(&server, &robot);
modeStop.addAsDefaultMode();
modeStop.activate();
// Connect to the robot.
if (!simpleConnector.connectRobot(&robot))
{
printf("Error: Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// set up the laser before handing it to the laser mode
simpleConnector.setupLaser(&sick);
robot.enableMotors();
// start the robot cycle running in a background thread
robot.runAsync(true);
// start the laser processing cycle in a background thread
sick.runAsync();
// connect the laser if it was requested
if (!simpleConnector.connectLaser(&sick))
{
printf("Error: Could not connect to laser... exiting\n");
Aria::shutdown();
return 1;
}
// log whatever we wanted to before the runAsync
simpleOpener.checkAndLog();
// run the server thread in the background
server.runAsync();
printf("Server is now running...\n");
// Add a key handler mostly that windows can exit by pressing
// escape, note that the key handler prevents you from running this program
// in the background on Linux.
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");
}
robot.waitForRunExit();
Aria::shutdown();
exit(0);
}
int main(int argc, char **argv)
{
// Initialize Aria and Arnl global information
Aria::init();
Arnl::init();
// The robot object
ArRobot robot;
// Parse the command line arguments.
ArArgumentParser parser(&argc, argv);
// Set up our simpleConnector, to connect to the robot and laser
//ArSimpleConnector simpleConnector(&parser);
ArRobotConnector robotConnector(&parser, &robot);
// Connect to the robot
if (!robotConnector.connectRobot())
{
ArLog::log(ArLog::Normal, "Error: Could not connect to robot... exiting");
Aria::exit(3);
}
// Set up where we'll look for files. Arnl::init() set Aria's default
// directory to Arnl's default directory; addDirectories() appends this
// "examples" directory.
char fileDir[1024];
ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(),
"examples");
// To direct log messages to a file, or to change the log level, use these calls:
//ArLog::init(ArLog::File, ArLog::Normal, "log.txt", true, true);
//ArLog::init(ArLog::File, ArLog::Verbose);
// Add a section to the configuration to change ArLog parameters
ArLog::addToConfig(Aria::getConfig());
// set up a gyro (if the robot is older and its firmware does not
// automatically incorporate gyro corrections, then this object will do it)
ArAnalogGyro gyro(&robot);
// Our networking server
ArServerBase server;
// Set up our simpleOpener, used to set up the networking server
ArServerSimpleOpener simpleOpener(&parser);
// the laser connector
ArLaserConnector laserConnector(&parser, &robot, &robotConnector);
// Tell the laser connector to always connect the first laser since
// this program always requires a laser.
parser.addDefaultArgument("-connectLaser");
// Load default arguments for this computer (from /etc/Aria.args, environment
// variables, and other places)
parser.loadDefaultArguments();
// Parse arguments
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
logOptions(argv[0]);
Aria::exit(1);
}
// This causes Aria::exit(9) to be called if the robot unexpectedly
// disconnects
ArGlobalFunctor1<int> shutdownFunctor(&Aria::exit, 9);
robot.addDisconnectOnErrorCB(&shutdownFunctor);
// Create an ArSonarDevice object (ArRangeDevice subclass) and
// connect it to the robot.
ArSonarDevice sonarDev;
robot.addRangeDevice(&sonarDev);
// This object will allow robot's movement parameters to be changed through
// a Robot Configuration section in the ArConfig global configuration facility.
ArRobotConfig robotConfig(&robot);
// Include gyro configuration options in the robot configuration section.
robotConfig.addAnalogGyro(&gyro);
// Start the robot thread.
robot.runAsync(true);
// connect the laser(s) if it was requested, this adds them to the
// robot too, and starts them running in their own threads
if (!laserConnector.connectLasers())
{
ArLog::log(ArLog::Normal, "Could not connect to all lasers... exiting\n");
Aria::exit(2);
}
// find the laser we should use for localization and/or mapping,
// which will be the first laser
robot.lock();
ArLaser *firstLaser = robot.findLaser(1);
if (firstLaser == NULL || !firstLaser->isConnected())
{
ArLog::log(ArLog::Normal, "Did not have laser 1 or it is not connected, cannot start localization and/or mapping... exiting");
Aria::exit(2);
}
robot.unlock();
/* Create and set up map object */
// Set up the map object, this will look for files in the examples
// directory (unless the file name starts with a /, \, or .
// You can take out the 'fileDir' argument to look in the program's current directory
// instead.
// When a configuration file is loaded into ArConfig later, if it specifies a
// map file, then that file will be loaded as the map.
ArMap map(fileDir);
// set it up to ignore empty file names (otherwise if a configuration omits
// the map file, the whole configuration change will fail)
map.setIgnoreEmptyFileName(true);
// ignore the case, so that if someone is using MobileEyes or
// MobilePlanner from Windows and changes the case on a map name,
// it will still work.
map.setIgnoreCase(true);
/* Create localization and path planning threads */
ArPathPlanningTask pathTask(&robot, &sonarDev, &map);
ArLog::log(ArLog::Normal, "Creating laser localization task");
// Laser Monte-Carlo Localization
ArLocalizationTask locTask(&robot, firstLaser, &map);
// Set some options on each laser that the laser connector
// connected to.
std::map<int, ArLaser *>::iterator laserIt;
for (laserIt = robot.getLaserMap()->begin();
laserIt != robot.getLaserMap()->end();
laserIt++)
{
int laserNum = (*laserIt).first;
ArLaser *laser = (*laserIt).second;
// Skip lasers that aren't connected
if(!laser->isConnected())
continue;
// add the disconnectOnError CB to shut things down if the laser
// connection is lost
laser->addDisconnectOnErrorCB(&shutdownFunctor);
// set the number of cumulative readings the laser will take
laser->setCumulativeBufferSize(200);
// add the lasers to the path planning task
pathTask.addRangeDevice(laser, ArPathPlanningTask::BOTH);
// set the cumulative clean offset (so that they don't all fire at once)
laser->setCumulativeCleanOffset(laserNum * 100);
// reset the cumulative clean time (to make the new offset take effect)
laser->resetLastCumulativeCleanTime();
// Add the packet count to the Aria info strings (It will be included in
// MobileEyes custom details so you can monitor whether the laser data is
// being received correctly)
std::string laserPacketCountName;
laserPacketCountName = laser->getName();
laserPacketCountName += " Packet Count";
Aria::getInfoGroup()->addStringInt(
laserPacketCountName.c_str(), 10,
new ArRetFunctorC<int, ArLaser>(laser,
&ArLaser::getReadingCount));
}
// Used for optional multirobot features (see below) (TODO move to multirobot
// example?)
ArClientSwitchManager clientSwitch(&server, &parser);
/* Start the server */
// Open the networking server
if (!simpleOpener.open(&server, fileDir, 240))
{
ArLog::log(ArLog::Normal, "Error: Could not open server.");
exit(2);
}
/* Create various services that provide network access to clients (such as
* MobileEyes), as well as add various additional features to ARNL */
// ARNL can optionally get information about the positions of other robots from a
// "central server" (see central server example program), if command
// line options specifying the address of the central server was given.
// If there is no central server, then the address of each other robot
// can instead be given in the configuration, and the multirobot systems
// will connect to each robot (or "peer") individually.
// TODO move this to multirobot example?
bool usingCentralServer = false;
if(clientSwitch.getCentralServerHostName() != NULL)
usingCentralServer = true;
// if we're using the central server then we want to create the
// multiRobot central classes
if (usingCentralServer)
{
// Make the handler for multi robot information (this sends the
// information to the central server)
//ArServerHandlerMultiRobot *handlerMultiRobot =
new ArServerHandlerMultiRobot(&server, &robot,
&pathTask,
&locTask, &map);
// Normally each robot, and the central server, must all have
// the same map name for the central server to share robot
// information. (i.e. they are operating in the same space).
// This changes the map name that ArServerHandlerMutliRobot
// reports to the central server, in case you want this individual
// robot to load a different map file name, but still report
// the common map file to the central server.
//handlerMultiRobot->overrideMapName("central.map");
// the range device that gets the multi robot information from
// the central server and presents it as virtual range readings
// to ARNL
ArMultiRobotRangeDevice *multiRobotRangeDevice = new ArMultiRobotRangeDevice(&server);
robot.addRangeDevice(multiRobotRangeDevice);
pathTask.addRangeDevice(multiRobotRangeDevice,
ArPathPlanningTask::BOTH);
// Set up options for drawing multirobot information in MobileEyes.
multiRobotRangeDevice->setCurrentDrawingData(
new ArDrawingData("polyDots", ArColor(125, 125, 0),
100, 73, 1000), true);
multiRobotRangeDevice->setCumulativeDrawingData(
new ArDrawingData("polyDots", ArColor(125, 0, 125),
100, 72, 1000), true);
// This sets up the localization to use the known poses of other robots
// for its localization in cases where numerous robots crowd out the map.
locTask.setMultiRobotCallback(multiRobotRangeDevice->getOtherRobotsCB());
}
// if we're not using a central server then create the multirobot peer classes
else
{
// set the path planning so it uses the explicit collision range for how far its planning
pathTask.setUseCollisionRangeForPlanningFlag(true);
// make our thing that gathers information from the other servers
ArServerHandlerMultiRobotPeer *multiRobotPeer = NULL;
ArMultiRobotPeerRangeDevice *multiRobotPeerRangeDevice = NULL;
multiRobotPeerRangeDevice = new ArMultiRobotPeerRangeDevice(&map);
// make our thing that sends information to the other servers
multiRobotPeer = new ArServerHandlerMultiRobotPeer(&server, &robot,
&pathTask, &locTask);
// hook the two together so they both know what priority this robot is
multiRobotPeer->setNewPrecedenceCallback(
multiRobotPeerRangeDevice->getSetPrecedenceCallback());
// hook the two together so they both know what priority this
// robot's fingerprint is
multiRobotPeer->setNewFingerprintCallback(
multiRobotPeerRangeDevice->getSetFingerprintCallback());
// hook the two together so that the range device can call on the
// server handler to change its fingerprint
multiRobotPeerRangeDevice->setChangeFingerprintCB(
multiRobotPeer->getChangeFingerprintCB());
// then add the robot to the places it needs to be
robot.addRangeDevice(multiRobotPeerRangeDevice);
pathTask.addRangeDevice(multiRobotPeerRangeDevice,
ArPathPlanningTask::BOTH);
// Set the range device so that we can see the information its using
// to avoid, you can comment these out in order to not see them
multiRobotPeerRangeDevice->setCurrentDrawingData(
new ArDrawingData("polyDots", ArColor(125, 125, 0),
100, 72, 1000), true);
multiRobotPeerRangeDevice->setCumulativeDrawingData(
new ArDrawingData("polyDots", ArColor(125, 0, 125),
100, 72, 1000), true);
// This sets up the localization to use the known poses of other robots
// for its localization in cases where numerous robots crowd out the map.
locTask.setMultiRobotCallback(
multiRobotPeerRangeDevice->getOtherRobotsCB());
}
/* Add additional range devices to the robot and path planning task (so it
avoids obstacles detected by these devices) */
// Add IR range device to robot and path planning task (so it avoids obstacles
// detected by this device)
robot.lock();
ArIRs irs;
robot.addRangeDevice(&irs);
pathTask.addRangeDevice(&irs, ArPathPlanningTask::CURRENT);
// Add bumpers range device to robot and path planning task (so it avoids obstacles
// detected by this device)
ArBumpers bumpers;
robot.addRangeDevice(&bumpers);
pathTask.addRangeDevice(&bumpers, ArPathPlanningTask::CURRENT);
// Add range device which uses forbidden regions given in the map to give virtual
// range device readings to ARNL. (so it avoids obstacles
// detected by this device)
ArForbiddenRangeDevice forbidden(&map);
robot.addRangeDevice(&forbidden);
pathTask.addRangeDevice(&forbidden, ArPathPlanningTask::CURRENT);
robot.unlock();
// Action to slow down robot when localization score drops but not lost.
ArActionSlowDownWhenNotCertain actionSlowDown(&locTask);
pathTask.getPathPlanActionGroup()->addAction(&actionSlowDown, 140);
// Action to stop the robot when localization is "lost" (score too low)
ArActionLost actionLostPath(&locTask, &pathTask);
pathTask.getPathPlanActionGroup()->addAction(&actionLostPath, 150);
// Arnl uses this object when it must replan its path because its
// path is completely blocked. It will use an older history of sensor
// readings to replan this new path. This should not be used with SONARNL
// since sonar readings are not accurate enough and may prevent the robot
// from planning through space that is actually clear.
ArGlobalReplanningRangeDevice replanDev(&pathTask);
// Service to provide drawings of data in the map display :
ArServerInfoDrawings drawings(&server);
drawings.addRobotsRangeDevices(&robot);
drawings.addRangeDevice(&replanDev);
/* Draw a box around the local path planning area use this
(You can enable this particular drawing from custom commands
which is set up down below in ArServerInfoPath) */
ArDrawingData drawingDataP("polyLine", ArColor(200,200,200), 1, 75);
ArFunctor2C<ArPathPlanningTask, ArServerClient *, ArNetPacket *>
drawingFunctorP(&pathTask, &ArPathPlanningTask::drawSearchRectangle);
drawings.addDrawing(&drawingDataP, "Local Plan Area", &drawingFunctorP);
/* Show the sample points used by MCL */
ArDrawingData drawingDataL("polyDots", ArColor(0,255,0), 100, 75);
ArFunctor2C<ArLocalizationTask, ArServerClient *, ArNetPacket *>
drawingFunctorL(&locTask, &ArLocalizationTask::drawRangePoints);
drawings.addDrawing(&drawingDataL, "Localization Points", &drawingFunctorL);
// "Custom" commands. You can add your own custom commands here, they will
// be available in MobileEyes' custom commands (enable in the toolbar or
// access through Robot Tools)
ArServerHandlerCommands commands(&server);
// These provide various kinds of information to the client:
ArServerInfoRobot serverInfoRobot(&server, &robot);
ArServerInfoSensor serverInfoSensor(&server, &robot);
ArServerInfoPath serverInfoPath(&server, &robot, &pathTask);
serverInfoPath.addSearchRectangleDrawing(&drawings);
serverInfoPath.addControlCommands(&commands);
// Provides localization info and allows the client (MobileEyes) to relocalize at a given
// pose:
ArServerInfoLocalization serverInfoLocalization(&server, &robot, &locTask);
ArServerHandlerLocalization serverLocHandler(&server, &robot, &locTask);
// If you're using MobileSim, ArServerHandlerLocalization sends it a command
// to move the robot's true pose if you manually do a localization through
// MobileEyes. To disable that behavior, use this constructor call instead:
// ArServerHandlerLocalization serverLocHandler(&server, &robot, true, false);
// The fifth argument determines whether to send the command to MobileSim.
// Provide the map to the client (and related controls):
ArServerHandlerMap serverMap(&server, &map);
// These objects add some simple (custom) commands to 'commands' for testing and debugging:
ArServerSimpleComUC uCCommands(&commands, &robot); // Send any command to the microcontroller
ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // configure logging
ArServerSimpleComLogRobotConfig configCommands(&commands, &robot); // trigger logging of the robot config parameters
// ArServerSimpleServerCommands serverCommands(&commands, &server); // monitor networking behavior (track packets sent etc.)
// service that allows the client to monitor the communication link status
// between the robot and the client.
//
ArServerHandlerCommMonitor handlerCommMonitor(&server);
// service that allows client to change configuration parameters in ArConfig
ArServerHandlerConfig handlerConfig(&server, Aria::getConfig(),
Arnl::getTypicalDefaultParamFileName(),
Aria::getDirectory());
/* Set up the possible modes for remote control from a client such as
* MobileEyes:
*/
// Mode To go to a goal or other specific point:
ArServerModeGoto modeGoto(&server, &robot, &pathTask, &map,
locTask.getRobotHome(),
locTask.getRobotHomeCallback());
// Mode To stop and remain stopped:
ArServerModeStop modeStop(&server, &robot);
// Cause the sonar to turn off automatically
// when the robot is stopped, and turn it back on when commands to move
// are sent. (Note, if using SONARNL to localize, then don't do this
// since localization may get lost)
ArSonarAutoDisabler sonarAutoDisabler(&robot);
// Teleoperation modes To drive by keyboard, joystick, etc:
ArServerModeRatioDrive modeRatioDrive(&server, &robot);
// ArServerModeDrive modeDrive(&server, &robot); // Older mode for compatability
// Prevent normal teleoperation driving if localization is lost using
// a high-priority action, which enables itself when the particular mode is
// active.
// (You have to enter unsafe drive mode to drive when lost.)
ArActionLost actionLostRatioDrive(&locTask, &pathTask, &modeRatioDrive);
modeRatioDrive.getActionGroup()->addAction(&actionLostRatioDrive, 110);
// Add drive mode section to the configuration, and also some custom (simple) commands:
modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings");
modeRatioDrive.addControlCommands(&commands);
// Wander mode (also prevent wandering if lost):
ArServerModeWander modeWander(&server, &robot);
ArActionLost actionLostWander(&locTask, &pathTask, &modeWander);
modeWander.getActionGroup()->addAction(&actionLostWander, 110);
// This provides a small table of interesting information for the client
// to display to the operator. You can add your own callbacks to show any
// data you want.
ArServerInfoStrings stringInfo(&server);
Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor());
// Provide a set of informational data (turn on in MobileEyes with
// View->Custom Details)
Aria::getInfoGroup()->addStringInt(
"Motor Packet Count", 10,
new ArConstRetFunctorC<int, ArRobot>(&robot,
&ArRobot::getMotorPacCount));
Aria::getInfoGroup()->addStringDouble(
"Laser Localization Score", 8,
new ArRetFunctorC<double, ArLocalizationTask>(
&locTask, &ArLocalizationTask::getLocalizationScore),
"%.03f");
Aria::getInfoGroup()->addStringInt(
"Laser Loc Num Samples", 8,
new ArRetFunctorC<int, ArLocalizationTask>(
&locTask, &ArLocalizationTask::getCurrentNumSamples),
"%4d");
// Display gyro status if gyro is enabled and is being handled by the firmware (gyro types 2, 3, or 4).
// (If the firmware detects an error communicating with the gyro or IMU it
// returns a flag, and stops using it.)
// (This gyro type parameter, and fault flag, are only in ARCOS, not Seekur firmware)
if(robot.getOrigRobotConfig() && robot.getOrigRobotConfig()->getGyroType() > 1)
{
Aria::getInfoGroup()->addStringString(
"Gyro/IMU Status", 10,
new ArGlobalRetFunctor1<const char*, ArRobot*>(&getGyroStatusString, &robot)
);
}
// Setup the dock if there is a docking system on board.
ArServerModeDock *modeDock = NULL;
modeDock = ArServerModeDock::createDock(&server, &robot, &locTask,
&pathTask);
if (modeDock != NULL)
{
modeDock->checkDock();
modeDock->addAsDefaultMode();
modeDock->addToConfig(Aria::getConfig());
modeDock->addControlCommands(&commands);
}
// Make Stop mode the default (If current mode deactivates without entering
// a new mode, then Stop Mode will be selected)
modeStop.addAsDefaultMode();
// TODO move up near where stop mode is created?
/* Services that allow the client to initiate scanning with the laser to
create maps in Mapper3 (So not possible with SONARNL): */
ArServerHandlerMapping handlerMapping(&server, &robot, firstLaser,
fileDir, "", true);
// make laser localization stop while mapping
handlerMapping.addMappingStartCallback(
new ArFunctor1C<ArLocalizationTask, bool>
(&locTask, &ArLocalizationTask::setIdleFlag, true));
// and then make it start again when we're doine
handlerMapping.addMappingEndCallback(
new ArFunctor1C<ArLocalizationTask, bool>
(&locTask, &ArLocalizationTask::setIdleFlag, false));
// Make it so our "lost" actions don't stop us while mapping
handlerMapping.addMappingStartCallback(actionLostPath.getDisableCB());
handlerMapping.addMappingStartCallback(actionLostRatioDrive.getDisableCB());
handlerMapping.addMappingStartCallback(actionLostWander.getDisableCB());
// And then let them make us stop as usual when done mapping
handlerMapping.addMappingEndCallback(actionLostPath.getEnableCB());
handlerMapping.addMappingEndCallback(actionLostRatioDrive.getEnableCB());
handlerMapping.addMappingEndCallback(actionLostWander.getEnableCB());
// don't let forbidden lines show up as obstacles while mapping
// (they'll just interfere with driving while mapping, and localization is off anyway)
handlerMapping.addMappingStartCallback(forbidden.getDisableCB());
// let forbidden lines show up as obstacles again as usual after mapping
handlerMapping.addMappingEndCallback(forbidden.getEnableCB());
/*
// If we are on a simulator, move the robot back to its starting position,
// and reset its odometry.
// This will allow localizeRobotAtHomeBlocking() below will (probably) work (it
// tries current odometry (which will be 0,0,0) and all the map
// home points.
// (Ignored by a real robot)
//robot.com(ArCommands::SIM_RESET);
*/
// create a pose storage class, this will let the program keep track
// of where the robot is between runs... after we try and restore
// from this file it will start saving the robot's pose into the
// file
ArPoseStorage poseStorage(&robot);
/// if we could restore the pose from then set the sim there (this
/// won't do anything to the real robot)... if we couldn't restore
/// the pose then just reset the position of the robot (which again
/// won't do anything to the real robot)
if (poseStorage.restorePose("robotPose"))
serverLocHandler.setSimPose(robot.getPose());
else
robot.com(ArCommands::SIM_RESET);
/* File transfer services: */
#ifdef WIN32
// Not implemented for Windows yet.
ArLog::log(ArLog::Normal, "Note, file upload/download services are not implemented for Windows; not enabling them.");
#else
// This block will allow you to set up where you get and put files
// to/from, just comment them out if you don't want this to happen
// /*
ArServerFileLister fileLister(&server, fileDir);
ArServerFileToClient fileToClient(&server, fileDir);
ArServerFileFromClient fileFromClient(&server, fileDir, "/tmp");
ArServerDeleteFileOnServer deleteFileOnServer(&server, fileDir);
// */
#endif
/* Video image streaming, and camera controls (Requires SAVserver or ACTS) */
// Forward any 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 seperate 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);
// make a camera to use in case we have video. the camera collection collects
// multiple ptz cameras
ArPTZ *camera = NULL;
ArServerHandlerCamera *handlerCamera = NULL;
ArCameraCollection *cameraCollection = NULL;
// if we have video then set up a camera
if (videoForwarder.isForwardingVideo())
{
cameraCollection = new ArCameraCollection();
cameraCollection->addCamera("Cam1", "PTZ", "Camera", "PTZ");
videoForwarder.setCameraName("Cam1");
videoForwarder.addToCameraCollection(*cameraCollection);
camera = new ArVCC4(&robot); //, invertedCamera, ArVCC4::COMM_UNKNOWN, true, true);
// To use an RVision SEE camera instead:
// camera = new ArRVisionPTZ(&robot);
camera->init();
handlerCamera = new ArServerHandlerCamera("Cam1",
&server,
&robot,
camera,
cameraCollection);
pathTask.addGoalFinishedCB(
new ArFunctorC<ArServerHandlerCamera>(
handlerCamera,
&ArServerHandlerCamera::cameraModeLookAtGoalClearGoal));
}
// After all of the cameras / videos have been created and added to the collection,
// then start the collection server.
//
if (cameraCollection != NULL) {
new ArServerHandlerCameraCollection(&server, cameraCollection);
}
/* Load configuration values, map, and begin! */
// When parsing the configuration file, also look at the program's command line options
// from the command-line argument parser as well as the configuration file.
// (So you can use any argument on the command line, namely -map.)
Aria::getConfig()->useArgumentParser(&parser);
puts("xxx");puts("aaa"); fflush(stdout);
// Read in parameter files.
ArLog::log(ArLog::Normal, "Loading config file %s into ArConfig (base directory %s)...", Arnl::getTypicalParamFileName(), Aria::getConfig()->getBaseDirectory());
if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
{
ArLog::log(ArLog::Normal, "Trouble loading configuration file, exiting");
Aria::exit(5);
}
// Warn about unknown params.
if (!simpleOpener.checkAndLog() || !parser.checkHelpAndWarnUnparsed())
{
logOptions(argv[0]);
Aria::exit(6);
}
// Warn if there is no map
if (map.getFileName() == NULL || strlen(map.getFileName()) <= 0)
{
ArLog::log(ArLog::Normal, "");
ArLog::log(ArLog::Normal, "### No map file is set up, you can make a map with the following procedure");
ArLog::log(ArLog::Normal, " 0) You can find this information in README.txt or docs/Mapping.txt");
ArLog::log(ArLog::Normal, " 1) Connect to this server with MobileEyes");
ArLog::log(ArLog::Normal, " 2) Go to Tools->Map Creation->Start Scan");
ArLog::log(ArLog::Normal, " 3) Give the map a name and hit okay");
ArLog::log(ArLog::Normal, " 4) Drive the robot around your space (see docs/Mapping.txt");
ArLog::log(ArLog::Normal, " 5) Go to Tools->Map Creation->Stop Scan");
ArLog::log(ArLog::Normal, " 6) Start up Mapper3");
ArLog::log(ArLog::Normal, " 7) Go to File->Open on Robot");
ArLog::log(ArLog::Normal, " 8) Select the .2d you created");
ArLog::log(ArLog::Normal, " 9) Create a .map");
ArLog::log(ArLog::Normal, " 10) Go to File->Save on Robot");
ArLog::log(ArLog::Normal, " 11) In MobileEyes, go to Tools->Robot Config");
ArLog::log(ArLog::Normal, " 12) Choose the Files section");
ArLog::log(ArLog::Normal, " 13) Enter the path and name of your new .map file for the value of the Map parameter.");
ArLog::log(ArLog::Normal, " 14) Press OK and your new map should become the map used");
ArLog::log(ArLog::Normal, "");
}
// Print a log message notifying user of the directory for map files
ArLog::log(ArLog::Normal, "");
ArLog::log(ArLog::Normal,
"Directory for maps and file serving: %s", fileDir);
ArLog::log(ArLog::Normal, "See the ARNL README.txt for more information");
ArLog::log(ArLog::Normal, "");
// Do an initial localization of the robot. It tries all the home points
// in the map, as well as the robot's current odometric position, as possible
// places the robot is likely to be at startup. If successful, it will
// also save the position it found to be the best localized position as the
// "Home" position, which can be obtained from the localization task (and is
// used by the "Go to home" network request).
locTask.localizeRobotAtHomeBlocking();
// Let the client switch manager (for multirobot) spin off into its own thread
// TODO move to multirobot example?
clientSwitch.runAsync();
// Start the networking server's thread
server.runAsync();
// Add a key handler so that you can exit by pressing
// escape. Note that this key handler, however, prevents this program from
// running in the background (e.g. as a system daemon or run from
// the shell with "&") -- it will lock up trying to read the keys;
// remove this if you wish to be able to run this program in the background.
ArKeyHandler *keyHandler;
if ((keyHandler = Aria::getKeyHandler()) == NULL)
{
keyHandler = new ArKeyHandler;
Aria::setKeyHandler(keyHandler);
robot.lock();
robot.attachKeyHandler(keyHandler);
robot.unlock();
puts("Server running. To exit, press escape.");
}
ArnlASyncTaskExample asyncTaskExample(&pathTask, &robot, &modeGoto, &parser);
// Enable the motors and wait until the robot exits (disconnection, etc.) or this program is
// canceled.
robot.enableMotors();
robot.waitForRunExit();
Aria::exit(0);
}
int main(int argc, char **argv)
{
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobot robot;
ArAnalogGyro gyro(&robot);
ArSonarDevice sonar;
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, "gotoActionExample: Could not connect to the robot.");
if(parser.checkHelpAndWarnUnparsed())
{
// -help not given
Aria::logOptions();
Aria::exit(1);
}
}
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
}
ArLog::log(ArLog::Normal, "gotoActionExample: Connected to robot.");
robot.addRangeDevice(&sonar);
robot.runAsync(true);
// Make a key handler, so that escape will shut down the program
// cleanly
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit\n");
// Collision avoidance actions at higher priority
ArActionLimiterForwards limiterAction("speed limiter near", 300, 600, 250);
ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100, 400);
ArActionLimiterTableSensor tableLimiterAction;
robot.addAction(&tableLimiterAction, 100);
robot.addAction(&limiterAction, 95);
robot.addAction(&limiterFarAction, 90);
// Goto action at lower priority
ArActionGoto gotoPoseAction("goto");
robot.addAction(&gotoPoseAction, 50);
// Stop action at lower priority, so the robot stops if it has no goal
ArActionStop stopAction("stop");
robot.addAction(&stopAction, 40);
// turn on the motors, turn off amigobot sounds
robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);
const int duration = 30000; //msec
ArLog::log(ArLog::Normal, "Going to four goals in turn for %d seconds, then cancelling goal and exiting.", duration/1000);
bool first = true;
int goalNum = 0;
ArTime start;
start.setToNow();
while (Aria::getRunning())
{
robot.lock();
// Choose a new goal if this is the first loop iteration, or if we
// achieved the previous goal.
if (first || gotoPoseAction.haveAchievedGoal())
{
first = false;
goalNum++;
if (goalNum > 4)
goalNum = 1; // start again at goal #1
// set our positions for the different goals
if (goalNum == 1)
gotoPoseAction.setGoal(ArPose(2500, 0));
else if (goalNum == 2)
gotoPoseAction.setGoal(ArPose(2500, 2500));
else if (goalNum == 3)
gotoPoseAction.setGoal(ArPose(0, 2500));
else if (goalNum == 4)
gotoPoseAction.setGoal(ArPose(0, 0));
ArLog::log(ArLog::Normal, "Going to next goal at %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if(start.mSecSince() >= duration) {
ArLog::log(ArLog::Normal, "%d seconds have elapsed. Cancelling current goal, waiting 3 seconds, and exiting.", duration/1000);
gotoPoseAction.cancelGoal();
robot.unlock();
ArUtil::sleep(3000);
break;
}
robot.unlock();
ArUtil::sleep(100);
}
// Robot disconnected or time elapsed, shut down
Aria::exit(0);
return 0;
}
int main(void)
{
ArSerialConnection con;
ArRobot robot;
int ret;
std::string str;
ArActionLimiterForwards limiter("speed limiter near", 225, 600, 250);
ArActionLimiterForwards limiterFar("speed limiter far", 225, 1100, 400);
ArActionTableSensorLimiter tableLimiter;
ArActionLimiterBackwards backwardsLimiter;
ArActionConstantVelocity stop("stop", 0);
ArSonarDevice sonar;
ArACTS_1_2 acts;
ArPTZ *ptz;
ptz = new ArVCC4(&robot, true);
ArGripper gripper(&robot);
Acquire acq(&acts, &gripper);
DriveTo driveTo(&acts, &gripper, ptz);
DropOff dropOff(&acts, &gripper, ptz);
PickUp pickUp(&acts, &gripper, ptz);
TakeBlockToWall takeBlock(&robot, &gripper, ptz, &acq, &driveTo, &pickUp,
&dropOff, &tableLimiter);
if (!acts.openPort(&robot))
{
printf("Could not connect to acts, exiting\n");
exit(0);
}
Aria::init();
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;
}
ptz->init();
ArUtil::sleep(8000);
printf("### 2222\n");
ptz->panTilt(0, -40);
printf("### whee\n");
ArUtil::sleep(8000);
robot.setAbsoluteMaxTransVel(400);
robot.setStateReflectionRefreshTime(250);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
ArUtil::sleep(200);
robot.addAction(&tableLimiter, 100);
robot.addAction(&limiter, 99);
robot.addAction(&limiterFar, 98);
robot.addAction(&backwardsLimiter, 97);
robot.addAction(&acq, 77);
robot.addAction(&driveTo, 76);
robot.addAction(&pickUp, 75);
robot.addAction(&dropOff, 74);
robot.addAction(&stop, 30);
robot.run(true);
Aria::shutdown();
return 0;
}
int main (int argc, char** argv) {
Aria::init();
ArRobot robot;
ArSonarDevice sonar;
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())
{
Aria::logOptions();
Aria::exit(1);
return 1;
}
}
ArSonarDevice sonarDev;
ArPose* poseList = readPostitions("positions.txt");
robot.runAsync(true);
robot.enableMotors();
robot.moveTo(ArPose(0,0,0));
robot.comInt(ArCommands::ENABLE, 1);
robot.addRangeDevice(&sonarDev);
ArActionGoto gotoPoseAction("goto", ArPose(0, 0, 0), 200);
ArActionAvoidFront avoidFront("avoid front");
ArActionStallRecover stallRecover("stall recover");
robot.addAction(&gotoPoseAction, 50);
robot.addAction(&avoidFront, 60);
robot.moveTo(ArPose(0,0,0));
int length = ARRAY_SIZE(poseList);
cout<<"do dai"<<length;
ArServerBase server;
ArServerSimpleOpener simpleOpener(&parser);
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);
}
ArServerInfoRobot serverInfo(&server, &robot);
GotoGoal gotoGoal(&robot, &sonar, &server, &serverInfo);
gotoGoal.init(argc, argv);
float angle = 0;
VideoCapture cap;
cap.open(0);
Rect trackWindow;
//var check find ball
bool checkObject = false;
int hsize = 16;
namedWindow( "threshold", 0 );
namedWindow( "trackbar", 0 );
namedWindow( "Histogram", 0 );
namedWindow( "main", 0 );
createTrackbar( "Vmin", "trackbar", &vmin, 256, 0 );
createTrackbar( "Vmax", "trackbar", &vmax, 256, 0 );
createTrackbar( "Smin", "trackbar", &smin, 256, 0 );
CascadeClassifier c;
c.load("cascade.xml");
Mat frame, hsv, hue, mask, hist, histimg = Mat::zeros(200, 320, CV_8UC3), backproj;
float vel = 0;
int i = 0;
while(1)
{
cap >> frame;
if( frame.empty() ){
cout<<"error camera"<<endl;
break;
}
frame.copyTo(image);
cvtColor(image, hsv, COLOR_BGR2HSV);
int _vmin = vmin, _vmax = vmax;
inRange(hsv, Scalar(0, smin, MIN(_vmin,_vmax)), Scalar(180, 256, MAX(_vmin, _vmax)), mask);
gotoPoseAction.setGoal(poseList[i]);
while (!gotoPoseAction.haveAchievedGoal())
{
ArLog::log(ArLog::Normal, "goal(%.2f, %0.2f) x = %.2f, y = %.2f", poseList[i].getX(), poseList[i].getY(), robot.getX(), robot.getY());
// if (!checkObject)
checkObject = detect(frame, c);
if (checkObject)
cout <<"Phat hien doi tuong"<<endl;
else
cout <<"Khong phat hien doi tuong"<<endl;
if (checkObject) {
if(trackObject(hsv, mask)) {
float d = distance();
if (d < 250) {
gotoGoal.move(-200);
} else if ( d >= 250 && d <= 300) {
gotoGoal.stop();
}
else {
vel = d * 0.7;
vel = (int) (vel/50) * 50;
if(vel > 200) {
vel = 200;
gotoGoal.setVel(vel);
}
angle = determindRotate();
cout <<"khoang cach: "<<d<<"\tGoc quay: "<<angle<<"\t van toc = "<<vel<<endl;
if (angle != 0) {
gotoGoal.stop();
gotoGoal.rotate(angle);
}
}
}
}
imshow("main", image);
imshow( "threshold", mask );
imshow( "Histogram", histimg );
}
i++;
}
ArUtil::sleep(2000);
Aria::shutdown();
}
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(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)
{
Aria::init();
// parse our args and make sure they were all accounted for
ArSimpleConnector connector(&argc, argv);
ArRobot robot;
// the laser. ArActionTriangleDriveTo will use this laser object since it is
// named "laser" when added to the ArRobot.
ArSick sick;
if (!connector.parseArgs() || argc > 1)
{
connector.logOptions();
exit(1);
}
// a key handler so we can do our key handling
ArKeyHandler keyHandler;
// let the global aria stuff know about it
Aria::setKeyHandler(&keyHandler);
// toss it on the robot
robot.attachKeyHandler(&keyHandler);
// add the laser to the robot
robot.addRangeDevice(&sick);
ArSonarDevice sonar;
robot.addRangeDevice(&sonar);
ArActionTriangleDriveTo triangleDriveTo;
ArFunctorC<ArActionTriangleDriveTo> lineGoCB(&triangleDriveTo,
&ArActionTriangleDriveTo::activate);
keyHandler.addKeyHandler('g', &lineGoCB);
keyHandler.addKeyHandler('G', &lineGoCB);
ArFunctorC<ArActionTriangleDriveTo> lineStopCB(&triangleDriveTo,
&ArActionTriangleDriveTo::deactivate);
keyHandler.addKeyHandler('s', &lineStopCB);
keyHandler.addKeyHandler('S', &lineStopCB);
ArActionLimiterForwards limiter("limiter", 150, 0, 0, 1.3);
robot.addAction(&limiter, 70);
ArActionLimiterBackwards limiterBackwards;
robot.addAction(&limiterBackwards, 69);
robot.addAction(&triangleDriveTo, 60);
ArActionKeydrive keydrive;
robot.addAction(&keydrive, 55);
ArActionStop stopAction;
robot.addAction(&stopAction, 50);
// try to connect, if we fail exit
if (!connector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
robot.comInt(ArCommands::SONAR, 1);
robot.comInt(ArCommands::ENABLE, 1);
// start the robot running, true so that if we lose connection the run stops
robot.runAsync(true);
// now set up the laser
connector.setupLaser(&sick);
sick.runAsync();
if (!sick.blockingConnect())
{
printf("Could not connect to SICK laser... exiting\n");
Aria::shutdown();
return 1;
}
printf("If you press the 'g' key it'll go find a triangle, if you press 's' it'll stop.\n");
robot.waitForRunExit();
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);
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)
{
Aria::init();
//ArLog::init(ArLog::StdOut, ArLog::Verbose);
// robot
ArRobot robot;
/// our server
ArServerBase server;
// set up our parser
ArArgumentParser parser(&argc, argv);
// set up our simple connector
ArSimpleConnector simpleConnector(&parser);
// set up a gyro
ArAnalogGyro gyro(&robot);
// load the default arguments
parser.loadDefaultArguments();
// parse the command line... fail and print the help if the parsing fails
// or if the help was requested
if (!simpleConnector.parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
simpleConnector.logOptions();
exit(1);
}
if (!server.loadUserInfo("userServerTest.userInfo"))
{
printf("Could not load user info, exiting\n");
exit(1);
}
server.logUsers();
// first open the server up
if (!server.open(7272))
{
printf("Could not open server port\n");
exit(1);
}
// sonar, must be added to the robot
ArSonarDevice sonarDev;
// add the sonar to the robot
robot.addRangeDevice(&sonarDev);
ArIRs irs;
robot.addRangeDevice(&irs);
ArBumpers bumpers;
robot.addRangeDevice(&bumpers);
// a laser in case one is used
ArSick sick(361, 180);
// add the laser to the robot
robot.addRangeDevice(&sick);
// attach stuff to the server
ArServerInfoRobot serverInfoRobot(&server, &robot);
ArServerInfoSensor serverInfoSensor(&server, &robot);
ArServerInfoDrawings drawings(&server);
drawings.addRobotsRangeDevices(&robot);
// ways of moving the robot
ArServerModeStop modeStop(&server, &robot);
ArServerModeDrive modeDrive(&server, &robot);
ArServerModeRatioDrive modeRatioDrive(&server, &robot);
ArServerModeWander modeWander(&server, &robot);
modeStop.addAsDefaultMode();
modeStop.activate();
// set up the simple commands
ArServerHandlerCommands commands(&server);
// add the commands for the microcontroller
ArServerSimpleComUC uCCommands(&commands, &robot);
// add the commands for logging
ArServerSimpleComMovementLogging loggingCommands(&commands, &robot);
// add the commands for the gyro
ArServerSimpleComGyro gyroCommands(&commands, &robot, &gyro);
// add the commands to enable and disable safe driving to the simple commands
modeDrive.addControlCommands(&commands);
// Forward any video if we have some to forward.. this will forward
// from SAV or ACTS, you can find both on our website
// http://robots.activmedia.com, ACTS is for color tracking and is
// charged for but SAV just does software A/V transmitting and is
// free to all our customers... just run ACTS or SAV before you
// start this program and this class here will forward video from it
// to MobileEyes
ArHybridForwarderVideo videoForwarder(&server, "localhost", 7070);
// make a camera to use in case we have video
ArPTZ *camera = NULL;
ArServerHandlerCamera *handlerCamera = NULL;
// if we have video then set up a camera
if (videoForwarder.isForwardingVideo())
{
bool invertedCamera = false;
camera = new ArVCC4(&robot, invertedCamera,
ArVCC4::COMM_UNKNOWN, true, true);
camera->init();
handlerCamera = new ArServerHandlerCamera(&server, &robot, camera);
}
server.logCommandGroups();
server.logCommandGroupsToFile("userServerTest.commandGroups");
// now let it spin off in its own thread
server.runAsync();
// set up the robot for connecting
if (!simpleConnector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// set up the laser before handing it to the laser mode
simpleConnector.setupLaser(&sick);
robot.enableMotors();
// start the robot running, true so that if we lose connection the run stops
robot.runAsync(true);
sick.runAsync();
// connect the laser if it was requested
if (!simpleConnector.connectLaser(&sick))
{
printf("Could not connect to laser... exiting\n");
Aria::shutdown();
return 1;
}
robot.waitForRunExit();
// now exit
Aria::shutdown();
exit(0);
}
int __cdecl _tmain (int argc, char** argv)
{
//------------ I N I C I O 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);
ActionGos go(500, 350);
robot.addAction(&go, 48);
ActionTurns turn(400, 110);
robot.addAction(&turn, 49);
ActionTurns turn2(400, 110);
robot.addAction(&turn2, 49);
// 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);
// ============================ INICIO CONFIG COM =================================//
CSerial serial;
LONG lLastError = ERROR_SUCCESS;
// Trata de abrir el com seleccionado
lLastError = serial.Open(_T("COM3"),0,0,false);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Imposible abrir el COM"));
// Inicia el puerto serial (9600,8N1)
lLastError = serial.Setup(CSerial::EBaud9600,CSerial::EData8,CSerial::EParNone,CSerial::EStop1);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Imposible setear la config del COM"));
// Register only for the receive event
lLastError = serial.SetMask(CSerial::EEventBreak |
CSerial::EEventCTS |
CSerial::EEventDSR |
CSerial::EEventError |
CSerial::EEventRing |
CSerial::EEventRLSD |
CSerial::EEventRecv);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port event mask"));
// Use 'non-blocking' reads, because we don't know how many bytes
// will be received. This is normally the most convenient mode
// (and also the default mode for reading data).
lLastError = serial.SetupReadTimeouts(CSerial::EReadTimeoutNonblocking);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port read timeout."));
// ============================ FIN CONFIG COM =================================//
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();
// Create the sound queue.
ArSoundsQueue soundQueue;
// Run the sound queue in a new thread
soundQueue.runAsync();
std::vector<const char*> filenames;
filenames.push_back("sound-r2a.wav");
soundQueue.play(filenames[0]);
}
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)
{
robot.unlock();
//Valor para el while
bool fContinue = true;
// <<<<<<------------- 1 Parte Secuencia: BAJA BRAZO ------------->>>>>> //
lLastError = serial.Write("b");
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to send data"));
//-------------------------E S C U C H A C O M ----------------------------//
do
{
// Wait for an event
lLastError = serial.WaitEvent();
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to wait for a COM-port event."));
// Save event
const CSerial::EEvent eEvent = serial.GetEventType();
// Handle break event
if (eEvent & CSerial::EEventBreak)
{
printf("\n### BREAK received ###\n");
}
// Handle CTS event
if (eEvent & CSerial::EEventCTS)
{
printf("\n### Clear to send %s ###\n", serial.GetCTS()?"on":"off");
}
// Handle DSR event
if (eEvent & CSerial::EEventDSR)
{
printf("\n### Data set ready %s ###\n", serial.GetDSR()?"on":"off");
}
// Handle error event
if (eEvent & CSerial::EEventError)
{
printf("\n### ERROR: ");
switch (serial.GetError())
{
case CSerial::EErrorBreak: printf("Break condition"); break;
case CSerial::EErrorFrame: printf("Framing error"); break;
case CSerial::EErrorIOE: printf("IO device error"); break;
case CSerial::EErrorMode: printf("Unsupported mode"); break;
case CSerial::EErrorOverrun: printf("Buffer overrun"); break;
case CSerial::EErrorRxOver: printf("Input buffer overflow"); break;
case CSerial::EErrorParity: printf("Input parity error"); break;
case CSerial::EErrorTxFull: printf("Output buffer full"); break;
default: printf("Unknown"); break;
}
printf(" ###\n");
}
// Handle ring event
if (eEvent & CSerial::EEventRing)
{
printf("\n### RING ###\n");
}
// Handle RLSD/CD event
if (eEvent & CSerial::EEventRLSD)
{
printf("\n### RLSD/CD %s ###\n", serial.GetRLSD()?"on":"off");
}
// Handle data receive event
if (eEvent & CSerial::EEventRecv)
{
// Read data, until there is nothing left
DWORD dwBytesRead = 0;
char szBuffer[101];
do
{
// Lee datos del Puerto COM
lLastError = serial.Read(szBuffer,sizeof(szBuffer)-1,&dwBytesRead);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to read from COM-port."));
if (dwBytesRead > 0)
{
//Preseteo color
int color = 0;
// Finaliza el dato, asi que sea una string valida
szBuffer[dwBytesRead] = '\0';
// Display the data
printf("%s", szBuffer);
// <<<<<<----------- 2 Parte Secuencia: CIERRA GRIPPER ----------->>>>>> //
if (strchr(szBuffer,76))
{
lLastError = serial.Write("c");
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to send data"));
}
// <<<<<<------------- 3 Parte Secuencia: SUBE BRAZO ------------->>>>>> //
if (strchr(szBuffer,117))
{
lLastError = serial.Write("s");
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to send data"));
}
// <<<<<<------------- 4 Parte Secuencia: COLOR ------------->>>>>> //
if (strchr(szBuffer,72))
{
lLastError = serial.Write("C");
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to send data"));
}
// <<<<<<---------- 5.1 Parte Secuencia: COLOR ROJO---------->>>>>> //
if (strchr(szBuffer,82))
{
color = 1;
//salir del bucle
fContinue = false;
}
// <<<<<<---------- 5.2 Parte Secuencia: COLOR AZUL ---------->>>>>> //
if (strchr(szBuffer,66))
{
color = 2;
//salir del bucle
fContinue = false;
}
// <<<<<<---------- 5.3 Parte Secuencia: COLOR VERDE ---------->>>>>> //
if (strchr(szBuffer,71))
{
color = 3;
//salir del bucle
fContinue = false;
}
}
}
while (dwBytesRead == sizeof(szBuffer)-1);
}
}
while (fContinue);
// Close the port again
serial.Close();
robot.lock();
}
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();
//------------ F I N M A I N D E L P R O G R A M A D E L R O B O T-----------//
return 0;
}
int main(int argc, char** argv)
{
Aria::init();
ArArgumentParser argParser(&argc, argv);
ArSimpleConnector con(&argParser);
ArRobot robot;
ArSonarDevice sonar;
argParser.loadDefaultArguments();
if(!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
return 1;
}
/* - the action group for teleoperation actions: */
teleop = new ArActionGroup(&robot);
// don't hit any tables (if the robot has IR table sensors)
teleop->addAction(new ArActionLimiterTableSensor, 100);
// limiter for close obstacles
teleop->addAction(new ArActionLimiterForwards("speed limiter near",
300, 600, 250), 95);
// limiter for far away obstacles
teleop->addAction(new ArActionLimiterForwards("speed limiter far",
300, 1100, 400), 90);
// limiter so we don't bump things backwards
teleop->addAction(new ArActionLimiterBackwards, 85);
// the joydrive action (drive from joystick)
ArActionJoydrive joydriveAct("joydrive", 400, 15);
teleop->addAction(&joydriveAct, 50);
// the keydrive action (drive from keyboard)
teleop->addAction(new ArActionKeydrive, 45);
/* - the action group for wander actions: */
wander = new ArActionGroup(&robot);
// if we're stalled we want to back up and recover
wander->addAction(new ArActionStallRecover, 100);
// react to bumpers
wander->addAction(new ArActionBumpers, 75);
// turn to avoid things closer to us
wander->addAction(new ArActionAvoidFront("Avoid Front Near", 225, 0), 50);
// turn avoid things further away
wander->addAction(new ArActionAvoidFront, 45);
// keep moving
wander->addAction(new ArActionConstantVelocity("Constant Velocity", 400), 25);
/* - use key commands to switch modes, and use keyboard
* and joystick as inputs for teleoperation actions. */
// create key handler if Aria does not already have one
ArKeyHandler *keyHandler = Aria::getKeyHandler();
if (keyHandler == NULL)
{
keyHandler = new ArKeyHandler;
Aria::setKeyHandler(keyHandler);
robot.attachKeyHandler(keyHandler);
}
// set the callbacks
ArGlobalFunctor teleopCB(&teleopMode);
ArGlobalFunctor wanderCB(&wanderMode);
keyHandler->addKeyHandler('w', &wanderCB);
keyHandler->addKeyHandler('W', &wanderCB);
keyHandler->addKeyHandler('t', &teleopCB);
keyHandler->addKeyHandler('T', &teleopCB);
// if we don't have a joystick, let 'em know
if (!joydriveAct.joystickInited())
printf("Note: Do not have a joystick, only the arrow keys on the keyboard will work.\n");
// set the joystick so it won't do anything if the button isn't pressed
joydriveAct.setStopIfNoButtonPressed(false);
/* - connect to the robot, then enter teleoperation mode. */
robot.addRangeDevice(&sonar);
if(!con.connectRobot(&robot))
{
ArLog::log(ArLog::Terse, "actionGroupExample: Could not connect to the robot.");
Aria::shutdown();
return 1;
}
robot.enableMotors();
teleopMode();
robot.run(true);
Aria::shutdown();
return 0;
}
int main(int argc, char **argv)
{
struct settings robot_settings;
robot_settings.min_distance = 500;
robot_settings.max_velocity = 500;
robot_settings.tracking_factor = 1.0;
ArRobot robot;
Aria::init();
//laser
int ret; //Don't know what this variable is for
ArSick sick; // Laser scanner
ArSerialConnection laserCon; // Scanner connection
std::string str; // Standard output
// sonar, must be added to the robot
ArSonarDevice sonar;
// add the sonar to the robot
robot.addRangeDevice(&sonar);
// add the laser to the robot
robot.addRangeDevice(&sick);
ArArgumentParser argParser(&argc, argv);
ArSimpleConnector con(&argParser);
// 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.");
robot.runAsync(false);
///////////////////////////////
// Attempt to connect to SICK using another hard-coded USB connection
sick.setDeviceConnection(&laserCon);
if((ret=laserCon.open("/dev/ttyUSB1")) !=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!");
printf("\r\nRobot Entering default resting state.\r\nUse the following commands to run the robot.\r\n");
printf("r Run the robot\r\n");
printf("s Stop the robot\r\n");
printf("t Enter test mode (the robot will do everything except actually move.\r\n");
printf("w Save current data to files, and show a plot of what the robot sees.\r\n");
printf("e Edit robot control parameters\r\n");
printf("q Quit the program\r\n");
printf("\r\n");
printf("\r\n");
printf("NOTE: You must have GNUPLOT installed on your computer, and create a directory entitled 'scan_data' under your current directory in order to use this script. Failure to do so might make the computer crash, which would cause the robot to go on a mad killing spree! Not really, but seriously, go ahead and get GNUPLOT and create that subdirectory before using the 'w' option.\r\n\r\n");
/////////////////////////////////////
char user_command = 0;
char plot_option = 0;
int robot_state = REST;
tracking_object target;
tracking_object l_target;
ofstream fobjects;
ofstream ftarget;
ofstream flog;
ofstream fltarget;
fobjects.open("./scan_data/objects_new.txt");
ftarget.open("./scan_data/target_new.txt");
fltarget.open("./scan_data/ltarget_new.txt");
fobjects << "\r\n";
ftarget << "\r\n";
fltarget << "\r\n";
fobjects.close();
ftarget.close();
fltarget.close();
flog.open("robot_log.txt");
std::vector<tracking_object> obj_vector;
std::vector<tracking_object> new_vector;
float last_v = 0;
ArTime start;
char test_flag = 0;
char target_lost = 0;
while(user_command != 'q')
{
switch (user_command)
{
case STOP:
robot_state = REST;
printf("robot has entered resting mode\r\n");
break;
case RUN:
robot_state = TRACKING;
printf("robot has entered tracking mode\r\n");
break;
case QUIT:
robot_state = -1;
printf("exiting... goodbye.\r\n");
break;
case WRITE:
plot_option = WRITE;
break;
case NO_WRITE:
plot_option = NO_WRITE;
break;
case TEST:
if(test_flag == TEST)
{
test_flag = 0;
printf("Exiting test mode\r\n");
}
else
{
test_flag = TEST;
printf("Entering test mode\r\n");
robot.lock();
robot.setVel(0);
robot.unlock();
}
break;
case EDIT:
edit_settings(&robot_settings);
break;
default:
robot_state = robot_state;
}
unsigned int i = 0;
unsigned int num_objects = 0;
int to_ind = -1;
float min_distance = 999999.9;
float new_heading = 0;
system("mv ./scan_data/objects_new.txt ./scan_data/objects.txt");
system("mv ./scan_data/target_new.txt ./scan_data/target.txt");
system("mv ./scan_data/ltarget_new.txt ./scan_data/ltarget.txt");
fobjects.open("./scan_data/objects_new.txt");
ftarget.open("./scan_data/target_new.txt");
fltarget.open("./scan_data/ltarget_new.txt");
switch (robot_state)
{
case REST:
robot.lock();
robot.setVel(0);
robot.unlock();
obj_vector = run_sick_scan(&sick, 2, plot_option);
target_lost = 0;
num_objects = obj_vector.size();
if(num_objects > 0)
{
for(i = 0; i < num_objects; i++)
{
print_object_to_stream(obj_vector[i], fobjects);
}
}
break;
case TRACKING:
flog << "TRACKING\r\n";
obj_vector = get_moving_objects(&sick, 2*DT, 10, plot_option);
num_objects = obj_vector.size();
target_lost = 0;
if(min_range(&sick, 45, 135) > ROBOT_SAFETY_MARGIN)
{
if(num_objects)
{
for(i = 0; i < obj_vector.size();i++)
{
print_object_to_stream(obj_vector[i], fobjects);
if(obj_vector[i].vmag > 0.1)
{
if(obj_vector[i].distance < min_distance)
{
min_distance = obj_vector[i].distance;
target = obj_vector[i];
to_ind = i;
}
}
}
if(to_ind > -1)
{
int l_edge = target.l_edge;
int r_edge = target.r_edge;
float difference = 9999999.9;
// Do another scan to make sure the object is actually there.
new_vector = get_moving_objects(&sick, DT, 10, 0, 5, 175);
num_objects = new_vector.size();
to_ind = -1;
if(num_objects)
{
for(i = 0; i < num_objects;i++)
{
if(new_vector[i].vmag > 0.1)
{
if(r_diff(target, new_vector[i])<difference)
{
difference = r_diff(target, new_vector[i]);
if(difference < 500.0)
to_ind = i;
}
}
}
}
if(to_ind > -1)
{
new_heading = (-90 + new_vector[to_ind].degree);
if(test_flag != TEST)
{
robot.lock();
robot.setDeltaHeading(new_heading);
robot.unlock();
}
robot_state = FOLLOWING;
target = new_vector[to_ind];
target.degree = target.degree - new_heading;
print_object_to_stream(target, ftarget);
print_object_to_stream(target, flog);
flog << new_heading;
}
}
}
}
else
{
robot_state = TOO_CLOSE;
printf("I'm too close to an obstacle, and I'm getting claustrophobic! I'm going to slowly back up now.\r\n");
}
break;
case FOLLOWING:
{
flog << "FOLLOWING\r\n";
i = 0;
int to_ind = -1;
float obj_difference = 9999999.9;
int num_scans = 0;
if(min_range(&sick, 45, 135) > ROBOT_SAFETY_MARGIN)
{
while((num_scans < 3)&&(to_ind == -1))
{
obj_vector = run_sick_scan(&sick, 2, 0, 10, 350);
num_objects = obj_vector.size();
if(num_objects)
{
for(i = 0; i < num_objects;i++)
{
if(r_diff(target, obj_vector[i]) < obj_difference)
{
obj_difference = r_diff(target, obj_vector[i]);
if (obj_difference < 500.0)
to_ind = i;
}
}
for(i = 0; i < num_objects;i++)
{
if((int)i == to_ind)
{
print_object_to_stream(obj_vector[i], ftarget);
}
else
{
print_object_to_stream(obj_vector[i], fobjects);
}
}
}
num_scans++;
}
float new_vel = 0;
if(to_ind > -1)
{
printf("Tracking target\r\n");
flog << "Following target\t";
target_lost = 0;
target = obj_vector[to_ind];
new_vel = (obj_vector[to_ind].distance - robot_settings.min_distance)*robot_settings.tracking_factor;
if(new_vel < 0)
new_vel = 0;
if(new_vel > robot_settings.max_velocity)
new_vel = robot_settings.max_velocity;
new_heading = (-90 + target.degree)*0.25;
new_heading = get_safe_path(&sick, new_heading, target.distance);
if(test_flag != TEST)
{
robot.lock();
robot.setVel(new_vel);
robot.unlock();
last_v = new_vel;
robot.lock();
robot.setDeltaHeading(new_heading);
robot.unlock();
target.degree = target.degree - new_heading;
}
if(new_vel < 1.0)
{
robot_state = TRACKING;
printf("entering tracking mode\r\n");
flog<<"entering tracking mode\r\n";
}
}
else
{
if(target_lost)
{
l_target.distance = l_target.distance - last_v*(start.mSecSince()/1000.0);
int temp_max_v = min_range(&sick, 5, 175);
if(temp_max_v < last_v)
last_v = temp_max_v;
if(last_v > robot_settings.max_velocity)
last_v = robot_settings.max_velocity;
if(test_flag != TEST)
{
robot.lock();
robot.setVel(last_v);
robot.unlock();
new_heading = -90.0 + l_target.degree;
new_heading = get_safe_path(&sick, new_heading, l_target.distance);
l_target.degree = target.degree - new_heading;
robot.lock();
robot.setDeltaHeading(new_heading);
robot.unlock();
}
print_object_to_stream(l_target, fltarget);
}
else
{
target_lost = 1;
l_target = target;
}
printf("target lost\r\n");
flog << "target lost\r\n";
start.setToNow();
if(target.distance < ROBOT_SAFETY_MARGIN)
{
robot_state = TRACKING;
target_lost = 0;
}
target_lost = 1;
}
printf("Velocity: %f\t",last_v);
flog << "Velocity:\t";
flog << last_v;
printf("Heading: %f\t",new_heading);
flog << "\tHeading\t";
flog << new_heading;
flog << "\r\n";
printf("\r\n");
}
else
{
robot_state = TOO_CLOSE;
printf("I'm too close to an obstacle, and I'm getting claustrophobic! I'm going to slowly back up now.\r\n");
}
break;
}
case TOO_CLOSE:
{
flog << "Too close\r\n";
if(min_range(&sick, 45, 135) > (ROBOT_SAFETY_MARGIN + 100))
{
robot_state = TRACKING;
printf("I feel better now! I'm going to reenter tracking mode.\r\n");
robot.lock();
robot.setVel(0);
robot.unlock();
}
else
{
if(test_flag != TEST)
{
printf("Backing up...\r\n");
robot.lock();
robot.setVel(-50);
robot.unlock();
}
}
break;
}
}
fobjects.close();
ftarget.close();
fltarget.close();
if(plot_option == WRITE)
{
system("./scan_data/plot_script.sh >/dev/null");
}
//////////////////////////////////////////////////////////////////////
// Everything past this point is code to grab the user input
user_command = 0;
fd_set rfds;
struct timeval tv;
int retval;
FD_ZERO(&rfds);
FD_SET(0, &rfds);
tv.tv_sec = 0;
tv.tv_usec = 100;
retval = select(1, &rfds, NULL, NULL, &tv);
if(retval == -1)
perror("select()");
else if(retval)
{
cin >> user_command;
printf("input detected from user\r\n");
}
plot_option = NO_WRITE;
//////////////////////////////////////////////////////////////////////
}
flog.close();
Aria::shutdown();
printf("Shutting down");
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);
}
}
// 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();
exit(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
// specified with the connectLaser option (so when you enter laser mode, you
// can then interactively choose which laser to use from the list which will
// show both connected and unconnected lasers.)
if (!laserConnector.connectLasers(false, false, true))
{
printf("Could not connect to lasers... 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.
ArModeLaser laser(&robot, "laser", 'l', 'L');
ArModeTeleop teleop(&robot, "teleop", 't', 'T');
ArModeUnguardedTeleop unguardedTeleop(&robot, "unguarded teleop", 'u', 'U');
ArModeWander wander(&robot, "wander", 'w', 'W');
ArModeGripper gripper(&robot, "gripper", 'g', 'G');
ArModeCamera camera(&robot, "camera", 'c', 'C');
ArModeSonar sonar(&robot, "sonar", 's', 'S');
ArModeBumps bumps(&robot, "bumps", 'b', 'B');
ArModePosition position(&robot, "position", 'p', 'P', &gyro);
ArModeIO io(&robot, "io", 'i', 'I');
ArModeActs actsMode(&robot, "acts", 'a', 'A');
ArModeCommand command(&robot, "command", 'd', 'D');
ArModeTCM2 tcm2(&robot, "tcm2", 'm', 'M', compass);
// 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);
}
int main ( int argc, char *argv[] ){
//cout << "running....\n";
try{
// Create the socket
ServerSocket server ( 30000 );
Aria::init();
Arnl::init();
ArRobot robot;
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArSonarDevice sonar;
ArSimpleConnector simpleConnector(&parser);
// Our server for mobile eyes
ArServerBase moServer;
// Set up our simpleOpener
ArServerSimpleOpener simpleOpener(&parser);
parser.loadDefaultArguments();
if (!Aria::parseArgs () || !parser.checkHelpAndWarnUnparsed()){
Aria::logOptions ();
Aria::exit (1);
}
//Add the sonar to the robot
robot.addRangeDevice(&sonar);
// Look for map in the current directory
ArMap arMap;
// set it up to ignore empty file names (otherwise the parseFile
// on the config will fail)
arMap.setIgnoreEmptyFileName (true);
// First open the server
if (!simpleOpener.open(&moServer)){
if (simpleOpener.wasUserFileBad())
ArLog::log(ArLog::Normal, "Bad user file");
else
ArLog::log(ArLog::Normal, "Could not open server port");
exit(2);
}
// Connect to the robot
if (!simpleConnector.connectRobot (&robot)){
ArLog::log (ArLog::Normal, "Could not connect to robot... exiting");
Aria::exit (3);
}
// Create the localization task (it will start its own thread here)
ArSonarLocalizationTask locTask(&robot, &sonar, &arMap);
ArLocalizationManager locManager(&robot, &arMap);
ArLog::log(ArLog::Normal, "Creating sonar localization task");
locManager.addLocalizationTask(&locTask);
// Set the initial pose to the robot's "Home" position from the map, or
// (0,0,0) if none, then let the localization thread take over.
locTask.localizeRobotAtHomeNonBlocking();
//Create the path planning task
ArPathPlanningTask pathTask(&robot,&sonar,&arMap);
ArLog::log(ArLog::Normal, "Robot Server: Connected.");
robot.enableMotors();
robot.clearDirectMotion();
// Start the robot processing cycle running in the background.
// True parameter means that if the connection is lost, then the
// run loop ends.
robot.runAsync(true);
// Read in parameter files.
Aria::getConfig ()->useArgumentParser (&parser);
if (!Aria::getConfig ()->parseFile (Arnl::getTypicalParamFileName ())){
ArLog::log (ArLog::Normal, "Trouble loading configuration file, exiting");
Aria::exit (5);
}
//Create the three states
robot.lock();
Follow follow = Follow(&robot,&sonar);
GoTo goTo(&robot,&pathTask,&arMap);
Search s(&robot,&sonar);
// Bumpers.
ArBumpers bumpers;
robot.addRangeDevice(&bumpers);
pathTask.addRangeDevice(&bumpers, ArPathPlanningTask::CURRENT);
// Forbidden regions from the map
ArForbiddenRangeDevice forbidden(&arMap);
robot.addRangeDevice(&forbidden);
pathTask.addRangeDevice(&forbidden, ArPathPlanningTask::CURRENT);
// Mode To stop and remain stopped:
ArServerModeStop modeStop(&moServer, &robot);
// Action to slow down robot when localization score drops but not lost.
ArActionSlowDownWhenNotCertain actionSlowDown(&locTask);
pathTask.getPathPlanActionGroup()->addAction(&actionSlowDown, 140);
// Action to stop the robot when localization is "lost" (score too low)
ArActionLost actionLostPath(&locTask, &pathTask);
pathTask.getPathPlanActionGroup()->addAction(&actionLostPath, 150);
// These provide various kinds of information to the client:
ArServerInfoRobot serverInfoRobot(&moServer, &robot);
ArServerInfoSensor serverInfoSensor(&moServer, &robot);
ArServerInfoPath serverInfoPath(&moServer, &robot, &pathTask);
// Provide the map to the client (and related controls):
// This uses both lines and points now, since everything except
// sonar localization uses both (path planning with sonar still uses both)
ArServerHandlerMap serverMap(&moServer, &arMap);
// Provides localization info and allows the client (MobileEyes) to relocalize at a given
// pose:
ArServerInfoLocalization serverInfoLocalization(&moServer, &robot, &locTask);
ArServerHandlerLocalization serverLocHandler(&moServer, &robot, &locTask);
robot.unlock();
moServer.runAsync();
//Main loop
while (true){
//The socket to accept connection
ServerSocket new_sock;
server.accept ( new_sock );
int state = 1; //1 = Follow, 2 = Search, 3 = GoTo
int lastPos[2]; //Storing last position of BB to search the target
int data[2]; //matrix with X,Y of BB
try{
while ( true ){
//receive data from tld
new_sock >> data;
//cout << data[0] << "," << data[1] << endl;
if(data[0] != -1)
lastPos[0] = data[0];
//cout << state <<endl; //for debugging
//Main logic
switch(state){
case 1:
cout << "Following target\n";
state = follow.run(data);
break;
case 2:
cout << "Searching for target\n";
state = s.seek(lastPos, data);
break;
case 3:
cout << "Going to ...\n";
state = goTo.run(data);
break;
default:
cout << "Not a case for state\n";
break;
}
std::cout << "Loc score: " << locTask.getLocalizationScore() << std::endl;
}
}
catch ( SocketException& ) {
cout << "Lost Connection" << endl;
robot.lock();
robot.stop();
robot.unlock();
}
}
}
catch ( SocketException& e ){
std::cout << "Exception was caught:" << e.description() << "\nExiting.\n";
}
ArLog::log(ArLog::Normal, "RobotServer: Exiting.");
return 0;
}
int main(int argc, char **argv)
{
Aria::init();
ArArgumentParser argParser(&argc, argv);
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(void)
{
// The connection we'll use to talk to the robot
ArTcpConnection con;
// the robot
ArRobot robot;
// the sonar device
ArSonarDevice sonar;
// some stuff for return values
int ret;
std::string str;
// the behaviors from above, and a stallRecover behavior that uses defaults
ActionGo go(500, 350);
ActionTurn turn(400, 30);
ArActionStallRecover recover;
// this needs to be done
Aria::init();
// open the connection, just using the defaults, if it fails, exit
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
// add the range device to the robot, you should add all the range
// devices and such before you add actions
robot.addRangeDevice(&sonar);
// 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;
}
// enable the motors, disable amigobot sounds
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
// add our actions in a good order, the integer here is the priority,
// with higher priority actions going first
robot.addAction(&recover, 100);
robot.addAction(&go, 50);
robot.addAction(&turn, 49);
// run the robot, the true here is to exit if it loses connection
robot.run(true);
// now just shutdown and go away
Aria::shutdown();
return 0;
}