void TestGuiWindowsEvents()
{
// Create some windows:
CDisplayWindow win2D("Bitmap window",300,300);
{
mrpt::utils::CImage img(300,300, 3);
img.filledRectangle(0,0,300,300,TColor(0,0,255));
img.textOut(50,50,"Hello world!", TColor(255,255,255));
win2D.showImage(img);
}
CDisplayWindow3D win3D("3D window",300,300);
{
mrpt::opengl::COpenGLScenePtr &scene = win3D.get3DSceneAndLock();
scene->insert( mrpt::opengl::CGridPlaneXY::Create() );
win3D.unlockAccess3DScene();
win3D.repaint();
}
CDisplayWindowPlots winPlot("Plots window",300,300);
// Tile windows nicely:
win2D.setPos(10,10);
win3D.setPos(340,10);
winPlot.setPos(10,340);
observer.observeBegin(win2D);
observer.observeBegin(win3D);
observer.observeBegin(winPlot);
// Wait until end:
cout << "Waiting for window events...\n";
cout << "** Close any window to end the program **\n";
while (win2D.isOpen() && win3D.isOpen() && winPlot.isOpen())
{
mrpt::system::sleep(100);
}
}
// ------------------------------------------------------
// TestObserverPattern
// ------------------------------------------------------
void TestObserverPattern()
{
CSimplePointsMapPtr map1 = CSimplePointsMap::Create();
CObservation2DRangeScan obs = getScan();
// Start observer:
MyObserver observer;
observer.observeBegin(*map1);
// Insert:
for (int i=0;i<3;i++)
map1->insertObservation(&obs);
// Clear:
map1->clear();
// Insert again:
map1->insertObservation(&obs);
// Destroy
map1.clear();
}
int main(int num_arg, char *argv[])
{
try
{
// Read function arguments
//----------------------------------------------------------------------
bool use_config_file = 0, enable_logfile = 0;
string filename;
if (num_arg < 2);
else if ( string(argv[1]) == "--help")
{
printf("\n\t Arguments of the function 'main' \n");
printf("==============================================================\n\n");
printf(" --help: Shows this menu... \n\n");
printf(" --config FICH.txt: Load FICH.txt as config file \n\n");
printf(" --create-config FICH.txt: Save the default config parameters \n\n");
printf(" \t\t\t in FICH.txt and close the program \n\n");
printf(" --save-logfile: Enable saving a log file with navigation data \n\n");
system::os::getch();
return 1;
}
else if ( string(argv[1]) == "--create-config")
{
filename = argv[2];
cout << endl << "Nombre del archivo: " << filename;
ofstream new_file(filename.c_str());
new_file << string(default_cfg_txt);
new_file.close();
cout << endl << "File saved" << endl;
system::os::getch();
return 1;
}
else
{
for (int i=1; i<num_arg; i++)
{
if ( string(argv[i]) == "--save-logfile")
enable_logfile = 1;
if ( string(argv[i]) == "--config")
{
use_config_file = 1;
filename = argv[i+1];
}
}
}
//Initial steps. Load configuration from file or default
//------------------------------------------------------
CMyReactInterface ReactInterface;
CReactiveNavigationSystem3D rn3d (ReactInterface, true, enable_logfile);
if (use_config_file == 0)
{
utils::CConfigFileMemory configNavigation(default_cfg_txt);
rn3d.loadConfigFile( configNavigation );
ReactInterface.loadMaps( configNavigation );
ReactInterface.loadConfiguration( configNavigation );
}
else
{
CConfigFile configNavigation(filename);
rn3d.loadConfigFile( configNavigation );
ReactInterface.loadMaps( configNavigation );
ReactInterface.loadConfiguration( configNavigation );
}
ReactInterface.initializeScene();
rn3d.initialize();
bool stop = 0;
bool moving_target = 0;
int pushed_key = 0;
TPoint3D last_Target_Pos(0,0,0);
CTicTac reactive_period;
reactive_period.Tic();
MyObserver observer;
observer.observeBegin(ReactInterface.window);
observer.mouse_click = 0;
while (!stop)
{
if (ReactInterface.window.keyHit())
pushed_key = ReactInterface.window.getPushedKey();
else
pushed_key = 0;
switch (pushed_key) {
case 'p':
//Pause navigation
rn3d.suspend();
break;
case 'r':
//Resume navigation
rn3d.resume();
break;
case 'm':
//Move the target
moving_target = 1;
break;
case 'e':
//Exit program
stop = 1;
break;
}
//Set the target when the user clicks the mouse
if (observer.mouse_click == 1)
{
observer.mouse_click = 0;
if (moving_target == 1)
{
moving_target = 0;
const CAbstractReactiveNavigationSystem::TNavigationParams nav_params = ReactInterface.createNewTarget(last_Target_Pos.x, last_Target_Pos.y, 0.3, 0);
rn3d.navigate(&nav_params);
}
}
//Execute navigation
rn3d.navigationStep();
ReactInterface.robotSim.simulateInterval( reactive_period.Tac() );
reactive_period.Tic();
if ((rn3d.IDLE == rn3d.getCurrentState())||(rn3d.SUSPENDED == rn3d.getCurrentState()))
{
CSimplePointsMap auxpoints;
ReactInterface.senseObstacles( auxpoints );
}
ReactInterface.updateScene();
system::sleep(5);
//Move target with the mouse
if (moving_target == 1)
{
int mouse_x,mouse_y;
if (ReactInterface.window.getLastMousePosition(mouse_x,mouse_y))
{
// Get the ray in 3D for the latest mouse (X,Y):
math::TLine3D ray;
ReactInterface.scene->getViewport("main")->get3DRayForPixelCoord(mouse_x,mouse_y,ray);
// Create a 3D plane, e.g. Z=0
const math::TPlane ground_plane(TPoint3D(0,0,0),TPoint3D(1,0,0),TPoint3D(0,1,0));
// Intersection of the line with the plane:
math::TObject3D inters;
math::intersect(ray,ground_plane, inters);
// Interpret the intersection as a point, if there is an intersection:
if (inters.getPoint(last_Target_Pos))
{
// Move an object to the position picked by the user:
ReactInterface.scene->getByClass<CDisk>(0)->setLocation(last_Target_Pos.x, last_Target_Pos.y, last_Target_Pos.z);
}
}
}
}
return 0;
}
catch (std::exception &e)
{
std::cout << "MRPT exception caught: " << e.what() << std::endl;
return -1;
}
catch (...)
{
printf("Untyped exception!!");
return -1;
}
}