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