/*!
* Gets the list of home poses.
*
* @param mapdata: Pointer to the map class.
*
* @return list of home objects.
*
*/
void
Advanced::getAllRobotHomes(ArMap* ariamap)
{
myHomeList.clear();
std::list<ArMapObject *>::iterator objIt;
ArMapObject* obj;
int i=0;
for (objIt = ariamap->getMapObjects()->begin();
objIt != ariamap->getMapObjects()->end();
objIt++)
{
//
// Get the forbidden lines and fill the occupancy grid there.
//
obj = (*objIt);
if (strcasecmp(obj->getType(), "RobotHome") == 0)
{
myHomeList.push_back(obj);
ArPose pose = obj->getPose();
printf("RobotHome[%d] = %s : %5.2f %5.2f %5.2f\n", i++, obj->getName(),
pose.getX(), pose.getY(), pose.getTh());
}
if (strcasecmp(obj->getType(), "Home") == 0)
{
myHomeList.push_back(obj);
ArPose pose = obj->getPose();
printf("Home[%d] = %s : %5.2f %5.2f %5.2f\n", i++, obj->getName(),
pose.getX(), pose.getY(), pose.getTh());
}
}
}
/** @internal */
AREXPORT void ArServerHandlerMap::serverGetGoals(ArServerClient *client,
ArNetPacket *packet)
{
std::list<ArMapObject *>::iterator objIt;
ArMapObject* obj;
ArPose goal;
ArNetPacket sendPacket;
for (objIt = myMap->getMapObjects()->begin();
objIt != myMap->getMapObjects()->end();
objIt++)
{
//
// Get the forbidden lines and fill the occupancy grid there.
//
obj = (*objIt);
if (obj == NULL)
break;
if (strcasecmp(obj->getType(), "GoalWithHeading") == 0 ||
strcasecmp(obj->getType(), "Goal") == 0)
{
sendPacket.strToBuf(obj->getName());
}
}
client->sendPacketTcp(&sendPacket);
}
/*!
* Callback function for the r key. Gets the robot to pick the next
* goal point and plan to it. and keep going around the list.
*/
void
rkeyCB(void)
{
if(roundRobinFlag == false){
roundRobinFlag = true;
}else{
roundRobinFlag = false;
return;
}
if(advancedptr->myLocaTask->getInitializedFlag()){
if(advancedptr->myGoalList.size()>0){
ArMapObject* front = advancedptr->myGoalList.front();
ArPose top = front->getPose();
bool headingFlag = false;
if(strcasecmp(front->getType(), "GoalWithHeading") == 0)
headingFlag = true;
else
headingFlag = false;
printf("Path planing to goal %s %5.2f %5.2f %5.2f: %d poses\n",
front->getName(),
top.getX(), top.getY(), top.getTh(),
advancedptr->myGoalList.size());
advancedptr->myGoalList.pop_front();
advancedptr->myGoalList.push_back(front);
//
// Setup the pathplanning task to this destination.
//
advancedptr->pathPlanFromCurrentToDest(top, headingFlag);
}
}else{
printf("Localize the robot first\n");
}
}
/*!
* This is the called when path to goal reached.
*
* @param goal: Goal it was assigned.
*/
void
Advanced::goalDone(ArPose goal)
{
static int goalCount = 0;
printf("goalDone:Main: %5.2f %5.2f %5.2f\07\n",
goal.getX(), goal.getY(), goal.getTh());
if(roundRobinFlag){
if(myLocaTask->getInitializedFlag()){
if(myGoalList.size()>0){
ArMapObject* front = myGoalList.front();
ArPose top = front->getPose();
bool headingFlag = false;
if(strcasecmp(front->getType(), "GoalWithHeading") == 0)
headingFlag = true;
else
headingFlag = false;
printf("Moving to next goal:%s %5.2f %5.2f %5.2f: %d poses %d Done\n",
front->getName(),
top.getX(), top.getY(), top.getTh(),
myGoalList.size(), goalCount++);
myGoalList.pop_front();
myGoalList.push_back(front);
myPathPlanningTask->pathPlanToPose(top, headingFlag);
}
}
}else{
printf("Localize the robot first\n");
}
}
/*!
* Callback function for the h key. Gets the robot to home pose.
*/
void
hkeyCB(void)
{
roundRobinFlag = false;
if(advancedptr->myLocaTask->getInitializedFlag()){
if(advancedptr->myHomeList.size()>0){
ArMapObject* front = advancedptr->myHomeList.front();
ArPose top = front->getPose();
printf("Homing to %s %5.2f %5.2f %5.2f\n",
front->getName(),
top.getX(), top.getY(), top.getTh());
bool headingFlag = true;
advancedptr->pathPlanFromCurrentToDest(top, headingFlag);
}
}else{
printf("Localize the robot first\n");
}
}
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);
}
}
void PathUtil::init(ArMap *map, std::vector<WayPoint*> *all1d) {
printf("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!AOSIDJHOAISDHJOIADHO\n\n\n");
ArLog::log(ArLog::Normal, "Goals: %i", map->findMapObjectsOfType("goal").size());
std::list<ArMapObject*> mapPoints = map->findMapObjectsOfType("goal");
std::list<ArMapObject*>::const_iterator iterator;
int distToField = 200;
std::vector<std::vector<WayPoint> > tmpWayPoints;
//init TTT field
for (int x = 0; x < 3; x++) {
std::vector<WayPoint> newVec;
tmpWayPoints.push_back(newVec);
for (int y = 0; y < 3; y++) {
WayPoint emptyWP(0, 0, -1);
emptyWP.blocked = true;
tmpWayPoints[x].push_back(emptyWP);
}
}
for (iterator = mapPoints.begin(); iterator != mapPoints.end(); iterator++) {
ArMapObject *curObj = *iterator;
//printf("Name: %i, %i, %i, %i\n", sizeof(curObj->getName()[0]), (int) sizeof(*(curObj->getName())), (int) (sizeof(*(curObj->getName())) / sizeof(curObj->getName()[0])), (int) ((sizeof(*(curObj->getName())) / sizeof(curObj->getName()[0])) > 1));
//is a TTT field, 1 char, '0' - '8'
if ((sizeof(*(curObj->getName())) / sizeof(curObj->getName()[0])) > 1 || curObj->getName()[0] < '0' || curObj->getName()[0] > '8') continue;
WayPoint *tmpWP = new WayPoint(curObj->getPose().getX(), curObj->getPose().getY(), (int) curObj->getName()[0] - '0');
tmpWP->blocked = true;
int xPos = tmpWP->id % 3;
int yPos = (int) floor(tmpWP->id / 3);
if (xPos != 1 || yPos != 1) tmpWP->blocked = false;
if (xPos == 0) tmpWP->y += distToField;
if (xPos == 2) tmpWP->y -= distToField;
if (yPos == 0) tmpWP->x -= distToField;
if (yPos == 2) tmpWP->x += distToField;
tmpWayPoints[xPos][yPos] = *tmpWP;
printf("Goal %i: %ix%i\n", (int) curObj->getName()[0] - '0', (int) curObj->getPose().getX(), (int) curObj->getPose().getY());
}
//create edges
for (int x = 0; x < (int) tmpWayPoints.size(); x++) {
for (int y = 0; y < (int) tmpWayPoints[x].size(); y++) {
if (x + 1 < (int) tmpWayPoints.size() && !tmpWayPoints[x + 1][y].blocked) tmpWayPoints[x][y].wayPoints.push_back(&tmpWayPoints[x + 1][y]);
if (x - 1 > -1 && !tmpWayPoints[x - 1][y].blocked) tmpWayPoints[x][y].wayPoints.push_back(&tmpWayPoints[x - 1][y]);
if (y + 1 < (int) tmpWayPoints[x].size() && !tmpWayPoints[x][y + 1].blocked) tmpWayPoints[x][y].wayPoints.push_back(&tmpWayPoints[x][y + 1]);
if (y - 1 > -1 && !tmpWayPoints[x][y - 1].blocked) tmpWayPoints[x][y].wayPoints.push_back(&tmpWayPoints[x][y - 1]);
//ArLog::log(ArLog::Normal, "%i: %ix%i", wayPoints[grid.X][grid.Y].id, grid.X, grid.Y);
//wayPoints[grid.X][grid.Y].listConnections();
}
}
//std::vector<WayPoint*> all1d;
int index = 0;
for (int x = 0; x < (int) tmpWayPoints.size(); x++) {
//std::vector<WayPoint*> tmpVec;
//wayPoints->push_back(&tmpVec);
for (int y = 0; y < (int) tmpWayPoints[x].size(); y++) {
//all1d->push_back(&tmpWayPoints[x][y]);
all1d->at(index)->x = tmpWayPoints[x][y].x;
all1d->at(index)->y = tmpWayPoints[x][y].y;
all1d->at(index)->id = tmpWayPoints[x][y].id;
all1d->at(index)->blocked = tmpWayPoints[x][y].blocked;
index++;
//tmpVec.push_back(&tmpWayPoints[x][y]);
//printf("Cur: %i, %i, %i\n", x, y, tmpWayPoints[x][y].id);
//ArLog::log(ArLog::Normal, "%i: %ix%i", (*(*wayPoints)[x])[y]->id, x, y);
//(*(*wayPoints)[x])[y]->listConnections();
}
}
//for(int i = 0; i < all1d->size(); i++) printf("%i/%i: %ix%i\n", i, all1d->at(i)->id, all1d->at(i)->x, all1d->at(i)->y);
/*
std::vector<WayPoint*> path;
std::vector<WayPoint*> best;
path.push_back(&tmpWayPoints[0][0]);
bool foundPath = findPath(&tmpWayPoints[0][0], &tmpWayPoints[2][2], &path, &best);
printf("Path found (%i): %i\n", (int) foundPath, (int) best.size());
for (int i = 0; i < best.size(); i++) printf("%i wp: %i\n", i, best[i]->id);
*/
}