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();
}
ArDeviceConnection *ArDeviceConnectionCreatorHelper::createTcpConnection(
const char *port, const char *defaultInfo, const char *logPrefix)
{
ArTcpConnection *tcpConn = new ArTcpConnection;
int ret;
tcpConn->setPort(port, atoi(defaultInfo));
ArLog::log(ourSuccessLogLevel,
"%sSet tcp connection to open %s (and port %d)",
logPrefix, port, atoi(defaultInfo));
return tcpConn;
/*
This code is commented out because it created problems with demo
(or any other program that used ArLaserConnector::connectLasers
with addAllLasersToRobot as true)
if ((ret = tcpConn->open(port, atoi(defaultInfo))) == 0)
{
ArLog::log(ourSuccessLogLevel,
"%sOpened tcp connection from %s (and port %d)",
logPrefix, port, atoi(defaultInfo));
return tcpConn;
}
else
{
ArLog::log(ArLog::Normal, "%sCould not open a tcp connection to host '%s' with default port %d (from '%s'), because %s",
logPrefix, port, atoi(defaultInfo), defaultInfo,
tcpConn->getOpenMessage(ret));
delete tcpConn;
return NULL;
}
*/
}
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)
{
Aria::init();
ArRobot robot;
ArSerialConnection serialConnection;
ArTcpConnection tcpConnection;
if (tcpConnection.open("localhost", 8101)) {
robot.setDeviceConnection(&tcpConnection);
} else {
serialConnection.setPort("/dev/ttyUSB0");
robot.setDeviceConnection(&serialConnection);
}
robot.blockingConnect();
printf("Setting robot to run async\n");
robot.runAsync(false);
printf("Turning off sound\n");
robot.comInt(ArCommands::SOUNDTOG, 0);
printf("Enabling motors\n");
robot.enableMotors();
// add a set of actions that combine together to effect the wander behavior
/*ArActionStallRecover recover;
ArActionBumpers bumpers;
ArActionAvoidFront avoidFrontNear("Avoid Front Near", 225, 0);
ArActionAvoidFront avoidFrontFar;
ArActionConstantVelocity constantVelocity("Constant Velocity", 400);
robot.addAction(&recover, 100);
robot.addAction(&bumpers, 75);
robot.addAction(&avoidFrontNear, 50);
robot.addAction(&avoidFrontFar, 49);
robot.addAction(&constantVelocity, 25);*/
printf("Locking\n");
robot.lock();
robot.setVel(100.0);
robot.unlock();
printf("Sleeping\n");
ArUtil::sleep(3*1000);
printf("Awake\n");
// wait for robot task loop to end before exiting the program
//while (true);
//robot.waitForRunExit();
Aria::exit(0);
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)
{
bool done;
double distToTravel = 2300;
// whether to use the sim for the laser or not, if you use the sim
// for hte laser, you have to use the sim for the robot too
bool useSim = false;
// the laser
ArSick sick;
// connection
ArDeviceConnection *con;
// Laser connection
ArSerialConnection laserCon;
// robot
ArRobot robot;
// set a default filename
//std::string filename = "c:\\log\\1scans.2d";
std::string filename = "1scans.2d";
// see if we want to use a different filename
//if (argc > 1)
//Lfilename = argv[1];
printf("Logging to file %s\n", filename.c_str());
// start the logger with good values
sick.configureShort(useSim, ArSick::BAUD38400,
ArSick::DEGREES180, ArSick::INCREMENT_HALF);
ArSickLogger logger(&robot, &sick, 300, 25, filename.c_str());
// mandatory init
Aria::init();
// add it to the robot
robot.addRangeDevice(&sick);
//ArAnalogGyro gyro(&robot);
// if we're not using the sim, make a serial connection and set it up
if (!useSim)
{
ArSerialConnection *serCon;
serCon = new ArSerialConnection;
serCon->setPort();
//serCon->setBaud(38400);
con = serCon;
}
// if we are using the sim, set up a tcp connection
else
{
ArTcpConnection *tcpCon;
tcpCon = new ArTcpConnection;
tcpCon->setPort();
con = tcpCon;
}
// set the connection on the robot
robot.setDeviceConnection(con);
// try to connect, if we fail exit
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// set up a key handler so escape exits and attach to the robot
ArKeyHandler keyHandler;
robot.attachKeyHandler(&keyHandler);
// run the robot, true here so that the run will exit if connection lost
robot.runAsync(true);
// if we're not using the sim, set up the port for the laser
if (!useSim)
{
laserCon.setPort(ArUtil::COM3);
sick.setDeviceConnection(&laserCon);
}
// now that we're connected to the robot, connect to the laser
sick.runAsync();
if (!sick.blockingConnect())
{
printf("Could not connect to SICK laser... exiting\n");
robot.disconnect();
Aria::shutdown();
return 1;
}
#ifdef WIN32
// wait until someone pushes the motor button to go
while (1)
{
robot.lock();
if (!robot.isRunning())
exit(0);
if (robot.areMotorsEnabled())
{
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(100);
}
#endif
// basically from here on down the robot just cruises around a bit
robot.lock();
// enable the motors, disable amigobot sounds
robot.comInt(ArCommands::ENABLE, 1);
ArTime startTime;
// move a couple meters
robot.move(distToTravel);
robot.unlock();
startTime.setToNow();
do {
ArUtil::sleep(100);
robot.lock();
robot.setHeading(0);
done = robot.isMoveDone(60);
robot.unlock();
} while (!done);
/*
// rotate a few times
robot.lock();
robot.setVel(0);
robot.setRotVel(60);
robot.unlock();
ArUtil::sleep(12000);
*/
robot.lock();
robot.setHeading(180);
robot.unlock();
do {
ArUtil::sleep(100);
robot.lock();
robot.setHeading(180);
done = robot.isHeadingDone();
robot.unlock();
} while (!done);
// move a couple meters
robot.lock();
robot.move(distToTravel);
robot.unlock();
startTime.setToNow();
do {
ArUtil::sleep(100);
robot.lock();
robot.setHeading(180);
done = robot.isMoveDone(60);
robot.unlock();
} while (!done);
robot.lock();
robot.setHeading(0);
robot.setVel(0);
robot.unlock();
startTime.setToNow();
do {
ArUtil::sleep(100);
robot.lock();
robot.setHeading(0);
done = robot.isHeadingDone();
robot.unlock();
} while (!done);
sick.lockDevice();
sick.disconnect();
sick.unlockDevice();
robot.lock();
robot.disconnect();
robot.unlock();
// now exit
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;
}
int main(int argc, char **argv)
{
// this is how long to wait after there's been no data to close the
// connection.. if its 0 and its using robot it'll set it to 5000 (5
// seconds), if its 0 and using laser, it'll set it to 60000 (60
// seconds, which is needed if the sick driver is controlling power
int timeout = 0;
// true will print out packets as they come and go, false won't
bool tracePackets = false;
// The socket objects
ArSocket masterSock, clientSock;
// The connections
ArTcpConnection clientConn;
ArSerialConnection robotConn;
// the receivers, first for the robot
ArRobotPacketReceiver clientRec(&clientConn);
ArRobotPacketReceiver robotRec(&robotConn);
// then for the laser
ArSickPacketReceiver clientSickRec(&clientConn, 0, false, true);
ArSickPacketReceiver robotSickRec(&robotConn);
// how about a packet
ArBasePacket *packet;
// our timer for how often we test the client
ArTime lastClientTest;
ArTime lastData;
// where we're forwarding from and to
int portNumber;
const char *portName;
// if we're using the robot or the laser
bool useRobot;
if (argc == 1)
{
printf("Using robot and port 8101 and serial connection %s, by default.\n", ArUtil::COM1);
useRobot = true;
portNumber = 8101;
portName = ArUtil::COM1;
}
else if (argc == 2)
{
// if laser isn't the last arg, somethings wrong
if (strcmp(argv[1], "laser") != 0)
{
usage(argv[0]);
return -1;
}
useRobot = false;
portNumber = 8102;
portName = ArUtil::COM3;
printf("Using laser and port %d and serial connection %s, by command line arguments.\n", portNumber, portName);
printf("(Note: Requests to change BAUD rate cannot be fulfilled; use 9600 rate only.)\n");
}
else if (argc == 3)
{
if ((portNumber = atoi(argv[1])) <= 0)
{
usage(argv[0]);
return -1;
}
portName = argv[2];
printf("Using robot and port %d and serial connection %s, by command line arguments.\n", portNumber, portName);
}
else if (argc == 4)
{
if ((portNumber = atoi(argv[1])) <= 0)
{
usage(argv[0]);
return -1;
}
// if laser isn't the last arg, somethings wrong
if (strcmp(argv[3], "laser") != 0)
{
usage(argv[0]);
return -1;
}
useRobot = false;
portName = argv[2];
printf("Using laser and port %d and serial connection %s, by command line arguments.\n", portNumber, portName);
printf("(Note: Requests to change BAUD rate cannot be fulfilled; use 9600 rate only.)\n");
}
else
{
usage(argv[0]);
return -1;
}
if (timeout == 0 && useRobot)
timeout = 5000;
else if (timeout == 0)
timeout = 60000;
// Initialize Aria. For Windows, this absolutely must be done. Because
// Windows does not initialize the socket layer for each program. Each
// program must initialize the sockets itself.
Aria::init(Aria::SIGHANDLE_NONE);
// Lets open the master socket
if (masterSock.open(portNumber, ArSocket::TCP))
printf("Opened the master port at %d\n", portNumber);
else
{
printf("Failed to open the master port at %d: %s\n",
portNumber, masterSock.getErrorStr().c_str());
return -1;
}
// just go forever
while (1)
{
// Lets wait for the client to connect to us.
if (masterSock.accept(&clientSock))
printf("Client has connected\n");
else
printf("Error in accepting a connection from the client: %s\n",
masterSock.getErrorStr().c_str());
// now set up our connection so our packet receivers work
clientConn.setSocket(&clientSock);
clientConn.setStatus(ArDeviceConnection::STATUS_OPEN);
lastClientTest.setToNow();
lastData.setToNow();
// open up the robot port
if (robotConn.open(portName) != 0)
{
printf("Could not open robot port %s.\n", portName);
return -1;
}
// while we're open, just read from one port and write to the other
while (clientSock.getFD() >= 0)
{
// get our packet
if (useRobot)
packet = clientRec.receivePacket(1);
else
packet = clientSickRec.receivePacket(1);
// see if we had one
if (packet != NULL)
{
if (tracePackets)
{
printf("Client ");
packet->log();
}
robotConn.writePacket(packet);
lastData.setToNow();
}
// get our packet
if (useRobot)
packet = robotRec.receivePacket(1);
else
packet = robotSickRec.receivePacket(1);
// see if we had one
if (packet != NULL)
{
if (tracePackets)
{
printf("Robot ");
packet->log();
}
clientConn.writePacket(packet);
lastData.setToNow();
}
ArUtil::sleep(1);
// If no datas gone by in timeout ms assume our connection is broken
if (lastData.mSecSince() > timeout)
{
printf("No data received in %d milliseconds, closing connection.\n",
timeout);
clientConn.close();
}
}
// Now lets close the connection to the client
clientConn.close();
printf("Socket to client closed\n");
robotConn.close();
}
// And lets close the master port
masterSock.close();
printf("Master socket closed and program exiting\n");
// Uninitialize Aria
Aria::uninit();
// All done
return(0);
}
