int main(void)
{
std::string str;
int ret;
ArGlobalFunctor sonarPrinterCB(&sonarPrinter);
ArTcpConnection con;
Aria::init();
robot = new ArRobot;
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
exit(0);
}
robot->setDeviceConnection(&con);
if (!robot->blockingConnect())
{
printf("Could not connect to robot->.. exiting\n");
exit(0);
}
robot->addRangeDevice(&sonar);
robot->addUserTask("Sonar printer", 50, &sonarPrinterCB);
robot->comInt(ArCommands::SONAR, 1);
robot->comInt(ArCommands::SOUNDTOG, 0);
robot->run(true);
Aria::shutdown();
}
int main(void)
{
ArTcpConnection con;
ArRobot robot;
int ret;
std::string str;
JoydriveAction jdAct;
FillerThread ft;
ft.create();
FillerThread ft2;
ft2.create();
Aria::init();
/*
if (!jdAct.joystickInited())
{
printf("Do not have a joystick, set up the joystick then rerun the program\n\n");
Aria::shutdown();
return 1;
}
*/
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
robot.setDeviceConnection(&con);
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
lastLoopTime.setToNow();
loopTime = robot.getCycleTime();
robot.addAction(&jdAct, 100);
robot.runAsync(true);
robot.waitForRunExit();
Aria::shutdown();
return 0;
}
int main(int argc, char **argv)
{
std::string str;
int ret;
// connection to the robot
ArTcpConnection con;
// the robot
ArRobot robot;
// ake the joydrive object, which also creates its own thread
Joydrive joyd(&robot);
// the connection handler
ConnHandler ch(&robot, &joyd);
// init aria, which will make a dedicated signal handling thread
Aria::init(Aria::SIGHANDLE_THREAD);
// open the connection with default args, exit if it fails
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
// set the connection on the robot
robot.setDeviceConnection(&con);
// run the robot in its own thread
robot.runAsync(false);
// have the robot connect asyncronously (so its loop is still running)
// if this fails it means that the robot isn't running in its own thread
if (!robot.asyncConnect())
{
printf(
"asyncConnect failed because robot is not running in its own thread.\n");
Aria::shutdown();
return 1;
}
// now we just wait for the robot to be done running
printf("Waiting for the robot's run to exit.\n");
robot.waitForRunExit();
// then we exit
printf("exiting main\n");
Aria::exit(0); // exit program
return 0;
}
int main(int argc, char **argv)
{
std::string str;
int ret;
// connection
ArTcpConnection con;
// robot, this starts it with state reflecting turned off
ArRobot robot(NULL, false);
// make the joydrive instance, which adds its task to the robot
Joydrive joyd(&robot);
// mandatory aria initialization
Aria::init();
// open the connection, if it fails, exit
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::exit(1);
return 1;
}
// set the robots connection
robot.setDeviceConnection(&con);
// try to connect, if we fail exit
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::exit(1);
return 1;
}
// turn off sonar, turn the motors on, turn off amigobot sound
robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
// run the robot, true so that if connection is lost the run stops
robot.run(true);
// now exit
Aria::exit(0); // exit program
return 0;
}
int main(int argc, char **argv)
{
std::string str;
int ret;
ArTcpConnection con;
ArRobot robot;
ActionTest at1(-50, 333);
ActionTest at2(25, 666);
ActionTest at3(25, 0);
ActionTest at4(0, -999);
Aria::init();
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
robot.setDeviceConnection(&con);
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
robot.addAction(&at1, 100);
robot.addAction(&at2, 100);
robot.addAction(&at3, 100);
robot.addAction(&at4, 100);
robot.run(true);
Aria::shutdown();
return 0;
}
int main(void)
{
ArTcpConnection con;
ArRobot robot;
ArSonarDevice sonar;
int ret;
std::string str;
ArActionStallRecover recover;
ArActionConstantVelocity constantVelocity("Constant Velocity", 400);
Aria::init();
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
robot.addRangeDevice(&sonar);
robot.setDeviceConnection(&con);
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
robot.addAction(&recover, 100);
robot.addAction(&constantVelocity, 25);
robot.run(true);
Aria::shutdown();
return 0;
}
// Create an ArGPS object. If some options were obtained from command-line
// parameters by parseArgs(), use those, otherwise get values from robot
// parameters (the .p file) if we have a valid robot with valid parameters.
AREXPORT ArGPS* ArGPSConnector::createGPS(ArRobot *robot)
{
// If we have a robot with parameters (i.e. have connected and read the .p
// file), use those values unless already set by parseArgs() from command-line
if(robot && robot->getRobotParams())
{
if(myPort == NULL) {
myPort = robot->getRobotParams()->getGPSPort();
if(strcmp(myPort, "COM1") == 0)
myPort = ArUtil::COM1;
if(strcmp(myPort, "COM2") == 0)
myPort = ArUtil::COM2;
if(strcmp(myPort, "COM3") == 0)
myPort = ArUtil::COM3;
if(strcmp(myPort, "COM4") == 0)
myPort = ArUtil::COM4;
}
if(myBaud == -1) {
myBaud = robot->getRobotParams()->getGPSBaud();
}
if(myDeviceType == Invalid) {
myDeviceType = deviceTypeFromString(robot->getRobotParams()->getGPSType());
}
}
else
{
if(myPort == NULL) myPort = ARGPS_DEFAULT_SERIAL_PORT;
if(myBaud == -1) myBaud = ARGPS_DEFAULT_SERIAL_BAUD;
if(myDeviceType == Invalid) myDeviceType = Standard;
}
// Create gps:
ArGPS* newGPS = NULL;
switch (myDeviceType)
{
case Novatel:
ArLog::log(ArLog::Normal, "ArGPSConnector: Using Novatel GPS");
newGPS = new ArNovatelGPS;
break;
case Trimble:
ArLog::log(ArLog::Normal, "ArGPSConnector: Using Trimble GPS");
newGPS = new ArTrimbleGPS;
break;
case NovatelSPAN:
ArLog::log(ArLog::Normal, "ArGPSConnector: Using Novatel SPAN GPS");
newGPS = new ArNovatelSPAN;
break;
default:
ArLog::log(ArLog::Normal, "ArGPSConnector: Using standard NMEA GPS");
newGPS = new ArGPS;
break;
}
if (myTCPHost == NULL)
{
// Setup serial connection
ArSerialConnection *serialCon = new ArSerialConnection;
ArLog::log(ArLog::Normal, "ArGPSConnector: Connecting to GPS on port %s at %d baud...", myPort, myBaud);
if (!serialCon->setBaud(myBaud)) { delete serialCon; return false; }
if (serialCon->open(myPort) != 0) {
ArLog::log(ArLog::Terse, "ArGPSConnector: Error: could not open GPS serial port %s.", myPort);
delete serialCon;
return NULL;
}
newGPS->setDeviceConnection(serialCon);
myDeviceCon = serialCon;
}
else
{
// Setup TCP connection
ArTcpConnection *tcpCon = new ArTcpConnection;
ArLog::log(ArLog::Normal, "ArGPSConnector: Opening TCP connection to %s:%d...", myTCPHost, myTCPPort);
int openState = tcpCon->open(myTCPHost, myTCPPort);
if (openState != 0) {
ArLog::log(ArLog::Terse, "ArGPSConnector: Error: could not open TCP connection to %s port %d: %s", tcpCon->getOpenMessage(openState));
delete tcpCon;
return NULL;
}
newGPS->setDeviceConnection(tcpCon);
myDeviceCon = tcpCon;
}
return newGPS;
}
int main(int argc, char **argv)
{
int ret;
std::string str;
// the serial connection (robot)
ArSerialConnection serConn;
// tcp connection (sim)
ArTcpConnection tcpConn;
// the robot
ArRobot robot;
// the laser
ArSick sick;
// the laser connection
ArSerialConnection laserCon;
bool useSimForLaser = false;
std::string hostname = "prod.local.net";
// timeouts in minutes
int wanderTime = 0;
int restTime = 0;
// check arguments
if (argc == 3 || argc == 4)
{
wanderTime = atoi(argv[1]);
restTime = atoi(argv[2]);
if (argc == 4)
hostname = argv[3];
}
else
{
printf("\nUsage:\n\tpeoplebotTest <wanderTime> <restTime> <hostname>\n\n");
printf("Times are in minutes. Hostname is the machine to pipe the ACTS display to\n\n");
wanderTime = 15;
restTime = 45;
}
printf("Wander time - %d minutes\nRest time - %d minutes\n", wanderTime, restTime);
printf("Sending display to %s.\n\n", hostname.c_str());
// sonar, must be added to the robot
ArSonarDevice sonar;
// the actions we'll use to wander
ArActionStallRecover recover;
ArActionBumpers bumpers;
ArActionAvoidFront avoidFrontNear("Avoid Front Near", 225, 0);
ArActionAvoidFront avoidFrontFar;
// Make a key handler, so that escape will shut down the program
// cleanly
ArKeyHandler keyHandler;
// mandatory init
Aria::init();
// Add the key handler to Aria so other things can find it
Aria::setKeyHandler(&keyHandler);
// Attach the key handler to a robot now, so that it actually gets
// some processing time so it can work, this will also make escape
// exit
robot.attachKeyHandler(&keyHandler);
// First we see if we can open the tcp connection, if we can we'll
// assume we're connecting to the sim, and just go on... if we
// can't open the tcp it means the sim isn't there, so just try the
// robot
// modify this next line if you're not using default tcp connection
tcpConn.setPort();
// see if we can get to the simulator (true is success)
if (tcpConn.openSimple())
{
// we could get to the sim, so set the robots device connection to the sim
printf("Connecting to simulator through tcp.\n");
robot.setDeviceConnection(&tcpConn);
}
else
{
// we couldn't get to the sim, so set the port on the serial
// connection and then set the serial connection as the robots
// device
// modify the next line if you're not using the first serial port
// to talk to your robot
serConn.setPort();
printf(
"Could not connect to simulator, connecting to robot through serial.\n");
robot.setDeviceConnection(&serConn);
}
// add the sonar to the robot
robot.addRangeDevice(&sonar);
// add the laser
robot.addRangeDevice(&sick);
// try to connect, if we fail exit
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// turn on the motors, turn off amigobot sounds
//robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
// turn off the sonar to start with
robot.comInt(ArCommands::SONAR, 0);
// add the actions
robot.addAction(&recover, 100);
robot.addAction(&bumpers, 75);
robot.addAction(&avoidFrontNear, 50);
robot.addAction(&avoidFrontFar, 49);
// start the robot running, true so that if we lose connection the run stops
robot.runAsync(true);
if (!useSimForLaser)
{
sick.setDeviceConnection(&laserCon);
if ((ret = laserCon.open("/dev/ttyS2")) != 0)
{
str = tcpConn.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
sick.configureShort(false);
}
else
{
sick.configureShort(true);
}
sick.runAsync();
if (!sick.blockingConnect())
{
printf("Could not connect to SICK laser... exiting\n");
Aria::shutdown();
return 1;
}
robot.lock();
robot.comInt(ArCommands::ENABLE, 1);
robot.unlock();
// add the peoplebot test
PeoplebotTest pbTest(&robot, wanderTime, restTime, hostname);
robot.waitForRunExit();
// now exit
Aria::shutdown();
return 0;
}
int main(void)
{
// The connection we'll use to talk to the robot
ArTcpConnection con;
// the robot
ArRobot robot;
// the sonar device
ArSonarDevice sonar;
// some stuff for return values
int ret;
std::string str;
// the behaviors from above, and a stallRecover behavior that uses defaults
ActionGo go(500, 350);
ActionTurn turn(400, 30);
ArActionStallRecover recover;
// this needs to be done
Aria::init();
// open the connection, just using the defaults, if it fails, exit
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
// add the range device to the robot, you should add all the range
// devices and such before you add actions
robot.addRangeDevice(&sonar);
// set the robot to use the given connection
robot.setDeviceConnection(&con);
// do a blocking connect, if it fails exit
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// enable the motors, disable amigobot sounds
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
// add our actions in a good order, the integer here is the priority,
// with higher priority actions going first
robot.addAction(&recover, 100);
robot.addAction(&go, 50);
robot.addAction(&turn, 49);
// run the robot, the true here is to exit if it loses connection
robot.run(true);
// now just shutdown and go away
Aria::shutdown();
return 0;
}
int main(int argc, char **argv)
{
std::string str;
int ret;
int num;
ArPose pose;
Aria::init();
if (argc != 2)
{
printf("Usage is '%s <num>' where <num> is one of:\n", argv[0]);
printf("1 Moves the robot back to the origin.\n");
printf("2 Moves the robot +1 meter in X direction.\n");
printf("3 Does a transform test, going from the origin to robot and back.\n");
printf("4 Does a transform test with a point 1 meter to the -x from the robot (ie left).\n");
printf("5 Does a transform, prints out sonar plar reading before and after.\n");
printf("6 Disconnects the robot, and reconnects to it.\n");
exit(1);
}
num = atoi(argv[1]);
ArTcpConnection con;
ArRobot robot(NULL, false);
Joydrive joyd(&robot, num);
ArFunctorC<Joydrive> driveCB(&joyd, &Joydrive::drive);
ArSonarDevice sonar;
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
robot.addUserTask("joydrive", 50, &driveCB);
robot.addRangeDevice(&sonar);
robot.setDeviceConnection(&con);
pose.setPose(4000, 2000, 90);
robot.moveTo(pose);
pose = robot.getPose();
printf("Position set to, before connect.\n");
pose.log();
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
pose = robot.getPose();
printf("Position robot at, after connect.\n");
pose.log();
//printf("Position being reset to 0.\n");
//pose.setPose(0, 0, 0);
//robot.moveTo(pose);
//robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
robot.run(true);
Aria::shutdown();
return 0;
}
