00001 #include "SIGEL_SlaveGUI/SIG_SimulationVisualisationWidget.h"
00002
00003 #include "SIGEL_Visualisation/SIG_SimulationVisualisation.h"
00004 #include "SIGEL_SlaveGUI/SIG_MovieSettingsDialog.h"
00005 #include "SIGEL_Tools/SIG_Exception.h"
00006
00007 #include <qmessagebox.h>
00008 #include <qspinbox.h>
00009 #include <qlineedit.h>
00010 #include <qpixmap.h>
00011 #include <qcombobox.h>
00012 #include <qcheckbox.h>
00013 #include <qcolordialog.h>
00014
00015
00016
00017
00018 SIG_SimulationVisualisationWidget::SIG_SimulationVisualisationWidget( QWidget *parent=0,
00019 char const *name=0,
00020 WFlags f=0 )
00021 : SIGEL_CommonGUI::SIG_VisualisationWidget( parent, name, f ),
00022 frameDelay(0),
00023 noOfFFSteps(0),
00024 traceRobot(true),
00025 robot(0),
00026 environment(0),
00027 simulationParameters(0),
00028 program(0),
00029 movieWidth(320),
00030 movieHeight(200),
00031 movieFrequency(1),
00032 movieMaxFrames(1000),
00033 movieQuality(50),
00034 currentFrame(0),
00035 currentFrameName(0),
00036 movieFilePrefix( "sigel_pic" ),
00037 fileFormat( "png" ),
00038 record(false),
00039 useLeadingZeros( true ),
00040 planeColor( 127, 127, 127 )
00041 {
00042 simulationTimer = new QTimer(this, "simulationTimer");
00043
00044 connect( simulationTimer,
00045 SIGNAL(timeout()),
00046 SLOT(slotSimulationProgress()) );
00047
00048 char *sigelRoot = std::getenv( "SIGEL_ROOT" );
00049 QString temp( sigelRoot );
00050 if( temp.right(1) != "/" )
00051 temp.append( "/" );
00052 movieDirectory = temp + "movie/";
00053 };
00054
00055 SIG_SimulationVisualisationWidget::~SIG_SimulationVisualisationWidget()
00056 { };
00057
00058 void SIG_SimulationVisualisationWidget::setShowAncorPoints( int state )
00059 {
00060 if (visualisation)
00061 {
00062 SIGEL_Visualisation::SIG_SimulationVisualisation *simulationVisualisation =
00063 static_cast< SIGEL_Visualisation::SIG_SimulationVisualisation* >( visualisation );
00064
00065 if (state > 0)
00066 simulationVisualisation->setPointsVisible( true );
00067 else
00068 simulationVisualisation->setPointsVisible( false );
00069
00070 SIGEL_CommonGUI::SIG_VisualisationWidget::setShowAncorPoints( state );
00071 };
00072 };
00073
00074 void SIG_SimulationVisualisationWidget::choosePlaneColor()
00075 {
00076 if (visualisation)
00077 {
00078 SIGEL_Visualisation::SIG_SimulationVisualisation &simulationVisualisation = static_cast<SIGEL_Visualisation::SIG_SimulationVisualisation&>(*visualisation);
00079
00080 QColor newPlaneColor = QColorDialog::getColor( planeColor, this );
00081
00082 if (newPlaneColor.isValid())
00083 {
00084 planeColor = newPlaneColor;
00085 simulationVisualisation.setPlaneColor( planeColor );
00086
00087 if (automaticRefresh)
00088 updateGL();
00089 };
00090 };
00091 };
00092
00093 void SIG_SimulationVisualisationWidget::setShowPlane( int state )
00094 {
00095 if (visualisation)
00096 {
00097 SIGEL_Visualisation::SIG_SimulationVisualisation &simulationVisualisation =
00098 static_cast< SIGEL_Visualisation::SIG_SimulationVisualisation& >( *visualisation );
00099
00100 switch (state)
00101 {
00102 case 0:
00103 simulationVisualisation.setShowPlane( false );
00104 break;
00105 case 2:
00106 simulationVisualisation.setShowPlane( true );
00107 break;
00108 };
00109
00110 if (automaticRefresh)
00111 updateGL();
00112 };
00113 };
00114
00115 void SIG_SimulationVisualisationWidget::setShowGrid( int state )
00116 {
00117 if (visualisation)
00118 {
00119 SIGEL_Visualisation::SIG_SimulationVisualisation &simulationVisualisation =
00120 static_cast< SIGEL_Visualisation::SIG_SimulationVisualisation& >( *visualisation );
00121
00122 switch (state)
00123 {
00124 case 0:
00125 simulationVisualisation.setShowGrid( false );
00126 break;
00127 case 2:
00128 simulationVisualisation.setShowGrid( true );
00129 break;
00130 };
00131
00132 if (automaticRefresh)
00133 updateGL();
00134 };
00135 };
00136
00137 void SIG_SimulationVisualisationWidget::setShowRobotPath( int state )
00138 {
00139 if (visualisation)
00140 {
00141 SIGEL_Visualisation::SIG_SimulationVisualisation &simulationVisualisation =
00142 static_cast< SIGEL_Visualisation::SIG_SimulationVisualisation& >( *visualisation );
00143
00144 switch (state)
00145 {
00146 case 0:
00147 simulationVisualisation.setShowRobotPath( false );
00148 break;
00149 case 2:
00150 simulationVisualisation.setShowRobotPath( true );
00151 break;
00152 };
00153
00154 if (automaticRefresh)
00155 updateGL();
00156 };
00157 };
00158
00159 void SIG_SimulationVisualisationWidget::mousePressEvent( QMouseEvent *event )
00160 {
00161 SIGEL_CommonGUI::SIG_VisualisationWidget::mousePressEvent( event );
00162 };
00163
00164 void SIG_SimulationVisualisationWidget::mouseMoveEvent( QMouseEvent *event )
00165 {
00166 SIGEL_CommonGUI::SIG_VisualisationWidget::mouseMoveEvent( event );
00167 };
00168
00169 void SIG_SimulationVisualisationWidget::makeTimeSteps(int noOfSteps)
00170 {
00171 if (visualisation)
00172 {
00173 SIGEL_Visualisation::SIG_SimulationVisualisation &simulationVisualisation = static_cast<SIGEL_Visualisation::SIG_SimulationVisualisation&>(*visualisation);
00174
00175 try
00176 {
00177 simulationVisualisation.makeTimeSteps( noOfSteps );
00178 }
00179 catch (SIGEL_Tools::SIG_Exception &e)
00180 {
00181 if (simulationRunning())
00182 {
00183 slotStartSimulation();
00184 emit signalSimulationAbort();
00185 };
00186
00187 QMessageBox::warning( this, "Simulation Exception!", e.getMessage() );
00188 };
00189
00190 emit signalSimulationProgress( simulationVisualisation.getSimulationTime() );
00191
00192 if ( traceRobot )
00193 {
00194 DL_vector robotsRealOrigin = robot->initialLocation;
00195 robotsRealOrigin.timesis( -1 );
00196
00197 simulationVisualisation.getRobotRotation().times( &robotsRealOrigin,
00198 &simulationVisualisation.viewSettings.lookPoint );
00199
00200 DL_vector robotPosition = simulationVisualisation.getRobotPosition();
00201
00202 simulationVisualisation.viewSettings.lookPoint.plusis( &robotPosition );
00203 };
00204
00205 if ( record )
00206 {
00207
00208 if( ((currentFrame % movieFrequency) == 0) && currentFrameName < movieMaxFrames )
00209 {
00210
00211 QString fileName = QString::number( currentFrameName );
00212
00213
00214 if( useLeadingZeros )
00215 {
00216
00217
00218 int currentLength = fileName.length();
00219
00220 int maxLength = QString::number( movieMaxFrames ).length();
00221
00222 int difference = maxLength - currentLength;
00223
00224
00225 for( int i=1; i<=difference; i++ )
00226 fileName.prepend( "0" );
00227 }
00228
00229 fileName.prepend( movieDirectory + movieFilePrefix );
00230
00231 fileName.append( "." + fileFormat );
00232 #ifdef SIG_DEBUG
00233 SIGEL_Tools::SIG_IO::cerr << "Writing " << fileName << " in format " << movieWidth << " x " << movieHeight << " in quality " << movieQuality << ".\n";
00234 #endif
00235
00236 QDir movieDir( movieDirectory );
00237 if(!movieDir.exists() )
00238 movieDir.mkdir( movieDirectory );
00239
00240 bool renderSuccess;
00241
00242 if ( fileFormat == "pov" )
00243 {
00244 QString includeFileName = movieFilePrefix + ".inc";
00245
00246 renderSuccess = simulationVisualisation.exportToPovray( includeFileName,
00247 fileName );
00248 }
00249 else
00250 renderSuccess = renderPixmap( movieWidth, movieHeight, true ).save( fileName, fileFormat.upper(), movieQuality );
00251
00252 if ( !renderSuccess )
00253 {
00254
00255
00256 record = false;
00257 QMessageBox::warning( this, "File error", "Unable to write file " + fileName + ".\nPerhaps you don't have permission to write the file.");
00258 };
00259 currentFrameName++;
00260 }
00261 currentFrame++;
00262 }
00263 };
00264 };
00265
00266 void SIG_SimulationVisualisationWidget::visualizeThis(SIGEL_Robot::SIG_Robot const &rrobot,
00267 SIGEL_Environment::SIG_Environment const &eenvironment,
00268 SIGEL_Simulation::SIG_SimulationParameters const &ssimulationParameters,
00269 SIGEL_Program::SIG_Program const &pprogram)
00270 {
00271 robot = &rrobot;
00272 environment = &eenvironment;
00273 simulationParameters = &ssimulationParameters;
00274 program = &pprogram;
00275
00276 makeCurrent();
00277
00278 delete visualisation;
00279
00280 visualisation = new SIGEL_Visualisation::SIG_SimulationVisualisation( *robot,
00281 *environment,
00282 *simulationParameters,
00283 *program );
00284
00285 SIGEL_Visualisation::SIG_SimulationVisualisation &simulationVisualisation = static_cast<SIGEL_Visualisation::SIG_SimulationVisualisation&>(*visualisation);
00286
00287 simulationVisualisation.setPlaneColor( planeColor );
00288
00289 emit signalSimulationProgress( QTime() );
00290
00291 initFloatingTextWidgets();
00292
00293 noOfFFSteps = static_cast<int>(5 / simulationParameters->getStepSize());
00294
00295 resizeGL( width(), height() );
00296
00297 slotSetTraceRobot( true );
00298 slotNavigateCenter();
00299 updateGL();
00300 };
00301
00302 void SIG_SimulationVisualisationWidget::resetRecorder()
00303 {
00304 record = false;
00305 currentFrame = 0;
00306 currentFrameName = 0;
00307 };
00308
00309 bool SIG_SimulationVisualisationWidget::simulationRunning()
00310 {
00311 return simulationTimer->isActive();
00312 };
00313
00314 void SIG_SimulationVisualisationWidget::slotStartSimulation()
00315 {
00316 if (simulationRunning())
00317 {
00318 automaticRefresh = true;
00319 simulationTimer->stop();
00320 }
00321 else
00322 {
00323 automaticRefresh = false;
00324 simulationTimer->start( frameDelay );
00325 };
00326 };
00327
00328 void SIG_SimulationVisualisationWidget::slotStopSimulation()
00329 {
00330 if (simulationRunning())
00331 {
00332 slotStartSimulation();
00333 };
00334
00335 visualizeThis( *robot,
00336 *environment,
00337 *simulationParameters,
00338 *program );
00339 };
00340
00341 void SIG_SimulationVisualisationWidget::slotStepSimulation()
00342 {
00343 if (!simulationRunning())
00344 {
00345 makeTimeSteps(1);
00346 updateGL();
00347 };
00348 };
00349
00350 void SIG_SimulationVisualisationWidget::slotFForwardSimulation()
00351 {
00352 if (!simulationRunning())
00353 {
00354 makeTimeSteps( noOfFFSteps );
00355 updateGL();
00356 };
00357 };
00358
00359 void SIG_SimulationVisualisationWidget::slotSetFrameDelay(int fframeDelay)
00360 {
00361 frameDelay = fframeDelay;
00362
00363 if (simulationTimer->isActive())
00364 simulationTimer->changeInterval( frameDelay );
00365 };
00366
00367 void SIG_SimulationVisualisationWidget::slotSimulationProgress()
00368 {
00369 makeTimeSteps(1);
00370 updateGL();
00371 };
00372
00373 void SIG_SimulationVisualisationWidget::slotSetTraceRobot( bool newValue )
00374 {
00375 traceRobot = newValue;
00376
00377 if (traceRobot)
00378 slotNavigateCenter();
00379 };
00380
00381 void SIG_SimulationVisualisationWidget::slotNavigateForward()
00382 {
00383 if (visualisation)
00384 {
00385 double const distance = 1;
00386
00387 double actXPos = visualisation->viewSettings.lookPoint.get( 0 );
00388 double actZPos = visualisation->viewSettings.lookPoint.get( 2 );
00389
00390 double viewDirectionAngle = yaw + 180;
00391 if (viewDirectionAngle > 360)
00392 viewDirectionAngle -= 360;
00393
00394 double radViewDirectionAngle = (viewDirectionAngle / 360) * 2 * pi;
00395
00396 double newXPos = actXPos + ( std::sin( radViewDirectionAngle ) * distance );
00397 double newZPos = actZPos + ( std::cos( radViewDirectionAngle ) * distance );
00398
00399 visualisation->viewSettings.lookPoint.set( 0 , newXPos );
00400 visualisation->viewSettings.lookPoint.set( 2 , newZPos );
00401
00402 if (automaticRefresh)
00403 updateGL();
00404 };
00405 };
00406
00407 void SIG_SimulationVisualisationWidget::slotNavigateBackward()
00408 {
00409 if (visualisation)
00410 {
00411 double const distance = 1;
00412
00413 double actXPos = visualisation->viewSettings.lookPoint.get( 0 );
00414 double actZPos = visualisation->viewSettings.lookPoint.get( 2 );
00415
00416 double radViewDirectionAngle = (yaw / 360) * 2 * pi;
00417
00418 double newXPos = actXPos + ( std::sin( radViewDirectionAngle ) * distance );
00419 double newZPos = actZPos + ( std::cos( radViewDirectionAngle ) * distance );
00420
00421 visualisation->viewSettings.lookPoint.set( 0 , newXPos );
00422 visualisation->viewSettings.lookPoint.set( 2 , newZPos );
00423
00424 if (automaticRefresh)
00425 updateGL();
00426 };
00427 };
00428
00429 void SIG_SimulationVisualisationWidget::slotNavigateRight()
00430 {
00431 if (visualisation)
00432 {
00433 double const distance = 1;
00434
00435 double actXPos = visualisation->viewSettings.lookPoint.get( 0 );
00436 double actZPos = visualisation->viewSettings.lookPoint.get( 2 );
00437
00438 double viewDirectionAngle = yaw + 90;
00439 if (viewDirectionAngle > 360)
00440 viewDirectionAngle -= 360;
00441
00442 double radViewDirectionAngle = (viewDirectionAngle / 360) * 2 * pi;
00443
00444 double newXPos = actXPos + ( std::sin( radViewDirectionAngle ) * distance );
00445 double newZPos = actZPos + ( std::cos( radViewDirectionAngle ) * distance );
00446
00447 visualisation->viewSettings.lookPoint.set( 0 , newXPos );
00448 visualisation->viewSettings.lookPoint.set( 2 , newZPos );
00449
00450 if (automaticRefresh)
00451 updateGL();
00452 };
00453 };
00454
00455 void SIG_SimulationVisualisationWidget::slotNavigateLeft()
00456 {
00457 if (visualisation)
00458 {
00459 double const distance = 1;
00460
00461 double actXPos = visualisation->viewSettings.lookPoint.get( 0 );
00462 double actZPos = visualisation->viewSettings.lookPoint.get( 2 );
00463
00464 double viewDirectionAngle = yaw - 90;
00465 if (viewDirectionAngle < 360)
00466 viewDirectionAngle += 360;
00467
00468 double radViewDirectionAngle = (viewDirectionAngle / 360) * 2 * pi;
00469
00470 double newXPos = actXPos + ( std::sin( radViewDirectionAngle ) * distance );
00471 double newZPos = actZPos + ( std::cos( radViewDirectionAngle ) * distance );
00472
00473 visualisation->viewSettings.lookPoint.set( 0 , newXPos );
00474 visualisation->viewSettings.lookPoint.set( 2 , newZPos );
00475
00476 if (automaticRefresh)
00477 updateGL();
00478 };
00479 };
00480
00481 void SIG_SimulationVisualisationWidget::slotNavigateDown()
00482 {
00483 if (visualisation)
00484 {
00485 double const distance = 1;
00486
00487 double actHeight = visualisation->viewSettings.lookPoint.get( 1 );
00488
00489 visualisation->viewSettings.lookPoint.set( 1, actHeight - distance );
00490
00491 if (automaticRefresh)
00492 updateGL();
00493 };
00494 };
00495
00496 void SIG_SimulationVisualisationWidget::slotNavigateUp()
00497 {
00498 if (visualisation)
00499 {
00500 double const distance = 1;
00501
00502 double actHeight = visualisation->viewSettings.lookPoint.get( 1 );
00503
00504 visualisation->viewSettings.lookPoint.set( 1, actHeight + distance );
00505
00506 if (automaticRefresh)
00507 updateGL();
00508 };
00509 };
00510
00511 void SIG_SimulationVisualisationWidget::slotNavigateCenter()
00512 {
00513 if (visualisation)
00514 {
00515 SIGEL_Visualisation::SIG_SimulationVisualisation *simulationVisualisation =
00516 static_cast< SIGEL_Visualisation::SIG_SimulationVisualisation* >( visualisation );
00517
00518 DL_vector robotsRealOrigin = robot->initialLocation;
00519 robotsRealOrigin.timesis( -1 );
00520
00521 simulationVisualisation->getRobotRotation().times( &robotsRealOrigin,
00522 &simulationVisualisation->viewSettings.lookPoint );
00523
00524 DL_vector robotPosition = simulationVisualisation->getRobotPosition();
00525
00526 simulationVisualisation->viewSettings.lookPoint.plusis( &robotPosition );
00527
00528 if (automaticRefresh)
00529 updateGL();
00530 };
00531 };
00532
00533 void SIG_SimulationVisualisationWidget::slotRecordClicked()
00534 {
00535 record = !record;
00536 };
00537
00538 void SIG_SimulationVisualisationWidget::slotAlterMovieSettingsClicked()
00539 {
00540 SIGEL_SlaveGUI::SIG_MovieSettingsDialog movieSettingsDialog( this, "movieSettingsDialog", true );
00541 movieSettingsDialog.spinboxWidth->setValue( movieWidth );
00542 movieSettingsDialog.spinboxHeight->setValue( movieHeight );
00543 movieSettingsDialog.spinboxFrequency->setValue( movieFrequency );
00544 movieSettingsDialog.lineeditDirectory->setText( movieDirectory );
00545 movieSettingsDialog.lineeditFilePrefix->setText( movieFilePrefix );
00546 movieSettingsDialog.spinboxMaxFrames->setValue( movieMaxFrames );
00547 if( fileFormat == "bmp" )
00548 movieSettingsDialog.comboboxFormat->setCurrentItem(0);
00549 if( fileFormat == "png" )
00550 movieSettingsDialog.comboboxFormat->setCurrentItem(1);
00551 if( fileFormat == "ppm" )
00552 movieSettingsDialog.comboboxFormat->setCurrentItem(2);
00553 if( fileFormat == "xbm" )
00554 movieSettingsDialog.comboboxFormat->setCurrentItem(3);
00555 if( fileFormat == "xpm" )
00556 movieSettingsDialog.comboboxFormat->setCurrentItem(4);
00557 if( fileFormat == "pov" )
00558 movieSettingsDialog.comboboxFormat->setCurrentItem(5);
00559 movieSettingsDialog.spinboxQuality->setValue( movieQuality );
00560 movieSettingsDialog.checkboxUseLeadingZeros->setChecked( useLeadingZeros );
00561 movieSettingsDialog.checkboxEnableMovie->setChecked( record );
00562
00563 switch( movieSettingsDialog.exec() )
00564 {
00565 case QDialog::Accepted:
00566 movieWidth = movieSettingsDialog.spinboxWidth->value();
00567 movieHeight = movieSettingsDialog.spinboxHeight->value();
00568 movieFrequency = movieSettingsDialog.spinboxFrequency->value();
00569 movieDirectory = movieSettingsDialog.lineeditDirectory->text();
00570 if( movieDirectory.right(1) != "/" )
00571 movieDirectory.append( "/" );
00572 movieFilePrefix = movieSettingsDialog.lineeditFilePrefix->text();
00573 fileFormat = movieSettingsDialog.comboboxFormat->currentText().lower();
00574 movieMaxFrames = movieSettingsDialog.spinboxMaxFrames->value();
00575 movieQuality = movieSettingsDialog.spinboxQuality->value();
00576 useLeadingZeros = movieSettingsDialog.checkboxUseLeadingZeros->isChecked();
00577 record = movieSettingsDialog.checkboxEnableMovie->isChecked();
00578
00579 if (record && (fileFormat == "pov") )
00580 {
00581 SIGEL_Visualisation::SIG_SimulationVisualisation &simulationVisualisation =
00582 static_cast< SIGEL_Visualisation::SIG_SimulationVisualisation& >( *visualisation );
00583
00584 QString fileName = movieDirectory + movieFilePrefix + ".inc";
00585
00586 QDir movieDir( movieDirectory );
00587 if(!movieDir.exists() )
00588 movieDir.mkdir( movieDirectory );
00589
00590 double aspectRatio = double( movieWidth ) / double ( movieHeight );
00591
00592 if ( !simulationVisualisation.createPovrayIncludeFile( fileName, aspectRatio ) )
00593 {
00594 record = false;
00595 QMessageBox::warning( this, "File error", "Unable to write file " + fileName + ".\nPerhaps you don't have permission to write the file.");
00596 };
00597 };
00598
00599 emit signalRecordingAllowed( record );
00600 break;
00601 }
00602 };
00603
00604 void SIG_SimulationVisualisationWidget::paintGL()
00605 {
00606 SIGEL_CommonGUI::SIG_VisualisationWidget::paintGL();
00607
00608 if (visualisation)
00609 {
00610 emit signalPosition( visualisation->viewSettings.lookPoint );
00611 };
00612 };
00613
00614