/*!
* Shuts down the system.
*
*/
void
Advanced::shutDown(void)
{
Aria::exit(0);
//
// Stop the path planning thread.
//
if(myPathPlanningTask){
myPathPlanningTask->stopRunning();
// delete myPathPlanningTask;
printf("Stopped Path Planning Thread\n");
}
//
// Stop the localization thread.
//
if(myLocaTask){
myLocaTask->stopRunning();
delete myLocaTask;
printf("Stopped Localization Thread\n");
}
//
// Stop the laser thread.
//
if(mySick)
{
mySick->lockDevice();
mySick->disconnect();
mySick->unlockDevice();
printf("Stopped Laser Thread\n");
}
//
// Stop the robot thread.
//
myRobot->lock();
myRobot->stopRunning();
myRobot->unlock();
printf("Stopped Robot Thread\n");
//
// Exit Aria
//
Aria::shutdown();
printf("Aria Shutdown\n");
}
// Collects a laser reading from the robot, and corrects the XY-rotation
vector<PointXY> CollectLaser(ArSick &sick, double maxRange)
{
vector<PointXY> laserPoints;
sick.lockDevice();
vector<ArSensorReading> * readings = sick.getRawReadingsAsVector();
for(vector<ArSensorReading>::const_iterator it = readings->begin(); it != readings->end(); it++)
{
if(it->getRange() < maxRange)
{
// Correct coordinates right here so we don't have to do it later!
double x = -it->getLocalY();
double y = it->getLocalX();
laserPoints.push_back(PointXY(x, y));
}
}
sick.unlockDevice();
//ArUtil::sleep(100);
return laserPoints;
}
int main(int argc, char **argv)
{
Aria::init();
// parse our args and make sure they were all accounted for
ArSimpleConnector connector(&argc, argv);
ArRobot robot;
// the laser. ArActionTriangleDriveTo will use this laser object since it is
// named "laser" when added to the ArRobot.
ArSick sick;
if (!connector.parseArgs() || argc > 1)
{
connector.logOptions();
exit(1);
}
// a key handler so we can do our key handling
ArKeyHandler keyHandler;
// let the global aria stuff know about it
Aria::setKeyHandler(&keyHandler);
// toss it on the robot
robot.attachKeyHandler(&keyHandler);
// add the laser to the robot
robot.addRangeDevice(&sick);
ArSonarDevice sonar;
robot.addRangeDevice(&sonar);
ArActionTriangleDriveTo triangleDriveTo;
ArFunctorC<ArActionTriangleDriveTo> lineGoCB(&triangleDriveTo,
&ArActionTriangleDriveTo::activate);
keyHandler.addKeyHandler('g', &lineGoCB);
keyHandler.addKeyHandler('G', &lineGoCB);
ArFunctorC<ArActionTriangleDriveTo> lineStopCB(&triangleDriveTo,
&ArActionTriangleDriveTo::deactivate);
keyHandler.addKeyHandler('s', &lineStopCB);
keyHandler.addKeyHandler('S', &lineStopCB);
ArActionLimiterForwards limiter("limiter", 150, 0, 0, 1.3);
robot.addAction(&limiter, 70);
ArActionLimiterBackwards limiterBackwards;
robot.addAction(&limiterBackwards, 69);
robot.addAction(&triangleDriveTo, 60);
ArActionKeydrive keydrive;
robot.addAction(&keydrive, 55);
ArActionStop stopAction;
robot.addAction(&stopAction, 50);
// try to connect, if we fail exit
if (!connector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
robot.comInt(ArCommands::SONAR, 1);
robot.comInt(ArCommands::ENABLE, 1);
// start the robot running, true so that if we lose connection the run stops
robot.runAsync(true);
// now set up the laser
connector.setupLaser(&sick);
sick.runAsync();
if (!sick.blockingConnect())
{
printf("Could not connect to SICK laser... exiting\n");
Aria::shutdown();
return 1;
}
printf("If you press the 'g' key it'll go find a triangle, if you press 's' it'll stop.\n");
robot.waitForRunExit();
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)
{
//----------------------initialized robot server------------------------------------------
Aria::init();
Arnl::init();
ArServerBase server;
//-----------------------------------------------------------------------------------
VCCHandler ptz(&robot); //create keyboard for control vcc50i
G_PTZHandler->reset();
ArUtil::sleep(300);
G_PTZHandler->panSlew(30);
//-----------------------------------------------------------------------------------
argc = 2 ;
argv[0] = "-map";
argv[1] = "map20121111.map";
// Parse the command line arguments.
ArArgumentParser parser(&argc, argv);
// Set up our simpleConnector
ArSimpleConnector simpleConnector(&parser);
// Set up our simpleOpener
ArServerSimpleOpener simpleOpener(&parser);
//*******
// Set up our client for the central server
// ArClientSwitchManager clientSwitch(&server, &parser);
//************
// Load default arguments for this computer (from /etc/Aria.args, environment
// variables, and other places)
// parser.loadDefaultArguments();
// set up a gyro
ArAnalogGyro gyro(&robot);
//gyro.activate();
// Parse arguments for the simple connector.
// if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
// {
// ArLog::log(ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]);
// Aria::logOptions();
// Aria::exit(1);
// }
// The laser object, will be used if we have one
// Add the laser to the robot
robot.addRangeDevice(&sick);
// Sonar, must be added to the robot, used by teleoperation and wander to
// detect obstacles, and for localization if SONARNL
ArSonarDevice sonarDev;
// Add the sonar to the robot
robot.addRangeDevice(&sonarDev);
// Set up where we'll look for files
char fileDir[1024];
ArUtil::addDirectories(fileDir, sizeof(fileDir), "./", "maps");
ArLog::log(ArLog::Normal, "Installation directory is: %s\nMaps directory is: %s\n", Aria::getDirectory(), fileDir);
// Set up the map, this will look for files in the examples
// directory (unless the file name starts with a /, \, or .
// You can take out the 'fileDir' argument to look in the current directory
// instead
ArMap arMap(fileDir);
// set it up to ignore empty file names (otherwise the parseFile
// on the config will fail)
arMap.setIgnoreEmptyFileName(true);
//********************************
//Localization
//********************************
ArLocalizationManager locManager(&robot, &arMap);
ArLog::log(ArLog::Normal, "Creating laser localization task");
ArLocalizationTask locTask (&robot, &sick, &arMap);
locManager.addLocalizationTask(&locTask);
//*******************************
//Path planning
//*******************************
// Make the path task planning task
ArPathPlanningTask pathTask(&robot, &sick, &sonarDev, &arMap);
G_PathPlanning = &pathTask;
// Set up things so data can be logged (only do it with the laser
// since it can overrun a 9600 serial connection which the sonar is
// more likely to have)
ArDataLogger dataLogger(&robot);
dataLogger.addToConfig(Aria::getConfig());
// add our logging to the config
// ArLog::addToConfig(Aria::getConfig());
// First open the server
if (!simpleOpener.open(&server, fileDir, 240))
{
if (simpleOpener.wasUserFileBad())
ArLog::log(ArLog::Normal, "Bad user file");
else
ArLog::log(ArLog::Normal, "Could not open server port");
exit(2);
}
// Connect the robot
if (!simpleConnector.connectRobot(&robot))
{
ArLog::log(ArLog::Normal, "Could not connect to robot... exiting");
Aria::exit(3);
}
//-----------------------------------------------
//**************************
// Set up a class that'll put the movement and gyro parameters into ArConfig
ArRobotConfig robotConfig(&robot);
robotConfig.addAnalogGyro(&gyro);
//*****************************
robot.enableMotors();
robot.clearDirectMotion();
// if we are connected to a simulator, reset it to its start position
robot.comInt(ArCommands::RESETSIMTOORIGIN, 1);
robot.moveTo(ArPose(0,0,0));
// Set up laser using connector (command line arguments, etc.)
simpleConnector.setupLaser(&sick);
// Start the robot thread.
robot.runAsync(true);
// Start the laser thread.
sick.runAsync();
// Try to connect the laser
if (!sick.blockingConnect())
ArLog::log(ArLog::Normal, "Warning: Couldn't connect to SICK laser, it won't be used");
else
ArLog::log(ArLog::Normal, "Connected to laser.");
//***************************************
// Add additional range devices to the robot and path planning task.
// IRs if the robot has them.
robot.lock();
// ArIRs irs;
// robot.addRangeDevice(&irs);
// pathTask.addRangeDevice(&irs, ArPathPlanningTask::CURRENT);
//******************************************
// Forbidden regions from the map
ArForbiddenRangeDevice forbidden(&arMap);
robot.addRangeDevice(&forbidden);
// This is the place to add a range device which will hold sensor data
// and delete it appropriately to replan around blocked paths.
ArGlobalReplanningRangeDevice replanDev(&pathTask);
// Create objects that add network services:
// Drawing in the map display:
ArServerInfoDrawings drawings(&server);
drawings.addRobotsRangeDevices(&robot);
drawings.addRangeDevice(&replanDev);
/* If you want to draw the destination put this code back in:
ArServerDrawingDestination destination(
&drawings, &pathTask, "destination",
500, 500,
new ArDrawingData("polyDots",
ArColor(0xff, 0xff, 0x0),
800, // size
49), // just below the robot
*/
/* If you want to see the local path planning area use this
(You can enable this particular drawing from custom commands
which is set up down below in ArServerInfoPath)
ArDrawingData drawingDataP("polyLine", ArColor(200,200,200), 1, 75);
ArFunctor2C<ArPathPlanningTask, ArServerClient *, ArNetPacket *>
drawingFunctorP(pathTask, &ArPathPlanningTask::drawSearchRectangle);
drawings.addDrawing(&drawingDataP, "Local Plan Area", &drawingFunctorP);
*/
/* If you want to see the points making up the local path in addition to the
* main path use this.
ArDrawingData drawingDataP2("polyDots", ArColor(0,128,0), 100, 70);
ArFunctor2C<ArPathPlanningTask, ArServerClient *, ArNetPacket *>
drawingFunctorP2(pathTask, &ArPathPlanningTask::drawPathPoints);
drawings.addDrawing(&drawingDataP2, "Path Points", &drawingFunctorP2);
*/
// Misc. simple commands:
ArServerHandlerCommands commands(&server);
// These provide various kinds of information to the client:
ArServerInfoRobot serverInfoRobot(&server, &robot);
ArServerInfoSensor serverInfoSensor(&server, &robot);
ArServerInfoPath serverInfoPath(&server, &robot, &pathTask);
serverInfoPath.addSearchRectangleDrawing(&drawings);
serverInfoPath.addControlCommands(&commands);
//-------------------------receive commands or events from client---------------------
// ArServerHandlerCommands commands(&server);
server.addData("RobotVideo" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&VideoServerBase::RobotVideoCB) ,"","");
server.addData("turn" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&turn_func) ,"","");
server.addData("RobotMotion" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_RobotMotion) ,"","");
server.addData("CameraMotion" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_CameraMotion) ,"","");
server.addData("RobotTurnLeft" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_RobotTurnLeft) ,"","");
server.addData("RobotTurnRight" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_RobotTurnRight) ,"","");
server.addData("TargetApproach" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_TargetApproach) ,"","");
server.addData("TargetApproachObstacles" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_TargetApproach_Obstacles) ,"","");
server.addData("GlassesCancel" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_GlassesCancel) ,"","");
server.addData("Calibration" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_Calibration) ,"","");
server.addClientRemovedCallback(new ArGlobalFunctor1< ArServerClient * >(&clientCloseCallback));
//-------------------------receive commands or events from client---------------------
//***********************
ArServerInfoLocalization serverInfoLocalization (&server, &robot, &locManager);
ArServerHandlerLocalization serverLocHandler (&server, &robot, &locManager);
// Provide the map to the client (and related controls):
// This uses both lines and points now, since everything except
// sonar localization uses both (path planning with sonar still uses both)
ArServerHandlerMap serverMap(&server, &arMap);
// Add some simple (custom) commands for testing and debugging:
ArServerSimpleComUC uCCommands(&commands, &robot); // Send any command to the microcontroller
ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // configure logging
ArServerSimpleComGyro gyroCommands(&commands, &robot, &gyro); // monitor the gyro
ArServerSimpleComLogRobotConfig configCommands(&commands, &robot); // trigger logging of the robot config parameters
ArServerSimpleServerCommands serverCommands(&commands, &server); // monitor networking behavior (track packets sent etc.)
/* Set up the possible modes for remote control from a client such as
* MobileEyes:
*/
// Mode To go to a goal or other specific point:
ArServerModeGoto modeGoto(&server, &robot, &pathTask, &arMap, ArPose(0,0,0));
// Add a simple (custom) command that allows you to give a list of
// goals to tour, instead of all. Useful for testing and debugging.
modeGoto.addTourGoalsInListSimpleCommand(&commands);
// Mode To stop and remain stopped:
ArServerModeStop modeStop(&server, &robot);
// cause the sonar to turn off automatically
// when the robot is stopped, and turn it back on when commands to move
// are sent. (Note, this should not be done if you need the sonar
// data to localize, or for other purposes while stopped)
ArSonarAutoDisabler sonarAutoDisabler(&robot);
// Teleoperation modes To drive by keyboard, joystick, etc:
ArServerModeRatioDrive modeRatioDrive(&server, &robot); // New, improved mode
ArServerModeDrive modeDrive(&server, &robot); // Older mode for compatability
// Drive mode's configuration and custom (simple) commands:
modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings");
modeDrive.addControlCommands(&commands);
modeRatioDrive.addControlCommands(&commands);
// Wander mode
// ArServerModeWander modeWander(&server, &robot);
//*********************************
// Prevent driving if localization is lost:
ArActionLost actionLostRatioDrive (&locManager, NULL, &modeRatioDrive);
modeRatioDrive.getActionGroup ()->addAction (&actionLostRatioDrive, 110);
// Prevent wandering if lost:
// ArActionLost
// actionLostWander (&locManager, NULL, &modeWander);
// modeWander.getActionGroup ()->addAction (&actionLostWander, 110);
// This provides a small table of interesting information for the client
// to display to the operator:
ArServerInfoStrings stringInfo(&server);
Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor());
Aria::getInfoGroup()->addStringInt(
"Motor Packet Count", 10,
new ArConstRetFunctorC<int, ArRobot>(&robot,
&ArRobot::getMotorPacCount));
// Make Stop mode the default (If current mode deactivates without entering
// a new mode, then Stop Mode will be selected)
modeStop.addAsDefaultMode();
// Create the service that allows the client to monitor the communication
// between the robot and the client.
//
ArServerHandlerCommMonitor handlerCommMonitor(&server);
// Create service that allows client to change configuration parameters in ArConfig
ArServerHandlerConfig handlerConfig(&server, Aria::getConfig(),
Arnl::getTypicalDefaultParamFileName(),
Aria::getDirectory());
// Read in parameter files. read the paras from input
Aria::getConfig()->useArgumentParser(&parser);
if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
{
ArLog::log(ArLog::Normal, "Trouble loading configuration file, exiting");
Aria::exit(5);
}
// Warn about unknown params.
if (!simpleOpener.checkAndLog() || !parser.checkHelpAndWarnUnparsed())
{
ArLog::log(ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]);
simpleConnector.logOptions();
simpleOpener.logOptions();
Aria::exit(6);
}
// Warn if there is no map
if (arMap.getFileName() == NULL || strlen(arMap.getFileName()) <= 0)
{
ArLog::log(ArLog::Normal, "");
ArLog::log(ArLog::Normal, "### Warning, No map file is set up, you can make a map with sickLogger or arnlServer, and Mapper3; More info in docs/Mapping.txt and README.txt. Set the map with the -map command line option, or by changing the config with MobileEyes or by editing the config file.");
ArLog::log(ArLog::Normal, "");
}
// find out where we'll want to put files
ArLog::log(ArLog::Normal, "");
ArLog::log(ArLog::Normal,
"Directory for maps and file serving: %s", fileDir);
ArLog::log(ArLog::Normal, "See the ARNL README.txt for more information");
ArLog::log(ArLog::Normal, "");
// If you want MobileSim to try and load up the same map as you are
// using in guiServer then uncomment out the next line and this object
// will send a command to MobileSim to do so, but make sure you start
// MobileSim from the Arnl/examples directory or use the --cwd option,
// so that the map names used by MobileSim match the map names used
// by guiServer
//ArSimMapSwitcher mapSwitcher(&robot, &arMap);
/******************************************************
* ****************************************************
* Camera
* ****************************************************
* *****************************************************/
robot.unlock();
// Localize robot at home.
locTask.localizeRobotAtHomeBlocking();
locTask.forceUpdatePose(ArPose(0,0,0));
resetMotion();
// robot.enableMotors();
// robot.runAsync(true);
//locTask.localizeRobotAtHomeBlocking();
server.runAsync();
VideoServerBase videoserver;
videoserver.runAsync();
robot.comInt(ArCommands::SOUNDTOG, 0);
//G_PathPlanning->pathPlanToPose(ArPose(1500, -1500 , -32),true,true);
//while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL );
//S_TargetApproach_Obstacles1();
//ArUtil::sleep(5000);
// G_PTZHandler->panRel(60);
// ArUtil::sleep(3000);
//while(1)
//S_TargetApproach1();
cout << "done!!!!!!!---------------------" <<endl;
robot.waitForRunExit();
Aria::exit(0);
}
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;
}
void S_TargetApproach( ArServerClient *serverclient, ArNetPacket *socket)
{
//Important: to halt current movement of camera, making the reading curret.
G_PTZHandler->haltPanTilt();
cout << "The last step: TargetApproach!" <<endl;
//G_PathPlanning->setCollisionRange(1000);
//G_PathPlanning->setFrontClearance(40);
//G_PathPlanning->setGoalDistanceTolerance(2500);
double camAngle = -1 * G_PTZHandler->getPan();
double robotHeading = robot.getPose().getTh();
double robotCurrentX = robot.getPose().getX() ;
double robotCurrentY = robot.getPose().getY();
double angle = 0.0;
double distance =0.0;
double targetX, targetY;
int disThreshold = 500;
//test mode
// G_PTZHandler->panRel(-30);
//G_PathPlanning->setFrontClearance(40);
//G_PathPlanning->setObsThreshold(1);
cout << "--------------Path Planning Information--------------------" <<endl;
cout << "SafeCollisionRange = " << G_PathPlanning->getSafeCollisionRange() << endl;
cout << "FrontClearance = " << G_PathPlanning->getFrontClearance() << endl
<< "GoalDistanceTolerance = " << G_PathPlanning->getGoalDistanceTolerance() << endl
<< "setGoalOccupiedFailDistance = " << G_PathPlanning->getGoalOccupiedFailDistance() << endl
<< "getObsThreshold = " << G_PathPlanning->getObsThreshold() <<endl
<< "getLocalPathFailDistance = " << G_PathPlanning->getLocalPathFailDistance() << endl;
//getCurrentGoal
//--------------- Set up the laser range device to read the distance from the target -----------------------------
// for(int i =0; i< 20;i++)
//sick.currentReadingPolar(robotHeading+ camAngle -2.5, robotHeading+ camAngle +2.5, &angle);
//Begin:
sick.lockDevice();
//if (distance == 0 || distance>disThreshold)
distance = sick.currentReadingPolar(/*robotHeading+*/ camAngle -2.5, /*robotHeading+*/ camAngle +2.5, &angle)-disThreshold;
//double distance = sick.currentReadingPolar(89, 90, &angle);
cout << "The closest reading is " << distance << " at " << angle << " degree , " << "disThreshold : " <<disThreshold << " " << robotHeading << " " << camAngle<< endl;
sick.unlockDevice();
//----------------------------------------------------------------------------------------------------------------
//basic_turn(camAngle);
cout << "Camera Angle is " << camAngle << endl;
cout << "before calculation, check originalX, originalY " << robotCurrentX << " " << robotCurrentY << " robotHeading is " << robotHeading << endl<<endl;
coordinateCalculation(robotCurrentX,robotCurrentY,&targetX,&targetY,camAngle,robotHeading,distance);
//
//cout << "before movement, check targetX, target Y" << targetX << " " << targetY << "robotHeading is "<< robotHeading << endl << endl;
cout << "targetX : " <<targetX << " targetY" <<targetY;
G_PathPlanning->pathPlanToPose(ArPose(targetX,targetY,camAngle),true,true);
cout << "RobotMotion is processing..." <<endl;
G_PTZHandler->reset();
ArUtil::sleep(200);
G_PTZHandler->tiltRel(-10);
while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL )
{
if (G_PathPlanning->getState() == ArPathPlanningTask::FAILED_MOVE)
{
G_PathPlanning->cancelPathPlan();cout << "x " << robot.getPose().getX()<< " y " <<robot.getPose().getY() <<endl; break;
}
else if(G_PathPlanning->getState() == ArPathPlanningTask::FAILED_PLAN)
{
// getchar();
//getchar();
//getchar();
// getchar();
// disThreshold+=50;
//
// goto Begin;
}
}
//serverclient->sendPacketTcp(socket);
ArUtil::sleep(3000);
cout << "RobotMotion is heading home..." <<endl;
G_PathPlanning->pathPlanToPose(ArPose(0,0,0),true,true);
while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL )
{
//cout << G_PathPlanning->getState() <<endl;
if (G_PathPlanning->getState() == ArPathPlanningTask::FAILED_MOVE)
{
G_PathPlanning->cancelPathPlan(); break;
}
// //else if(G_PathPlanning->getState() == ArPathPlanningTask::FAILED_PLAN)
// // {G_PathPlanning->pathPlanToPose(ArPose(-300,30,0),true,true);}
}
}
int main(int argc, char **argv)
{
int t, cnt;
double laser_dist[900];
double laser_angle[900];
std::list<ArSensorReading *> *readings;
std::list<ArSensorReading *>::iterator it;
ArKeyHandler keyHandler;
Aria::init();
// Add the key handler to Aria so other things can find it
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
// add the laser to the robot
robot.addRangeDevice(&sick);
// Parse all our args
ArSimpleConnector connector(&argc, argv);
if (!connector.parseArgs() || argc > 1)
{
connector.logOptions();
exit(1);
}
robot.addRangeDevice(&sick);
// try to connect, if we fail exit
if (!connector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// start the robot running, true so that if we lose connection the run stops
robot.runAsync(true);
// now set up the laser
sick.configureShort(true,ArSick::BAUD38400,ArSick::DEGREES180,ArSick::INCREMENT_ONE);
connector.setupLaser(&sick);
sick.runAsync();
if (!sick.blockingConnect())
{
printf("Could not connect to SICK laser... exiting\n");
Aria::shutdown();
return 1;
}
cnt = 1;
while(cnt<10000){
readings=(list<ArSensorReading *,allocator<ArSensorReading *> > *)sick.getRawReadings();//CurrentBuffer..
while (readings == NULL){
readings = (list<ArSensorReading *, allocator<ArSensorReading *> > *)sick.getRawReadings();
}
t=0;
for(it=readings->begin(); it!=readings->end(); it++){
//cout << "t: " << t << endl;
laser_dist[t]=(*it)->getRange();
laser_angle[t]=-90+t;
//cout << "laser angle: " << laser_angle[t] << " laser dist.: " << laser_dist[t] <<" "<<"\n";
t++;
}
cout << "count: " << cnt << endl; //for some reason this line needs to be here
cnt++;
}
for (t=0; t<181; t++){
cout << "laser angle: " << laser_angle[t] << " laser dist.: " << laser_dist[t] <<" "<<"\n";
}
robot.waitForRunExit();
Aria::shutdown();
return 0;
}
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{
int main(int argc, char **argv)
{
Aria::init();
ArSimpleConnector connector(&argc, argv);
ArRobot robot;
ArSick sick;
if (!Aria::parseArgs() || argc > 1)
{
Aria::logOptions();
Aria::exit(1); // exit program with error code 1
return 1;
}
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
robot.addRangeDevice(&sick);
// Create the ArLineFinder object. Set it to log lots of information about its
// processing.
ArLineFinder lineFinder(&sick);
lineFinder.setVerbose(true);
// Add key callbacks that simply call the ArLineFinder::getLinesAndSaveThem()
// function, which searches for lines in the current set of laser sensor
// readings, and saves them in files with the names 'points' and 'lines'.
ArFunctorC<ArLineFinder> findLineCB(&lineFinder,
&ArLineFinder::getLinesAndSaveThem);
keyHandler.addKeyHandler('f', &findLineCB);
keyHandler.addKeyHandler('F', &findLineCB);
ArLog::log(ArLog::Normal, "lineFinderExample: connecting to robot...");
if (!connector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::exit(1); // exit program with error code 1
return 1;
}
robot.runAsync(true);
// now set up the laser
ArLog::log(ArLog::Normal, "lineFinderExample: connecting to SICK laser...");
connector.setupLaser(&sick);
sick.runAsync();
if (!sick.blockingConnect())
{
printf("Could not connect to SICK laser... exiting\n");
Aria::exit(1);
return 1;
}
printf("If you press the 'f' key the points and lines found will be saved\n");
printf("Into the 'points' and 'lines' file in the current working directory\n");
robot.waitForRunExit();
Aria::exit(0);
return 0;
}
int main(int argc, char **argv)
{
// robot
ArRobot robot;
// the laser
ArSick sick;
// 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", 1600, 0, 0, 1.3);
// limiter for far away obstacles
//ArActionLimiterForwards limiterFar("speed limiter near", 300, 1000, 450, 1.1);
//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", 1500);
// turn the orbot if its slowed down
ArActionTurn turn;
// mandatory init
Aria::init();
// Parse all our args
ArSimpleConnector connector(&argc, argv);
if (!connector.parseArgs() || argc > 1)
{
connector.logOptions();
exit(1);
}
// add the sonar to the robot
//robot.addRangeDevice(&sonar);
// add the laser to the robot
robot.addRangeDevice(&sick);
// try to connect, if we fail exit
if (!connector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
robot.comInt(ArCommands::SONAR, 0);
// turn on the motors, turn off amigobot sounds
//robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::SOUNDTOG, 0);
// add the actions
//robot.addAction(&recover, 100);
//robot.addAction(&bumpers, 75);
robot.addAction(&limiter, 49);
//robot.addAction(&limiter, 48);
//robot.addAction(&tableLimiter, 50);
robot.addAction(&turn, 30);
robot.addAction(&constantVelocity, 20);
robot.setStateReflectionRefreshTime(50);
limiter.activate();
turn.activate();
constantVelocity.activate();
robot.clearDirectMotion();
//robot.setStateReflectionRefreshTime(50);
robot.setRotVelMax(50);
robot.setTransAccel(1500);
robot.setTransDecel(100);
// start the robot running, true so that if we lose connection the run stops
robot.runAsync(true);
connector.setupLaser(&sick);
// 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");
Aria::shutdown();
return 1;
}
sick.lockDevice();
sick.setMinRange(250);
sick.unlockDevice();
robot.lock();
ArGlobalFunctor1<ArRobot *> userTaskCB(&userTask, &robot);
robot.addUserTask("iotest", 100, &userTaskCB);
requestTime.setToNow();
robot.comInt(ArCommands::IOREQUEST, 1);
robot.comInt(ArCommands::ENABLE, 1);
robot.unlock();
robot.waitForRunExit();
// now exit
Aria::shutdown();
return 0;
}
int main(int argc, char** argv)
{
Aria::init();
ArLog::init(ArLog::StdErr, ArLog::Normal);
ArArgumentParser argParser(&argc, argv);
ArSimpleConnector connector(&argParser);
ArGPSConnector gpsConnector(&argParser);
ArRobot robot;
ArActionLimiterForwards nearLimitAction("limit near", 300, 600, 250);
ArActionLimiterForwards farLimitAction("limit far", 300, 1100, 400);
ArActionLimiterBackwards limitBackwardsAction;
ArActionJoydrive joydriveAction;
ArActionKeydrive keydriveAction;
ArSonarDevice sonar;
ArSick laser;
argParser.loadDefaultArguments();
if(!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
return -1;
}
robot.addRangeDevice(&sonar);
robot.addRangeDevice(&laser);
ArLog::log(ArLog::Normal, "gpsRobotTaskExample: Connecting to robot...");
if(!connector.connectRobot(&robot))
{
ArLog::log(ArLog::Terse, "gpsRobotTaskExample: Could not connect to the robot. Exiting.");
return -2;
}
ArLog::log(ArLog::Normal, "gpsRobotTaskExample: Connected to the robot.");
// Connect to GPS
ArLog::log(ArLog::Normal, "gpsRobotTaskExample: Connecting to GPS, it may take a few seconds...");
ArGPS *gps = gpsConnector.createGPS();
if(!gps || !gps->connect());
{
ArLog::log(ArLog::Terse, "gpsRobotTaskExample: Error connecting to GPS device. Try -gpsType, -gpsPort, and/or -gpsBaud command-line arguments. Use -help for help. Exiting.");
return -3;
}
// Create an GPSLogTask which will register a task with the robot.
GPSLogTask gpsTask(&robot, gps, joydriveAction.getJoyHandler()->haveJoystick() ? joydriveAction.getJoyHandler() : NULL);
// Add actions
robot.addAction(&nearLimitAction, 100);
robot.addAction(&farLimitAction, 90);
robot.addAction(&limitBackwardsAction, 80);
robot.addAction(&joydriveAction, 50);
robot.addAction(&keydriveAction, 40);
// allow keydrive action to drive robot even if joystick button isn't pressed
joydriveAction.setStopIfNoButtonPressed(false);
// Start the robot running
robot.runAsync(true);
// Connect to the laser
connector.setupLaser(&laser);
laser.runAsync();
if(!laser.blockingConnect())
ArLog::log(ArLog::Normal, "gpsRobotTaskExample: Warning, could not connect to SICK laser, will not use it.");
robot.lock();
robot.enableMotors();
robot.comInt(47, 1); // enable joystick driving on some robots
// Add exit callback to reset/unwrap steering wheels on seekur (critical if the robot doesn't have sliprings); does nothing for other robots
Aria::addExitCallback(new ArRetFunctor1C<bool, ArRobot, unsigned char>(&robot, &ArRobot::com, (unsigned char)120));
Aria::addExitCallback(new ArRetFunctor1C<bool, ArRobot, unsigned char>(&robot, &ArRobot::com, (unsigned char)120));
robot.unlock();
ArLog::log(ArLog::Normal, "gpsRobotTaskExample: Running... (drive robot with joystick or arrow keys)");
robot.waitForRunExit();
return 0;
}
int main(int argc, char **argv)
{
double speed = 1000;
double squareSide = 2000;
// 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
// robot
robot = new ArRobot;
// the laser
ArSick sick;
// set up our simpleConnector
ArSimpleConnector simpleConnector(&argc, argv);
// set up a key handler so escape exits and attach to the robot
ArKeyHandler keyHandler;
robot->attachKeyHandler(&keyHandler);
// parse its arguments
if (simpleConnector.parseArgs())
{
simpleConnector.logOptions();
keyHandler.restore();
exit(1);
}
// if there are more arguments left then it means we didn't
// understand an option
/*
if (argc > 1)
{
simpleConnector.logOptions();
keyHandler.restore();
exit(1);
}
*/
// 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);
// set up the robot for connecting
if (!simpleConnector.connectRobot(robot))
{
printf("Could not connect to robot->.. exiting\n");
Aria::shutdown();
return 1;
}
robot->setRotVelMax(300);
robot->setRotAccel(300);
robot->setRotDecel(300);
robot->setAbsoluteMaxTransVel(2000);
robot->setTransVelMax(2000);
robot->setTransAccel(500);
robot->setTransDecel(500);
/*
robot->comInt(82, 30); // rotkp
robot->comInt(83, 200); // rotkv
robot->comInt(84, 0); // rotki
robot->comInt(85, 15); // transkp
robot->comInt(86, 450); // transkv
robot->comInt(87, 4); // transki
*/
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
// run the robot, true here so that the run will exit if connection lost
robot->runAsync(true);
// set up the laser before handing it to the laser mode
simpleConnector.setupLaser(&sick);
// 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;
}
robot->lock();
robot->addUserTask("printer", 50, new ArGlobalFunctor(&printer));
robot->unlock();
#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
printf("Starting moving\n");
robot->lock();
// enable the motors, disable amigobot sounds
robot->comInt(ArCommands::ENABLE, 1);
robot->setHeading(0);
robot->setVel(1000);
robot->unlock();
ArUtil::sleep(speed / 500.0 * 1000.0);
printf("Should be up to speed, moving on first side\n");
ArUtil::sleep(squareSide / speed * 1000);
printf("Turning to second side\n");
robot->lock();
robot->setHeading(90);
robot->setVel(speed);
robot->unlock();
ArUtil::sleep(squareSide / speed * 1000);
printf("Turning to third side\n");
robot->lock();
robot->setHeading(180);
robot->setVel(speed);
robot->unlock();
ArUtil::sleep(squareSide / speed * 1000);
printf("Turning to last side\n");
robot->lock();
robot->setHeading(-90);
robot->setVel(speed);
robot->unlock();
ArUtil::sleep(squareSide / speed * 1000);
printf("Pointing back original direction and stopping\n");
robot->lock();
robot->setHeading(0);
robot->setVel(0);
robot->unlock();
ArUtil::sleep(300);
printf("Stopped\n");
sick.lockDevice();
sick.disconnect();
sick.unlockDevice();
robot->lock();
robot->disconnect();
robot->unlock();
// now exit
Aria::shutdown();
return 0;
}
int main( int argc, char **argv ){
// parse our args and make sure they were all accounted for
ArSimpleConnector connector(&argc, argv);
ArRobot robot;
ArSick sick;
double dist, angle = 0;
// Allow for esc to release robot
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit\n");
if( !connector.parseArgs() || argc > 1 ){
connector.logOptions();
exit(1);
}
// add the laser to the robot
robot.addRangeDevice(&sick);
// try to connect, if we fail exit
if( !connector.connectRobot(&robot) ){
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// start the robot running, true so that if we lose connection the run stops
robot.runAsync(true);
// now set up the laser
connector.setupLaser(&sick);
sick.runAsync();
if( !sick.blockingConnect() ){
printf("Could not connect to SICK laser... exiting\n");
Aria::shutdown();
return 1;
}
robot.comInt(ArCommands::ENABLE, 1);
ArPose pose(0, -1000, 0);
robot.moveTo(pose);
ArPose prev_pose = robot.getPose();
double total_distance = 0;
printf("Connected\n");
ArUtil::sleep(1000);
PathLog log("../Data/reactive.dat");
int iterations_wo_movement = 0;
while( iterations_wo_movement < CONSECUTIVE_NON_MOTIONS ){
// Get updated measurement
sick.lockDevice();
dist = sick.currentReadingPolar(-90, 90, &angle);
sick.unlockDevice();
dist = (dist > 30000) ? 0 : dist - IDEAL_DISTANCE;
trackRobot(&robot, dist, angle);
ArUtil::sleep(500);
pose = robot.getPose();
log.write(pose);
total_distance += getDistance(prev_pose, pose);
prev_pose = pose;
// Determine if the robot is done tracking
isRobotTracking(&iterations_wo_movement, dist, angle);
}
ArUtil::sleep(1000);
log.close();
ofstream output;
output.open("../Data/reactive_dist.dat", ios::out | ios::trunc);
output << "Reactive 1 " << total_distance << endl;
output.close();
Aria::exit(0);
return 0;
}
int main(int argc, char **argv)
{
Aria::init();
ArArgumentParser argParser(&argc, argv);
argParser.loadDefaultArguments();
ArRobot robot;
ArSick laser;
std::ofstream stream; // for loggin
time_t timer;
char buffer[120];
//for loggin
ArRobotConnector robotConnector(&argParser, &robot);
ArLaserConnector laserConnector(&argParser, &robot, &robotConnector);
int32_t count = 0;
readyLog(stream);
if (!robotConnector.connectRobot())
{
ArLog::log(ArLog::Terse, "Could not connect to the robot.");
if (argParser.checkHelpAndWarnUnparsed())
{
// -help not given, just exit.
Aria::logOptions();
Aria::exit(1);
return 1;
}
}
// Trigger argument parsing
if (!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
return 1;
}
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
puts("This program will make the robot wander around. It uses some avoidance\n"
"actions if obstacles are detected, otherwise it just has a\n"
"constant forward velocity.\n\nPress CTRL-C or Escape to exit.");
//ArSonarDevice sonar;
//robot.addRangeDevice(&sonar);
robot.addRangeDevice(&laser);
robot.runAsync(true);
// try to connect to laser. if fail, warn but continue, using sonar only
if (!laserConnector.connectLasers())
{
ArLog::log(ArLog::Normal, "Warning: unable to connect to requested lasers, will wander using robot sonar only.");
}
double sampleAngle, dist;
auto sampleRange = laser.currentReadingPolar(-20, 20, &sampleAngle);
auto degreeStr = laser.getDegreesChoicesString();
std::cout << "auto sampleRange = laser.currentReadingPolar(-20, 20, &sampleAngle);" << sampleRange << std::endl;
std::cout << "auto degreeStr = laser.getDegreesChoicesString(); : " << degreeStr << std::endl;
// turn on the motors, turn off amigobot sounds
robot.enableMotors();
//robot.getLaserMap()
robot.comInt(ArCommands::SOUNDTOG, 0);
// add a set of actions that combine together to effect the wander behavior
ArActionStallRecover recover;
ArActionBumpers bumpers;
ArActionAvoidFront avoidFrontNear("Avoid Front Near", 100, 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);
stream << "IMPORTANT !! == robot.getRobotRadius() : " << robot.getRobotRadius << std::endl;
std::string timestamp;
while (1)
{
//
//
time(&timer);
strftime(buffer, 80, " - timestamp : %I:%M:%S", localtime(&timer));
timestamp = buffer;
dist = robot.checkRangeDevicesCurrentPolar(-70, 70, &sampleAngle) - robot.getRobotRadius();
stream << count << timestamp << "checkRangeDevicesCurrentPolar(-70, 70, &angle) : " << dist << std::endl;
Sleep(500);
count++;
}
// wwill add processor here
// wait for robot task loop to end before exiting the program
robot.waitForRunExit();
Aria::exit(0);
return 0;
}
