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