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