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();
}
