AREXPORT void arrobot_setpose(double x, double y, double th)
{
AR_DEBUG_LOGINFO();
AR_ASSERT_RETURN(robot);
robot->lock();
robot->moveTo(ArPose(x, y, th));
robot->unlock();
}
/*****************************************************************
** Robot Search Subroutine
******************************************************************/
void S_RobotMotion( ArServerClient *serverclient, ArNetPacket *socket)
{
G_PTZHandler->reset();
ArUtil::sleep(500);
G_PTZHandler->tiltRel(-10);
double robotHeading=0;
double robotDstX = robot.getPose().getX();
double robotDstY = robot.getPose().getY();
//---------Generate the next motion by random numbers------------
cout << "-------- Generate the next motion by random numbers --------" <<endl;
srand( time( NULL ) );
switch(rand()%4)
{
case 0: //¡û
robotDstY += 1000;
robotHeading = 90;
break;
case 1: //¡ü
robotDstX += 1000;
robotHeading = 0;
break;
case 2: //¡ú
robotDstY -= 1000;
robotHeading = -90;
break;
case 3: //¡ý
robotDstX -= 1000;
robotHeading = 180;
break;
}
G_PathPlanning->pathPlanToPose(ArPose(robotDstX, robotDstY, robotHeading), true,true);
cout << "RobotMotion is processing..." <<endl;
while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL )
{
if (G_PathPlanning->getState() == ArPathPlanningTask::ABORTED_PATHPLAN) { G_PathPlanning->cancelPathPlan();break;}
else if(G_PathPlanning->getState() == ArPathPlanningTask::FAILED_PLAN) {G_PathPlanning->pathPlanToPose(ArPose(-300,-100,0),true,true);}
}
serverclient->sendPacketTcp(socket);
//-------------------------------------------------------------------------------------------------------
}
void basic_turn(int turnAngal)
{
// ArTime start;
// G_PTZHandler->reset();
//ArUtil::sleep(500);
//G_PTZHandler->tiltRel(-10);
//CameraMoveCount=0;
//robot.lock();
double robotHeading = robot.getPose().getTh();
robotHeading += turnAngal;
double robotCurrentX = robot.getPose().getX() ;
double robotCurrentY = robot.getPose().getY();
G_PathPlanning->pathPlanToPose(ArPose(robotCurrentX, robotCurrentY , robotHeading),true,true);
while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL );
//robot.setHeading(robot.getTh()+turnAngal);
// robot.unlock();
//start.setToNow();
//while (1)
//{
// robot.lock();
// if (robot.isHeadingDone())
// {
// printf(" Finished turn\n");
//
// ArUtil::sleep(50);
// cout << " current heading: " << " " << robot.getTh()<<endl;
// robot.unlock();
// break;
// }
// if (start.mSecSince() > 5000)
// {
// printf(" Turn timed out\n");
//
// cout << " current heading: " << " " << robot.getTh()<<endl;
// robot.unlock();
// break;
// }
// robot.unlock();
// ArUtil::sleep(10);
//}
}
void SimpleTask::task(void)
{
if (!myGoalDone)
return;
myGoalDone = false;
myState++;
if (myState == 1)
myPathTask->pathPlanToPose(ArPose(1000, 0, 0), true);
//else if (myState == 1)
//myPathTask->pathPlanToGoal("Goal1");
else
{
myState = 0;
myPathTask->pathPlanToPose(myHome, true);
}
}
ArPose GPSMapTools::getCurrentPosFromGPS()
{
ArLLACoords gpsCoords(myGPS->getLatitude(), myGPS->getLongitude(), myGPS->getAltitude());
double x, y, z;
x = y = z = NAN;
// XXX need to have previously saved origin in myMapGPSCoords; either recreate
// here each time, or reset whenever a new map is either created or loaded
// elsewhere, and in ctor if there is a map in arnl.
if(!myHaveMapGPSCoords)
{
// get origin from ArMap and setup MAp->GPS coords translator
if(myMap->hasOriginLatLongAlt())
{
ArPose p = myMap->getOriginLatLong();
myOrigin = ArLLACoords(p.getX(), p.getY(), myMap->getOriginAltitude());
resetMapCoords(myOrigin);
}
}
myMapGPSCoords.convertLLA2MapCoords(gpsCoords, x, y, z);
return ArPose(x, y);
}
void GPSMapTools::setOrigin()
{
if(!checkGPS("setting map origin")) return;
if(!checkMap("setting map origin")) return;
if(myRobot)
{
ArLog::log(ArLog::Normal, "GPSMapTools: Resetting robot odometric position to 0,0,0.");
myRobot->moveTo(0,0,0);
myRobot->com(ArCommands::SIM_RESET);
}
if(myMap->hasOriginLatLongAlt())
{
ArLog::log(ArLog::Terse, "GPSMapTools: setting map origin: Warning: Map already has an origin point, it will be replaced by current position.");
}
myOrigin = ArLLACoords(myGPS->getLatitude(), myGPS->getLongitude(), myGPS->getAltitude());
resetMapCoords(myOrigin);
myMap->lock();
myMap->setOriginLatLongAlt(true, ArPose(myGPS->getLatitude(), myGPS->getLongitude()), myGPS->getAltitude());
myMap->writeFile(myMap->getFileName(), true);
myMap->unlock();
reloadMapFile();
}
void S_TargetApproach( ArServerClient *serverclient, ArNetPacket *socket)
{
//Important: to halt current movement of camera, making the reading curret.
G_PTZHandler->haltPanTilt();
cout << "The last step: TargetApproach!" <<endl;
//G_PathPlanning->setCollisionRange(1000);
//G_PathPlanning->setFrontClearance(40);
//G_PathPlanning->setGoalDistanceTolerance(2500);
double camAngle = -1 * G_PTZHandler->getPan();
double robotHeading = robot.getPose().getTh();
double robotCurrentX = robot.getPose().getX() ;
double robotCurrentY = robot.getPose().getY();
double angle = 0.0;
double distance =0.0;
double targetX, targetY;
int disThreshold = 500;
//test mode
// G_PTZHandler->panRel(-30);
//G_PathPlanning->setFrontClearance(40);
//G_PathPlanning->setObsThreshold(1);
cout << "--------------Path Planning Information--------------------" <<endl;
cout << "SafeCollisionRange = " << G_PathPlanning->getSafeCollisionRange() << endl;
cout << "FrontClearance = " << G_PathPlanning->getFrontClearance() << endl
<< "GoalDistanceTolerance = " << G_PathPlanning->getGoalDistanceTolerance() << endl
<< "setGoalOccupiedFailDistance = " << G_PathPlanning->getGoalOccupiedFailDistance() << endl
<< "getObsThreshold = " << G_PathPlanning->getObsThreshold() <<endl
<< "getLocalPathFailDistance = " << G_PathPlanning->getLocalPathFailDistance() << endl;
//getCurrentGoal
//--------------- Set up the laser range device to read the distance from the target -----------------------------
// for(int i =0; i< 20;i++)
//sick.currentReadingPolar(robotHeading+ camAngle -2.5, robotHeading+ camAngle +2.5, &angle);
//Begin:
sick.lockDevice();
//if (distance == 0 || distance>disThreshold)
distance = sick.currentReadingPolar(/*robotHeading+*/ camAngle -2.5, /*robotHeading+*/ camAngle +2.5, &angle)-disThreshold;
//double distance = sick.currentReadingPolar(89, 90, &angle);
cout << "The closest reading is " << distance << " at " << angle << " degree , " << "disThreshold : " <<disThreshold << " " << robotHeading << " " << camAngle<< endl;
sick.unlockDevice();
//----------------------------------------------------------------------------------------------------------------
//basic_turn(camAngle);
cout << "Camera Angle is " << camAngle << endl;
cout << "before calculation, check originalX, originalY " << robotCurrentX << " " << robotCurrentY << " robotHeading is " << robotHeading << endl<<endl;
coordinateCalculation(robotCurrentX,robotCurrentY,&targetX,&targetY,camAngle,robotHeading,distance);
//
//cout << "before movement, check targetX, target Y" << targetX << " " << targetY << "robotHeading is "<< robotHeading << endl << endl;
cout << "targetX : " <<targetX << " targetY" <<targetY;
G_PathPlanning->pathPlanToPose(ArPose(targetX,targetY,camAngle),true,true);
cout << "RobotMotion is processing..." <<endl;
G_PTZHandler->reset();
ArUtil::sleep(200);
G_PTZHandler->tiltRel(-10);
while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL )
{
if (G_PathPlanning->getState() == ArPathPlanningTask::FAILED_MOVE)
{
G_PathPlanning->cancelPathPlan();cout << "x " << robot.getPose().getX()<< " y " <<robot.getPose().getY() <<endl; break;
}
else if(G_PathPlanning->getState() == ArPathPlanningTask::FAILED_PLAN)
{
// getchar();
//getchar();
//getchar();
// getchar();
// disThreshold+=50;
//
// goto Begin;
}
}
//serverclient->sendPacketTcp(socket);
ArUtil::sleep(3000);
cout << "RobotMotion is heading home..." <<endl;
G_PathPlanning->pathPlanToPose(ArPose(0,0,0),true,true);
while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL )
{
//cout << G_PathPlanning->getState() <<endl;
if (G_PathPlanning->getState() == ArPathPlanningTask::FAILED_MOVE)
{
G_PathPlanning->cancelPathPlan(); break;
}
// //else if(G_PathPlanning->getState() == ArPathPlanningTask::FAILED_PLAN)
// // {G_PathPlanning->pathPlanToPose(ArPose(-300,30,0),true,true);}
}
}
int main(int argc, char **argv)
{
Aria::init();
ArServerBase server;
ArArgumentParser parser(&argc, argv);
ArServerSimpleOpener simpleOpener(&parser);
// parse the command line... fail and print the help if the parsing fails
// or if help was requested
parser.loadDefaultArguments();
if (!simpleOpener.parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
simpleOpener.logOptions();
exit(1);
}
// first open the server up
if (!simpleOpener.open(&server))
{
if (simpleOpener.wasUserFileBad())
printf("Error: Bad user/password/permissions file.\n");
else
printf("Error: Could not open server port. Use -help to see options.\n");
exit(1);
}
// This is the service that provides drawing data to the client.
ArServerInfoDrawings drawings(&server);
// Add our custom drawings
drawings.addDrawing(
// shape: color: size: layer:
new ArDrawingData("polyLine", ArColor(255, 0, 0), 2, 49),
"exampleDrawing_Home",
new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleHomeDrawingNetCallback)
);
drawings.addDrawing(
new ArDrawingData("polyDots", ArColor(0, 255, 0), 250, 48),
"exampleDrawing_Dots",
new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleDotsDrawingNetCallback)
);
drawings.addDrawing(
new ArDrawingData("polySegments", ArColor(0, 0, 0), 4, 52),
"exampleDrawing_XMarksTheSpot",
new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleXDrawingNetCallback)
);
drawings.addDrawing(
new ArDrawingData("polyArrows", ArColor(255, 0, 255), 500, 100),
"exampleDrawing_Arrows",
new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleArrowsDrawingNetCallback)
);
Circle circle(&drawings, "exampleDrawing_circle",
new ArDrawingData("polySegments", ArColor(255, 150, 0), 3, 120));
circle.setPos(ArPose(0, -5000));
circle.setRadius(1000);
circle.setNumPoints(360);
// log whatever we wanted to before the runAsync
simpleOpener.checkAndLog();
// run the server thread in the background
server.runAsync();
printf("Server is now running...\n");
// Add a key handler mostly that windows can exit by pressing
// escape, note that the key handler prevents you from running this program
// in the background on Linux.
ArKeyHandler *keyHandler;
if ((keyHandler = Aria::getKeyHandler()) == NULL)
{
keyHandler = new ArKeyHandler;
Aria::setKeyHandler(keyHandler);
printf("To exit, press escape.\n");
}
double circleRadius = 1000;
while(true)
{
ArUtil::sleep(100);
circleRadius += 50;
if(circleRadius > 5000)
circleRadius = 0;
circle.setRadius(circleRadius);
}
Aria::shutdown();
exit(0);
}
int main(int argc, char **argv)
{
int ret;
std::string str;
ArSerialConnection con;
double dist, angle;
std::list<ArPoseWithTime *> *readings;
std::list<ArPoseWithTime *>::iterator it;
double farDist, farAngle;
bool found;
ArGlobalFunctor failedConnectCB(&failedConnect);
Aria::init();
sick = new ArSick;
// open the connection, if it fails, exit
if ((ret = con.open("/dev/ttyS2")) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
sick->configure(false);
sick->setDeviceConnection(&con);
sick->setFilterNearDist(0);
sick->setMinRange(0);
sick->addFailedConnectCB(&failedConnectCB, ArListPos::FIRST);
sick->runAsync();
ArUtil::sleep(100);
sick->lockDevice();
sick->asyncConnect();
sick->unlockDevice();
while (!sick->isConnected())
ArUtil::sleep(100);
printf("Connected\n");
// while (sick->isConnected())
int times = 0;
while (sick->getRunning())
{
//dist = sick->getCurrentBuffer().getClosestPolar(-90, 90, ArPose(0, 0), 30000, &angle);
sick->lockDevice();
dist = sick->currentReadingPolar(-90, 90, &angle);
if (dist < sick->getMaxRange())
printf("Closest reading %.2f mm away at %.2f degrees\n", dist, angle);
else
printf("No close reading.\n");
readings = sick->getCurrentBuffer();
int i = 0;
for (it = readings->begin(), found = false; it != readings->end(); it++)
{
i++;
dist = (*it)->findDistanceTo(ArPose(0, 0));
angle = (*it)->findAngleTo(ArPose(0, 0));
if (!found || dist > farDist)
{
found = true;
farDist = dist;
farAngle = angle;
}
}
if (found)
printf("Furthest reading %.2f mm away at %.2f degrees\n",
farDist, farAngle);
else
printf("No far reading found.\n");
printf("%d readings\n\n", i);
sick->unlockDevice();
ArUtil::sleep(100);
}
sick->lockDevice();
sick->stopRunning();
sick->disconnect();
sick->unlockDevice();
system("echo 'succeeded' >> results");
return 0;
}
ArPose RosArnlNode::rosPoseToArPose(const geometry_msgs::Pose& p)
{
return ArPose( p.position.x * 1000.0, p.position.y * 1000.0, tf::getYaw(p.orientation) / (M_PI/180.0) );
}
AREXPORT void ArLaser::setSensorPosition(
double x, double y, double th, double z)
{
setSensorPosition(ArPose(x, y, th), z);
}
int _tmain(int argc, char** argv)
{
//-------------- M A I N D E L P R O G R A M A D E L R O B O T------------//
//----------------------------------------------------------------------------------//
//inicializaion de variables
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArSimpleConnector simpleConnector(&parser);
ArRobot robot;
ArSonarDevice sonar;
ArAnalogGyro gyro(&robot);
robot.addRangeDevice(&sonar);
ActionTurns turn(400, 55);
robot.addAction(&turn, 49);
ActionTurns turn2(400, 55);
robot.addAction(&turn2, 49);
turn.deactivate();
turn2.deactivate();
// presionar tecla escape para salir del programa
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("Presionar ESC para salir\n");
// uso de sonares para evitar colisiones con las paredes u
// obstaculos grandes, mayores a 8cm de alto
ArActionLimiterForwards limiterAction("limitador velocidad cerca", 300, 600, 250);
ArActionLimiterForwards limiterFarAction("limitador velocidad lejos", 300, 1100, 400);
ArActionLimiterTableSensor tableLimiterAction;
robot.addAction(&tableLimiterAction, 100);
robot.addAction(&limiterAction, 95);
robot.addAction(&limiterFarAction, 90);
// Inicializon la funcion de goto
ArActionGoto gotoPoseAction("goto");
robot.addAction(&gotoPoseAction, 50);
// Finaliza el goto si es que no hace nada
ArActionStop stopAction("stop");
robot.addAction(&stopAction, 40);
// Parser del CLI
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
exit(1);
}
// Conexion del robot
if (!simpleConnector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::exit(1);
}
robot.runAsync(true);
// enciende motores, apaga sonidos
robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);
// Imprimo algunos datos del robot como posicion velocidad y bateria
robot.lock();
ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV",
robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage());
robot.unlock();
const int duration = 100000; //msec
ArLog::log(ArLog::Normal, "Completados los puntos en %d segundos", duration/1000);
bool first = true;
int goalNum = 0;
int color = 3;
ArTime start;
start.setToNow();
while (Aria::getRunning())
{
robot.lock();
// inicia el primer punto
if (first || gotoPoseAction.haveAchievedGoal())
{
first = false;
goalNum++; //cambia de 0 a 1 el contador
printf("El contador esta en: --> %d <---\n",goalNum);
if (goalNum > 20)
goalNum = 1;
//comienza la secuencia de puntos
if (goalNum == 1)
{
gotoPoseAction.setGoal(ArPose(1150, 0));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
// Imprimo algunos datos del robot como posicion velocidad y bateria
robot.lock();
ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV",
robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage());
robot.unlock();
}
else if (goalNum == 2)
{
printf("Gira 90 grados izquierda\n");
robot.unlock();
turn.myActivate = 1;
turn.myDirection = 1;
turn.activate();
ArUtil::sleep(1000);
turn.deactivate();
turn.myActivate = 0;
turn.myDirection = 0;
robot.lock();
}
else if (goalNum == 3)
{
gotoPoseAction.setGoal(ArPose(1150, 2670));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
// Imprimo algunos datos del robot como posicion velocidad y bateria
robot.lock();
ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV",
robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage());
robot.unlock();
}
else if (goalNum == 4)
{
printf("Gira 90 grados izquierda\n");
robot.unlock();
turn2.myActivate = 1;
turn2.myDirection = 1;
turn2.activate();
ArUtil::sleep(1000);
turn2.deactivate();
turn2.myActivate = 0;
turn2.myDirection = 0;
robot.lock();
}
else if (goalNum == 5)
{
gotoPoseAction.setGoal(ArPose(650, 2670));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
else if (goalNum == 6)
{
printf("Gira 90 grados izquierda\n");
robot.unlock();
turn2.myActivate = 1;
turn2.myDirection = 1;
turn2.activate();
ArUtil::sleep(1000);
turn2.deactivate();
turn2.myActivate = 0;
turn2.myDirection = 0;
robot.lock();
}
else if (goalNum == 7)
{
gotoPoseAction.setGoal(ArPose(650, 0));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
else if (goalNum == 8)
{
gotoPoseAction.setGoal(ArPose(1800,1199));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
else if (goalNum == 9)
{
gotoPoseAction.setGoal(ArPose(2600, 1199));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
else if (goalNum == 10)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(2800, 850));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 2)
{
gotoPoseAction.setGoal(ArPose(3500, 1199));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(2800, 1550));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 11)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(2800, 613));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 2)
{
printf("Gira 180 grados derecha\n");
robot.unlock();
turn2.myActivate = 1;
turn2.myDirection = 2;
turn2.activate();
ArUtil::sleep(2000);
turn2.deactivate();
turn2.myActivate = 0;
turn2.myDirection = 0;
robot.lock();
goalNum = 19;
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(2800, 1785));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 12)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(3300, 413));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(3300, 1985));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 13)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(3500, 413));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(3500, 1985));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 14)
{
//secuencia de drop de la figura
}
else if (goalNum == 15)
{
printf("Gira 180 grados derecha\n");
robot.unlock();
turn2.myActivate = 1;
turn2.myDirection = 2;
turn2.activate();
ArUtil::sleep(2000);
turn2.deactivate();
turn2.myActivate = 0;
turn2.myDirection = 0;
robot.lock();
}
else if (goalNum == 16)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(3300, 413));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(3300, 1985));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 17)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(2800, 603));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(2800, 1795));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 18)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(2800, 860));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(2800, 1540));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 19)
{
gotoPoseAction.setGoal(ArPose(2600, 1199));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
else if (goalNum == 20)
{
gotoPoseAction.setGoal(ArPose(1800, 1199));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
if(start.mSecSince() >= duration) {
ArLog::log(ArLog::Normal, "No puede llegar al punto, y la aplicacion saldra en %d", duration/1000);
gotoPoseAction.cancelGoal();
robot.unlock();
ArUtil::sleep(3000);
break;
}
robot.unlock();
ArUtil::sleep(10);
}
// Robot desconectado al terminal el sleep
Aria::shutdown();
//----------------------------------------------------------------------------------//
return 0;
}
int main(int argc, char **argv)
{
Aria::init();
argc = 3;
argv[0] = "";
argv[1] = "-rp";
argv[2] = "/dev/ttyUSB0";
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobot robot;
ArAnalogGyro gyro(&robot);
ArSonarDevice sonar;
ArRobotConnector robotConnector(&parser, &robot);
if(!robotConnector.connectRobot())
{
if(parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
}
}
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
}
imgdb.setRobot(&robot);
int Rc;
pthread_t slam_thread;
Rc = pthread_create(&slam_thread, NULL, slam_event, NULL);
if (Rc) { printf("Create slam thread failed!\n"); exit(-1); }
ImageList* current = NULL;
while (!current) current = imgdb.getEdge();
// current->person_point
robot.addRangeDevice(&sonar);
robot.runAsync(true);
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit\n");
robot.setAbsoluteMaxRotVel(30);
ArActionStallRecover recover;
ArActionBumpers bumpers;
double minDistance=10000;
// ArPose endPoint(point.x, point.z);
// current->collect_x
// current->collect_y
// robot.getX()
// robot.getY()
// current = imgdb.getEdge();
// for (int i = 0; i < current->edge.size(); i++) {
// for (int j = 0; j < current->edge[i].size() - 1; j++) {
// double dis = sqrt(pow(current->edge[i][j].z,2)+(pow(current->edge[i][j].x,2)));
// if(dis<minDistance){
// minDistance=dis;
// }
// }
// }
// 引入避障action,前方安全距离为500mm,侧边安全距离为340mm
Avoid avoidSide("Avoid side", ArPose(0, 0), 800, 100, 1000*minDistance);
robot.addAction(&recover, 100);
robot.addAction(&bumpers, 95);
robot.addAction(&avoidSide, 80);
ArActionStop stopAction("stop");
robot.addAction(&stopAction, 50);
robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);
const int duration = 500000;
bool first = true;
int goalNum = 0;
ArTime start;
start.setToNow();
while (Aria::getRunning())
{
current = imgdb.getEdge();
for (int i = 0; i < current->edge.size(); i++) {
for (int j = 0; j < current->edge[i].size() - 1; j++) {
// printf("i=%d\n",i);
double dis = sqrt(pow(current->edge[i][j].z,2)+(pow(current->edge[i][j].x,2)));
// printf("dis=%f,mindis=%f\n",dis,minDistance);
if(dis<minDistance){
minDistance=dis;
printf("min=%f\n",minDistance);
}
}
}
//引入避障action,前方安全距离为500mm,侧边安全距离为340mm
// Avoid avoidSide("Avoid side", ArPose(0, 0), 800, 200, minDistance);
// robot.addAction(&avoidSide,80);
robot.lock();
if (first || avoidSide.haveAchievedGoal())
{
first = false;
goalNum++;
printf("count goalNum = %d\n", goalNum);
if (goalNum > 2){
goalNum = 1; // start again at goal #1
}
// avoidSide.setGoal(ArPose(10000,0));
if(goalNum==1){
printf("zuobiaox=%f,zuobiaoy=%f\n",-current->person_point.z,current->person_point.x);
avoidSide.setGoal(ArPose(1000*current->person_point.z, -1000*current->person_point.x));
printf("goalNum == 1\n\n");
}
else if(goalNum==2){
printf("zuobiaox=%f,zuobiaoy=%f\n",-current->person_point.z,current->person_point.x);
avoidSide.setGoal(ArPose(1000*current->person_point.z, -1000*current->person_point.x));
printf("goalNum == 2\n\n");
}
}
if(start.mSecSince() >= duration) {
ArLog::log(ArLog::Normal, "%d seconds have elapsed. Cancelling current goal, waiting 3 seconds, and exiting.", duration/1000);
avoidSide.cancelGoal();
robot.unlock();
ArUtil::sleep(3000);
break;
}
robot.unlock();
ArUtil::sleep(100);
}
Aria::exit(0);
return 0;
}
void GPSMapTools::addHomeHere(ArArgumentBuilder *args)
{
if(!checkGPS("adding home")) return;
if(!checkMap("adding home")) return;
ArPose p = getCurrentPosFromGPS();
ArLog::log(ArLog::Normal, "GPSMapTools: Adding home in map at GPS position (x=%.2f, y=%.2f)", p.getX(), p.getY());
myMap->lock();
ArMapObject *newobj;
myMap->getMapObjects()->push_back(newobj = new ArMapObject("Home", p, "Home", "ICON", args->getFullString(), false, ArPose(0, 0), ArPose(0, 0)));
printf("\tnew map object is:\n\t%s\n", newobj->toString());
newobj->log();
myMap->writeFile(myMap->getFileName(), true);
myMap->unlock();
reloadMapFile();
}
int __cdecl _tmain (int argc, char** argv)
{
//------------ I N I C I O M A I N D E L P R O G R A M A D E L R O B O T-----------//
//inicializaion de variables
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArSimpleConnector simpleConnector(&parser);
ArRobot robot;
ArSonarDevice sonar;
ArAnalogGyro gyro(&robot);
robot.addRangeDevice(&sonar);
ActionGos go(500, 350);
robot.addAction(&go, 48);
ActionTurns turn(400, 110);
robot.addAction(&turn, 49);
ActionTurns turn2(400, 110);
robot.addAction(&turn2, 49);
// presionar tecla escape para salir del programa
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("Presionar ESC para salir\n");
// uso de sonares para evitar colisiones con las paredes u
// obstaculos grandes, mayores a 8cm de alto
ArActionLimiterForwards limiterAction("limitador velocidad cerca", 300, 600, 250);
ArActionLimiterForwards limiterFarAction("limitador velocidad lejos", 300, 1100, 400);
ArActionLimiterTableSensor tableLimiterAction;
robot.addAction(&tableLimiterAction, 100);
robot.addAction(&limiterAction, 95);
robot.addAction(&limiterFarAction, 90);
// Inicializon la funcion de goto
ArActionGoto gotoPoseAction("goto");
robot.addAction(&gotoPoseAction, 50);
// Finaliza el goto si es que no hace nada
ArActionStop stopAction("stop");
robot.addAction(&stopAction, 40);
// Parser del CLI
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
exit(1);
}
// Conexion del robot
if (!simpleConnector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::exit(1);
}
robot.runAsync(true);
// enciende motores, apaga sonidos
robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);
// Imprimo algunos datos del robot como posicion velocidad y bateria
robot.lock();
ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV",
robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage());
robot.unlock();
const int duration = 100000; //msec
ArLog::log(ArLog::Normal, "Completados los puntos en %d segundos", duration/1000);
// ============================ INICIO CONFIG COM =================================//
CSerial serial;
LONG lLastError = ERROR_SUCCESS;
// Trata de abrir el com seleccionado
lLastError = serial.Open(_T("COM3"),0,0,false);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Imposible abrir el COM"));
// Inicia el puerto serial (9600,8N1)
lLastError = serial.Setup(CSerial::EBaud9600,CSerial::EData8,CSerial::EParNone,CSerial::EStop1);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Imposible setear la config del COM"));
// Register only for the receive event
lLastError = serial.SetMask(CSerial::EEventBreak |
CSerial::EEventCTS |
CSerial::EEventDSR |
CSerial::EEventError |
CSerial::EEventRing |
CSerial::EEventRLSD |
CSerial::EEventRecv);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port event mask"));
// Use 'non-blocking' reads, because we don't know how many bytes
// will be received. This is normally the most convenient mode
// (and also the default mode for reading data).
lLastError = serial.SetupReadTimeouts(CSerial::EReadTimeoutNonblocking);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port read timeout."));
// ============================ FIN CONFIG COM =================================//
bool first = true;
int goalNum = 0;
int color = 3;
ArTime start;
start.setToNow();
while (Aria::getRunning())
{
robot.lock();
// inicia el primer punto
if (first || gotoPoseAction.haveAchievedGoal())
{
first = false;
goalNum++; //cambia de 0 a 1 el contador
printf("El contador esta en: --> %d <---\n",goalNum);
if (goalNum > 20)
goalNum = 1;
//comienza la secuencia de puntos
if (goalNum == 1)
{
gotoPoseAction.setGoal(ArPose(1150, 0));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
// Imprimo algunos datos del robot como posicion velocidad y bateria
robot.lock();
ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV",
robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage());
robot.unlock();
// Create the sound queue.
ArSoundsQueue soundQueue;
// Run the sound queue in a new thread
soundQueue.runAsync();
std::vector<const char*> filenames;
filenames.push_back("sound-r2a.wav");
soundQueue.play(filenames[0]);
}
else if (goalNum == 2)
{
printf("Gira 90 grados izquierda\n");
robot.unlock();
turn.myActivate = 1;
turn.myDirection = 1;
turn.activate();
ArUtil::sleep(1000);
turn.deactivate();
turn.myActivate = 0;
turn.myDirection = 0;
robot.lock();
}
else if (goalNum == 3)
{
gotoPoseAction.setGoal(ArPose(1150, 2670));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
// Imprimo algunos datos del robot como posicion velocidad y bateria
robot.lock();
ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV",
robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage());
robot.unlock();
}
else if (goalNum == 4)
{
printf("Gira 90 grados izquierda\n");
robot.unlock();
turn2.myActivate = 1;
turn2.myDirection = 1;
turn2.activate();
ArUtil::sleep(1000);
turn2.deactivate();
turn2.myActivate = 0;
turn2.myDirection = 0;
robot.lock();
}
else if (goalNum == 5)
{
gotoPoseAction.setGoal(ArPose(650, 2670));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
else if (goalNum == 6)
{
printf("Gira 90 grados izquierda\n");
robot.unlock();
turn2.myActivate = 1;
turn2.myDirection = 1;
turn2.activate();
ArUtil::sleep(1000);
turn2.deactivate();
turn2.myActivate = 0;
turn2.myDirection = 0;
robot.lock();
}
else if (goalNum == 7)
{
gotoPoseAction.setGoal(ArPose(650, 0));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
else if (goalNum == 8)
{
gotoPoseAction.setGoal(ArPose(1800,1199));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
else if (goalNum == 9)
{
gotoPoseAction.setGoal(ArPose(2600, 1199));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
else if (goalNum == 10)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(2800, 850));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 2)
{
gotoPoseAction.setGoal(ArPose(3500, 1199));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(2800, 1550));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 11)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(2800, 613));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 2)
{
printf("Gira 180 grados derecha\n");
robot.unlock();
turn2.myActivate = 1;
turn2.myDirection = 2;
turn2.activate();
ArUtil::sleep(2000);
turn2.deactivate();
turn2.myActivate = 0;
turn2.myDirection = 0;
robot.lock();
goalNum = 19;
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(2800, 1785));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 12)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(3300, 413));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(3300, 1985));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 13)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(3500, 413));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(3500, 1985));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 14)
{
robot.unlock();
//Valor para el while
bool fContinue = true;
// <<<<<<------------- 1 Parte Secuencia: BAJA BRAZO ------------->>>>>> //
lLastError = serial.Write("b");
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to send data"));
//-------------------------E S C U C H A C O M ----------------------------//
do
{
// Wait for an event
lLastError = serial.WaitEvent();
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to wait for a COM-port event."));
// Save event
const CSerial::EEvent eEvent = serial.GetEventType();
// Handle break event
if (eEvent & CSerial::EEventBreak)
{
printf("\n### BREAK received ###\n");
}
// Handle CTS event
if (eEvent & CSerial::EEventCTS)
{
printf("\n### Clear to send %s ###\n", serial.GetCTS()?"on":"off");
}
// Handle DSR event
if (eEvent & CSerial::EEventDSR)
{
printf("\n### Data set ready %s ###\n", serial.GetDSR()?"on":"off");
}
// Handle error event
if (eEvent & CSerial::EEventError)
{
printf("\n### ERROR: ");
switch (serial.GetError())
{
case CSerial::EErrorBreak: printf("Break condition"); break;
case CSerial::EErrorFrame: printf("Framing error"); break;
case CSerial::EErrorIOE: printf("IO device error"); break;
case CSerial::EErrorMode: printf("Unsupported mode"); break;
case CSerial::EErrorOverrun: printf("Buffer overrun"); break;
case CSerial::EErrorRxOver: printf("Input buffer overflow"); break;
case CSerial::EErrorParity: printf("Input parity error"); break;
case CSerial::EErrorTxFull: printf("Output buffer full"); break;
default: printf("Unknown"); break;
}
printf(" ###\n");
}
// Handle ring event
if (eEvent & CSerial::EEventRing)
{
printf("\n### RING ###\n");
}
// Handle RLSD/CD event
if (eEvent & CSerial::EEventRLSD)
{
printf("\n### RLSD/CD %s ###\n", serial.GetRLSD()?"on":"off");
}
// Handle data receive event
if (eEvent & CSerial::EEventRecv)
{
// Read data, until there is nothing left
DWORD dwBytesRead = 0;
char szBuffer[101];
do
{
// Lee datos del Puerto COM
lLastError = serial.Read(szBuffer,sizeof(szBuffer)-1,&dwBytesRead);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to read from COM-port."));
if (dwBytesRead > 0)
{
//Preseteo color
int color = 0;
// Finaliza el dato, asi que sea una string valida
szBuffer[dwBytesRead] = '\0';
// Display the data
printf("%s", szBuffer);
// <<<<<<----------- 2 Parte Secuencia: CIERRA GRIPPER ----------->>>>>> //
if (strchr(szBuffer,76))
{
lLastError = serial.Write("c");
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to send data"));
}
// <<<<<<------------- 3 Parte Secuencia: SUBE BRAZO ------------->>>>>> //
if (strchr(szBuffer,117))
{
lLastError = serial.Write("s");
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to send data"));
}
// <<<<<<------------- 4 Parte Secuencia: COLOR ------------->>>>>> //
if (strchr(szBuffer,72))
{
lLastError = serial.Write("C");
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to send data"));
}
// <<<<<<---------- 5.1 Parte Secuencia: COLOR ROJO---------->>>>>> //
if (strchr(szBuffer,82))
{
color = 1;
//salir del bucle
fContinue = false;
}
// <<<<<<---------- 5.2 Parte Secuencia: COLOR AZUL ---------->>>>>> //
if (strchr(szBuffer,66))
{
color = 2;
//salir del bucle
fContinue = false;
}
// <<<<<<---------- 5.3 Parte Secuencia: COLOR VERDE ---------->>>>>> //
if (strchr(szBuffer,71))
{
color = 3;
//salir del bucle
fContinue = false;
}
}
}
while (dwBytesRead == sizeof(szBuffer)-1);
}
}
while (fContinue);
// Close the port again
serial.Close();
robot.lock();
}
else if (goalNum == 15)
{
printf("Gira 180 grados derecha\n");
robot.unlock();
turn2.myActivate = 1;
turn2.myDirection = 2;
turn2.activate();
ArUtil::sleep(2000);
turn2.deactivate();
turn2.myActivate = 0;
turn2.myDirection = 0;
robot.lock();
}
else if (goalNum == 16)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(3300, 413));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(3300, 1985));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 17)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(2800, 603));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(2800, 1795));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 18)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(2800, 860));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(2800, 1540));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 19)
{
gotoPoseAction.setGoal(ArPose(2600, 1199));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
else if (goalNum == 20)
{
gotoPoseAction.setGoal(ArPose(1800, 1199));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
if(start.mSecSince() >= duration) {
ArLog::log(ArLog::Normal, "No puede llegar al punto, y la aplicacion saldra en %d", duration/1000);
gotoPoseAction.cancelGoal();
robot.unlock();
ArUtil::sleep(3000);
break;
}
robot.unlock();
ArUtil::sleep(10);
}
// Robot desconectado al terminal el sleep
Aria::shutdown();
//------------ F I N M A I N D E L P R O G R A M A D E L R O B O T-----------//
return 0;
}
int main(void)
{
ArPose pose;
ArLine xLine(-2000, 0, 2000, 0);
ArLine yLine(100, 500, 100, -500);
ArLineSegment xLineSeg(-2000, 0, 2000, 0);
ArLineSegment yLineSeg(100, 500, 100, -500);
// test all our segments
testIntersection(&xLine, &yLine, 100, 0, "xLine and yLine");
testIntersection(&xLineSeg, &yLine, 100, 0, "xLineSeg and yLine");
testIntersection(&yLineSeg, &xLine, 100, 0, "yLineSeg and xLine");
testIntersection(&xLineSeg, &yLineSeg, 100, 0, "xLineSeg and yLineSeg");
// test the perp on all the segments
testPerp(&xLineSeg, ArPose(-2000, 50), ArPose(-2000, 0), "xLineSeg end1");
testPerp(&xLineSeg, ArPose(2000, 50), ArPose(2000, 0), "xLineSeg end2");
testPerp(&xLineSeg, ArPose(357, 50), ArPose(357, 0), "xLineSeg middle");
testNotPerp(&xLineSeg, ArPose(2001, 0), "xLineSeg beyond end2");
testNotPerp(&xLineSeg, ArPose(3000, 0), "xLineSeg way beyond end2");
testNotPerp(&xLineSeg, ArPose(-2001, 0), "xLineSeg beyond end1");
testNotPerp(&xLineSeg, ArPose(-3000, 0), "xLineSeg way beyond end1");
testPerp(&xLineSeg, ArPose(1000, 0), ArPose(1000, 0), "xLineSeg point on line");
printf("All tests completed successfully\n");
return 0;
}
AREXPORT bool ArLineFinder::combineLines(void)
{
int start = 0;
int len = myLines->size();
// this is the min line distance
std::map<int, ArLineFinderSegment *> *newLines;
int numNewLines = 0;
int numNewMerges = 0;
ArLineFinderSegment *newLine;
newLines = new std::map<int, ArLineFinderSegment *>;
if (myPrinting)
ArLog::log(ArLog::Normal, "new iteration\n");
bool nextMerged = false;
for (start = 0; start < len; start++)
{
if (nextMerged)
{
nextMerged = false;
continue;
}
if (start + 1 == len)
{
if (myPrinting)
ArLog::log(ArLog::Normal, "inserted last one %g",
ArPose((*myLines)[start]->getX1(),
(*myLines)[start]->getY1()).findDistanceTo(
ArPose((*myLines)[start]->getX2(), (*myLines)[start]->getY2())));
(*newLines)[numNewLines] = new ArLineFinderSegment(*((*myLines)[start]));
numNewLines++;
continue;
}
newLine = averageSegments((*myLines)[start], (*myLines)[start+1]);
if (newLine != NULL)
{
if (myPrinting)
ArLog::log(ArLog::Normal, "merged %g %g to %g",
(*myLines)[start]->getLength(),
(*myLines)[start+1]->getLength(),
newLine->getLength());
(*newLines)[numNewLines] = newLine;
numNewLines++;
numNewMerges++;
nextMerged = true;
}
else
{
if (myPrinting)
ArLog::log(ArLog::Normal, "inserted anyways %g",
(*myLines)[start]->getLength());
(*newLines)[numNewLines] = new ArLineFinderSegment(*((*myLines)[start]));
numNewLines++;
}
}
// move the new lines over and delete the old ones
if (myLines != NULL && myLines->begin() != myLines->end())
{
ArUtil::deleteSetPairs(myLines->begin(), myLines->end());
delete myLines;
myLines = NULL;
}
else if (myLines != NULL)
{
delete myLines;
myLines = NULL;
}
myLines = newLines;
// if we didn't merge any just return
if (numNewMerges == 0)
return true;
// otherwise do it again
return combineLines();
}
int main(int argc, char **argv)
{
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobot robot;
ArAnalogGyro gyro(&robot);
ArSonarDevice sonar;
ArRobotConnector robotConnector(&parser, &robot);
ArLaserConnector laserConnector(&parser, &robot, &robotConnector);
// Connect to the robot, get some initial data from it such as type and name,
// and then load parameter files for this robot.
if(!robotConnector.connectRobot())
{
ArLog::log(ArLog::Terse, "gotoActionExample: Could not connect to the robot.");
if(parser.checkHelpAndWarnUnparsed())
{
// -help not given
Aria::logOptions();
Aria::exit(1);
}
}
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
}
ArLog::log(ArLog::Normal, "gotoActionExample: Connected to robot.");
robot.addRangeDevice(&sonar);
robot.runAsync(true);
// Make a key handler, so that escape will shut down the program
// cleanly
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit\n");
// Collision avoidance actions at higher priority
ArActionLimiterForwards limiterAction("speed limiter near", 300, 600, 250);
ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100, 400);
ArActionLimiterTableSensor tableLimiterAction;
robot.addAction(&tableLimiterAction, 100);
robot.addAction(&limiterAction, 95);
robot.addAction(&limiterFarAction, 90);
// Goto action at lower priority
ArActionGoto gotoPoseAction("goto");
robot.addAction(&gotoPoseAction, 50);
// Stop action at lower priority, so the robot stops if it has no goal
ArActionStop stopAction("stop");
robot.addAction(&stopAction, 40);
// turn on the motors, turn off amigobot sounds
robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);
const int duration = 30000; //msec
ArLog::log(ArLog::Normal, "Going to four goals in turn for %d seconds, then cancelling goal and exiting.", duration/1000);
bool first = true;
int goalNum = 0;
ArTime start;
start.setToNow();
while (Aria::getRunning())
{
robot.lock();
// Choose a new goal if this is the first loop iteration, or if we
// achieved the previous goal.
if (first || gotoPoseAction.haveAchievedGoal())
{
first = false;
goalNum++;
if (goalNum > 4)
goalNum = 1; // start again at goal #1
// set our positions for the different goals
if (goalNum == 1)
gotoPoseAction.setGoal(ArPose(2500, 0));
else if (goalNum == 2)
gotoPoseAction.setGoal(ArPose(2500, 2500));
else if (goalNum == 3)
gotoPoseAction.setGoal(ArPose(0, 2500));
else if (goalNum == 4)
gotoPoseAction.setGoal(ArPose(0, 0));
ArLog::log(ArLog::Normal, "Going to next goal at %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if(start.mSecSince() >= duration) {
ArLog::log(ArLog::Normal, "%d seconds have elapsed. Cancelling current goal, waiting 3 seconds, and exiting.", duration/1000);
gotoPoseAction.cancelGoal();
robot.unlock();
ArUtil::sleep(3000);
break;
}
robot.unlock();
ArUtil::sleep(100);
}
// Robot disconnected or time elapsed, shut down
Aria::exit(0);
return 0;
}
int APIENTRY _tWinMain(HINSTANCE hInstance,
HINSTANCE hPrevInstance,
LPTSTR lpCmdLine,
int nCmdShow)
{
UNREFERENCED_PARAMETER(hPrevInstance);
UNREFERENCED_PARAMETER(lpCmdLine);
//-------------- M A I N D E L P R O G R A M A D E L R O B O T------------//
//----------------------------------------------------------------------------------------------------------
//inicializaion de variables
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArSimpleConnector simpleConnector(&parser);
ArRobot robot;
ArSonarDevice sonar;
ArAnalogGyro gyro(&robot);
robot.addRangeDevice(&sonar);
// presionar tecla escape para salir del programa
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit\n");
// uso de sonares para evitar colisiones con las paredes u
// obstaculos grandes, mayores a 8cm de alto
ArActionLimiterForwards limiterAction("speed limiter near", 300, 600, 250);
ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100, 400);
ArActionLimiterTableSensor tableLimiterAction;
robot.addAction(&tableLimiterAction, 100);
robot.addAction(&limiterAction, 95);
robot.addAction(&limiterFarAction, 90);
// Inicializon la funcion de goto
ArActionGoto gotoPoseAction("goto");
robot.addAction(&gotoPoseAction, 50);
// Finaliza el goto si es que no hace nada
ArActionStop stopAction("stop");
robot.addAction(&stopAction, 40);
// Parser del CLI
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
exit(1);
}
// Conexion del robot
if (!simpleConnector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::exit(1);
}
robot.runAsync(true);
// enciende motores, apaga sonidos
robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);
const int duration = 100000; //msec
ArLog::log(ArLog::Normal, "Completados los puntos en %d segundos", duration/1000);
bool first = true;
int horiz = 1800;
int vert = 380;
int goalNum = 0;
ArTime start;
start.setToNow();
while (Aria::getRunning())
{
robot.lock();
// inicia el primer punto
if (first || gotoPoseAction.haveAchievedGoal())
{
first = false;
goalNum++; //cambia de 0 a 1 el contador
if (goalNum > 7)
goalNum = 1;
//comienza la secuencia de puntos
if (goalNum == 1)
gotoPoseAction.setGoal(ArPose(horiz, vert*0));
else if (goalNum == 2)
gotoPoseAction.setGoal(ArPose(0, vert*1));
else if (goalNum == 3)
gotoPoseAction.setGoa l(ArPose(horiz, vert*2)+5);
else if (goalNum == 4)
gotoPoseAction.setGoal(ArPose(0, vert*3));
else if (goalNum == 5)
gotoPoseAction.setGoal(ArPose(horiz, vert*4+5));
else if (goalNum == 6)
gotoPoseAction.setGoal(ArPose(0, vert*5));
else if (goalNum == 7)
gotoPoseAction.setGoal(ArPose(0, vert*0));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if(start.mSecSince() >= duration) {
ArLog::log(ArLog::Normal, "%d seconds have elapsed. Cancelling current goal, waiting 3 seconds, and exiting.", duration/1000);
gotoPoseAction.cancelGoal();
robot.unlock();
ArUtil::sleep(3000);
break;
}
robot.unlock();
ArUtil::sleep(100);
}
// Robot desconectado al terminal el sleep
Aria::shutdown();
return 0;
//----------------------------------------------------------------------------------------------------------
// TODO: Place code here.
MSG msg;
HACCEL hAccelTable;
// Initialize global strings
LoadString(hInstance, IDS_APP_TITLE, szTitle, MAX_LOADSTRING);
LoadString(hInstance, IDC_VENTANAROBOT, szWindowClass, MAX_LOADSTRING);
MyRegisterClass(hInstance);
// Perform application initialization:
if (!InitInstance (hInstance, nCmdShow))
{
return FALSE;
}
hAccelTable = LoadAccelerators(hInstance, MAKEINTRESOURCE(IDC_VENTANAROBOT));
// Main message loop:
while (GetMessage(&msg, NULL, 0, 0))
{
if (!TranslateAccelerator(msg.hwnd, hAccelTable, &msg))
{
TranslateMessage(&msg);
DispatchMessage(&msg);
}
}
return (int) msg.wParam;
}
