AREXPORT ArTCMCompassDirect::ArTCMCompassDirect(const char *serialPortName) :
myCreatedOwnDeviceConnection(true),
myNMEAParser("ArTCMCompassDirect"),
myHCHDMHandler(this, &ArTCMCompassDirect::handleHCHDM)
{
ArSerialConnection *newSerialCon = new ArSerialConnection();
newSerialCon->setPort(serialPortName);
newSerialCon->setBaud(9600);
myDeviceConnection = newSerialCon;
myNMEAParser.addHandler("HCHDM", &myHCHDMHandler);
}
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;
}
void SetupRobot(void)
{
puts("attempting to connect to robot");
RobotConnectoin.setPort("COM8");
RobotConnectoin.setBaud(9600);
robot.setDeviceConnection(&RobotConnectoin);
if(!robot.blockingConnect()){puts("not connected to robot");Aria::shutdown();}
robot.addRangeDevice(&sonarDev);
robot.addRangeDevice(&bumpers);
robot.enableMotors();
robot.enableSonar();
robot.requestEncoderPackets();
robot.setCycleChained(false);
// robot.setRotVelMax(robot.getRotVelMax());
}
int main(int argc, char **argv)
{
Aria::init();
ArLog::init(ArLog::StdErr, ArLog::Normal);
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
}
ArSerialConnection con;
con.setPort(ArUtil::COM4);
con.setBaud(19200);
if(!con.openSimple())
ArLog::log(ArLog::Terse, "could not open COM4");
char buf[512];
while (true)
{
int n = con.read(buf, 512, 10);
if(n < 0)
{
ArLog::log(ArLog::Terse, "Error reading.");
Aria::exit(n);
}
if(n == 0)
continue;
// log for debugging:
char cmd = 0;
int x = 0;
int size = 0;
for(int i = 0; i < n; ++i)
{
if(buf[i] == 0xc1 || buf[i] == 0x5a) puts("");
printf("0x%hhX (%u) ", buf[i], buf[i]);
}
}
Aria::exit(0);
}
int main(int argc, char **argv)
{
// just some stuff for returns
std::string str;
// robots connection
ArSerialConnection con;
// the robot, this turns state reflection off
ArRobot robot(NULL, false);
// the joydrive as defined above, this also adds itself as a user task
KeyPTU ptu(&robot);
// mandatory init
Aria::init();
ArLog::init(ArLog::StdOut, ArLog::Terse, NULL, true);
con.setPort(ArUtil::COM1);
// set the connection on the robot
robot.setDeviceConnection(&con);
// connect, if we fail, exit
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// turn off the sonar, enable the motors, turn off amigobot sounds
robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
printf("Press '?' for available commands\r\n");
// run, if we lose connection to the robot, exit
robot.run(true);
// shutdown and go away
Aria::shutdown();
return 0;
}
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(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;
}
ArDeviceConnection *ArDeviceConnectionCreatorHelper::createSerialConnection(
const char *port, const char *defaultInfo, const char *logPrefix)
{
ArSerialConnection *serConn = new ArSerialConnection;
std::string serPort;
if (strcasecmp(port, "COM1") == 0)
serPort = ArUtil::COM1;
else if (strcasecmp(port, "COM2") == 0)
serPort = ArUtil::COM2;
else if (strcasecmp(port, "COM3") == 0)
serPort = ArUtil::COM3;
else if (strcasecmp(port, "COM4") == 0)
serPort = ArUtil::COM4;
else if (strcasecmp(port, "COM5") == 0)
serPort = ArUtil::COM5;
else if (strcasecmp(port, "COM6") == 0)
serPort = ArUtil::COM6;
else if (strcasecmp(port, "COM7") == 0)
serPort = ArUtil::COM7;
else if (strcasecmp(port, "COM8") == 0)
serPort = ArUtil::COM8;
else if (strcasecmp(port, "COM9") == 0)
serPort = ArUtil::COM9;
else if (strcasecmp(port, "COM10") == 0)
serPort = ArUtil::COM10;
else if (strcasecmp(port, "COM11") == 0)
serPort = ArUtil::COM11;
else if (strcasecmp(port, "COM12") == 0)
serPort = ArUtil::COM12;
else if (strcasecmp(port, "COM13") == 0)
serPort = ArUtil::COM13;
else if (strcasecmp(port, "COM14") == 0)
serPort = ArUtil::COM14;
else if (strcasecmp(port, "COM15") == 0)
serPort = ArUtil::COM15;
else if (strcasecmp(port, "COM16") == 0)
serPort = ArUtil::COM16;
else if (port != NULL)
serPort = port;
ArLog::log(ourSuccessLogLevel, "%Set serial port to open %s",
logPrefix, serPort.c_str());
serConn->setPort(serPort.c_str());
return serConn;
/*
This code is commented out because it created problems with demo
(or any other program that used ArLaserConnector::connectLasers
with addAllLasersToRobot as true)
int ret;
if ((ret = serConn->open(serPort.c_str())) == 0)
{
ArLog::log(ourSuccessLogLevel, "%sOpened serial port %s",
logPrefix, serPort.c_str());
return serConn;
}
else
{
ArLog::log(ArLog::Normal, "%sCould not open serial port %s (from %s), because %s",
logPrefix, serPort.c_str(), port,
serConn->getOpenMessage(ret));
delete serConn;
return NULL;
}
*/
}