Robotsystem from Robot Group Leipzig: simulation.cpp Source File

- Main Page
- Modules
- Namespaces
- Classes
- Files
- Directories
- Examples

- File List
- File Members

# simulation.cpp

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005 by Robot Group Leipzig                             *
    00003  *    martius@informatik.uni-leipzig.de                                    *
    00004  *    fhesse@informatik.uni-leipzig.de                                     *
    00005  *    der@informatik.uni-leipzig.de                                        *
    00006  *    frankguettler@gmx.de                                                 *
    00007  *                                                                         *
    00008  *   This program is free software; you can redistribute it and/or modify  *
    00009  *   it under the terms of the GNU General Public License as published by  *
    00010  *   the Free Software Foundation; either version 2 of the License, or     *
    00011  *   (at your option) any later version.                                   *
    00012  *                                                                         *
    00013  *   This program is distributed in the hope that it will be useful,       *
    00014  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00015  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00016  *   GNU General Public License for more details.                          *
    00017  *                                                                         *
    00018  *   You should have received a copy of the GNU General Public License     *
    00019  *   along with this program; if not, write to the                         *
    00020  *   Free Software Foundation, Inc.,                                       *
    00021  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00022  *                                                                         *
    00023  *   $Log: simulation.cpp,v $
    00024  *   Revision 1.40.4.26  2006/03/29 15:07:29  martius
    00025  *   Dummy Primitive for Environment
    00026  *
    00027  *   Revision 1.40.4.25  2006/03/19 13:38:08  robot3
    00028  *   cameramanipulator for race mode included
    00029  *
    00030  *   Revision 1.40.4.24  2006/03/08 11:21:20  robot3
    00031  *   Following Cameramanipulator is now on key 2
    00032  *
    00033  *   Revision 1.40.4.23  2006/03/06 16:53:49  robot3
    00034  *   now ExtendedViewer is used because of the new getCurrentCameraManipulator(),
    00035  *   code optimizations
    00036  *
    00037  *   Revision 1.40.4.22  2006/03/04 15:04:33  robot3
    00038  *   cameramanipulator is now updated with every draw intervall
    00039  *
    00040  *   Revision 1.40.4.21  2006/03/03 12:11:32  robot3
    00041  *   neccessary changes made for new cameramanipulators
    00042  *
    00043  *   Revision 1.40.4.20  2006/02/22 15:26:23  martius
    00044  *   frame grabbing with osg works again
    00045  *
    00046  *   Revision 1.40.4.19  2006/02/20 10:50:20  martius
    00047  *   pause, random, windowsize, Ctrl-keys
    00048  *
    00049  *   Revision 1.40.4.18  2006/02/14 17:36:03  martius
    00050  *   prevent overflow in time sync
    00051  *
    00052  *   Revision 1.40.4.17  2006/02/14 17:31:12  martius
    00053  *   much better time syncronisation
    00054  *
    00055  *   Revision 1.40.4.16  2006/02/01 14:00:32  martius
    00056  *   remerging of non-fullscreen start
    00057  *
    00058  *   Revision 1.40.4.15  2006/02/01 10:24:34  robot3
    00059  *   new camera manipulator added
    00060  *
    00061  *   Revision 1.40.4.13  2006/01/12 22:32:51  martius
    00062  *   key eventhandler integrated
    00063  *
    00064  *   Revision 1.40.4.12  2006/01/12 15:16:53  martius
    00065  *   transparency
    00066  *
    00067  *   Revision 1.40.4.11  2005/12/29 16:49:48  martius
    00068  *   end is obsolete
    00069  *   tidyUp is used for deletion
    00070  *
    00071  *   Revision 1.40.4.10  2005/12/29 12:54:19  martius
    00072  *   multiple tesselhints
    00073  *
    00074  *   Revision 1.40.4.9  2005/12/15 17:02:04  martius
    00075  *   light is in sky and standart cams removed
    00076  *   config has a default implentation now
    00077  *
    00078  *   Revision 1.40.4.8  2005/12/14 15:36:25  martius
    00079  *   do not check for unused vars
    00080  *
    00081  *   Revision 1.40.4.7  2005/12/13 18:10:33  martius
    00082  *   changelog
    00083  *
    00084  *   Revision 1.40.4.6  2005/12/11 23:35:07  martius
    00085  *   *** empty log message ***
    00086  *
    00087  *   Revision 1.40.4.5  2005/12/09 16:53:16  martius
    00088  *   camera is working now
    00089  *
    00090  *   Revision 1.40.4.4  2005/12/06 17:38:13  martius
    00091  *   *** empty log message ***
    00092  *
    00093  *   Revision 1.40.4.3  2005/12/06 10:13:23  martius
    00094  *   openscenegraph integration started
    00095  *
    00096  *   Revision 1.40.4.2  2005/11/15 12:29:14  martius
    00097  *   new selforg structure and OdeAgent, OdeRobot ...
    00098  *
    00099  *   Revision 1.40.4.1  2005/11/14 17:37:00  martius
    00100  *   changed makefile structure to have and include directory
    00101  *   mode to selforg
    00102  *
    00103  *   Revision 1.40  2005/11/10 09:20:48  martius
    00104  *   10 minute notification
    00105  *
    00106  *   Revision 1.39  2005/10/21 11:59:58  martius
    00107  *   realtimefactor written small
    00108  *
    00109  *   Revision 1.38  2005/10/18 15:31:32  fhesse
    00110  *   one comment improved
    00111  *
    00112  *   Revision 1.37  2005/10/06 17:11:26  martius
    00113  *   switched to stl lists
    00114  *
    00115  *   Revision 1.36  2005/09/27 13:59:03  martius
    00116  *   doInternals after control
    00117  *
    00118  *   Revision 1.35  2005/09/23 09:55:16  martius
    00119  *   odeConfig gets OdeHandle explicit
    00120  *
    00121  *   Revision 1.34  2005/09/22 13:17:11  martius
    00122  *   OdeHandle and GlobalData finished
    00123  *   doInternalStuff included
    00124  *
    00125  *   Revision 1.33  2005/09/22 11:21:57  martius
    00126  *   removed global variables
    00127  *   OdeHandle and GlobalData are used instead
    00128  *   sensor prepared
    00129  *
    00130  *   Revision 1.32  2005/09/20 10:54:34  robot3
    00131  *   camera module:
    00132  *   -pressing key c now centers on focused robot
    00133  *   -pressing key b now moves 5.0f behind the robot
    00134  *   -tiny bugfixing (nullpointer crashes etc.)
    00135  *
    00136  *   Revision 1.31  2005/09/19 16:01:58  martius
    00137  *   use dsSetSimulationTime
    00138  *
    00139  *   Revision 1.30  2005/09/13 15:36:38  martius
    00140  *   disabled advanced modes
    00141  *   new grabframe interface used
    00142  *
    00143  *   Revision 1.29  2005/09/13 13:26:00  martius
    00144  *   random seed adjustable
    00145  *   usage with -h
    00146  *
    00147  *   Revision 1.28  2005/09/11 15:16:41  martius
    00148  *   fast frame capturing enabled
    00149  *
    00150  *   Revision 1.27  2005/09/02 17:18:15  martius
    00151  *   camera modes changed
    00152  *
    00153  *   Revision 1.26  2005/08/25 07:36:16  fhesse
    00154  *   unistd.h included to be able to use usleep
    00155  *
    00156  *   Revision 1.25  2005/08/23 11:39:31  robot1
    00157  *   advancedFollowing mode integrated (for switching)
    00158  *
    00159  *   Revision 1.24  2005/08/22 20:34:22  martius
    00160  *   Display robot name when looking at it.
    00161  *   Init robot view always
    00162  *
    00163  *   Revision 1.23  2005/08/22 12:41:05  robot1
    00164  *   advancedTV mode integrated (for switching)
    00165  *
    00166  *   Revision 1.22  2005/08/16 10:09:07  robot1
    00167  *   tiny bugfixing
    00168  *
    00169  *   Revision 1.21  2005/08/12 12:43:57  robot1
    00170  *   command for switching between agents implemented
    00171  *
    00172  *   Revision 1.20  2005/08/12 11:55:01  robot1
    00173  *   camera module integrated
    00174  *
    00175  *   Revision 1.19  2005/08/03 20:34:39  martius
    00176  *   basic random number initialisation
    00177  *   contains returns index instead of bool
    00178  *
    00179  *   Revision 1.18  2005/07/29 10:22:47  martius
    00180  *   drawInterval honored
    00181  *   real time syncronisation
    00182  *
    00183  *   Revision 1.17  2005/07/27 13:23:16  martius
    00184  *   new color and position construction
    00185  *
    00186  *   Revision 1.16  2005/07/21 12:18:43  fhesse
    00187  *   window size 640x480
    00188  *
    00189  *   Revision 1.15  2005/07/18 08:35:21  martius
    00190  *   drawcallback is additionalcallback now
    00191  *
    00192  *   Revision 1.14  2005/07/15 11:35:52  fhesse
    00193  *   added parameter gravity
    00194  *
    00195  *   Revision 1.13  2005/07/13 08:39:21  robot8
    00196  *   added the possibility to use an additional command function, which handels special Inputs if the ODE simulation window has the focus
    00197  *
    00198  *   Revision 1.12  2005/07/11 11:19:38  robot8
    00199  *   adding the line, where the pointer to the additional draw function is set to the value of the parameter drawCallback
    00200  *
    00201  *   Revision 1.11  2005/07/08 10:14:05  martius
    00202  *   added contains (helper for stringlist search)
    00203  *
    00204  *   Revision 1.10  2005/07/07 10:23:36  martius
    00205  *   added user draw callback
    00206  *
    00207  *   Revision 1.9  2005/06/30 13:23:38  robot8
    00208  *   completing the call of the dynamic collisionCallback-function for  standard collisions
    00209  *
    00210  *   Revision 1.8  2005/06/29 09:27:03  martius
    00211  *   *** empty log message ***
    00212  *
    00213  *   Revision 1.7  2005/06/29 09:25:17  martius
    00214  *   customized callback for collision
    00215  *
    00216  *   Revision 1.6  2005/06/22 15:39:49  fhesse
    00217  *   path to textures
    00218  *
    00219  *   Revision 1.5  2005/06/20 10:03:26  fhesse
    00220  *   collision treatment by agents included
    00221  *
    00222  *   Revision 1.4  2005/06/17 09:33:53  martius
    00223  *   aligned values on showParams
    00224  *
    00225  *   Revision 1.3  2005/06/15 14:01:31  martius
    00226  *   moved all general code from main to simulation
    00227  *                                                                 *
    00228  ***************************************************************************/
    00229 #include <stdlib.h>
    00230 #include <signal.h>
    00231 #include <iostream>
    00232 #include <sys/stat.h>
    00233 #include <sys/time.h>
    00234 #include <unistd.h>
    00235 #include <selforg/configurable.h>
    00236 #include <selforg/abstractcontroller.h>
    00237
    00238 #include "simulation.h"
    00239
    00240 #include <osg/ShapeDrawable>
    00241 #include <osg/ArgumentParser>
    00242 #include <osg/BlendFunc>
    00243 #include <osg/AlphaFunc>
    00244 #include <osgDB/ReaderWriter>
    00245 #include <osgDB/FileUtils>
    00246
    00247 #include "odeagent.h"
    00248 #include "primitive.h"
    00249
    00250 #include "grabframe.h"
    00251
    00252 #include "abstractobstacle.h"
    00253 #include "cameramanipulator.h"
    00254 #include "cameramanipulatorTV.h"
    00255 #include "cameramanipulatorFollow.h"
    00256 #include "cameramanipulatorRace.h"
    00257
    00258 namespace lpzrobots {
    00259
    00260   using namespace std;
    00261   using namespace osg;
    00262   using namespace osgProducer;
    00263
    00264
    00265   int Simulation::ctrl_C = 0;
    00266
    00267   Simulation::Simulation(){
    00268     nextLeakAnnounce = 20;
    00269     leakAnnCounter = 1;
    00270     sim_step = 0;
    00271     state    = none;
    00272     pause    = false;
    00273     viewer   = 0;
    00274     cam      = 0;
    00275     arguments= 0;
    00276   }
    00277
    00278   Simulation::~Simulation(){
    00279     if(state!=running) return;
    00280     dJointGroupDestroy ( odeHandle.jointGroup );
    00281     dWorldDestroy ( odeHandle.world );
    00282     dSpaceDestroy ( odeHandle.space );
    00283     dCloseODE ();
    00284
    00285     state=closed;
    00286      // tweak this, because Simulation is inherited from Referenced
    00287     osgGA::GUIEventHandler::unref_nodelete();
    00288     // tweak this, because Simulation is inherited from Referenced
    00289     Producer::Camera::Callback::unref_nodelete();
    00290   }
    00291
    00292
    00293
    00294   bool Simulation::init(int argc, char** argv){
    00295
    00296     /**************** ODE-Section   ***********************/
    00297     odeHandle.world = dWorldCreate ();
    00298
    00299     // Create the primary world-space, which is used for collision detection
    00300     odeHandle.space = dHashSpaceCreate (0);
    00301     // the jointGroup is used for collision handling,
    00302     //  where a lot of joints are created every step
    00303     odeHandle.jointGroup = dJointGroupCreate ( 1000000 );
    00304
    00305     globalData.odeConfig.setOdeHandle(odeHandle);
    00306
    00307     //set Gravity to Earth level
    00308     dWorldSetGravity ( odeHandle.world , 0 , 0 , globalData.odeConfig.gravity );
    00309     dWorldSetERP ( odeHandle.world , 1 );
    00310
    00311     cmd_handler_init();
    00312
    00313     globalData.environment = new DummyPrimitive();
    00314
    00315     // add ode config to config list
    00316     globalData.configs.push_back(&(globalData.odeConfig));
    00317
    00318     /**************** OpenSceneGraph-Section   ***********************/
    00319
    00320     osgDB::FilePathList l = osgDB::getDataFilePathList();
    00321     l.push_back("../../osg/data");
    00322     osgDB::setDataFilePathList(l);
    00323
    00324     processCmdLine(argc, argv);
    00325     // use an ArgumentParser object to manage the program arguments.
    00326     arguments = new ArgumentParser(&argc, argv);
    00327
    00328     // set up the usage document, in case we need to print out how to use this program.
    00329     arguments->getApplicationUsage()->setDescription(
    00330                   arguments->getApplicationName() + " Lpzrobots Simulator");
    00331     arguments->getApplicationUsage()->setCommandLineUsage(arguments->getApplicationName());
    00332     arguments->getApplicationUsage()->addCommandLineOption(
    00333                  "-h or --help", "Display this information");
    00334
    00335     // construct the viewer.
    00336     viewer = new ExtendedViewer(*arguments);
    00337
    00338     // set up the value with sensible default event handlers.
    00339     unsigned int options =  Viewer::SKY_LIGHT_SOURCE |
    00340       Viewer::STATE_MANIPULATOR |
    00341       Viewer::STATS_MANIPULATOR |
    00342       Viewer::VIEWER_MANIPULATOR |
    00343       Viewer::ESCAPE_SETS_DONE;
    00344     viewer->setUpViewer(options);
    00345
    00346     viewer->getEventHandlerList().push_front(this);
    00347
    00348     // if user request help write it out to cout.
    00349     if (arguments->read("-h") || arguments->read("--help")) {
    00350       arguments->getApplicationUsage()->write(std::cout);
    00351       return false;
    00352     }
    00353     // any option left unread are converted into errors to write out later.
    00354     //    arguments->reportRemainingOptionsAsUnrecognized();
    00355
    00356     // report any errors if they have occured when parsing the program aguments.
    00357     if (arguments->errors()) {
    00358       arguments->writeErrorMessages(std::cout);
    00359       return false;
    00360     }
    00361
    00362     osgHandle.tesselhints[0] = new TessellationHints();
    00363     osgHandle.tesselhints[1] = new TessellationHints();
    00364     osgHandle.tesselhints[2] = new TessellationHints();
    00365     osgHandle.tesselhints[0]->setDetailRatio(0.1f); // Low
    00366     osgHandle.tesselhints[1]->setDetailRatio(1.0f); // Middle
    00367     osgHandle.tesselhints[2]->setDetailRatio(3.0f); // High
    00368
    00369     osgHandle.color = Color(1,1,1,1);
    00370
    00371     osgHandle.scene=makeScene();
    00372     if (!osgHandle.scene) return false;
    00373
    00374     osgHandle.normalState = new StateSet();
    00375
    00376     // set up blending for transparent stateset
    00377     osg::StateSet* stateset = new StateSet();
    00378     osg::BlendFunc* transBlend = new osg::BlendFunc;
    00379     transBlend->setFunction(osg::BlendFunc::SRC_ALPHA, osg::BlendFunc::ONE_MINUS_SRC_ALPHA);
    00380     stateset->setAttributeAndModes(transBlend, osg::StateAttribute::ON);
    00381     stateset->setRenderingHint(osg::StateSet::TRANSPARENT_BIN);
    00382     //stateset->setRenderBinDetails(5,"RenderBin");
    00383     stateset->setMode(GL_CULL_FACE,osg::StateAttribute::ON); // disable backface because of problems
    00384     osgHandle.transparentState = stateset;
    00385
    00386     // setup the camera manipulators
    00387     CameraManipulator* defaultCameramanipulator =
    00388       new CameraManipulator(osgHandle.scene, globalData);
    00389     CameraManipulator* cameramanipulatorFollow =
    00390       new CameraManipulatorFollow(osgHandle.scene, globalData);
    00391     CameraManipulator* cameramanipulatorTV =
    00392       new CameraManipulatorTV(osgHandle.scene, globalData);
    00393     CameraManipulator* cameramanipulatorRace =
    00394       new CameraManipulatorRace(osgHandle.scene, globalData);
    00395     unsigned int pos = viewer->addCameraManipulator(defaultCameramanipulator);
    00396     viewer->addCameraManipulator(cameramanipulatorFollow);
    00397     viewer->addCameraManipulator(cameramanipulatorTV);
    00398     viewer->addCameraManipulator(cameramanipulatorRace);
    00399     viewer->selectCameraManipulator(pos); // this is the default camera type
    00400
    00401     // get details on keyboard and mouse bindings used by the viewer.
    00402     viewer->getUsage(*(arguments->getApplicationUsage()));
    00403
    00404     state=initialised;
    00405     return true;
    00406   }
    00407
    00408
    00409   bool Simulation::run(int argc, char** argv){
    00410     if(!init(argc, argv)) return false;
    00411
    00412     // information on terminal, can be removed if the printout is undesired
    00413     printf ( "\nWelcome to the virtual ODE - robot simulator of the Robot Group Leipzig\n" );
    00414     printf ( "------------------------------------------------------------------------\n" );
    00415     printf ( "Press Ctrl-C for an basic commandline interface (on the console).\n\n" );
    00416     printf ( "Press h      for help.\n\n" );
    00417
    00418     //********************Simulation start*****************
    00419     state=running;
    00420     globalData.time=0;
    00421     resetSyncTimer();
    00422
    00423     start(odeHandle, osgHandle, globalData);
    00424
    00425     // add model to viewer.
    00426     viewer->setSceneData(root);
    00427
    00428     Producer::CameraConfig* cfg = viewer->getCameraConfig();
    00429     cam = cfg->getCamera(0);
    00430
    00431     Producer::RenderSurface* rs = cam->getRenderSurface();
    00432     rs->setWindowName( "LpzRobots - Selforg" );
    00433
    00434     // the following starts the system in windowed mode
    00435     int x = rs->getWindowOriginX();
    00436     int y = rs->getWindowOriginY();
    00437     rs->setWindowRectangle(x,y,windowWidth, windowHeight);
    00438     rs->fullScreen(false);
    00439
    00440     cam->addPostDrawCallback(this);
    00441
    00442     // create the windows and run the threads.
    00443     viewer->realize();
    00444
    00445     while (!viewer->done())
    00446       {
    00447         // wait for all cull and draw threads to complete.
    00448         viewer->sync();
    00449
    00450         loop();
    00451
    00452         // update the scene by traversing it with the the update visitor which will
    00453         // call all node update callbacks and animations.
    00454         viewer->update();
    00455
    00456
    00457         // fire off the cull and draw traversals of the scene.
    00458         viewer->frame();
    00459       }
    00460
    00461     // wait for all cull and draw threads to complete before exit.
    00462     viewer->sync();
    00463     tidyUp(globalData);
    00464     end(globalData);
    00465     return true;
    00466
    00467   }
    00468
    00469   void Simulation::config(GlobalData& globalData){
    00470     changeParams(globalData.configs);
    00471   }
    00472
    00473   void Simulation::end(GlobalData& globalData){
    00474   }
    00475
    00476   void Simulation::loop(){
    00477     // we run the physical simulation as often as "drawinterval",
    00478     //  the drawing of all object should occur if t==0
    00479     for(int t = 0; t < globalData.odeConfig.drawInterval; t++){
    00480       // Parametereingabe
    00481       if (control_c_pressed()){
    00482         cmd_begin_input();
    00483         config(globalData);
    00484         cmd_end_input();
    00485         resetSyncTimer();
    00486       }
    00487
    00488       // the simulation is just run if pause is not enabled
    00489       if (!pause) {
    00490         globalData.time += globalData.odeConfig.simStepSize;
    00491         sim_step++;
    00492         // print simulation time every 10 min.
    00493         if(sim_step% ( int(1/globalData.odeConfig.simStepSize) * 600) ==0) {
    00494           printf("Simulation time: %li min\n", sim_step/ ( long(1/globalData.odeConfig.simStepSize)*60));
    00495         }
    00496         // for all agents: robots internal stuff and control step if at controlInterval
    00497         for(OdeAgentList::iterator i=globalData.agents.begin(); i != globalData.agents.end(); i++){
    00498           if ( (sim_step % globalData.odeConfig.controlInterval ) == 0 ){
    00499             (*i)->step(globalData.odeConfig.noise);
    00500           }
    00501           (*i)->getRobot()->doInternalStuff(globalData);
    00502         }
    00503
    00504         /**********************Simulationsschritt an sich**********************/
    00505         dSpaceCollide ( odeHandle.space , this , &nearCallback );
    00506         dWorldStep ( odeHandle.world , globalData.odeConfig.simStepSize );
    00507         //ODE-Engine geht einen Schritt weiter
    00508         dJointGroupEmpty (odeHandle.jointGroup);
    00509       }
    00510
    00511       addCallback(globalData, t==0, pause);
    00512
    00513       if(t==0){
    00514         /************************** Update the scene ***********************/
    00515         for(ObstacleList::iterator i=globalData.obstacles.begin(); i != globalData.obstacles.end(); i++){
    00516           (*i)->update();
    00517         }
    00518         for(OdeAgentList::iterator i=globalData.agents.begin(); i != globalData.agents.end(); i++){
    00519           (*i)->getRobot()->update();
    00520         }
    00521         // update the camera
    00522         osgGA::MatrixManipulator* mm =viewer->getCurrentCameraManipulator();
    00523         if(mm){
    00524           CameraManipulator* cameramanipulator = dynamic_cast<CameraManipulator*>(mm);
    00525           if(cameramanipulator)
    00526             cameramanipulator->update();
    00527         }
    00528
    00529 //      // grab frame if in captureing mode
    00530 //      if(videostream.opened && !pause){
    00531 //        grabAndWriteFrame(videostream);
    00532 //      }
    00533       }
    00534
    00535     }
    00536     /************************** Time Syncronisation ***********************/
    00537     // Time syncronisation of real time and simulations time (not if on capture mode, or pause)
    00538     if(globalData.odeConfig.realTimeFactor!=0.0 && !pause){
    00539       long elaped = timeOfDayinMS() - realtimeoffset;
    00540       // difference between actual time and current time in milliseconds
    00541       long diff = long((globalData.time*1000.0 - simtimeoffset)
    00542                        / globalData.odeConfig.realTimeFactor  ) - elaped;
    00543       if(diff > 10000 || diff < -10000)  // check for overflow or other weird things
    00544         resetSyncTimer();
    00545       else{
    00546         if(diff > 4){ // if less the 3 milliseconds we don't call usleep since it needs time
    00547           usleep((diff-2)*1000);
    00548           nextLeakAnnounce=4;
    00549         }else if (diff < 0){
    00550           // we do not bother the user all the time
    00551           if(leakAnnCounter%nextLeakAnnounce==0 && diff < -100 && !videostream.isOpen()){
    00552             printf("Time leak of %li ms (Suggestion: realtimefactor=%g , next in annoucement in %i )\n",
    00553                    -diff, globalData.odeConfig.realTimeFactor*0.5, nextLeakAnnounce);
    00554             nextLeakAnnounce=min(nextLeakAnnounce*2,512);
    00555             leakAnnCounter=0;
    00556             resetSyncTimer();
    00557           }
    00558           leakAnnCounter++;
    00559         }
    00560       }
    00561     }else if (pause) {
    00562       usleep(1000);
    00563     }
    00564   }
    00565
    00566   bool Simulation::handle(const osgGA::GUIEventAdapter& ea,osgGA::GUIActionAdapter&) {
    00567     bool handled = false;
    00568     switch(ea.getEventType()) {
    00569     case(osgGA::GUIEventAdapter::KEYDOWN):
    00570       {
    00571         handled = command(odeHandle, osgHandle, globalData, ea.getKey(), true);
    00572         if(handled) break;
    00573         //      printf("Key: %i\n", ea.getKey());
    00574         switch(ea.getKey()){
    00575         case 18:  // Ctrl - r
    00576           if(videostream.isOpen()){
    00577             printf("Stop video recording!\n");
    00578             videostream.close();
    00579           }else{
    00580             char dir[128];
    00581             char filename[140];
    00582             createNewDir("video", dir);
    00583             printf("Start video recording in %s!\n", dir);
    00584             sprintf(filename, "%s/frame", dir);
    00585             videostream.open(filename);
    00586           }
    00587           break;
    00588         case 16: // Ctrl - p
    00589           pause = !pause;
    00590           printf( pause ? "Pause\n" : "Continue\n" );
    00591           resetSyncTimer();
    00592           handled = true;
    00593           break;
    00594         }
    00595       }
    00596     case(osgGA::GUIEventAdapter::KEYUP):
    00597       {
    00598           handled = command(odeHandle, osgHandle, globalData, ea.getKey(), false);
    00599       }
    00600     default:
    00601       break;
    00602     }
    00603     return handled;
    00604   }
    00605
    00606   void Simulation::getUsage (osg::ApplicationUsage& au) const {
    00607     au.addKeyboardMouseBinding("Simulation: Ctrl-r","Start/Stop video recording");
    00608     au.addKeyboardMouseBinding("Simulation: Ctrl-p","Pause on/off");
    00609     bindingDescription(au);
    00610   }
    00611
    00612   void Simulation::accept(osgGA::GUIEventHandlerVisitor& v) {
    00613     v.visit(*this);
    00614   }
    00615
    00616   ///////////////// Camera::Callback interface
    00617  void Simulation::operator() (const Producer::Camera &c){
    00618     // grab frame if in captureing mode
    00619     if(videostream.isOpen() && !pause){
    00620       if(!videostream.grabAndWriteFrame(c)){
    00621         fprintf(stderr,"Stop video recording because of failture!\n");
    00622         videostream.close();
    00623       }
    00624     }
    00625   }
    00626
    00627   /// clears obstacle and agents lists and delete entries
    00628   void Simulation::tidyUp(GlobalData& global){
    00629     // clear obstacles list
    00630     for(ObstacleList::iterator i=global.obstacles.begin(); i != global.obstacles.end(); i++){
    00631       delete (*i);
    00632     }
    00633     global.obstacles.clear();
    00634
    00635     // clear agents list
    00636     for(OdeAgentList::iterator i=global.agents.begin(); i != global.agents.end(); i++){
    00637       delete (*i)->getRobot();
    00638       delete (*i)->getController();
    00639       delete (*i);
    00640     }
    00641     if(global.environment) delete global.environment;
    00642     global.agents.clear();
    00643   }
    00644
    00645
    00646   void Simulation::processCmdLine(int argc, char** argv){
    00647     if(contains(argv, argc, "-h")) usage(argv[0]);
    00648
    00649     int seedIndex = contains(argv, argc, "-r");
    00650     long seed=0;
    00651     // initialize random number generator
    00652     if(seedIndex && argc > seedIndex) {
    00653       seed=atoi(argv[seedIndex]);
    00654     }else{
    00655
    00656       seed=time(0);
    00657     }
    00658     printf("Use random number seed: %li\n", seed);
    00659     srand(seed);
    00660
    00661     int resolindex = contains(argv, argc, "-x");
    00662     windowWidth = 640;
    00663     windowHeight = 480;
    00664     if(resolindex && argc > resolindex) {
    00665       sscanf(argv[resolindex],"%ix%i", &windowWidth,&windowHeight);
    00666       windowWidth = windowWidth < 64 ? 64 : (windowWidth > 1600 ? 1600 : windowWidth);
    00667       windowHeight = windowHeight < 64 ? 64 : (windowHeight > 1200 ? 1200 : windowHeight);
    00668     }
    00669
    00670     pause = contains(argv, argc, "-pause")!=0;
    00671
    00672   }
    00673
    00674   // Diese Funktion wird immer aufgerufen, wenn es im definierten Space zu einer Kollission kam
    00675   //
    00676   void Simulation::nearCallback(void *data, dGeomID o1, dGeomID o2){
    00677     Simulation* me = (Simulation*) data;
    00678     if (!me) return;
    00679
    00680     bool collision_treated=false;
    00681     // call robots collision treatments
    00682     for(OdeAgentList::iterator i= me->globalData.agents.begin();
    00683         (i != me->globalData.agents.end()) && !collision_treated; i++){
    00684       collision_treated=(*i)->getRobot()->collisionCallback(data, o1, o2);
    00685     }
    00686
    00687     if (collision_treated) return; // exit if collision was treated by a robot
    00688
    00689     if(!(me->collCallback(me->odeHandle, data,o1,o2))){
    00690       // using standard collision treatment
    00691
    00692       int i,n;
    00693       const int N = 40;
    00694       dContact contact[N];
    00695       n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
    00696       if (n > 0) {
    00697         for (i=0; i<n; i++)
    00698           {
    00699             contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
    00700               dContactSoftERP | dContactSoftCFM | dContactApprox1;
    00701             contact[i].surface.mu = 0.8; //normale Reibung von Reifen auf Asphalt
    00702             contact[i].surface.slip1 = 0.005;
    00703             contact[i].surface.slip2 = 0.005;
    00704             contact[i].surface.soft_erp = 1;
    00705             contact[i].surface.soft_cfm = 0.00001;
    00706             dJointID c = dJointCreateContact (me->odeHandle.world,
    00707                                               me->odeHandle.jointGroup,&contact[i]);
    00708             dJointAttach ( c , dGeomGetBody(contact[i].geom.g1) , dGeomGetBody(contact[i].geom.g2)) ;
    00709           }
    00710       }
    00711     }
    00712   }
    00713
    00714
    00715
    00716
    00717   /// internals
    00718
    00719   void Simulation::control_c(int i){
    00720     ctrl_C++ ;
    00721     // if (Control_C > 100)exit(0);
    00722   }
    00723
    00724   void Simulation::cmd_handler_exit(void){
    00725     signal(SIGINT,SIG_DFL);
    00726     ctrl_C=0;
    00727   }
    00728
    00729   void Simulation::cmd_handler_init(){
    00730     signal(SIGINT,control_c);
    00731     atexit(cmd_handler_exit);
    00732   }
    00733
    00734   bool Simulation::control_c_pressed(){
    00735     return ctrl_C!=0;
    00736   }
    00737
    00738   void Simulation::cmd_begin_input(){
    00739     cmd_handler_exit();
    00740   }
    00741
    00742   void Simulation::cmd_end_input(){
    00743     cmd_handler_init();
    00744   }
    00745
    00746   long Simulation::timeOfDayinMS(){
    00747     struct timeval t;
    00748     gettimeofday(&t, 0);
    00749     return t.tv_sec*1000 + t.tv_usec/1000;
    00750   }
    00751
    00752   void Simulation::resetSyncTimer(){
    00753     realtimeoffset = timeOfDayinMS();
    00754     simtimeoffset = int(globalData.time*1000);
    00755   }
    00756
    00757
    00758
    00759   // Helper
    00760   int contains(char **list, int len,  const char *str){
    00761     for(int i=0; i<len; i++){
    00762       if(strcmp(list[i],str) == 0) return i+1;
    00763     }
    00764     return 0;
    00765   }
    00766
    00767   void Simulation::usage(const char* progname){
    00768     printf("Parameter: %s [-r SEED] [-x WxH] [-pause]\n", progname);
    00769     printf("\t-r SEED\t\tuse SEED as random number seed\n");
    00770     printf("\t-x WxH\t\twindow size of width(W) x height(H) is used (640x480 default)\n");
    00771     printf("\t-pause \t\tstart in pause mode\n");
    00772   }
    00773
    00774   // Commandline interface stuff
    00775   void showParams(const ConfigList& configs)
    00776   {
    00777     for(vector<Configurable*>::const_iterator i=configs.begin(); i != configs.end(); i++){
    00778       (*i)->print(stdout, 0);
    00779     }
    00780   }
    00781
    00782   void changeParams(ConfigList& configs){
    00783     char buffer[1024];
    00784     std::cout << "Type: Parameter=Value\n";
    00785     fgets( buffer, 1024, stdin);
    00786     if ( strchr(buffer,'?')!=0){
    00787       showParams(configs);
    00788       return;
    00789     }
    00790
    00791     char *p = strchr(buffer,'=');
    00792     if (p){
    00793       *p=0; // terminate key string
    00794       double v=strtod(p+1,0);
    00795       for(ConfigList::iterator i=configs.begin(); i != configs.end(); i++){
    00796         if ((*i)->setParam(buffer,v))
    00797           printf(" %s=\t%f \n", buffer, (*i)->getParam(buffer));
    00798       }
    00799     }
    00800   }
    00801
    00802   void createNewDir(const char* base, char *newdir){
    00803     struct stat s;
    00804     for(int i=0; i<1000; i++){
    00805       sprintf(newdir,"%s%03i", base, i);
    00806       if(stat(newdir,&s)!=0){ // file/dir does not exist -> take it
    00807         mkdir(newdir, S_IREAD | S_IWRITE | S_IEXEC | S_IRGRP | S_IXGRP );
    00808         return;
    00809       }
    00810     }
    00811     assert(1); // should not happen
    00812   }
    00813
    00814   void Simulation::setCameraHomePos(const osg::Vec3& eye, const osg::Vec3& view){
    00815     osgGA::MatrixManipulator* mm =viewer->getCurrentCameraManipulator();
    00816     if(mm){
    00817       CameraManipulator* cameramanipulator = dynamic_cast<CameraManipulator*>(mm);
    00818       if(cameramanipulator)
    00819         cameramanipulator->setHome(eye, view);
    00820     }
    00821   }
    00822
    00823
    00824 }
    00825