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);
}
void PeoplebotTest::userTask(void)
{
switch (myState)
{
case IDLE:
// start wandering
printf("Starting to wander for the first time\n");
myStateTime.setToNow();
myState = WANDERING;
myRobot->addAction(myConstantVelocity, 25);
printf("Opening up ACTS\n");
myCmd = "DISPLAY=";
myCmd += myHostname.c_str();
myCmd += ":0; /usr/local/acts/bin/acts -G bttv -n 0 &> /dev/null &";
system(myCmd.c_str());
break;
case WANDERING:
if (timeout(myWanderingTimeout))
{
myRobot->comInt(ArCommands::SONAR,0);
myRobot->remAction(myConstantVelocity);
myRobot->setVel(0);
myRobot->setRotVel(0);
myState = RESTING;
mySonar = 0;
printf("Going to rest now\n");
printf("Killing ACTS\n");
system("killall -9 acts &> /dev/null");
myStateTime.setToNow();
myTotalWanderTime += myWanderingTimeout;
}
else if (myRobot->getVel() > 0 && mySonar != 1)
{
// ping front sonar
//printf("Enabling front sonar\n");
mySonar = 1;
myRobot->comInt(ArCommands::SONAR, 0);
myRobot->comInt(ArCommands::SONAR, 1);
myRobot->comInt(ArCommands::SONAR, 4);
}
else if (myRobot->getVel() < 0 && mySonar != -1)
{
// ping rear sonar
//printf("Enabling rear sonar\n");
mySonar = -1;
myRobot->comInt(ArCommands::SONAR, 0);
myRobot->comInt(ArCommands::SONAR, 5);
}
break;
case RESTING:
if (timeout(myRestingTimeout))
{
printf("Going to wander now\n");
myState = WANDERING;
myStateTime.setToNow();
myTotalRestTime += myRestingTimeout;
myRobot->clearDirectMotion();
myRobot->addAction(myConstantVelocity, 25);
printf("Opening up ACTS\n");
myCmd = "DISPLAY=";
myCmd += myHostname.c_str();
myCmd += ":0; /usr/local/acts/bin/acts -G bttv -n 0 &> /dev/null &";
system(myCmd.c_str());
}
break;
case OTHER:
default:
break;
};
}
void TakeBlockToWall::handler(void)
{
Color tempColor;
switch (myState)
{
case STATE_START:
setState(STATE_ACQUIRE_BLOCK);
myDropWall = COLOR_FIRST_WALL;
myLapWall = COLOR_SECOND_WALL;
printf("!! Started state handling!\n");
//handler();
return;
break;
case STATE_ACQUIRE_BLOCK:
if (myNewState)
{
printf("!! Acquire block\n");
myNewState = false;
myAMPTU->panTilt(0, -40);
myAcquire->activate();
myAcquire->setChannel(COLOR_BLOCK);
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myGripper->getGripState() == 2 &&
myGripper->getBreakBeamState() != 0)
{
printf("###### AcquireBlock: Successful (have cube?)\n");
setState(STATE_PICKUP_BACKUP);
//handler();
return;
}
else if (myGripper->getBreakBeamState() != 0)
{
printf("###### AcquireBlock: Successful (cube in gripper?)\n");
setState(STATE_PICKUP_BLOCK);
//handler();
return;
}
if (myAcquire->getState() == Acquire::STATE_FAILED ||
myStateStartTime.mSecSince() > 35000)
{
printf("###### AcqiureBlock: failed\n");
setState(STATE_BACKUP);
//handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("###### AcquireBlock: successful\n");
setState(STATE_PICKUP_BLOCK);
//handler();
return;
}
break;
case STATE_PICKUP_BLOCK:
if (myNewState)
{
printf("!! Pickup block\n");
myNewState = false;
myAMPTU->panTilt(0, -35);
myAcquire->deactivate();
myPickUp->activate();
myPickUp->setChannel(COLOR_BLOCK);
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myPickUp->getState() == PickUp::STATE_FAILED)
{
printf("###### PickUpBlock: failed\n");
setState(STATE_BACKUP);
//handler();
return;
}
else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED)
{
printf("###### PickUpBlock: successful\n");
setState(STATE_PICKUP_BACKUP);
//handler();
return;
}
break;
case STATE_BACKUP:
if (myNewState)
{
myNewState = false;
myRobot->move(BACKUP_DIST * .75);
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
{
printf("###### Backup: Failed, going forwards\n");
myRobot->clearDirectMotion();
setState(STATE_FORWARD);
}
if (myStateStartTime.mSecSince() > BACKUP_TIME ||
myStateStartPos.findDistanceTo(myRobot->getPose()) > BACKUP_DIST * .95 * .75)
{
printf("###### Backup: Succeeded\n");
myRobot->clearDirectMotion();
setState(STATE_ACQUIRE_BLOCK2);
//handler();
return;
}
break;
case STATE_FORWARD:
if (myNewState)
{
myNewState = false;
myRobot->move(-BACKUP_DIST * .75);
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
{
printf("###### Forward: Failed\n");
myRobot->clearDirectMotion();
setState(STATE_FAILED);
}
if (myStateStartTime.mSecSince() > BACKUP_TIME ||
myStateStartPos.findDistanceTo(myRobot->getPose()) >
ArMath::fabs(BACKUP_DIST * .95 * .75))
{
printf("###### Forward: Succeeded\n");
myRobot->clearDirectMotion();
setState(STATE_ACQUIRE_BLOCK2);
//handler();
return;
}
break;
case STATE_ACQUIRE_BLOCK2:
if (myNewState)
{
printf("!! Acquire block 2\n");
myNewState = false;
myAMPTU->panTilt(0, -40);
myAcquire->activate();
myAcquire->setChannel(COLOR_BLOCK);
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myGripper->getGripState() == 2 &&
myGripper->getBreakBeamState() != 0)
{
printf("###### AcquireBlock2: Successful (have cube?)\n");
setState(STATE_PICKUP_BACKUP);
//handler();
return;
}
else if (myGripper->getBreakBeamState() != 0)
{
printf("###### AcquireBlock2: Successful (cube in gripper?)\n");
setState(STATE_PICKUP_BLOCK2);
//handler();
return;
}
if (myAcquire->getState() == Acquire::STATE_FAILED ||
myStateStartTime.mSecSince() > 35000)
{
printf("###### AcqiureBlock2: failed\n");
setState(STATE_FAILED);
//handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("###### AcquireBlock2: successful\n");
setState(STATE_PICKUP_BLOCK2);
//handler();
return;
}
break;
case STATE_PICKUP_BLOCK2:
if (myNewState)
{
printf("!! Pickup block 2\n");
myNewState = false;
myAcquire->deactivate();
myPickUp->activate();
myAMPTU->panTilt(0, -55);
myPickUp->setChannel(COLOR_BLOCK);
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myPickUp->getState() == PickUp::STATE_FAILED)
{
printf("###### PickUpBlock2: failed\n");
setState(STATE_FAILED);
//handler();
return;
}
else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED)
{
printf("###### PickUpBlock2: successful\n");
setState(STATE_PICKUP_BACKUP);
//handler();
return;
}
break;
case STATE_PICKUP_BACKUP:
if (myNewState)
{
myNewState = false;
myRobot->move(BACKUP_DIST);
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myStateStartTime.mSecSince() > BACKUP_TIME ||
myStateStartPos.findDistanceTo(myRobot->getPose()) >
ArMath::fabs(BACKUP_DIST * .95))
{
printf("###### PickUp_BackUp: done\n");
myRobot->clearDirectMotion();
setState(STATE_ACQUIRE_DROP_WALL);
//handler();
return;
}
break;
case STATE_ACQUIRE_DROP_WALL:
if (myNewState)
{
printf("!! Acquire Drop wall, channel %d\n", myDropWall);
myNewState = false;
myAMPTU->panTilt(0, -30);
myAcquire->activate();
myAcquire->setChannel(myDropWall);
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myGripper->getGripState() != 2 ||
myGripper->getBreakBeamState() == 0)
{
printf("###### AcquireDropWall:: failed (lost cube %d %d)\n",
myGripper->getGripState(), myGripper->getBreakBeamState());
setState(STATE_BACKUP);
//handler();
return;
}
if (myAcquire->getState() == Acquire::STATE_FAILED ||
myStateStartTime.mSecSince() > 35000)
{
printf("###### AcquireDropWall:: failed\n");
setState(STATE_FAILED);
//handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("###### AcquireDropWall: successful\n");
setState(STATE_DRIVETO_DROP_WALL);
//handler();
return;
}
break;
case STATE_DRIVETO_DROP_WALL:
if (myNewState)
{
printf("!! DropOff Drop wall, channel %d\n", myDropWall);
myNewState = false;
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->activate();
myDropOff->setChannel(myDropWall);
myTableLimiter->deactivate();
}
if (myDropOff->getState() == DropOff::STATE_FAILED)
{
printf("###### DropOffDropWall: failed\n");
setState(STATE_FAILED);
//handler();
return;
}
else if (myDropOff->getState() == DropOff::STATE_SUCCEEDED)
{
printf("###### DropOffDropWall: succesful\n");
setState(STATE_DROP_BACKUP);
//handler();
return;
}
break;
case STATE_DROP_BACKUP:
if (myNewState)
{
myNewState = false;
myRobot->move(BACKUP_DIST);
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myStateStartTime.mSecSince() > BACKUP_TIME ||
myStateStartPos.findDistanceTo(myRobot->getPose()) >
ArMath::fabs(BACKUP_DIST * .95))
{
printf("###### Drop_Backup: done\n");
myRobot->clearDirectMotion();
setState(STATE_ACQUIRE_LAP_WALL);
//handler();
return;
}
break;
case STATE_ACQUIRE_LAP_WALL:
if (myNewState)
{
printf("!! Acquire Lap wall, channel %d\n", myLapWall);
myNewState = false;
myAMPTU->panTilt(0, -30);
myAcquire->activate();
myAcquire->setChannel(myLapWall);
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->activate();
}
if (myAcquire->getState() == Acquire::STATE_FAILED ||
myStateStartTime.mSecSince() > 35000)
{
printf("###### AcquireLapWall:: failed\n");
setState(STATE_SWITCH);
//handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("###### AcquireLapWall: successful\n");
setState(STATE_DRIVETO_LAP_WALL);
//handler();
return;
}
break;
case STATE_DRIVETO_LAP_WALL:
if (myNewState)
{
printf("!! Driveto Lap wall, channel %d\n", myLapWall);
myNewState = false;
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->activate();
myDriveTo->setChannel(myLapWall);
myDropOff->deactivate();
myTableLimiter->activate();
}
if (myDriveTo->getState() == DriveTo::STATE_FAILED)
{
printf("###### DriveToLapWall: failed\n");
setState(STATE_BACKUP_LAP_WALL);
//handler();
return;
}
else if (myDriveTo->getState() == DriveTo::STATE_SUCCEEDED)
{
printf("###### DriveToLapWall: succesful\n");
setState(STATE_BACKUP_LAP_WALL);
//handler();
return;
}
break;
case STATE_BACKUP_LAP_WALL:
if (myNewState)
{
myNewState = false;
myRobot->move(BACKUP_DIST * .75);
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
{
printf("###### BackupLapWall: Failed, going forwards\n");
myRobot->clearDirectMotion();
setState(STATE_FORWARD_LAP_WALL);
}
if (myStateStartTime.mSecSince() > BACKUP_TIME ||
myStateStartPos.findDistanceTo(myRobot->getPose()) >
ArMath::fabs(BACKUP_DIST * .95 * .75))
{
printf("###### BackupLapWall: Succeeded\n");
myRobot->clearDirectMotion();
setState(STATE_SWITCH);
//handler();
return;
}
break;
case STATE_FORWARD_LAP_WALL:
if (myNewState)
{
myNewState = false;
myRobot->move(-BACKUP_DIST * .75);
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
{
printf("###### ForwardLapWall: Failed\n");
myRobot->clearDirectMotion();
setState(STATE_FAILED);
}
if (myStateStartTime.mSecSince() > BACKUP_TIME ||
myStateStartPos.findDistanceTo(myRobot->getPose()) >
ArMath::fabs(BACKUP_DIST * .95 * .75))
{
printf("###### ForwardLapWall: Succeeded\n");
myRobot->clearDirectMotion();
setState(STATE_SWITCH);
//handler();
return;
}
break;
case STATE_SWITCH:
printf("!! Switching walls around.\n");
tempColor = myDropWall;
myDropWall = myLapWall;
myLapWall = tempColor;
setState(STATE_ACQUIRE_BLOCK);
//handler();
return;
case STATE_FAILED:
printf("@@@@@ Failed to complete the task!\n");
myRobot->comInt(ArCommands::SONAR, 0);
ArUtil::sleep(50);
myRobot->comStr(ArCommands::SAY, "\52\77\37\62\42\70");
ArUtil::sleep(500);
Aria::shutdown();
myRobot->disconnect();
myRobot->stopRunning();
return;
}
}
int main ( int argc, char *argv[] ){
//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[])
{
// Initialize location of Aria, Arnl and their args.
Aria::init();
Arnl::init();
// The robot object
ArRobot robot;
#ifndef SONARNL
// The laser object
ArSick sick(181, 361);
#endif
// Parse them command line arguments.
ArArgumentParser parser(&argc, argv);
// Set up our simpleConnector
ArSimpleConnector simpleConnector(&parser);
// Load default arguments for this computer
parser.loadDefaultArguments();
// Parse its arguments for the simple connector.
simpleConnector.parseArgs();
// sonar, must be added to the robot, for teleop and wander
ArSonarDevice sonarDev;
// add the sonar to the robot
robot.addRangeDevice(&sonarDev);
ArMap arMap;
#ifndef SONARNL
// Initialize the localization
ArLocalizationTask locTask(&robot, &sick, &arMap);
// Make the path task
ArPathPlanningTask pathTask(&robot, &sick, &sonarDev, &arMap);
#else
// Initialize the localization
ArSonarLocalizationTask locTask(&robot, &sonarDev, &arMap);
// Make the path task
ArPathPlanningTask pathTask(&robot, NULL, &sonarDev, &arMap);
#endif
// Stop the robot as soon as localization fails.
ArFunctor1C<ArPathPlanningTask, int>
locaFailed(&pathTask, &ArPathPlanningTask::trackingFailed);
locTask.addFailedLocalizationCB(&locaFailed);
// Read in param files.
Aria::getConfig()->useArgumentParser(&parser);
if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
{
printf("Trouble loading configuration file, exiting\n");
exit(1);
}
// Warn about unknown params.
if (!parser.checkHelpAndWarnUnparsed())
{
printf("\nUsage: %s -map mapfilename\n\n", argv[0]);
simpleConnector.logOptions();
exit(2);
}
// Our server
ArServerBase server;
// First open the server up
if (!server.open(7272))
{
printf("Could not open server port\n");
exit(1);
}
// Connect the robot
if (!simpleConnector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
robot.com2Bytes(31, 14, 0);
robot.com2Bytes(31, 15, 0);
ArUtil::sleep(100);
robot.com2Bytes(31, 14, 1);
robot.com2Bytes(31, 15, 1);
robot.enableMotors();
robot.clearDirectMotion();
#ifndef SONARNL
// Set up the laser before handing it to the laser mode
simpleConnector.setupLaser(&sick);
// Add the laser to the robot
robot.addRangeDevice(&sick);
#endif
// Start the robot thread.
robot.runAsync(true);
#ifndef SONARNL
// Start the laser thread.
sick.runAsync();
// Connect the laser
if (!sick.blockingConnect())
{
printf("Couldn't connect to sick, exiting\n");
Aria::shutdown();
return 1;
}
#endif
ArUtil::sleep(300);
// If you want to set the number of samples change the line below
locTask.setNumSamples(2000);
// Localize the robot to home
if(locTask.localizeRobotAtHomeBlocking())
{
printf("Successfully localized at home.\n");
}
else
{
printf("WARNING: Unable to localize at home position!\n");
}
robot.lock();
// attach stuff to the server
ArServerInfoRobot serverInfoRobot(&server, &robot);
ArServerInfoSensor serverInfoSensor(&server, &robot);
ArServerInfoPath serverInfoPath(&server, &robot, &pathTask);
ArServerInfoLocalization serverInfoLocalization(&server, &robot, &locTask);
#ifndef SONARNL
// Set it up to handle maps.
ArServerHandlerMap serverMap(&server, &arMap, ArServerHandlerMap::POINTS);
#else
ArServerHandlerMap serverMap(&server, &arMap, ArServerHandlerMap::LINES);
#endif
// Set up a service that allows the client to monitor the communication
// between the robot and the client.
//
ArServerHandlerCommMonitor serverCommMonitor(&server);
//ArServerModeGoto modeGoto(&server, &robot, &pathTask);
//ArServerModeStop modeStop(&server, &robot);
//ArServerModeDrive modeDrive(&server, &robot);
SimpleTask simpleTask(&robot, &pathTask);
robot.unlock();
// Read in param files.
Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName());
// Now let it spin off in its own thread
server.run();
exit(0);
}
int main(int argc, char **argv)
{
// 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;
}
/*!
* Main function which initializes the localization tasks and pathplanning
* tasks.
*
* @param argc No of command line args.
* @param argv Array of command line args.
*
* @return 1 if successful.
*/
int
main(int argc, char *argv[])
{
char* mapname;
if(argc<2){
printf("%s\n",USAGE);
exit(1);
}
Aria::init();
Arnl::init();
// The robot object
ArRobot robot;
#ifndef SONARNL
// The laser object
ArSick sick(181, 361);
#endif
// Set up our simpleConnector
ArSimpleConnector simpleConnector(&argc, argv);
// Parse its arguments
simpleConnector.parseArgs();
// Parse them command line arguments.
ArArgumentParser parser(&argc, argv);
// Sonar, must be added to the robot, for teleop and wander
ArSonarDevice sonarDev;
// Add the sonar to the robot
robot.addRangeDevice(&sonarDev);
// Set up the robot for connecting
if (!simpleConnector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
robot.enableMotors();
robot.clearDirectMotion();
#ifndef SONARNL
// Set up the laser before handing it to the laser mode
simpleConnector.setupLaser(&sick);
// Add the laser to the robot
robot.addRangeDevice(&sick);
#endif
// Start the robot thread.
robot.runAsync(true);
#ifndef SONARNL
// Start the laser thread.
sick.runAsync();
// Connect the laser
if (!sick.blockingConnect())
{
printf("Couldn't connect to sick, exiting\n");
Aria::shutdown();
return 1;
}
#endif
//
// Set up the big holder.
//
#ifndef SONARNL
advancedptr = new Advanced(&robot, &sick, &sonarDev);
#else
advancedptr = new Advanced(&robot, NULL, &sonarDev);
#endif
//
// Set up the callbacks for localization task.
//
ArFunctor1C<Advanced, int>
failed(advancedptr, &Advanced::trackingFailed);
//
// Set up the callbacks for pathplanning task.
//
ArFunctor1C<Advanced, ArPose>
goal_done(advancedptr, &Advanced::goalDone);
ArFunctor1C<Advanced, ArPose>
goal_failed(advancedptr, &Advanced::goalFailed);
// Read in config file. Do this before creating the localization
// and path planning tasks, so we can override certain config
// parameters such as the map file name when we do so.
if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
{
printf("Trouble loading configuration file, exiting\n");
exit(1);
}
//
// Get mapname and initialize localization task and pathplanning tasks.
//
if((mapname = parser.checkParameterArgument("-map"))){
//
// Start the localization thread. Should be done first.
//
if(!advancedptr->initializeLocalizationTask(mapname)){
printf("Cannot start the localization task thread.\n");
exit(1);
}else{
printf("Started the Localization Thread\n");
}
//
// Get the Aria map from the localization task .
// (Alternatively use a ArMap* made separately)
//
advancedptr->myMap = advancedptr->myLocaTask->getAriaMap();
//
// Set up the path plan structure.
// Cannot precede localization task setup.
//
if(!advancedptr->initializePathPlanningTask()){
printf("Cannot set up for path planning task thread.\n");
exit(1);
}else{
printf("Started Path Planning Thread\n");
}
//
// Functors to be used by the Arnl library if the threads
// signal failure or success.
//
advancedptr->myLocaTask->addFailedLocalizationCB(&failed);
advancedptr->myPathPlanningTask->addGoalDoneCB(&goal_done);
advancedptr->myPathPlanningTask->addGoalFailedCB(&goal_failed);
}else{
printf(USAGE);
exit(1);
}
//
// Get user input.
//
interact();
}
