/*!
* 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)
{
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 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)
{
// 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)
{
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;
}
