int main(void) { std::string str; int ret; ArGlobalFunctor sonarPrinterCB(&sonarPrinter); ArTcpConnection con; Aria::init(); robot = new ArRobot; if ((ret = con.open()) != 0) { str = con.getOpenMessage(ret); printf("Open failed: %s\n", str.c_str()); exit(0); } robot->setDeviceConnection(&con); if (!robot->blockingConnect()) { printf("Could not connect to robot->.. exiting\n"); exit(0); } robot->addRangeDevice(&sonar); robot->addUserTask("Sonar printer", 50, &sonarPrinterCB); robot->comInt(ArCommands::SONAR, 1); robot->comInt(ArCommands::SOUNDTOG, 0); robot->run(true); Aria::shutdown(); }
int main(int argc, char** argv) { ArGlobalFunctor sonarPrinterCB(&sonarPrinter); Aria::init(); ArSimpleConnector connector(&argc, argv); if(!connector.parseArgs()) { connector.logOptions(); return 1; } robot = new ArRobot; robot->addRangeDevice(&sonar); if(!connector.connectRobot(robot)) { printf("Could not connect to robot.\n"); return 2; } robot->comInt(ArCommands::SONAR, 1); robot->comInt(ArCommands::SOUNDTOG, 0); printf("Closest readings within quadrants:\n"); robot->addUserTask("Sonar printer", 50, &sonarPrinterCB); robot->run(true); Aria::shutdown(); }
/*! \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; } }