void handleGetMap(ArNetPacket *packet)
{
char buffer[512];
if (packet->getDataReadLength() == packet->getDataLength())
{
printf("Empty packet signifying end of map (for central forward)\n");
return;
}
packet->bufToStr(buffer, sizeof(buffer));
// if we got an end of line char instead of a line it means the map is over
if (buffer[0] == '\0')
{
printf("Map took %g seconds\n", start.mSecSince() / 1000.0);
arMap.parsingComplete();
arMap.writeFile("mapExample.map");
//client.disconnect();
//exit(0);
}
else
{
//printf("line '%s'\n", buffer);
arMap.parseLine(buffer);
}
}
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();
ArLog::init(ArLog::StdOut, ArLog::Verbose);
ArMap testMap;
ArTime timer;
ArGlobalFunctor mapChangedCB(&mapChanged);
testMap.addMapChangedCB(&mapChangedCB);
if (argc <= 1)
{
printf("mapTest: Usage %s <map> <map2:optional>\n", argv[0]);
Aria::exit(1);
}
timer.setToNow();
if (!testMap.readFile(argv[1]))
{
printf("mapTest: Could not read map '%s'\n", argv[1]);
Aria::exit(2);
}
printf("mapTest: Took %ld ms to read file\n", timer.mSecSince());
/*
printf("mapTest: ChangeTimes (in ms): mapObjects %ld points %ld mapInfo %ld\n",
testMap.getMapObjectsChanged().mSecSince(),
testMap.getPointsChanged().mSecSince(),
testMap.getMapInfoChanged().mSecSince());
*/
timer.setToNow();
if(!testMap.writeFile("mapTest.map"))
{
printf("mapTest: Error could not write new map to mapTest.map");
Aria::exit(3);
}
printf("mapTest: Took %ld ms to write file mapTest.map\n", timer.mSecSince());
std::list<ArMapObject *>::iterator objIt;
ArMapObject *obj;
for (objIt = testMap.getMapObjects()->begin();
objIt != testMap.getMapObjects()->end();
objIt++)
{
obj = (*objIt);
printf("mapTest: Map object: %s named \"%s\". Pose: %0.2f,%0.2f,%0.2f. ", obj->getType(), obj->getName(), obj->getPose().getX(), obj->getPose().getY(), obj->getPose().getTh());
if(obj->hasFromTo())
printf("mapTest: Extents: From %0.2f,%0.2f to %0.2f,%0.2f.", obj->getFromPose().getX(), obj->getFromPose().getY(), obj->getToPose().getX(), obj->getToPose().getY());
printf("mapTest: \n");
/*
if (strcasecmp(obj->getType(), "Goal") == 0 ||
strcasecmp(obj->getType(), "GoalWithHeading") == 0)
{
printf("mapTest: Map object: Goal %s\n", obj->getName());
}
else if (strcasecmp(obj->getType(), "ForbiddenLine") == 0 &&
obj->hasFromTo())
{
printf("mapTest: Map object: Forbidden line from %.0f %.0f to %.0f %.0f\n",
obj->getFromPose().getX(), obj->getFromPose().getY(),
obj->getToPose().getX(), obj->getToPose().getY());
}
*/
}
std::list<ArArgumentBuilder*>* objInfo = testMap.getMapInfo();
for(std::list<ArArgumentBuilder*>::const_iterator i = objInfo->begin();
i != objInfo->end();
i++)
{
printf("mapTest: MapInfo object definition:\n----\n");
(*i)->log();
printf("mapTest: ----\n");
}
printf("mapTest: First 5 data points:\n");
std::vector<ArPose>::iterator pIt;
ArPose pose;
int n = 0;
for (pIt = testMap.getPoints()->begin();
pIt != testMap.getPoints()->end();
pIt++)
{
pose = (*pIt);
if (n > 5)
exit(0);
printf("mapTest: \t%.0f %.0f\n", pose.getX(), pose.getY());
n++;
// the points are gone through
}
if (argc >= 3)
{
timer.setToNow();
if (!testMap.readFile(argv[2]))
{
printf("mapTest: Could not read map '%s'\n", argv[2]);
}
printf("mapTest: Took %ld ms to read file2\n", timer.mSecSince());
/*
ArUtil::sleep(30);
printf("mapTest: ChangeTimes (in ms): mapObjects %ld points %ld mapInfo %ld\n",
testMap.getMapObjectsChanged().mSecSince(),
testMap.getPointsChanged().mSecSince(),
testMap.getMapInfoChanged().mSecSince());
*/
timer.setToNow();
testMap.writeFile("mapTest2.map");
printf("mapTest: Took %ld ms to write file2\n", timer.mSecSince());
}
// now test it with the config stuff
ArArgumentBuilder builder;
builder.setExtraString("Map");
builder.add(argv[1]);
printf("mapTest: Trying config with map (%s)\n", argv[1]);
Aria::getConfig()->parseArgument(&builder);
Aria::getConfig()->callProcessFileCallBacks(true, NULL, 0);
printf("mapTest: Trying config again with same map (%s)\n", argv[1]);
Aria::getConfig()->parseArgument(&builder);
Aria::getConfig()->callProcessFileCallBacks(true, NULL, 0);
if (argc >= 3)
{
ArArgumentBuilder builder2;
builder2.setExtraString("Map");
builder2.add(argv[2]);
printf("mapTest: Trying config with map2 (%s)\n", argv[2]);
Aria::getConfig()->parseArgument(&builder2);
Aria::getConfig()->callProcessFileCallBacks(true, NULL, 0);
}
}
int main(int argc, char **argv)
{
Aria::init();
char *worldName;
char *mapName;
if (argc != 3)
{
ArLog::log(ArLog::Normal, "Usage: %s <WorldFile> <MapFile>", argv[0]);
ArLog::log(ArLog::Normal, "Example: %s columbia.wld columbia.map", argv[0]);
exit(1);
}
worldName = argv[1];
mapName = argv[2];
FILE *file;
if ((file = ArUtil::fopen(worldName, "r")) == NULL)
{
ArLog::log(ArLog::Normal, "Could not open world file '%s' to convert", worldName);
exit(1);
}
char line[10000];
std::vector<ArLineSegment> lines;
bool haveHome = false;
ArPose homePose;
// read until the end of the file
while (fgets(line, sizeof(line), file) != NULL)
{
ArArgumentBuilder builder;
builder.add(line);
// Four ints is a line
if (builder.getArgc() == 4 && builder.isArgInt(0) &&
builder.isArgInt(1) && builder.isArgInt(2) &&
builder.isArgInt(3))
{
lines.push_back(
ArLineSegment(builder.getArgInt(0), builder.getArgInt(1),
builder.getArgInt(2), builder.getArgInt(3)));
}
// "position X Y Th" becomes a RobotHome
if( !strcmp(builder.getArg(0), "position") &&
builder.getArgc() == 4 && builder.isArgInt(1) &&
builder.isArgInt(2) && builder.isArgInt(3) )
{
haveHome = true;
homePose.setX(builder.getArgInt(1));
homePose.setY(builder.getArgInt(2));
homePose.setTh(builder.getArgInt(3));
printf("Will make a Home point out of start position: ");
homePose.log();
}
}
ArMap armap;
armap.setLines(&lines);
ArPose nopose;
ArMapObject home("RobotHome", homePose, NULL, "ICON", "Home", false, nopose, nopose);
std::list<ArMapObject*> objects;
if(haveHome)
{
objects.push_back(&home);
armap.setMapObjects(&objects);
}
if (!armap.writeFile(mapName))
{
ArLog::log(ArLog::Normal, "Could not save map file '%s'", mapName);
exit(1);
}
ArLog::log(ArLog::Normal, "Converted %s world file to %s map file.", worldName, mapName);
exit(0);
}
