void SensorDetectPopup::popupClosed(ArTypes::Byte4 popupID, int button) { // A client closed the popup ArLog::log(ArLog::Normal, "popupExample: a client closed popup dialog window with id=%d. Button=%d...", popupID, button); myPopupDisplayed = false; if(button < 0) { ArLog::log(ArLog::Normal, "\t...popup timed out or closed due to an error."); return; } if (button == 0) { ArLog::log(ArLog::Normal, "\t...OK pressed."); return; } if(button == 1) { ArLog::log(ArLog::Normal, "\t...180 degree rotate requested."); myRobot->lock(); myRobot->setDeltaHeading(180); myRobot->unlock(); return; } if(button == 2) { ArLog::log(ArLog::Normal, "\t...exit requested."); myRobot->stopRunning(); Aria::shutdown(); Aria::exit(0); } }
ArActionDesired* ActionReadSonar::fire(ArActionDesired currentDesired) { ArRobot *robot = this->getRobot(); int total = robot->getNumSonar(); // get the total number of sonar on the robot ArSensorReading* value; // This class abstracts range and angle read from sonar //cout << " 0 : " << robot->getSonarReading(0)->getSensorTh() << " 1 : " << robot->getSonarReading(1)->getSensorTh() // << " 2 : " << robot->getSonarReading(2)->getSensorTh() << " 3 : " << robot->getSonarReading(3)->getSensorTh() // << " 4 : " << robot->getSonarReading(4)->getSensorTh() << " 5 : " << robot->getSonarReading(5)->getSensorTh() // << " 6 : " << robot->getSonarReading(6)->getSensorTh() << " 7 : " << robot->getSonarReading(7)->getSensorTh() // << "r :" << robot->getTh() << endl; double limit = 800; double distance; // reset the actionDesired (must be done), to clear // its previous values. myDesired.reset(); // if the sonar is null we can't do anything, so deactivate if (mySonar == NULL) { deactivate(); return NULL; } // gets value of object between -20 degrees and 20 degrees of foward double angle = 0; distance = mySonar->currentReadingPolar(-20, 20, &angle); //cout << "distance from nearest object =" << distance << endl; if (distance <= limit) { int heading = 15; //cout << "angle :" << angle << endl; if (angle > 10) { heading = -heading; } else if (angle < -10) { heading = heading; } //cout << "x" << robot->getX() << " ," << robot->getY() << endl; cout << "distance from nearest object =" << distance << endl; robot->lock(); robot->setVel(0); robot->unlock(); robot->lock(); robot->setDeltaHeading(heading); robot->unlock(); ArUtil::sleep(50); } return &myDesired; }
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) { Aria::init(); ArArgumentParser argParser(&argc, argv); ArSimpleConnector con(&argParser); ArRobot robot; // 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."); // Run the robot processing cycle in its own thread. Note that after starting this // thread, we must lock and unlock the ArRobot object before and after // accessing it. robot.runAsync(false); // Send the robot a series of motion commands directly, sleeping for a // few seconds afterwards to give the robot time to execute them. printf("directMotionExample: Setting rot velocity to 100 deg/sec then sleeping 3 seconds\n"); robot.lock(); robot.setRotVel(100); robot.unlock(); ArUtil::sleep(3*1000); printf("Stopping\n"); robot.lock(); robot.setRotVel(0); robot.unlock(); ArUtil::sleep(200); printf("directMotionExample: Telling the robot to go 300 mm on left wheel and 100 mm on right wheel for 5 seconds\n"); robot.lock(); robot.setVel2(300, 100); robot.unlock(); ArTime start; start.setToNow(); while (1) { robot.lock(); if (start.mSecSince() > 5000) { robot.unlock(); break; } robot.unlock(); ArUtil::sleep(50); } printf("directMotionExample: Telling the robot to move forwards one meter, then sleeping 5 seconds\n"); robot.lock(); robot.move(1000); robot.unlock(); start.setToNow(); while (1) { robot.lock(); if (robot.isMoveDone()) { printf("directMotionExample: Finished distance\n"); robot.unlock(); break; } if (start.mSecSince() > 5000) { printf("directMotionExample: Distance timed out\n"); robot.unlock(); break; } robot.unlock(); ArUtil::sleep(50); } printf("directMotionExample: Telling the robot to move backwards one meter, then sleeping 5 seconds\n"); robot.lock(); robot.move(-1000); robot.unlock(); start.setToNow(); while (1) { robot.lock(); if (robot.isMoveDone()) { printf("directMotionExample: Finished distance\n"); robot.unlock(); break; } if (start.mSecSince() > 10000) { printf("directMotionExample: Distance timed out\n"); robot.unlock(); break; } robot.unlock(); ArUtil::sleep(50); } printf("directMotionExample: Telling the robot to turn to 180, then sleeping 4 seconds\n"); robot.lock(); robot.setHeading(180); robot.unlock(); start.setToNow(); while (1) { robot.lock(); if (robot.isHeadingDone(5)) { printf("directMotionExample: Finished turn\n"); robot.unlock(); break; } if (start.mSecSince() > 5000) { printf("directMotionExample: Turn timed out\n"); robot.unlock(); break; } robot.unlock(); ArUtil::sleep(100); } printf("directMotionExample: Telling the robot to turn to 90, then sleeping 2 seconds\n"); robot.lock(); robot.setHeading(90); robot.unlock(); start.setToNow(); while (1) { robot.lock(); if (robot.isHeadingDone(5)) { printf("directMotionExample: Finished turn\n"); robot.unlock(); break; } if (start.mSecSince() > 5000) { printf("directMotionExample: turn timed out\n"); robot.unlock(); break; } robot.unlock(); ArUtil::sleep(100); } printf("directMotionExample: Setting vel2 to 200 mm/sec on both wheels, then sleeping 3 seconds\n"); robot.lock(); robot.setVel2(200, 200); robot.unlock(); ArUtil::sleep(3000); printf("directMotionExample: Stopping the robot, then sleeping for 2 seconds\n"); robot.lock(); robot.stop(); robot.unlock(); ArUtil::sleep(2000); printf("directMotionExample: Setting velocity to 200 mm/sec then sleeping 3 seconds\n"); robot.lock(); robot.setVel(200); robot.unlock(); ArUtil::sleep(3000); printf("directMotionExample: Stopping the robot, then sleeping for 2 seconds\n"); robot.lock(); robot.stop(); robot.unlock(); ArUtil::sleep(2000); printf("directMotionExample: Setting vel2 with 0 on left wheel, 200 mm/sec on right, then sleeping 5 seconds\n"); robot.lock(); robot.setVel2(0, 200); robot.unlock(); ArUtil::sleep(5000); printf("directMotionExample: Telling the robot to rotate at 50 deg/sec then sleeping 5 seconds\n"); robot.lock(); robot.setRotVel(50); robot.unlock(); ArUtil::sleep(5000); printf("directMotionExample: Telling the robot to rotate at -50 deg/sec then sleeping 5 seconds\n"); robot.lock(); robot.setRotVel(-50); robot.unlock(); ArUtil::sleep(5000); printf("directMotionExample: Setting vel2 with 0 on both wheels, then sleeping 3 seconds\n"); robot.lock(); robot.setVel2(0, 0); robot.unlock(); ArUtil::sleep(3000); printf("directMotionExample: Now having the robot change heading by -125 degrees, then sleeping for 6 seconds\n"); robot.lock(); robot.setDeltaHeading(-125); robot.unlock(); ArUtil::sleep(6000); printf("directMotionExample: Now having the robot change heading by 45 degrees, then sleeping for 6 seconds\n"); robot.lock(); robot.setDeltaHeading(45); robot.unlock(); ArUtil::sleep(6000); printf("directMotionExample: Setting vel2 with 200 on left wheel, 0 on right wheel, then sleeping 5 seconds\n"); robot.lock(); robot.setVel2(200, 0); robot.unlock(); ArUtil::sleep(5000); printf("directMotionExample: Done, shutting down Aria and exiting.\n"); Aria::shutdown(); return 0; }