int main(int argc, char **argv)
{
std::string str;
int ret;
ArGlobalRetFunctor1<bool, ArRobotPacket *> tcm2PrinterCB(&tcm2Printer);
ArSerialConnection con;
Aria::init();
robot = new ArRobot;
robot->addPacketHandler(&tcm2PrinterCB, ArListPos::FIRST);
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);
}
printf("%6s %6s %6s %6s %6s %6s %6s %10s %4s %4s %6s\n",
"comp", "pitch", "roll", "magX", "magY", "magZ", "temp", "error",
"calH", "calV", "calM");
robot->comInt(ArCommands::TCM2, 3);
robot->run(true);
Aria::shutdown();
}
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;
}
int main(int argc, char **argv)
{
std::string str;
int ret;
ArTime start;
// connection to the robot
ArSerialConnection con;
// the robot
ArRobot robot;
// the connection handler from above
ConnHandler ch(&robot);
// init area with a dedicated signal handling thread
Aria::init(Aria::SIGHANDLE_THREAD);
// open the connection with the defaults, exit if failed
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
// set the robots connection
robot.setDeviceConnection(&con);
// try to connect, if we fail, the connection handler should bail
if (!robot.blockingConnect())
{
// this should have been taken care of by the connection handler
// but just in case
printf(
"asyncConnect failed because robot is not running in its own thread.\n");
Aria::shutdown();
return 1;
}
// run the robot in its own thread, so it gets and processes packets and such
robot.runAsync(false);
int i;
while (Aria::getRunning())
{
robot.lock();
robot.comStr(ArCommands::TTY3, "1234567890");
robot.unlock();
}
robot.disconnect();
// shutdown and ge tout
Aria::shutdown();
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)
{
// just some stuff for returns
std::string str;
int ret;
// 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
Joydrive joyd(&robot);
// mandatory init
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::shutdown();
return 1;
}
// set the connection o nthe 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);
// 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)
{
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(void)
{
ArSerialConnection serConn;
if (serConn.open(ArUtil::COM1) != 0)
{
printf("Could not open serial port\n");
exit(1);
}
while (1)
{
printf("\rDCD %d", serConn.getDCD());
fflush(stdout);
}
return 0;
}
int main()
{
ArModuleLoader::Status status;
ArSerialConnection con;
ArRobot robot;
int ret;
std::string str;
Aria::init();
status=ArModuleLoader::load("./joydriveActionMod", &robot);
printStatus(status);
if (status == ArModuleLoader::STATUS_INIT_FAILED)
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);
robot.run(true);
status=ArModuleLoader::close("./joydriveActionMod");
printStatus(status);
Aria::shutdown();
return 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(void)
{
int ret;
std::string str;
CBTest cbTest;
ArFunctorC<CBTest> connectCB(&cbTest, &CBTest::connected);
ArFunctorC<CBTest> failedConnectCB(&cbTest, &CBTest::failedConnect);
ArFunctorC<CBTest> disconnectCB(&cbTest, &CBTest::disconnected);
ArFunctorC<CBTest> disconnectErrorCB(&cbTest, &CBTest::disconnectedError);
ArSerialConnection con;
ArRobot robot;
printf("If a robot is attached to your port you should see:\n");
printf("Failed connect, Connected, Disconnected Error, Connected, Disconnected\n");
printf("If no robot is attached you should see:\n");
printf("Failed connect, Failed connect, Failed connect\n");
printf("-------------------------------------------------------\n");
ArLog::init(ArLog::None, ArLog::Terse);
srand(time(NULL));
robot.setDeviceConnection(&con);
robot.addConnectCB(&connectCB, ArListPos::FIRST);
robot.addFailedConnectCB(&failedConnectCB, ArListPos::FIRST);
robot.addDisconnectNormallyCB(&disconnectCB, ArListPos::FIRST);
robot.addDisconnectOnErrorCB(&disconnectErrorCB, ArListPos::FIRST);
// this should fail since there isn't an open port yet
robot.blockingConnect();
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
exit(0);
}
robot.blockingConnect();
con.close();
robot.loopOnce();
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
exit(0);
}
robot.blockingConnect();
robot.disconnect();
exit(0);
}
bool ArUrg::internalConnect(void)
{
bool ret = true;
char buf[1024];
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
bool alreadyAtAutobaud = false;
// empty the buffer...
buf[0] = '\0';
while (readLine(buf, sizeof(buf), 1));
if (!(ret = sendCommandAndRecvStatus(
"V", "version request",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "0") != 0)
{
// if we didn't get it, try it at what the autobaud rate is
if (serConn != NULL)
{
alreadyAtAutobaud = true;
serConn->setBaud(atoi(getAutoBaudChoice()));
ArUtil::sleep(100);
if (!(ret = sendCommandAndRecvStatus(
"V", "version request after falling back to autobaudchoice",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "0") != 0)
{
if (ret && strcasecmp(buf, "0") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on version response after falling back to autobaudchoice",
getName());
return false;
}
}
// if we don't have a serial port, then we can't change the baud,
// so just fail
else
{
if (ret && strcasecmp(buf, "0") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on version response (%s)",
getName(), buf);
return false;
}
}
if (!alreadyAtAutobaud && serConn != NULL)
{
// empty the buffer from the last version request
while (readLine(buf, sizeof(buf), 100));
// now change the baud...
sprintf(buf, "S%06d7654321", atoi(getAutoBaudChoice()));
if (!writeLine(buf))
return false;
ArUtil::sleep(100);
//serConn->setBaud(115200);
serConn->setBaud(atoi(getAutoBaudChoice()));
// wait a second for the baud to change...
ArUtil::sleep(100);
// empty the buffer from the baud change
while (readLine(buf, sizeof(buf), 100));
if (!(ret = sendCommandAndRecvStatus(
"V", "version request after switching to autobaudchoice",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "0") != 0)
{
if (ret && strcasecmp(buf, "0") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on version response after switching to autobaudchoice",
getName());
return false;
}
ArLog::log(ArLog::Normal, "%s: Switched to %s baud rate",
getName(), getAutoBaudChoice());
}
while (readLine(buf, sizeof(buf), 10000))
{
/// MPL put this in instead of the following because of the
/// behavior change of readline
if (strlen(buf) == 0)
break;
if (strncasecmp(buf, "VEND:", strlen("VEND:")) == 0)
myVendor = &buf[5];
else if (strncasecmp(buf, "PROD:", strlen("PROD:")) == 0)
myProduct = &buf[5];
else if (strncasecmp(buf, "FIRM:", strlen("FIRM:")) == 0)
myFirmwareVersion = &buf[5];
else if (strncasecmp(buf, "PROT:", strlen("PROT:")) == 0)
myProtocolVersion = &buf[5];
else if (strncasecmp(buf, "SERI:", strlen("SERI:")) == 0)
mySerialNumber = &buf[5];
else if (strncasecmp(buf, "STAT:", strlen("STAT:")) == 0)
myStat = &buf[5];
}
if (myVendor.empty() || myProduct.empty() || myFirmwareVersion.empty() ||
myProtocolVersion.empty() || mySerialNumber.empty())
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Missing information in version response",
getName());
return false;
}
log();
myLogMore = true;
// myLogMore = false;
ArUtil::sleep(100);
printf("myRequestString %s\n", myRequestString);
if (!(ret = sendCommandAndRecvStatus(
myRequestString, "request distance reading",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "0") != 0)
{
if (ret && strcasecmp(buf, "0") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on distance reading response (%c)",
getName(), buf[0]);
return false;
}
myLogMore = false;
ArTime started;
started.setToNow();
while (started.secSince() < 10 && readLine(buf, sizeof(buf), 10000))
{
if (strlen(buf) == 0)
return true;
}
ArLog::log(ArLog::Normal, "%s::blockingConnect: Did not get distance reading back",
getName(), buf[0]);
return false;
}
AREXPORT bool ArUrg::blockingConnect(void)
{
if (!getRunning())
runAsync();
myConnMutex.lock();
if (myConn == NULL)
{
ArLog::log(ArLog::Terse,
"%s: Could not connect because there is no connection defined",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
if (serConn != NULL)
serConn->setBaud(atoi(getStartingBaudChoice()));
if (myConn->getStatus() != ArDeviceConnection::STATUS_OPEN &&
!myConn->openSimple())
{
ArLog::log(ArLog::Terse,
"%s: Could not connect because the connection was not open and could not open it",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
myConnMutex.unlock();
lockDevice();
myTryingToConnect = true;
unlockDevice();
laserPullUnsetParamsFromRobot();
laserCheckParams();
setParams(getStartDegrees(), getEndDegrees(), getIncrement(), getFlipped());
ArUtil::sleep(100);
bool connected = false;
if (internalConnect())
connected = true;
if (connected)
{
lockDevice();
myIsConnected = true;
myTryingToConnect = false;
unlockDevice();
ArLog::log(ArLog::Normal, "%s: Connected to laser", getName());
laserConnect();
return true;
}
else
{
failedToConnect();
return false;
}
}
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)
{
std::string str;
int ret;
int successes = 0, failures = 0;
int action;
bool exitOnFailure = true;
ArSerialConnection con;
ArRobot robot;
//ArLog::init(ArLog::StdOut, ArLog::Verbose);
srand(time(NULL));
robot.runAsync(false);
// if (!exitOnFailure)
// ArLog::init(ArLog::None, ArLog::Terse);
//else
//ArLog::init(ArLog::None);
while (1)
{
if (con.getStatus() != ArDeviceConnection::STATUS_OPEN &&
(ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
++failures;
if (exitOnFailure)
{
printf("Failed\n");
exit(0);
}
else
{
ArUtil::sleep(200);
robot.unlock();
continue;
}
}
robot.lock();
robot.setDeviceConnection(&con);
robot.unlock();
ArUtil::sleep((rand() % 5) * 100);
if (robot.asyncConnect())
{
robot.waitForConnectOrConnFail();
robot.lock();
if (!robot.isConnected())
{
if (exitOnFailure)
{
printf("Failed after %d tries.\n", successes);
exit(0);
}
printf("Failed to connect successfully");
++failures;
}
robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::SOUNDTOG, 0);
//robot.comInt(ArCommands::PLAYLIST, 0);
robot.comInt(ArCommands::ENCODER, 1);
ArUtil::sleep(((rand() % 20) + 3) * 100);
++successes;
// okay, now try to leave it in a messed up state
action = rand() % 8;
robot.dropConnection();
switch (action) {
case 0:
printf("Discon 0 ");
robot.disconnect();
ArUtil::sleep(100);
robot.com(0);
break;
case 1:
printf("Discon 1 ");
robot.disconnect();
ArUtil::sleep(100);
robot.com(0);
ArUtil::sleep(100);
robot.com(1);
break;
case 2:
printf("Discon 2 ");
robot.disconnect();
ArUtil::sleep(100);
robot.com(0);
ArUtil::sleep(100);
robot.com(1);
ArUtil::sleep(100);
robot.com(2);
break;
case 3:
printf("Discon 10 ");
robot.disconnect();
ArUtil::sleep(100);
robot.com(10);
break;
case 4:
printf("Discon ");
robot.disconnect();
break;
default:
printf("Leave ");
break;
}
robot.unlock();
}
else
{
if (exitOnFailure)
{
printf("Failed after %d tries.\n", successes);
exit(0);
}
printf("Failed to start connect ");
++failures;
}
if ((rand() % 2) == 0)
{
printf(" ! RadioDisconnect ! ");
con.write("|||\15", strlen("!!!\15"));
ArUtil::sleep(100);
con.write("WMD\15", strlen("WMD\15"));
ArUtil::sleep(200);
}
if ((rand() % 2) == 0)
{
printf(" ! ClosePort !\n");
con.close();
}
else
printf("\n");
printf("#### %d successes %d failures, %% %.2f success\n", successes, failures,
(float)successes/(float)(successes+failures)*100);
ArUtil::sleep((rand() % 2)* 1000);
}
return 0;
}
AREXPORT bool ArSZSeries::blockingConnect(void) {
if (!getRunning())
runAsync();
myConnMutex.lock();
if (myConn == NULL) {
ArLog::log(ArLog::Terse,
"%s: Could not connect because there is no connection defined",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
// myPrevSensorIntTime = myConn->getTimeRead(0);
// PS 9/9/11 - moved this here to fix issue with setting baud in mt400.p
laserPullUnsetParamsFromRobot();
laserCheckParams();
// PS 9/9/11 - add setting baud
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
if (serConn != NULL)
serConn->setBaud(atoi(getStartingBaudChoice()));
if (myConn->getStatus() != ArDeviceConnection::STATUS_OPEN
&& !myConn->openSimple()) {
ArLog::log(
ArLog::Terse,
"%s: Could not connect because the connection was not open and could not open it",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
// PS - set logging level and laser type in packet receiver class
myReceiver.setmyInfoLogLevel(myInfoLogLevel);
myReceiver.setmyName(getName());
myReceiver.setDeviceConnection(myConn);
myConnMutex.unlock();
lockDevice();
myTryingToConnect = true;
unlockDevice();
// PS 9/9/11 - moved up top
//laserPullUnsetParamsFromRobot();
//laserCheckParams();
int size = ArMath::roundInt((270/.3) + 1);
ArLog::log(myInfoLogLevel,
"%s::blockingConnect() Setting current buffer size to %d",
getName(), size);
setCurrentBufferSize(size);
ArTime timeDone;
if (myPowerControlled)
{
if (!timeDone.addMSec(60 * 1000))
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect() error adding msecs (60 * 1000)",
getName());
}
}
else
{
if (!timeDone.addMSec(30 * 1000))
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect() error adding msecs (30 * 1000)",
getName());
}
}
ArSZSeriesPacket *packet;
ArSZSeriesPacket sendPacket;
#if 0
sendPacket.empty();
sendPacket.uByteToBuf(0xA0); // stop continous sending
sendPacket.uByteToBuf(0x00);
sendPacket.uByteToBuf(0x1D);
sendPacket.uByteToBuf(0x7E);
sendPacket.finalizePacket();
if ((myConn->write(sendPacket.getBuf(), sendPacket.getLength())) == -1)
{
ArLog::log(ArLog::Terse,
"%s::blockingConnect() Could not send Stop Continuous mode to laser", getName());
failedToConnect();
return false;
}
#endif
// Build the Start Continuous sending packet and set it
// once we get a response, then we are connected, note
// the response needs to be a real reading
sendPacket.empty();
// command id = 0x91
//sendPacket.uByteToBuf(145);
sendPacket.uByteToBuf(0x91);
// note communication ID default is 0
// this value is set via the SZ Configurator
// ???? not sure what to do if it fails
// and put in CRC - from manual, this is
// specific to the communication ID = 0
sendPacket.uByteToBuf(0);
sendPacket.uByteToBuf(43);
sendPacket.uByteToBuf(218);
unsigned short crc = myReceiver.CRC16((unsigned char *)sendPacket.getBuf(), 2);
#if 0
// other communications IDs and CRC
// communication ID =1
sendPacket.uByteToBuf(1);
sendPacket.uByteToBuf(59);
sendPacket.uByteToBuf(251);
// communication ID =2
sendPacket.uByteToBuf(2);
sendPacket.uByteToBuf(11);
sendPacket.uByteToBuf(152);
// communication ID =3
sendPacket.uByteToBuf(3);
sendPacket.uByteToBuf(27);
sendPacket.uByteToBuf(185);
#endif
sendPacket.finalizePacket();
IFDEBUG(
int i;
char x[100000];
printf("buffer with len = %d: ",sendPacket.getLength());
for (i = 0;i < sendPacket.getLength();i++)
{
printf("0x%x ",sendPacket.getBuf()[i] & 0xff);
//sprintf(&x[i], "%2x", (char *)sendPacket.getBuf()[i]);
}
printf("\n");
//ArLog::log(ArLog::Terse,
// "%s::blockingConnect() write Buffer = %s", getName(), x);
); // end IFDEBUG
// 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;
}
AREXPORT bool ArUrg_2_0::blockingConnect(void)
{
if (!getRunning())
runAsync();
myConnMutex.lock();
if (myConn == NULL)
{
ArLog::log(ArLog::Terse,
"%s: Could not connect because there is no connection defined",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
// if we have a starting baud and are a serial port, then change the
// baud rate... not by default this will set it to 0 baud which'll
// cause the serial stuff not to touch it
if (serConn != NULL)
serConn->setBaud(atoi(getStartingBaudChoice()));
if (myConn->getStatus() != ArDeviceConnection::STATUS_OPEN &&
!myConn->openSimple())
{
ArLog::log(ArLog::Terse,
"%s: Could not connect because the connection was not open and could not open it",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
myConnMutex.unlock();
lockDevice();
myTryingToConnect = true;
unlockDevice();
laserPullUnsetParamsFromRobot();
laserCheckParams();
ArUtil::sleep(100);
bool connected = false;
if (internalConnect())
connected = true;
if (connected)
{
lockDevice();
myIsConnected = true;
myTryingToConnect = false;
unlockDevice();
ArLog::log(ArLog::Normal, "%s: Connected to laser", getName());
laserConnect();
return true;
}
else
{
failedToConnect();
return false;
}
}
int main(int argc, char **argv)
{
struct settings robot_settings;
robot_settings.min_distance = 500;
robot_settings.max_velocity = 500;
robot_settings.tracking_factor = 1.0;
ArRobot robot;
Aria::init();
//laser
int ret; //Don't know what this variable is for
ArSick sick; // Laser scanner
ArSerialConnection laserCon; // Scanner connection
std::string str; // Standard output
// sonar, must be added to the robot
ArSonarDevice sonar;
// add the sonar to the robot
robot.addRangeDevice(&sonar);
// add the laser to the robot
robot.addRangeDevice(&sick);
ArArgumentParser argParser(&argc, argv);
ArSimpleConnector con(&argParser);
// the connection handler from above
ConnHandler ch(&robot);
if(!Aria::parseArgs())
{
Aria::logOptions();
Aria::shutdown();
return 1;
}
if(!con.connectRobot(&robot))
{
ArLog::log(ArLog::Normal, "directMotionExample: Could not connect to the robot. Exiting.");
return 1;
}
ArLog::log(ArLog::Normal, "directMotionExample: Connected.");
robot.runAsync(false);
///////////////////////////////
// Attempt to connect to SICK using another hard-coded USB connection
sick.setDeviceConnection(&laserCon);
if((ret=laserCon.open("/dev/ttyUSB1")) !=0){
//If connection fails, shutdown
Aria::shutdown();
return 1;
}
//Configure the SICK
sick.configureShort(false,/*not using sim*/ArSick::BAUD38400,ArSick::DEGREES180,ArSick::INCREMENT_HALF);
//Run the sick
sick.runAsync();
// Presumably test to make sure that the connection is good
if(!sick.blockingConnect()){
printf("Could not get sick...exiting\n");
Aria::shutdown();
return 1;
}
printf("We are connected to the laser!");
printf("\r\nRobot Entering default resting state.\r\nUse the following commands to run the robot.\r\n");
printf("r Run the robot\r\n");
printf("s Stop the robot\r\n");
printf("t Enter test mode (the robot will do everything except actually move.\r\n");
printf("w Save current data to files, and show a plot of what the robot sees.\r\n");
printf("e Edit robot control parameters\r\n");
printf("q Quit the program\r\n");
printf("\r\n");
printf("\r\n");
printf("NOTE: You must have GNUPLOT installed on your computer, and create a directory entitled 'scan_data' under your current directory in order to use this script. Failure to do so might make the computer crash, which would cause the robot to go on a mad killing spree! Not really, but seriously, go ahead and get GNUPLOT and create that subdirectory before using the 'w' option.\r\n\r\n");
/////////////////////////////////////
char user_command = 0;
char plot_option = 0;
int robot_state = REST;
tracking_object target;
tracking_object l_target;
ofstream fobjects;
ofstream ftarget;
ofstream flog;
ofstream fltarget;
fobjects.open("./scan_data/objects_new.txt");
ftarget.open("./scan_data/target_new.txt");
fltarget.open("./scan_data/ltarget_new.txt");
fobjects << "\r\n";
ftarget << "\r\n";
fltarget << "\r\n";
fobjects.close();
ftarget.close();
fltarget.close();
flog.open("robot_log.txt");
std::vector<tracking_object> obj_vector;
std::vector<tracking_object> new_vector;
float last_v = 0;
ArTime start;
char test_flag = 0;
char target_lost = 0;
while(user_command != 'q')
{
switch (user_command)
{
case STOP:
robot_state = REST;
printf("robot has entered resting mode\r\n");
break;
case RUN:
robot_state = TRACKING;
printf("robot has entered tracking mode\r\n");
break;
case QUIT:
robot_state = -1;
printf("exiting... goodbye.\r\n");
break;
case WRITE:
plot_option = WRITE;
break;
case NO_WRITE:
plot_option = NO_WRITE;
break;
case TEST:
if(test_flag == TEST)
{
test_flag = 0;
printf("Exiting test mode\r\n");
}
else
{
test_flag = TEST;
printf("Entering test mode\r\n");
robot.lock();
robot.setVel(0);
robot.unlock();
}
break;
case EDIT:
edit_settings(&robot_settings);
break;
default:
robot_state = robot_state;
}
unsigned int i = 0;
unsigned int num_objects = 0;
int to_ind = -1;
float min_distance = 999999.9;
float new_heading = 0;
system("mv ./scan_data/objects_new.txt ./scan_data/objects.txt");
system("mv ./scan_data/target_new.txt ./scan_data/target.txt");
system("mv ./scan_data/ltarget_new.txt ./scan_data/ltarget.txt");
fobjects.open("./scan_data/objects_new.txt");
ftarget.open("./scan_data/target_new.txt");
fltarget.open("./scan_data/ltarget_new.txt");
switch (robot_state)
{
case REST:
robot.lock();
robot.setVel(0);
robot.unlock();
obj_vector = run_sick_scan(&sick, 2, plot_option);
target_lost = 0;
num_objects = obj_vector.size();
if(num_objects > 0)
{
for(i = 0; i < num_objects; i++)
{
print_object_to_stream(obj_vector[i], fobjects);
}
}
break;
case TRACKING:
flog << "TRACKING\r\n";
obj_vector = get_moving_objects(&sick, 2*DT, 10, plot_option);
num_objects = obj_vector.size();
target_lost = 0;
if(min_range(&sick, 45, 135) > ROBOT_SAFETY_MARGIN)
{
if(num_objects)
{
for(i = 0; i < obj_vector.size();i++)
{
print_object_to_stream(obj_vector[i], fobjects);
if(obj_vector[i].vmag > 0.1)
{
if(obj_vector[i].distance < min_distance)
{
min_distance = obj_vector[i].distance;
target = obj_vector[i];
to_ind = i;
}
}
}
if(to_ind > -1)
{
int l_edge = target.l_edge;
int r_edge = target.r_edge;
float difference = 9999999.9;
// Do another scan to make sure the object is actually there.
new_vector = get_moving_objects(&sick, DT, 10, 0, 5, 175);
num_objects = new_vector.size();
to_ind = -1;
if(num_objects)
{
for(i = 0; i < num_objects;i++)
{
if(new_vector[i].vmag > 0.1)
{
if(r_diff(target, new_vector[i])<difference)
{
difference = r_diff(target, new_vector[i]);
if(difference < 500.0)
to_ind = i;
}
}
}
}
if(to_ind > -1)
{
new_heading = (-90 + new_vector[to_ind].degree);
if(test_flag != TEST)
{
robot.lock();
robot.setDeltaHeading(new_heading);
robot.unlock();
}
robot_state = FOLLOWING;
target = new_vector[to_ind];
target.degree = target.degree - new_heading;
print_object_to_stream(target, ftarget);
print_object_to_stream(target, flog);
flog << new_heading;
}
}
}
}
else
{
robot_state = TOO_CLOSE;
printf("I'm too close to an obstacle, and I'm getting claustrophobic! I'm going to slowly back up now.\r\n");
}
break;
case FOLLOWING:
{
flog << "FOLLOWING\r\n";
i = 0;
int to_ind = -1;
float obj_difference = 9999999.9;
int num_scans = 0;
if(min_range(&sick, 45, 135) > ROBOT_SAFETY_MARGIN)
{
while((num_scans < 3)&&(to_ind == -1))
{
obj_vector = run_sick_scan(&sick, 2, 0, 10, 350);
num_objects = obj_vector.size();
if(num_objects)
{
for(i = 0; i < num_objects;i++)
{
if(r_diff(target, obj_vector[i]) < obj_difference)
{
obj_difference = r_diff(target, obj_vector[i]);
if (obj_difference < 500.0)
to_ind = i;
}
}
for(i = 0; i < num_objects;i++)
{
if((int)i == to_ind)
{
print_object_to_stream(obj_vector[i], ftarget);
}
else
{
print_object_to_stream(obj_vector[i], fobjects);
}
}
}
num_scans++;
}
float new_vel = 0;
if(to_ind > -1)
{
printf("Tracking target\r\n");
flog << "Following target\t";
target_lost = 0;
target = obj_vector[to_ind];
new_vel = (obj_vector[to_ind].distance - robot_settings.min_distance)*robot_settings.tracking_factor;
if(new_vel < 0)
new_vel = 0;
if(new_vel > robot_settings.max_velocity)
new_vel = robot_settings.max_velocity;
new_heading = (-90 + target.degree)*0.25;
new_heading = get_safe_path(&sick, new_heading, target.distance);
if(test_flag != TEST)
{
robot.lock();
robot.setVel(new_vel);
robot.unlock();
last_v = new_vel;
robot.lock();
robot.setDeltaHeading(new_heading);
robot.unlock();
target.degree = target.degree - new_heading;
}
if(new_vel < 1.0)
{
robot_state = TRACKING;
printf("entering tracking mode\r\n");
flog<<"entering tracking mode\r\n";
}
}
else
{
if(target_lost)
{
l_target.distance = l_target.distance - last_v*(start.mSecSince()/1000.0);
int temp_max_v = min_range(&sick, 5, 175);
if(temp_max_v < last_v)
last_v = temp_max_v;
if(last_v > robot_settings.max_velocity)
last_v = robot_settings.max_velocity;
if(test_flag != TEST)
{
robot.lock();
robot.setVel(last_v);
robot.unlock();
new_heading = -90.0 + l_target.degree;
new_heading = get_safe_path(&sick, new_heading, l_target.distance);
l_target.degree = target.degree - new_heading;
robot.lock();
robot.setDeltaHeading(new_heading);
robot.unlock();
}
print_object_to_stream(l_target, fltarget);
}
else
{
target_lost = 1;
l_target = target;
}
printf("target lost\r\n");
flog << "target lost\r\n";
start.setToNow();
if(target.distance < ROBOT_SAFETY_MARGIN)
{
robot_state = TRACKING;
target_lost = 0;
}
target_lost = 1;
}
printf("Velocity: %f\t",last_v);
flog << "Velocity:\t";
flog << last_v;
printf("Heading: %f\t",new_heading);
flog << "\tHeading\t";
flog << new_heading;
flog << "\r\n";
printf("\r\n");
}
else
{
robot_state = TOO_CLOSE;
printf("I'm too close to an obstacle, and I'm getting claustrophobic! I'm going to slowly back up now.\r\n");
}
break;
}
case TOO_CLOSE:
{
flog << "Too close\r\n";
if(min_range(&sick, 45, 135) > (ROBOT_SAFETY_MARGIN + 100))
{
robot_state = TRACKING;
printf("I feel better now! I'm going to reenter tracking mode.\r\n");
robot.lock();
robot.setVel(0);
robot.unlock();
}
else
{
if(test_flag != TEST)
{
printf("Backing up...\r\n");
robot.lock();
robot.setVel(-50);
robot.unlock();
}
}
break;
}
}
fobjects.close();
ftarget.close();
fltarget.close();
if(plot_option == WRITE)
{
system("./scan_data/plot_script.sh >/dev/null");
}
//////////////////////////////////////////////////////////////////////
// Everything past this point is code to grab the user input
user_command = 0;
fd_set rfds;
struct timeval tv;
int retval;
FD_ZERO(&rfds);
FD_SET(0, &rfds);
tv.tv_sec = 0;
tv.tv_usec = 100;
retval = select(1, &rfds, NULL, NULL, &tv);
if(retval == -1)
perror("select()");
else if(retval)
{
cin >> user_command;
printf("input detected from user\r\n");
}
plot_option = NO_WRITE;
//////////////////////////////////////////////////////////////////////
}
flog.close();
Aria::shutdown();
printf("Shutting down");
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(void)
{
ArSerialConnection con;
ArRobot robot;
int ret;
std::string str;
ArActionLimiterForwards limiter("speed limiter near", 300, 600, 250);
ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 400);
ArActionLimiterBackwards backwardsLimiter;
ArActionConstantVelocity stop("stop", 0);
ArActionConstantVelocity backup("backup", -200);
ArSonarDevice sonar;
ArACTS_1_2 acts;
ArSonyPTZ sony(&robot);
ArGripper gripper(&robot, ArGripper::GENIO);
Acquire acq(&acts, &gripper);
DriveTo driveTo(&acts, &gripper, &sony);
PickUp pickUp(&acts, &gripper, &sony);
TakeBlockToWall takeBlock(&robot, &gripper, &sony, &acq, &driveTo, &pickUp,
&backup);
Aria::init();
if (!acts.openPort(&robot))
{
printf("Could not connect to acts\n");
exit(1);
}
robot.addRangeDevice(&sonar);
//con.setBaud(38400);
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;
}
sony.init();
ArUtil::sleep(1000);
//robot.setAbsoluteMaxTransVel(400);
robot.setStateReflectionRefreshTime(250);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
ArUtil::sleep(200);
robot.addAction(&limiter, 100);
robot.addAction(&limiterFar, 99);
robot.addAction(&backwardsLimiter, 98);
robot.addAction(&acq, 77);
robot.addAction(&driveTo, 76);
robot.addAction(&pickUp, 75);
robot.addAction(&backup, 50);
robot.addAction(&stop, 30);
robot.run(true);
Aria::shutdown();
return 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;
}
int main(int argc, char **argv)
{
std::string str;
int ret;
time_t lastTime;
int trans, rot;
ArJoyHandler joyHandler;
ArSerialConnection con;
ArRobot robot(NULL, false);
joyHandler.init();
joyHandler.setSpeeds(100, 700);
if (joyHandler.haveJoystick())
{
printf("Have a joystick\n\n");
}
else
{
printf("Do not have a joystick, set up the joystick then rerun the program\n\n");
// exit(0);
}
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.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
//robot.comInt(ArCommands::ENCODER, 1);
//robot.comStrN(ArCommands::SAY, "\1\6\2\105", 4);
lastTime = time(NULL);
while (1)
{
if (!robot.isConnected())
{
printf("No longer connected to robot, exiting.\n");
exit(0);
}
robot.loopOnce();
if (lastTime != time(NULL))
{
printf("\rx %6.1f y %6.1f tth %6.1f vel %7.1f mpacs %3d", robot.getX(),
robot.getY(), robot.getTh(), robot.getVel(),
robot.getMotorPacCount());
fflush(stdout);
lastTime = time(NULL);
}
if (joyHandler.haveJoystick() && (joyHandler.getButton(1) ||
joyHandler.getButton(2)))
{
if (ArMath::fabs(robot.getVel()) < 10.0)
robot.comInt(ArCommands::ENABLE, 1);
joyHandler.getAdjusted(&rot, &trans);
robot.comInt(ArCommands::VEL, trans);
robot.comInt(ArCommands::RVEL, -rot);
}
else
{
robot.comInt(ArCommands::VEL, 0);
robot.comInt(ArCommands::RVEL, 0);
}
ArUtil::sleep(100);
}
}
bool ArUrg_2_0::internalConnect(void)
{
bool ret = true;
char buf[1024];
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
bool alreadyAtAutobaud = false;
// empty the buffer...
/*
sendCommandAndRecvStatus(
"RS", "reset",
buf, sizeof(buf), 1000);
readLine(buf, sizeof(buf), 1, true, false);
sendCommandAndRecvStatus(
"SCIP2.0", "SCIP 2.0 request",
buf, sizeof(buf), 1000);
*/
writeLine("RS");
ArUtil::sleep(100);
writeLine("SCIP2.0");
ArUtil::sleep(100);
ArTime startedFlushing;
while (readLine(buf, sizeof(buf), 1, true, false) ||
startedFlushing.mSecSince() < 1000);
buf[0] = '\0';
if (!(ret = sendCommandAndRecvStatus(
"VV", "version request",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
// if we didn't get it and have an autobaud, try it at what the autobaud rate is
if (serConn != NULL && atoi(getAutoBaudChoice()) > 0)
{
alreadyAtAutobaud = true;
serConn->setBaud(atoi(getAutoBaudChoice()));
ArUtil::sleep(100);
writeLine("RS");
ArUtil::sleep(100);
writeLine("SCIP2.0");
ArUtil::sleep(100);
startedFlushing.setToNow();
while (readLine(buf, sizeof(buf), 1, true, false) ||
startedFlushing.mSecSince() < 1000);
if (!(ret = sendCommandAndRecvStatus(
"VV", "version request after falling back to autobaudchoice",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
if (ret && strcasecmp(buf, "00") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on version response after falling back to autobaudchoice",
getName());
return false;
}
}
// if we don't have a serial port or no autobaud then we can't
// change the baud, so just fail
else
{
if (ret && strcasecmp(buf, "00") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on version response (%s)",
getName(), buf);
return false;
}
}
// if we want to autobaud, then give it a whirl
if (!alreadyAtAutobaud && serConn != NULL && atoi(getAutoBaudChoice()) > 0)
{
// empty the buffer from the last version request
while (readLine(buf, sizeof(buf), 100, true, false));
// now change the baud...
sprintf(buf, "SS%06d", atoi(getAutoBaudChoice()));
if (!writeLine(buf))
return false;
ArUtil::sleep(100);
//serConn->setBaud(115200);
serConn->setBaud(atoi(getAutoBaudChoice()));
// wait a second for the baud to change...
ArUtil::sleep(100);
// empty the buffer from the baud change
while (readLine(buf, sizeof(buf), 100, true, false));
if (!(ret = sendCommandAndRecvStatus(
"VV", "version request after switching to autobaudchoice",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
if (ret && strcasecmp(buf, "00") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on version response after switching to autobaudchoice",
getName());
return false;
}
ArLog::log(ArLog::Verbose, "%s: Switched to %s baud rate",
getName(), getAutoBaudChoice());
}
while (readLine(buf, sizeof(buf), 10000, false, true))
{
if (strlen(buf) == 0)
break;
if (strncasecmp(buf, "VEND:", strlen("VEND:")) == 0)
myVendor = &buf[5];
else if (strncasecmp(buf, "PROD:", strlen("PROD:")) == 0)
myProduct = &buf[5];
else if (strncasecmp(buf, "FIRM:", strlen("FIRM:")) == 0)
myFirmwareVersion = &buf[5];
else if (strncasecmp(buf, "PROT:", strlen("PROT:")) == 0)
myProtocolVersion = &buf[5];
else if (strncasecmp(buf, "SERI:", strlen("SERI:")) == 0)
mySerialNumber = &buf[5];
else if (strncasecmp(buf, "STAT:", strlen("STAT:")) == 0)
myStat = &buf[5];
}
if (myVendor.empty() || myProduct.empty() || myFirmwareVersion.empty() ||
myProtocolVersion.empty() || mySerialNumber.empty())
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Missing information in version response",
getName());
return false;
}
if (!(ret = sendCommandAndRecvStatus(
"PP", "parameter info request",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad response to parameter info request",
getName());
return false;
}
while (readLine(buf, sizeof(buf), 10000, false, true))
{
if (strlen(buf) == 0)
break;
if (strncasecmp(buf, "MODL:", strlen("MODL:")) == 0)
myModel = &buf[5];
else if (strncasecmp(buf, "DMIN:", strlen("DMIN:")) == 0)
myDMin = atoi(&buf[5]);
else if (strncasecmp(buf, "DMAX:", strlen("DMAX:")) == 0)
myDMax = atoi(&buf[5]);
else if (strncasecmp(buf, "ARES:", strlen("ARES:")) == 0)
myARes = atoi(&buf[5]);
else if (strncasecmp(buf, "AMIN:", strlen("AMIN:")) == 0)
myAMin = atoi(&buf[5]);
else if (strncasecmp(buf, "AMAX:", strlen("AMAX:")) == 0)
myAMax = atoi(&buf[5]);
else if (strncasecmp(buf, "AFRT:", strlen("AFRT:")) == 0)
myAFront = atoi(&buf[5]);
else if (strncasecmp(buf, "SCAN:", strlen("SCAN:")) == 0)
myScan = atoi(&buf[5]);
}
if (myModel.empty() || myDMin == 0 || myDMax == 0 || myARes == 0 ||
myAMin == 0 || myAMax == 0 || myAFront == 0 || myScan == 0)
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Missing information in parameter info response",
getName());
return false;
}
myStepSize = 360.0 / myARes;
myStepFirst = myAFront * myStepSize;
if (myMaxRange > myDMax)
setMaxRange(myDMax);
//log();
setParams(getStartDegrees(), getEndDegrees(), getIncrement(), getFlipped());
//myLogMore = true;
// myLogMore = false;
ArUtil::sleep(100);
//printf("myRequestString %s\n", myRequestString);
if (!(ret = sendCommandAndRecvStatus(
myRequestString, "request distance reading",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
if (ret && strcasecmp(buf, "00") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on distance reading response (%s)",
getName(), buf);
return false;
}
//myLogMore = false;
ArTime started;
started.setToNow();
while (started.secSince() < 10 &&
readLine(buf, sizeof(buf), 10000, true, false))
{
if (strlen(buf) == 0)
return true;
}
ArLog::log(ArLog::Normal, "%s::blockingConnect: Did not get distance reading back",
getName());
return false;
}
int main(int argc, char **argv)
{
int ret;
std::string str;
ArSerialConnection con;
const std::list<ArSensorReading *> *readings;
std::list<ArSensorReading *>::const_iterator it;
int i;
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->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())
while (1)
{
//dist = sick->getCurrentBuffer().getClosestPolar(-90, 90, ArPose(0, 0), 30000, &angle);
sick->lockDevice();
if (!sick->getRunning() || !sick->isConnected())
{
break;
}
printf("\r");
readings = sick->getRawReadings();
if (readings != NULL)
{
for (i = 0, it = readings->begin(); it != readings->end(); it++, i++)
{
if (abs(i - readings->size()/2) < 3)
printf("(%.2f %d) ", (*it)->getSensorTh(), (*it)->getRange());
}
}
// switch the commenting in out of the fflush and the
// printf("\n"); if you want to print on the same line or have it
// scrolling
fflush(stdout);
//printf("\n");
sick->unlockDevice();
ArUtil::sleep(100);
}
sick->lockDevice();
sick->stopRunning();
sick->disconnect();
sick->unlockDevice();
return 0;
}
int main(void)
{
ArSerialConnection con;
ArRobot robot;
int ret;
std::string str;
ArActionLimiterForwards limiter("speed limiter near", 225, 600, 250);
ArActionLimiterForwards limiterFar("speed limiter far", 225, 1100, 400);
ArActionTableSensorLimiter tableLimiter;
ArActionLimiterBackwards backwardsLimiter;
ArActionConstantVelocity stop("stop", 0);
ArSonarDevice sonar;
ArACTS_1_2 acts;
ArPTZ *ptz;
ptz = new ArVCC4(&robot, true);
ArGripper gripper(&robot);
Acquire acq(&acts, &gripper);
DriveTo driveTo(&acts, &gripper, ptz);
DropOff dropOff(&acts, &gripper, ptz);
PickUp pickUp(&acts, &gripper, ptz);
TakeBlockToWall takeBlock(&robot, &gripper, ptz, &acq, &driveTo, &pickUp,
&dropOff, &tableLimiter);
if (!acts.openPort(&robot))
{
printf("Could not connect to acts, exiting\n");
exit(0);
}
Aria::init();
robot.addRangeDevice(&sonar);
//con.setBaud(38400);
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;
}
ptz->init();
ArUtil::sleep(8000);
printf("### 2222\n");
ptz->panTilt(0, -40);
printf("### whee\n");
ArUtil::sleep(8000);
robot.setAbsoluteMaxTransVel(400);
robot.setStateReflectionRefreshTime(250);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
ArUtil::sleep(200);
robot.addAction(&tableLimiter, 100);
robot.addAction(&limiter, 99);
robot.addAction(&limiterFar, 98);
robot.addAction(&backwardsLimiter, 97);
robot.addAction(&acq, 77);
robot.addAction(&driveTo, 76);
robot.addAction(&pickUp, 75);
robot.addAction(&dropOff, 74);
robot.addAction(&stop, 30);
robot.run(true);
Aria::shutdown();
return 0;
}
int main(int argc, char **argv)
{
std::string str;
int ret;
int dist;
ArTime start;
ArPose startPose;
bool vel2 = false;
// connection to the robot
ArSerialConnection con;
// the robot
ArRobot robot;
// the connection handler from above
ConnHandler ch(&robot);
// init area with a dedicated signal handling thread
Aria::init(Aria::SIGHANDLE_THREAD);
if (argc != 2 || (dist = atoi(argv[1])) == 0)
{
printf("Usage: %s <distInMM>\n", argv[0]);
exit(0);
}
if (dist < 1000)
{
printf("You must go at least a meter\n");
exit(0);
}
// open the connection with the defaults, exit if failed
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
// set the robots connection
robot.setDeviceConnection(&con);
// try to connect, if we fail, the connection handler should bail
if (!robot.blockingConnect())
{
// this should have been taken care of by the connection handler
// but just in case
printf(
"asyncConnect failed because robot is not running in its own thread.\n");
Aria::shutdown();
return 1;
}
// run the robot in its own thread, so it gets and processes packets and such
robot.runAsync(false);
// just a big long set of printfs, direct motion commands and sleeps,
// it should be self-explanatory
robot.lock();
/*
robot.setAbsoluteMaxTransVel(2000);
robot.setTransVelMax(2000);
robot.setTransAccel(1000);
robot.setTransDecel(1000);
robot.comInt(82, 30); // rotkp
robot.comInt(83, 200); // rotkv
robot.comInt(84, 0); // rotki
robot.comInt(85, 30); // transkp
robot.comInt(86, 450); // transkv
robot.comInt(87, 4); // transki
*/
printf("Driving %d mm (going full speed for that far minus a meter then stopping)\n", dist);
if (vel2)
robot.setVel2(2200, 2200);
else
robot.setVel(2200);
robot.unlock();
start.setToNow();
startPose = robot.getPose();
while (1)
{
robot.lock();
printf("\r vel: %.0f x: %.0f y: %.0f: dist: %.0f heading: %.2f",
robot.getVel(), robot.getX(), robot.getY(),
startPose.findDistanceTo(robot.getPose()),
robot.getTh());
if (startPose.findDistanceTo(robot.getPose()) > abs(dist) - 1000)
{
printf("\nFinished distance\n");
robot.setVel(0);
robot.unlock();
break;
}
if (start.mSecSince() > 10000)
{
printf("\nDistance timed out\n");
robot.setVel(0);
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(50);
}
if (vel2)
robot.setVel2(0, 0);
else
robot.setVel(0);
start.setToNow();
while (1)
{
robot.lock();
if (vel2)
robot.setVel2(0, 0);
else
robot.setVel(0);
if (fabs(robot.getVel()) < 20)
{
printf("Stopped\n");
robot.unlock();
break;
}
if (start.mSecSince() > 2000)
{
printf("\nStop timed out\n");
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(50);
}
robot.lock();
robot.disconnect();
robot.unlock();
// shutdown and ge tout
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;
}
*/
}
AREXPORT bool ArUrg_2_0::setParamsBySteps(int startingStep, int endingStep,
int clusterCount, bool flipped)
{
if (startingStep > endingStep || clusterCount < 1)
{
ArLog::log(ArLog::Normal,
"%s::setParamsBySteps: Bad params (starting %d ending %d clusterCount %d)",
getName(), startingStep, endingStep, clusterCount);
return false;
}
if (startingStep < myAMin)
startingStep = myAMin;
if (endingStep > myAMax)
endingStep = myAMax;
myDataMutex.lock();
myStartingStep = startingStep;
myEndingStep = endingStep;
myClusterCount = clusterCount;
myFlipped = flipped;
//sprintf(myRequestString, "G%03d%03d%02d", myStartingStep, endingStep,
//clusterCount);
int baudRate = 0;
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
if (serConn != NULL)
baudRate = serConn->getBaud();
// only use the three data bytes if our range needs it, and if the baud rate can support it
if (myMaxRange > 4095 && (baudRate == 0 || baudRate > 57600))
{
myUseThreeDataBytes = true;
sprintf(myRequestString, "MD%04d%04d%02d%01d%02d",
myStartingStep, myEndingStep, myClusterCount,
0, // scan interval
0 // number of scans to send (forever)
);
}
else
{
myUseThreeDataBytes = false;
if (myMaxRange > 4094)
myMaxRange = 4094;
sprintf(myRequestString, "MS%04d%04d%02d%01d%02d",
myStartingStep, myEndingStep, myClusterCount,
0, // scan interval
0 // number of scans to send (forever)
);
}
myClusterMiddleAngle = 0;
if (myClusterCount > 1)
//myClusterMiddleAngle = myClusterCount * 0.3515625 / 2.0;
myClusterMiddleAngle = myClusterCount * myStepSize / 2.0;
if (myRawReadings != NULL)
{
ArUtil::deleteSet(myRawReadings->begin(), myRawReadings->end());
myRawReadings->clear();
delete myRawReadings;
myRawReadings = NULL;
}
myRawReadings = new std::list<ArSensorReading *>;
ArSensorReading *reading;
int onStep;
double angle;
for (onStep = myStartingStep;
onStep < myEndingStep;
onStep += myClusterCount)
{
/// FLIPPED
if (!myFlipped)
//angle = ArMath::subAngle(ArMath::subAngle(135,
// onStep * 0.3515625),
angle = ArMath::subAngle(ArMath::subAngle(myStepFirst,
onStep * myStepSize),
myClusterMiddleAngle);
else
//angle = ArMath::addAngle(ArMath::addAngle(-135,
// onStep * 0.3515625),
angle = ArMath::addAngle(ArMath::addAngle(-myStepFirst,
onStep * myStepSize),
myClusterMiddleAngle);
reading = new ArSensorReading;
reading->resetSensorPosition(ArMath::roundInt(mySensorPose.getX()),
ArMath::roundInt(mySensorPose.getY()),
ArMath::addAngle(angle,
mySensorPose.getTh()));
myRawReadings->push_back(reading);
//printf("%.1f ", reading->getSensorTh());
}
myDataMutex.unlock();
return true;
}
int main(int argc, char **argv)
{
int ret; //Don't know what this variable is for
ArRobot robot;// Robot object
ArSick sick; // Laser scanner
ArSerialConnection laserCon; // Scanner connection
ArSerialConnection con; // Robot connection
std::string str; // Standard output
// sonar, must be added to the robot
ArSonarDevice sonar;
// the actions we'll use to wander
// recover from stalls
ArActionStallRecover recover;
// react to bumpers
ArActionBumpers bumpers;
// limiter for close obstacles
ArActionLimiterForwards limiter("speed limiter near", 300, 600, 250, 1.1);
// limiter for far away obstacles
ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 600, 1.1);
// limiter for the table sensors
ArActionLimiterTableSensor tableLimiter;
// actually move the robot
ArActionConstantVelocity constantVelocity("Constant Velocity", 400);
// turn the orbot if its slowed down
ArActionTurn turn;
// mandatory init
Aria::init();
// Parse all our args
ArSimpleConnector connector(&argc, argv);
connector.parseArgs();
if (argc > 1)
{
connector.logOptions();
exit(1);
}
// add the sonar to the robot
robot.addRangeDevice(&sonar);
// add the laser to the robot
robot.addRangeDevice(&sick);
// NOTE: HARDCODED USB PORT!
// Attempt to open hard-coded USB to robot
if ((ret = con.open("/dev/ttyUSB2")) != 0){
// If connection fails, exit
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
// 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;
}
// turn on the motors, turn off amigobot sounds
//robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::SOUNDTOG, 0);
// start the robot running, true so that if we lose connection the run stops
robot.runAsync(true);
// Attempt to connect to SICK using another hard-coded USB connection
sick.setDeviceConnection(&laserCon);
if((ret=laserCon.open("/dev/ttyUSB3")) !=0) {
//If connection fails, shutdown
Aria::shutdown();
return 1;
}
//Configure the SICK
sick.configureShort(false,/*not using sim*/ArSick::BAUD38400,ArSick::DEGREES180,ArSick::INCREMENT_HALF);
//Run the sick
sick.runAsync();
// Presumably test to make sure that the connection is good
if(!sick.blockingConnect()){
printf("Could not get sick...exiting\n");
Aria::shutdown();
return 1;
}
printf("We are connected to the laser!");
/*
robot.lock();
robot.comInt(ArCommands::ENABLE, 1);
robot.unlock();
*/
int range [361] = {0};
int drange [360] = {0};
int i = 0;
int obj_range [2];
int old_range [360]={0};
clock_t now, prev;
while(1){
range [361] = {0};
drange [360] = {0};
i = 0;
obj_range[2];
std::list<ArSensorReading *> *readings;
std::list<ArSensorReading *>::iterator it;
sick.lockDevice();
readings=(list<ArSensorReading *,allocator<ArSensorReading *> > *)sick.getRawReadings();
if(NULL!=readings){
if ((readings->end() != readings->begin())){
for (it = readings->begin(); it!= readings->end(); it++){
// std::cout << (*it)->getRange()<<" ";
range[i] = ((*it)->getRange());
if(i){
drange[i-1] = range[i] - range[i-1];
printf("%f %i %i\r\n", (float)i/2.0, range[i], drange[i-1]);
}
i++;
}
int i = 0;
//detect the object range
while (i < 360) {
if (range[i]>Default_Distance + alpha) {
;
} else {
if (obj_range[0]=0)
obj_range[0]=i;
else
obj_range[1]=i;
}
}
if (!now)
prev=now;
now=clock();
duration=now-prev;
/******moving straight*******/
float speed = avg_speed(obj_range,old_range,range,(float)duration)
/*while(i < 360){
int r_edge = 0;
int l_edge = 0;
float obsticle_degree = 0;
if(drange[i] > D_DISTANCE){
r_edge = i;
while(drange[i] > -(D_DISTANCE)){
i++;
}
l_edge = i;
obsticle_degree = (r_edge + (l_edge - r_edge)/2.0)/2.0;
printf("\r\n object detected at %f\r\n", obsticle_degree);
}
std::cout<<std::endl;
}*/
}
else{
std::cout << "(readings->end() == readings -> begin())" << std::endl;
}
}
else{
