/*! \example movePioneer.cpp example showing how to connect and send direct basic motion commands to a Pioneer mobile robot. WARNING: this program does no sensing or avoiding of obstacles, the robot WILL collide with any objects in the way! Make sure the robot has about 2-3 meters of free space around it before starting the program. This program will work either with the MobileSim simulator or on a real robot's onboard computer. (Or use -remoteHost to connect to a wireless ethernet-serial bridge.) */ int main(int argc, char **argv) { try { std::cout << "\nWARNING: this program does no sensing or avoiding of obstacles, \n" "the robot WILL collide with any objects in the way! Make sure the \n" "robot has approximately 3 meters of free space on all sides.\n" << std::endl; vpRobotPioneer robot; ArArgumentParser parser(&argc, argv); parser.loadDefaultArguments(); // ArRobotConnector connects to the robot, get some initial data from it such as type and name, // and then loads parameter files for this robot. ArRobotConnector robotConnector(&parser, &robot); if(!robotConnector.connectRobot()) { ArLog::log(ArLog::Terse, "Could not connect to the robot."); if(parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(1); } } if (!Aria::parseArgs()) { Aria::logOptions(); Aria::shutdown(); return false; } std::cout << "Robot connected" << std::endl; robot.useSonar(false); // disable the sonar device usage // Robot velocities vpColVector v(2), v_mes(2); for (int i=0; i < 100; i++) { double t = vpTime::measureTimeMs(); v = 0; v[0] = i/1000.; // Translational velocity in m/s //v[1] = vpMath::rad(i/5.); // Rotational velocity in rad/sec robot.setVelocity(vpRobot::REFERENCE_FRAME, v); v_mes = robot.getVelocity(vpRobot::REFERENCE_FRAME); std::cout << "Trans. vel= " << v_mes[0] << " m/s, Rot. vel=" << vpMath::deg(v_mes[1]) << " deg/s" << std::endl; v_mes = robot.getVelocity(vpRobot::ARTICULAR_FRAME); std::cout << "Left wheel vel= " << v_mes[0] << " m/s, Right wheel vel=" << v_mes[1] << " m/s" << std::endl; std::cout << "Battery=" << robot.getBatteryVoltage() << std::endl; vpTime::wait(t, 40); } ArLog::log(ArLog::Normal, "simpleMotionCommands: Stopping."); robot.lock(); robot.stop(); robot.unlock(); ArUtil::sleep(1000); robot.lock(); ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV", robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getRotVel(), robot.getBatteryVoltage()); robot.unlock(); std::cout << "Ending robot thread..." << std::endl; robot.stopRunning(); // wait for the thread to stop robot.waitForRunExit(); // exit ArLog::log(ArLog::Normal, "simpleMotionCommands: Exiting."); return 0; } catch(vpException e) { std::cout << "Catch an exception: " << e << std::endl; return 1; } }
/*! \example sonarPioneerReader.cpp example showing how to connect and read sonar data from a Pioneer mobile robot-> */ int main(int argc, char **argv) { try { ArArgumentParser parser(&argc, argv); parser.loadDefaultArguments(); robot = new vpRobotPioneer; // ArRobotConnector connects to the robot, get some initial data from it such as type and name, // and then loads parameter files for this robot. ArRobotConnector robotConnector(&parser, robot); if(!robotConnector.connectRobot()) { ArLog::log(ArLog::Terse, "Could not connect to the robot"); if(parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(1); } } if (!Aria::parseArgs()) { Aria::logOptions(); Aria::shutdown(); return false; } std::cout << "Robot connected" << std::endl; #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI) // Create a display to show sensor data if (isInitialized == false) { I.resize((unsigned int)half_size*2, (unsigned int)half_size*2); I = 255; #if defined(VISP_HAVE_X11) d = new vpDisplayX; #elif defined (VISP_HAVE_GDI) d = new vpDisplayGDI; #endif d->init(I, -1, -1, "Sonar range data"); isInitialized = true; } #endif // Activates the sonar ArGlobalFunctor sonarPrinterCB(&sonarPrinter); robot->addRangeDevice(&sonar); robot->addUserTask("Sonar printer", 50, &sonarPrinterCB); robot->useSonar(true); // activates the sonar device usage // Robot velocities vpColVector v_mes(2); for (int i=0; i < 1000; i++) { double t = vpTime::measureTimeMs(); v_mes = robot->getVelocity(vpRobot::REFERENCE_FRAME); std::cout << "Trans. vel= " << v_mes[0] << " m/s, Rot. vel=" << vpMath::deg(v_mes[1]) << " deg/s" << std::endl; v_mes = robot->getVelocity(vpRobot::ARTICULAR_FRAME); std::cout << "Left wheel vel= " << v_mes[0] << " m/s, Right wheel vel=" << v_mes[1] << " m/s" << std::endl; std::cout << "Battery=" << robot->getBatteryVoltage() << std::endl; #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI) if (isInitialized) { // A mouse click to exit // Before exiting save the last sonar image if (vpDisplay::getClick(I, false) == true) { { // Set the default output path std::string opath; #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX opath = "/tmp"; #elif defined(_WIN32) opath = "C:\\temp"; #endif std::string username = vpIoTools::getUserName(); // Append to the output path string, the login name of the user opath = vpIoTools::createFilePath(opath, username); // Test if the output path exist. If no try to create it if (vpIoTools::checkDirectory(opath) == false) { try { // Create the dirname vpIoTools::makeDirectory(opath); } catch (...) { std::cerr << std::endl << "ERROR:" << std::endl; std::cerr << " Cannot create " << opath << std::endl; exit(-1); } } std::string filename = opath + "/sonar.png"; vpImage<vpRGBa> C; vpDisplay::getImage(I, C); vpImageIo::write(C, filename); } break; } } #endif vpTime::wait(t, 40); } ArLog::log(ArLog::Normal, "simpleMotionCommands: Stopping."); robot->lock(); robot->stop(); robot->unlock(); ArUtil::sleep(1000); robot->lock(); ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV", robot->getX(), robot->getY(), robot->getTh(), robot->getVel(), robot->getRotVel(), robot->getBatteryVoltage()); robot->unlock(); std::cout << "Ending robot thread..." << std::endl; robot->stopRunning(); #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI) if (isInitialized) { if (d != NULL) delete d; } #endif // wait for the thread to stop robot->waitForRunExit(); delete robot; // exit ArLog::log(ArLog::Normal, "simpleMotionCommands: Exiting."); return 0; } catch(vpException &e) { std::cout << "Catch an exception: " << e << std::endl; return 1; } }