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) { 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) { 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) { bool done; double distToTravel = 2300; // whether to use the sim for the laser or not, if you use the sim // for hte laser, you have to use the sim for the robot too bool useSim = false; // the laser ArSick sick; // connection ArDeviceConnection *con; // Laser connection ArSerialConnection laserCon; // robot ArRobot robot; // set a default filename //std::string filename = "c:\\log\\1scans.2d"; std::string filename = "1scans.2d"; // see if we want to use a different filename //if (argc > 1) //Lfilename = argv[1]; printf("Logging to file %s\n", filename.c_str()); // start the logger with good values sick.configureShort(useSim, ArSick::BAUD38400, ArSick::DEGREES180, ArSick::INCREMENT_HALF); ArSickLogger logger(&robot, &sick, 300, 25, filename.c_str()); // mandatory init Aria::init(); // add it to the robot robot.addRangeDevice(&sick); //ArAnalogGyro gyro(&robot); // if we're not using the sim, make a serial connection and set it up if (!useSim) { ArSerialConnection *serCon; serCon = new ArSerialConnection; serCon->setPort(); //serCon->setBaud(38400); con = serCon; } // if we are using the sim, set up a tcp connection else { ArTcpConnection *tcpCon; tcpCon = new ArTcpConnection; tcpCon->setPort(); con = tcpCon; } // set the connection on the robot robot.setDeviceConnection(con); // try to connect, if we fail exit if (!robot.blockingConnect()) { printf("Could not connect to robot... exiting\n"); Aria::shutdown(); return 1; } // set up a key handler so escape exits and attach to the robot ArKeyHandler keyHandler; robot.attachKeyHandler(&keyHandler); // run the robot, true here so that the run will exit if connection lost robot.runAsync(true); // if we're not using the sim, set up the port for the laser if (!useSim) { laserCon.setPort(ArUtil::COM3); sick.setDeviceConnection(&laserCon); } // now that we're connected to the robot, connect to the laser sick.runAsync(); if (!sick.blockingConnect()) { printf("Could not connect to SICK laser... exiting\n"); robot.disconnect(); Aria::shutdown(); return 1; } #ifdef WIN32 // wait until someone pushes the motor button to go while (1) { robot.lock(); if (!robot.isRunning()) exit(0); if (robot.areMotorsEnabled()) { robot.unlock(); break; } robot.unlock(); ArUtil::sleep(100); } #endif // basically from here on down the robot just cruises around a bit robot.lock(); // enable the motors, disable amigobot sounds robot.comInt(ArCommands::ENABLE, 1); ArTime startTime; // move a couple meters robot.move(distToTravel); robot.unlock(); startTime.setToNow(); do { ArUtil::sleep(100); robot.lock(); robot.setHeading(0); done = robot.isMoveDone(60); robot.unlock(); } while (!done); /* // rotate a few times robot.lock(); robot.setVel(0); robot.setRotVel(60); robot.unlock(); ArUtil::sleep(12000); */ robot.lock(); robot.setHeading(180); robot.unlock(); do { ArUtil::sleep(100); robot.lock(); robot.setHeading(180); done = robot.isHeadingDone(); robot.unlock(); } while (!done); // move a couple meters robot.lock(); robot.move(distToTravel); robot.unlock(); startTime.setToNow(); do { ArUtil::sleep(100); robot.lock(); robot.setHeading(180); done = robot.isMoveDone(60); robot.unlock(); } while (!done); robot.lock(); robot.setHeading(0); robot.setVel(0); robot.unlock(); startTime.setToNow(); do { ArUtil::sleep(100); robot.lock(); robot.setHeading(0); done = robot.isHeadingDone(); robot.unlock(); } while (!done); sick.lockDevice(); sick.disconnect(); sick.unlockDevice(); robot.lock(); robot.disconnect(); robot.unlock(); // now exit Aria::shutdown(); return 0; }
int main(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{