int main(int argc, char** argv)
{
// set up our simpleConnector
ArSimpleConnector simpleConnector(&argc, argv);
// robot
ArRobot robot;
// a key handler so we can do our key handling
ArKeyHandler keyHandler;
ArLog::init(ArLog::StdOut,ArLog::Verbose);
// if there are more arguments left then it means we didn't
// understand an option
if (!simpleConnector.parseArgs() || argc > 1)
{
simpleConnector.logOptions();
keyHandler.restore();
exit(1);
}
// mandatory init
Aria::init();
ArLog::init(ArLog::StdOut, ArLog::Terse, NULL, true);
// let the global aria stuff know about it
Aria::setKeyHandler(&keyHandler);
// toss it on the robot
robot.attachKeyHandler(&keyHandler);
// set up the robot for connecting
if (!simpleConnector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
keyHandler.restore();
return 1;
}
// turn on the motors for the velocity response test
robot.comInt(ArCommands::ENABLE, 1);
velTime.setToNow();
// turn off the sonar
robot.comInt(ArCommands::SONAR, 0);
ArGlobalFunctor1<ArRobot *> userTaskCB(&userTask, &robot);
robot.addUserTask("iotest", 100, &userTaskCB);
robot.comInt(ArCommands::IOREQUEST, 1);
requestTime.setToNow();
//start the robot running, true so that if we lose connection the run stops
robot.run(true);
// now exit
Aria::shutdown();
return 0;
}
int main()
{
ArSyncTask *task;
UserTaskTest test;
ArRobot robot;
ArFunctorC<UserTaskTest> userTaskOne(&test, &UserTaskTest::userTaskOne);
ArFunctorC<UserTaskTest> userTaskTwo(&test, &UserTaskTest::userTaskTwo);
ArFunctorC<UserTaskTest> userTaskThree(&test, &UserTaskTest::userTaskThree);
ArFunctorC<UserTaskTest> userTaskFour(&test, &UserTaskTest::userTaskFour);
printf("Before tasks added:\n");
robot.logUserTasks();
printf("---------------------------------------------------------------\n");
// the order you add tasks doesn't matter, it goes off of the integer
// you give the function call to add them
// Caveat: if you give the function call the same number it goes off of order
robot.addUserTask("procTwo", 20, &userTaskTwo);
robot.addUserTask("procFour", 40, &userTaskFour, &test.myTaskFourState);
robot.addUserTask("procThree", 30, &userTaskThree);
robot.addUserTask("procOne", 10, &userTaskOne);
printf("After tasks added:\n");
robot.logUserTasks();
printf("---------------------------------------------------------------\n");
printf("What happens when these are run:\n");
robot.loopOnce();
printf("---------------------------------------------------------------\n");
printf("After tasks removed by name:\n");
printf("---------------------------------------------------------------\n");
robot.remUserTask("procOne");
robot.remUserTask("procTwo");
robot.remUserTask("procThree");
robot.remUserTask("procFour");
robot.logUserTasks();
printf("\nAfter they are added again, procThree is found by name and set to SUSPEND and procFour is found by functor and set to FAILURE:\n");
printf("---------------------------------------------------------------\n");
printf("---------------------------------------------------------------\n");
robot.addUserTask("procTwo", 20, &userTaskTwo);
robot.addUserTask("procFour", 40, &userTaskFour, &test.myTaskFourState);
robot.addUserTask("procThree", 30, &userTaskThree);
robot.addUserTask("procOne", 10, &userTaskOne);
task = robot.findUserTask("procThree");
if (task != NULL)
task->setState(ArTaskState::SUSPEND);
task = robot.findUserTask(&userTaskFour);
if (task != NULL)
task->setState(ArTaskState::FAILURE);
robot.logUserTasks();
task = robot.findUserTask(&userTaskFour);
if (task != NULL)
{
printf("---------------------------------------------------------------\n");
printf("Did the real state on procFourState get set:\n");
printf("getState: %d realState: %d\n", task->getState(),
test.myTaskFourState);
}
printf("---------------------------------------------------------------\n");
printf("What happens when these are run:\n");
robot.loopOnce();
printf("---------------------------------------------------------------\n");
printf("After tasks removed by functor:\n");
printf("---------------------------------------------------------------\n");
robot.remUserTask(&userTaskOne);
robot.remUserTask(&userTaskTwo);
robot.remUserTask(&userTaskThree);
robot.remUserTask(&userTaskFour);
robot.logUserTasks();
}
int main(int argc, char **argv)
{
// robot
ArRobot robot;
// the laser
ArSick sick;
// sonar, must be added to the robot
//ArSonarDevice sonar;
// the actions we'll use to wander
// recover from stalls
//ArActionStallRecover recover;
// react to bumpers
//ArActionBumpers bumpers;
// limiter for close obstacles
ArActionLimiterForwards limiter("speed limiter near", 1600, 0, 0, 1.3);
// limiter for far away obstacles
//ArActionLimiterForwards limiterFar("speed limiter near", 300, 1000, 450, 1.1);
//ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 600, 1.1);
// limiter for the table sensors
//ArActionLimiterTableSensor tableLimiter;
// actually move the robot
ArActionConstantVelocity constantVelocity("Constant Velocity", 1500);
// turn the orbot if its slowed down
ArActionTurn turn;
// mandatory init
Aria::init();
// Parse all our args
ArSimpleConnector connector(&argc, argv);
if (!connector.parseArgs() || argc > 1)
{
connector.logOptions();
exit(1);
}
// add the sonar to the robot
//robot.addRangeDevice(&sonar);
// add the laser to the robot
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;
}
robot.comInt(ArCommands::SONAR, 0);
// turn on the motors, turn off amigobot sounds
//robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::SOUNDTOG, 0);
// add the actions
//robot.addAction(&recover, 100);
//robot.addAction(&bumpers, 75);
robot.addAction(&limiter, 49);
//robot.addAction(&limiter, 48);
//robot.addAction(&tableLimiter, 50);
robot.addAction(&turn, 30);
robot.addAction(&constantVelocity, 20);
robot.setStateReflectionRefreshTime(50);
limiter.activate();
turn.activate();
constantVelocity.activate();
robot.clearDirectMotion();
//robot.setStateReflectionRefreshTime(50);
robot.setRotVelMax(50);
robot.setTransAccel(1500);
robot.setTransDecel(100);
// start the robot running, true so that if we lose connection the run stops
robot.runAsync(true);
connector.setupLaser(&sick);
// now that we're connected to the robot, connect to the laser
sick.runAsync();
if (!sick.blockingConnect())
{
printf("Could not connect to SICK laser... exiting\n");
Aria::shutdown();
return 1;
}
sick.lockDevice();
sick.setMinRange(250);
sick.unlockDevice();
robot.lock();
ArGlobalFunctor1<ArRobot *> userTaskCB(&userTask, &robot);
robot.addUserTask("iotest", 100, &userTaskCB);
requestTime.setToNow();
robot.comInt(ArCommands::IOREQUEST, 1);
robot.comInt(ArCommands::ENABLE, 1);
robot.unlock();
robot.waitForRunExit();
// now exit
Aria::shutdown();
return 0;
}
