void SamInit(void) { puts("in laser"); laser.setSerialPort("COM10"); puts("set serial \n Starting laser, this may take a minute"); bool TurnedOn = false; while(!TurnedOn) // sometimes the coms are busy, wait untill we get what we want { try{TurnedOn = laser.turnOn();} catch (...){} } puts("laser turned on"); RecognisePort("Out"); StartModule("/Laser"); myfirst.open("/Laser_Out"); //myPortStatus myfirst.setReporter(myPortStatus); }
// ------------------------------------------------------ // Test_HOKUYO // ------------------------------------------------------ void Test_HOKUYO() { CHokuyoURG laser; string serName, type; string ip; unsigned int port; cout << "Specify the type of the Hokuyo connection, usb or ethernet: "; getline(cin,type); while ( (lowerCase(type) != "usb" ) && ( lowerCase(type) != "ethernet" ) ) { cout << "Incorrect type" << endl; cout << "Specify the type of the Hokuyo connection, usb or ethernet: "; getline(cin,type); } cout << endl << endl << "HOKUYO laser range finder test application." << endl << endl; if ( lowerCase(type) == "usb" ) { 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.setSerialPort( serName ); } else { cout << "Enter the ip direction: "; getline(cin,ip); cout << "Enter the port number: "; cin >> port; // Set the laser serial port: laser.setIPandPort( ip, port ); } // Config: Use defaults + selected port ( serial or ethernet ) 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(15); }; laser.turnOff(); }