/** @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);
}
/*!
* 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());
}
}
}
/*!
* 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");
}
}
AREXPORT void ArForbiddenRangeDevice::processMap(void)
{
std::list<ArMapObject *>::const_iterator it;
ArMapObject *obj;
myDataMutex.lock();
ArUtil::deleteSet(mySegments.begin(), mySegments.end());
mySegments.clear();
for (it = myMap->getMapObjects()->begin();
it != myMap->getMapObjects()->end();
it++)
{
obj = (*it);
if (strcmp(obj->getType(), "ForbiddenLine") == 0 &&
obj->hasFromTo())
{
mySegments.push_back(new ArLineSegment(obj->getFromPose(),
obj->getToPose()));
}
if (strcmp(obj->getType(), "ForbiddenArea") == 0 &&
obj->hasFromTo())
{
double angle = obj->getPose().getTh();
double sa = ArMath::sin(angle);
double ca = ArMath::cos(angle);
double fx = obj->getFromPose().getX();
double fy = obj->getFromPose().getY();
double tx = obj->getToPose().getX();
double ty = obj->getToPose().getY();
ArPose P0((fx*ca - fy*sa), (fx*sa + fy*ca));
ArPose P1((tx*ca - fy*sa), (tx*sa + fy*ca));
ArPose P2((tx*ca - ty*sa), (tx*sa + ty*ca));
ArPose P3((fx*ca - ty*sa), (fx*sa + ty*ca));
mySegments.push_back(new ArLineSegment(P0, P1));
mySegments.push_back(new ArLineSegment(P1, P2));
mySegments.push_back(new ArLineSegment(P2, P3));
mySegments.push_back(new ArLineSegment(P3, P0));
}
}
myDataMutex.unlock();
}
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);
}
}