void SamIter(void) { bool thereIsObservation,hardError; CObservation2DRangeScan myscan; laser.doProcessSimple( thereIsObservation, myscan, hardError ); Bottle &Send = myfirst.prepare(); Send.clear(); if(thereIsObservation ) { //scan only 512 points for 180 deg, ignore the first,last 85 points for(int x=0;x<myscan.scan.size()-85;x++) { //if(myscan.scan[x]<mindistance){myscan.scan[x]=0;} if(x>84) { Send.addDouble(myscan.scan[x]); if(myscan.scan[x]==1.0) std::cout << "obst sensed at " << x <<std::endl; //if(myscan.scan[x]<mindistance){myscan.scan[x]=0;} } //std::cout << "Scan size Vlaser" << myscan.scan.size() <<std::endl; } myfirst.writeStrict(); } //std::cout<< " laser size " <<Send.size() <<std::endl; }
bool receiveDataFromSensor(CObservation2DRangeScan* pObs, int laser_model) { bool thereIsObservation, hardError; int count =0; while(true) { if(laser_model == SICK){ sick.doProcessSimple(thereIsObservation, *pObs, hardError); }else if(laser_model == HOKUYO){ hokuyo.doProcessSimple(thereIsObservation, *pObs, hardError); } if(thereIsObservation && !hardError) break; else{ count++; } if(count>10) { cout << "Receive Data Error... and Exit" << endl; // sick.stop(); return false; } } return true; }
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); }
void initLaser(){ cout << "[init Laser start....]" << endl; // if(LASER_MODEL == SICK){ if(sick.start()){ cout << "Sick initialized successfully" << endl; } else { cout << "Error in initLaser" << endl; exit; } // }else if(LASER_MODEL == HOKUYO){ hokuyo.m_com_port = "ttyACM0"; hokuyo.turnOn(); // } cout << "[init Laser end....]" << endl; }
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(); }