void Disconnect() { laser.turnOff(); }
// ------------------------------------------------------ // Test_HOKUYO // ------------------------------------------------------ void Test_HOKUYO() { CHokuyoURG laser; string serName; cout << "HOKUYO laser range finder test application." << endl << endl; if (SERIAL_NAME.empty()) { cout << "Enter the serial port name (e.g. COM1, ttyS0, ttyUSB0, ttyACM0): "; getline(cin,serName); } else { cout << "Using serial port: " << SERIAL_NAME << endl; serName = SERIAL_NAME; } // Set the laser serial port: laser.m_com_port = serName; // Config: Use defaults + selected serial port printf("[TEST] Turning laser ON...\n"); if (laser.turnOn()) printf("[TEST] Initialization OK!\n"); else { printf("[TEST] Initialization failed!\n"); return; } #if MRPT_HAS_WXWIDGETS CDisplayWindowPlots win("Laser scans"); #endif cout << "Press any key to stop capturing..." << endl; CTicTac tictac; tictac.Tic(); while (!mrpt::system::os::kbhit()) { bool thereIsObservation,hardError; CObservation2DRangeScan obs; laser.doProcessSimple( thereIsObservation, obs, hardError ); if (hardError) printf("[TEST] Hardware error=true!!\n"); if (thereIsObservation) { double FPS = 1.0 / tictac.Tac(); printf("Scan received: %u ranges, FOV: %.02fdeg, %.03fHz: mid rang=%fm\n", (unsigned int)obs.scan.size(), RAD2DEG(obs.aperture), FPS, obs.scan[obs.scan.size()/2]); obs.sensorPose = CPose3D(0,0,0); mrpt::slam::CSimplePointsMap theMap; theMap.insertionOptions.minDistBetweenLaserPoints = 0; theMap.insertObservation( &obs ); //map.save2D_to_text_file("_out_scan.txt"); /* COpenGLScene scene3D; opengl::CPointCloudPtr points = opengl::CPointCloud::Create(); points->loadFromPointsMap(&map); scene3D.insert(points); CFileStream("_out_point_cloud.3Dscene",fomWrite) << scene3D; */ #if MRPT_HAS_WXWIDGETS vector_float xs,ys,zs; theMap.getAllPoints(xs,ys,zs); win.plot(xs,ys,".b3"); win.axis_equal(); #endif tictac.Tic(); } mrpt::system::sleep(5); }; laser.turnOff(); }