|
MRXT: The Multi-Robot eXploration Tool
Multi-Robot autonomous exploration and mapping simulator.
|
|
MRXT: The Multi-Robot eXploration Tool
Multi-Robot autonomous exploration and mapping simulator.
|
00001 00002 #include <stdio.h> 00003 #include "mainwindow.h" 00004 #include "team.h" 00005 #include "simulatedModel.h" 00006 #include "slamInterface.h" 00007 #include "ui_RobotSettingsDialog.h" 00008 #include "ui_scenePropertiesDialog.h" 00009 #include "ui_slamOptionsDialog.h" 00010 #include "ui_strategyOptionsDialog.h" 00011 #include "ui_About.h" 00012 #include "ui_appConfigDialog.h" 00013 #include "StringTokenizer.h" 00014 #include <QFileDialog> 00015 #include <QDir> 00016 #include <QTime> 00017 #include <QGraphicsItemGroup> 00018 #include <QMessageBox> 00019 00020 void waitEnding::run(){ 00021 t->waitForTaskFinished(); 00022 emit simulationFinished(true); 00023 } 00024 00025 // 00026 // Constructor 00027 // 00028 MainWindow::MainWindow(QWidget *parent) : 00029 QMainWindow(parent), 00030 numrobots(1), 00031 expConf(0), 00032 scenarioConf(0), 00033 scene(0), 00034 myteam(0), 00035 slam(0), 00036 mapLoaded(false), 00037 strategiesGroup(0), 00038 view(&qscene), 00039 scale(20), 00040 scaleInitialized(false), 00041 waitTh(0) 00042 { 00043 00044 setupUi(this); 00045 show(); 00046 00047 // cargar config files 00048 appConf = new ConfigFile( ( QDir::homePath()+="/.mrxt/config/app.config").toStdString().c_str() ); 00049 slamConf = new ConfigFile( ( QDir::homePath()+="/.mrxt/config/slam.config").toStdString().c_str() ); 00050 00051 qRegisterMetaType<rlines>("rlines"); 00052 qRegisterMetaType<rposes>("rposes"); 00053 00054 connect( actionOpen_Scenario, SIGNAL(triggered(bool)), this, SLOT(openScenario(bool))); 00055 connect( actionSaveOmap, SIGNAL(triggered(bool)), this, SLOT(saveOmap(bool))); 00056 connect( actionSaveVmap, SIGNAL(triggered(bool)), this, SLOT(saveVmap(bool))); 00057 connect( actionSaveSLAMlog, SIGNAL(triggered(bool)), this, SLOT(saveSLAMlog(bool))); 00058 connect( actionSaveGTlog, SIGNAL(triggered(bool)), this, SLOT(saveGTlog(bool))); 00059 connect( actionQuit, SIGNAL(triggered(bool)), this, SLOT(close())); 00060 connect( spinBox, SIGNAL(valueChanged(int)), this, SLOT(changeNumRobots(int))); 00061 connect( randomPosesButton, SIGNAL(clicked(bool)), this, SLOT(randomPoses(bool))); 00062 connect( actionPlay, SIGNAL(triggered(bool)), this, SLOT(startSimulation(bool))); 00063 connect( actionStop, SIGNAL(triggered(bool)), this, SLOT(stopSimulation(bool))); 00064 connect( actionSceneProperties, SIGNAL(triggered(bool)), this, SLOT(openScenePropDiag(bool))); 00065 connect( actionRobotSettings, SIGNAL(triggered(bool)), this, SLOT(openRobotSetDiag(bool))); 00066 connect( actionStrategy_options, SIGNAL(triggered(bool)), this, SLOT(openStrategyOptDiag(bool))); 00067 connect( actionSLAM_options, SIGNAL(triggered(bool)), this, SLOT(openSLAMOptSetDiag(bool))); 00068 connect( actionAbout, SIGNAL(triggered(bool)), this, SLOT(openAboutDiag(bool))); 00069 connect( actionConfiguration, SIGNAL(triggered(bool)), this, SLOT(openAppConfigDiag(bool))); 00070 connect( zoomSlider, SIGNAL(valueChanged (int)), this, SLOT(sceneZoom(int))); 00071 00072 QActionGroup* strategiesGroup = new QActionGroup(this); 00073 strategiesGroup->addAction(actionNearest_Frontier); 00074 strategiesGroup->addAction(actionCost_Utility); 00075 strategiesGroup->addAction(actionMarket_Based); 00076 strategiesGroup->addAction(actionBehaviour_Based); 00077 strategiesGroup->addAction(actionHybrid); 00078 strategiesGroup->addAction(actionCoordinated); 00079 strategiesGroup->addAction(actionIntegrated); 00080 strategiesGroup->setExclusive(true); 00081 actionNearest_Frontier->setChecked(true); 00082 changeStrategy(); 00083 00084 QActionGroup* slamTechniquesGroup = new QActionGroup(this); 00085 slamTechniquesGroup->addAction(actionEKF); 00086 slamTechniquesGroup->addAction(actionRBPF); 00087 slamTechniquesGroup->setExclusive(true); 00088 if (slamConf->read<int>("SLAM")==1) actionEKF->setChecked(true); 00089 if (slamConf->read<int>("SLAM")==0) actionRBPF->setChecked(true); 00090 actionSaveVmap->setEnabled(false); 00091 actionSaveOmap->setEnabled(false); 00092 actionSaveSLAMlog->setEnabled(false); 00093 actionSaveGTlog->setEnabled(false); 00094 actionStop->setEnabled(false); 00095 00096 connect( actionNearest_Frontier, SIGNAL(triggered(bool)), this, SLOT(changeStrategy(bool))); 00097 connect( actionCost_Utility, SIGNAL(triggered(bool)), this, SLOT(changeStrategy(bool))); 00098 connect( actionMarket_Based, SIGNAL(triggered(bool)), this, SLOT(changeStrategy(bool))); 00099 connect( actionBehaviour_Based, SIGNAL(triggered(bool)), this, SLOT(changeStrategy(bool))); 00100 connect( actionHybrid, SIGNAL(triggered(bool)), this, SLOT(changeStrategy(bool))); 00101 connect( actionCoordinated, SIGNAL(triggered(bool)), this, SLOT(changeStrategy(bool))); 00102 connect( actionIntegrated, SIGNAL(triggered(bool)), this, SLOT(changeStrategy(bool))); 00103 connect( actionEKF, SIGNAL(triggered(bool)), this, SLOT(changeSlam(bool))); 00104 connect( actionRBPF, SIGNAL(triggered(bool)), this, SLOT(changeSlam(bool))); 00105 00106 playButton->setDefaultAction(actionPlay); 00107 stopButton->setDefaultAction(actionStop); 00108 stopButton->setDisabled(true); 00109 00110 actionSceneProperties->setDisabled(true); 00111 actionRobotSettings->setDisabled(true); 00112 00113 } 00114 00115 // 00116 // Destroyer 00117 // 00118 MainWindow::~MainWindow(){ 00119 if (scene) delete scene; 00120 if (expConf) delete expConf; 00121 if (myteam) delete myteam; 00122 if (scenarioConf) delete scenarioConf; 00123 } 00124 00125 00126 /////////////////////////////////////////////////////////////////// DIALOGS /////////////////////////////////////////////////// 00127 00128 // 00129 // Open Scenario Dialog 00130 // 00131 void MainWindow::openScenario(bool checked){ 00132 00133 QString fileName; 00134 fileName = QFileDialog::getOpenFileName(this, tr("Open Map File"), QDir::homePath()+="/.mrxt/maps", tr("map files (*.map)")); 00135 00136 try{ 00137 if (fileName.size() > 0){ 00138 scenarioConf = new ConfigFile(fileName.toStdString().c_str()); 00139 scene = new simulatedModel(numrobots,*scenarioConf, appConf->read<double>("SAMPLE_TIME")); 00140 00141 connect( scene, SIGNAL(changedPositions(rposes)), this, SLOT(updatePoses(rposes))); 00142 00143 // shows the scenario 00144 // testing the drwaing capabilities 00145 scrollArea->setWidget(&view); 00146 drawScene(); 00147 drawRobots(); 00148 00149 actionSceneProperties->setEnabled(true); 00150 actionRobotSettings->setEnabled(true); 00151 mapLoaded = true; 00152 } 00153 } 00154 catch ( ... ){ 00155 //printf("error loading map\n"); 00156 mapLoaded = false; 00157 } 00158 } 00159 00160 00161 void MainWindow::saveOmap(bool checked){ 00162 QString fileName = QFileDialog::getSaveFileName(this, tr("Save Occupancy Grid Map"), QDir::homePath()+="/.mrxt/outfiles", tr("Images (*.jpg)")); 00163 try{ 00164 QFile::copy(QDir::homePath()+="/.mrxt/outfiles/tempLogomap.jpg", fileName); 00165 } 00166 catch( ... ){ 00167 printf("Error saving file"); 00168 } 00169 } 00170 00171 void MainWindow::saveVmap(bool checked){ 00172 QString fileName = QFileDialog::getSaveFileName(this, tr("Save Landmarks Map"),QDir::homePath()+="/.mrxt/outfiles", tr("matlab m-file (*.m)")); 00173 try{ 00174 QFile::copy(QDir::homePath()+="/.mrxt/outfiles/tempLogvmap.m",fileName); 00175 } 00176 catch( ... ){ 00177 printf("Error saving file"); 00178 } 00179 } 00180 00181 void MainWindow::saveSLAMlog(bool checked){ 00182 QString fileName = QFileDialog::getSaveFileName(this, tr("Save SLAM log"), QDir::homePath()+="/.mrxt/outfiles", tr("matlab m-file (*.m)")); 00183 try{ 00184 QFile::copy(QDir::homePath()+="/.mrxt/outfiles/tempLogslam.m",fileName); 00185 } 00186 catch( ... ){ 00187 printf("Error saving file"); 00188 } 00189 } 00190 void MainWindow::saveGTlog(bool checked){ 00191 QString fileName = QFileDialog::getSaveFileName(this, tr("Save Ground Truth log"), QDir::homePath()+="/.mrxt/outfiles", tr("matlab m-file (*.m)")); 00192 try{ 00193 QFile::copy(QDir::homePath()+="/.mrxt/outfiles/tempLogGT.m",fileName); 00194 } 00195 catch( ... ){ 00196 printf("Error saving file"); 00197 } 00198 } 00199 00200 // 00201 // Strategy Options Dialog 00202 // 00203 void MainWindow::openStrategyOptDiag(bool){ 00204 if (expConf) { 00205 Ui::StrategyOptionsDialog stratDiag; 00206 QDialog diag; 00207 stratDiag.setupUi(&diag); 00208 stratDiag.tabWidget->clear(); 00209 00210 if (actionHybrid->isChecked()){ 00211 stratDiag.tabWidget->insertTab(0, stratDiag.HTab,"Hybrid"); 00212 stratDiag.HFSSpinBox->setValue(expConf->read<double>("SIGMAGOFRO")); 00213 stratDiag.HUSSpinBox->setValue(expConf->read<double>("SIGMAGOUZ")); 00214 stratDiag.HOSSpinBox->setValue(expConf->read<double>("SIGMAAVOBS")); 00215 stratDiag.HRSSpinBox->setValue(expConf->read<double>("SIGMAAVROB")); 00216 stratDiag.HPSSpinBox->setValue(expConf->read<double>("SIGMAGOPRE")); 00217 stratDiag.HGSSpinBox->setValue(expConf->read<double>("SIGMAGOGOAL")); 00218 stratDiag.HFASpinBox->setValue(expConf->read<double>("WEIGHTGOFRO")); 00219 stratDiag.HUASpinBox->setValue(expConf->read<double>("WEIGHTGOUZ")); 00220 stratDiag.HOASpinBox->setValue(expConf->read<double>("WEIGHTAVOBS")); 00221 stratDiag.HRASpinBox->setValue(expConf->read<double>("WEIGHTAVROB")); 00222 stratDiag.HPASpinBox->setValue(expConf->read<double>("WEIGHTGOPRE")); 00223 stratDiag.HGASpinBox->setValue(expConf->read<double>("WEIGHTGOGOAL")); 00224 stratDiag.HVMAXSpinBox->setValue(expConf->read<double>("VMAX")); 00225 stratDiag.HWMAXSpinBox->setValue(expConf->read<double>("WMAX")); 00226 stratDiag.HK1SpinBox->setValue(expConf->read<double>("K1")); 00227 stratDiag.HK2SpinBox->setValue(expConf->read<double>("K2")); 00228 stratDiag.HLPWSpinBox->setValue(expConf->read<double>("LOCALPOTENTIALWIDTH")); 00229 stratDiag.HLPHSpinBox->setValue(expConf->read<double>("LOCALPOTENTIALHEIGHT")); 00230 if (expConf->read<bool>("USEESZ")) stratDiag.HUseESZCB->setChecked(true); 00231 else stratDiag.HUseESZCB->setChecked(false); 00232 stratDiag.HESZRadiusSpinBox->setValue(expConf->read<double>("ESZRADIUS")); 00233 stratDiag.HESZDilationSpinBox->setValue(expConf->read<int>("ESZDILATIONRADIUS")); 00234 00235 stratDiag.HTreeRadiusSpinBox->setValue(expConf->read<double>("TREERADIUS")); 00236 stratDiag.HTreeDilationSpinBox->setValue(expConf->read<int>("TREEDILATIONRADIUS")); 00237 stratDiag.HMFLSpinBox->setValue(expConf->read<int>("MIN_FRONTIER_LENGTH")); 00238 stratDiag.HMGLSpinBox->setValue(expConf->read<int>("MIN_GATEWAY_LENGTH")); 00239 stratDiag.HURSpinBox->setValue(expConf->read<double>("UTILITY_RADIUS")); 00240 00241 } 00242 else if (actionBehaviour_Based->isChecked()){ 00243 stratDiag.tabWidget->insertTab(0, stratDiag.BBTab, "BehaviourBased"); 00244 stratDiag.BBFSSpinBox->setValue(expConf->read<double>("SIGMAGOFRO")); 00245 stratDiag.BBUSSpinBox->setValue(expConf->read<double>("SIGMAGOUZ")); 00246 stratDiag.BBOSSpinBox->setValue(expConf->read<double>("SIGMAAVOBS")); 00247 stratDiag.BBRSSpinBox->setValue(expConf->read<double>("SIGMAAVROB")); 00248 stratDiag.BBPSSpinBox->setValue(expConf->read<double>("SIGMAGOPRE")); 00249 stratDiag.BBGSSpinBox->setValue(expConf->read<double>("SIGMAGOGOAL")); 00250 stratDiag.BBFASpinBox->setValue(expConf->read<double>("WEIGHTGOFRO")); 00251 stratDiag.BBUASpinBox->setValue(expConf->read<double>("WEIGHTGOUZ")); 00252 stratDiag.BBOASpinBox->setValue(expConf->read<double>("WEIGHTAVOBS")); 00253 stratDiag.BBRASpinBox->setValue(expConf->read<double>("WEIGHTAVROB")); 00254 stratDiag.BBPASpinBox->setValue(expConf->read<double>("WEIGHTGOPRE")); 00255 stratDiag.BBGASpinBox->setValue(expConf->read<double>("WEIGHTGOGOAL")); 00256 stratDiag.BBVMAXSpinBox->setValue(expConf->read<double>("VMAX")); 00257 stratDiag.BBWMAXSpinBox->setValue(expConf->read<double>("WMAX")); 00258 stratDiag.BBK1SpinBox->setValue(expConf->read<double>("K1")); 00259 stratDiag.BBK2SpinBox->setValue(expConf->read<double>("K2")); 00260 stratDiag.BBLPWSpinBox->setValue(expConf->read<double>("LOCALPOTENTIALWIDTH")); 00261 stratDiag.BBLPHSpinBox->setValue(expConf->read<double>("LOCALPOTENTIALHEIGHT")); 00262 if (expConf->read<bool>("USEESZ")) stratDiag.BBUseESZCB->setChecked(true); 00263 else stratDiag.BBUseESZCB->setChecked(false); 00264 stratDiag.BBESZRadiusSpinBox->setValue(expConf->read<double>("ESZRADIUS")); 00265 stratDiag.BBESZDilationSpinBox->setValue(expConf->read<int>("ESZDILATIONRADIUS")); 00266 00267 if (expConf->read<bool>("INTEGRATE_SLAM")) stratDiag.BBCBIntegrated->setChecked(true); 00268 else stratDiag.BBCBIntegrated->setChecked(false); 00269 if (expConf->read<bool>("ESCAPE_FROM_LOCAL_MINIMA")) stratDiag.BBCBEscapeActivated->setChecked(true); 00270 else stratDiag.BBCBEscapeActivated->setChecked(false); 00271 stratDiag.BBIOSpinBox->setValue(expConf->read<int>("INFLATE_OBSTACLES")); 00272 } 00273 else if (actionNearest_Frontier->isChecked()){ 00274 stratDiag.tabWidget->insertTab(0, stratDiag.NFTab, "NearestFrontier"); 00275 stratDiag.NFOSSpinBox->setValue(expConf->read<double>("SIGMAAVOBS")); 00276 stratDiag.NFGSSpinBox->setValue(expConf->read<double>("SIGMAGOGOAL")); 00277 stratDiag.NFOASpinBox->setValue(expConf->read<double>("WEIGHTAVOBS")); 00278 stratDiag.NFGASpinBox->setValue(expConf->read<double>("WEIGHTGOGOAL")); 00279 stratDiag.NFVMAXSpinBox->setValue(expConf->read<double>("VMAX")); 00280 stratDiag.NFWMAXSpinBox->setValue(expConf->read<double>("WMAX")); 00281 stratDiag.NFK1SpinBox->setValue(expConf->read<double>("K1")); 00282 stratDiag.NFK2SpinBox->setValue(expConf->read<double>("K2")); 00283 stratDiag.NFLPWSpinBox->setValue(expConf->read<double>("LOCALPOTENTIALWIDTH")); 00284 stratDiag.NFLPHSpinBox->setValue(expConf->read<double>("LOCALPOTENTIALHEIGHT")); 00285 00286 stratDiag.NFRPSpinBox->setValue(expConf->read<double>("REPLANNING_PERIOD")); 00287 stratDiag.NFIOSpinBox->setValue(expConf->read<int>("INFLATE_OBSTACLES")); 00288 00289 00290 } 00291 else if (actionCost_Utility->isChecked()){ 00292 stratDiag.tabWidget->insertTab(0, stratDiag.CUTab, "Cost-Utility"); 00293 stratDiag.CUOSSpinBox->setValue(expConf->read<double>("SIGMAAVOBS")); 00294 stratDiag.CUGSSpinBox->setValue(expConf->read<double>("SIGMAGOGOAL")); 00295 stratDiag.CUOASpinBox->setValue(expConf->read<double>("WEIGHTAVOBS")); 00296 stratDiag.CUGASpinBox->setValue(expConf->read<double>("WEIGHTGOGOAL")); 00297 stratDiag.CUVMAXSpinBox->setValue(expConf->read<double>("VMAX")); 00298 stratDiag.CUWMAXSpinBox->setValue(expConf->read<double>("WMAX")); 00299 stratDiag.CUK1SpinBox->setValue(expConf->read<double>("K1")); 00300 stratDiag.CUK2SpinBox->setValue(expConf->read<double>("K2")); 00301 stratDiag.CULPWSpinBox->setValue(expConf->read<double>("LOCALPOTENTIALWIDTH")); 00302 stratDiag.CULPHSpinBox->setValue(expConf->read<double>("LOCALPOTENTIALHEIGHT")); 00303 00304 stratDiag.CURPSpinBox->setValue(expConf->read<double>("REPLANNING_PERIOD")); 00305 stratDiag.CUIOSpinBox->setValue(expConf->read<int>("INFLATE_OBSTACLES")); 00306 stratDiag.CUURSpinBox->setValue(expConf->read<double>("UTILITY_RADIUS")); 00307 stratDiag.CUUWSpinBox->setValue(expConf->read<double>("UTILITY_WEIGHT")); 00308 stratDiag.CUCWSpinBox->setValue(expConf->read<double>("COST_WEIGHT")); 00309 00310 } 00311 else if (actionCoordinated->isChecked()){ 00312 stratDiag.tabWidget->insertTab(0, stratDiag.CTab, "Coordinated"); 00313 stratDiag.COSSpinBox->setValue(expConf->read<double>("SIGMAAVOBS")); 00314 stratDiag.CGSSpinBox->setValue(expConf->read<double>("SIGMAGOGOAL")); 00315 stratDiag.COASpinBox->setValue(expConf->read<double>("WEIGHTAVOBS")); 00316 stratDiag.CGASpinBox->setValue(expConf->read<double>("WEIGHTGOGOAL")); 00317 stratDiag.CVMAXSpinBox->setValue(expConf->read<double>("VMAX")); 00318 stratDiag.CWMAXSpinBox->setValue(expConf->read<double>("WMAX")); 00319 stratDiag.CK1SpinBox->setValue(expConf->read<double>("K1")); 00320 stratDiag.CK2SpinBox->setValue(expConf->read<double>("K2")); 00321 stratDiag.CLPWSpinBox->setValue(expConf->read<double>("LOCALPOTENTIALWIDTH")); 00322 stratDiag.CLPHSpinBox->setValue(expConf->read<double>("LOCALPOTENTIALHEIGHT")); 00323 00324 stratDiag.CRPSpinBox->setValue(expConf->read<double>("REPLANNING_PERIOD")); 00325 stratDiag.CIOSpinBox->setValue(expConf->read<int>("INFLATE_OBSTACLES")); 00326 stratDiag.CIRSpinBox->setValue(expConf->read<double>("INFLUENCE_RADIUS")); 00327 00328 } 00329 else if (actionMarket_Based->isChecked()){ 00330 stratDiag.tabWidget->insertTab(0, stratDiag.MBTab, "Market Based"); 00331 stratDiag.MBOSSpinBox->setValue(expConf->read<double>("SIGMAAVOBS")); 00332 stratDiag.MBGSSpinBox->setValue(expConf->read<double>("SIGMAGOGOAL")); 00333 stratDiag.MBOASpinBox->setValue(expConf->read<double>("WEIGHTAVOBS")); 00334 stratDiag.MBGASpinBox->setValue(expConf->read<double>("WEIGHTGOGOAL")); 00335 stratDiag.MBVMAXSpinBox->setValue(expConf->read<double>("VMAX")); 00336 stratDiag.MBWMAXSpinBox->setValue(expConf->read<double>("WMAX")); 00337 stratDiag.MBK1SpinBox->setValue(expConf->read<double>("K1")); 00338 stratDiag.MBK2SpinBox->setValue(expConf->read<double>("K2")); 00339 stratDiag.MBLPWSpinBox->setValue(expConf->read<double>("LOCALPOTENTIALWIDTH")); 00340 stratDiag.MBLPHSpinBox->setValue(expConf->read<double>("LOCALPOTENTIALHEIGHT")); 00341 00342 stratDiag.MBRPSpinBox->setValue(expConf->read<double>("REPLANNING_PERIOD")); 00343 stratDiag.MBIOSpinBox->setValue(expConf->read<int>("INFLATE_OBSTACLES")); 00344 stratDiag.MBURSpinBox->setValue(expConf->read<double>("UTILITY_RADIUS")); 00345 stratDiag.MBUWSpinBox->setValue(expConf->read<double>("UTILITY_WEIGHT")); 00346 stratDiag.MBCWSpinBox->setValue(expConf->read<double>("COST_WEIGHT")); 00347 00348 00349 } 00350 else if (actionIntegrated->isChecked()){ 00351 stratDiag.tabWidget->insertTab(0, stratDiag.ITab,"Integrated"); 00352 stratDiag.IOSSpinBox->setValue(expConf->read<double>("SIGMAAVOBS")); 00353 stratDiag.IGSSpinBox->setValue(expConf->read<double>("SIGMAGOGOAL")); 00354 stratDiag.IOASpinBox->setValue(expConf->read<double>("WEIGHTAVOBS")); 00355 stratDiag.IGASpinBox->setValue(expConf->read<double>("WEIGHTGOGOAL")); 00356 stratDiag.IVMAXSpinBox->setValue(expConf->read<double>("VMAX")); 00357 stratDiag.IWMAXSpinBox->setValue(expConf->read<double>("WMAX")); 00358 stratDiag.IK1SpinBox->setValue(expConf->read<double>("K1")); 00359 stratDiag.IK2SpinBox->setValue(expConf->read<double>("K2")); 00360 stratDiag.ILPWSpinBox->setValue(expConf->read<double>("LOCALPOTENTIALWIDTH")); 00361 stratDiag.ILPHSpinBox->setValue(expConf->read<double>("LOCALPOTENTIALHEIGHT")); 00362 00363 stratDiag.IRPSpinBox->setValue(expConf->read<double>("REPLANNING_PERIOD")); 00364 stratDiag.IIOSpinBox->setValue(expConf->read<int>("INFLATE_OBSTACLES")); 00365 stratDiag.IURSpinBox->setValue(expConf->read<double>("UTILITY_RADIUS")); 00366 stratDiag.IUWSpinBox->setValue(expConf->read<double>("UTILITY_WEIGHT")); 00367 stratDiag.ICWSpinBox->setValue(expConf->read<double>("COST_WEIGHT")); 00368 stratDiag.ILWSpinBox->setValue(expConf->read<double>("LOCALIZATION_WEIGHT")); 00369 stratDiag.ICRSpinBox->setValue(expConf->read<double>("CAMERA_RANGE")); 00370 00371 } 00372 00373 diag.exec(); 00374 00375 if (diag.result()==QDialog::Accepted){ 00376 00377 if (actionHybrid->isChecked()){ 00378 expConf->add<double>("SIGMAGOFRO", stratDiag.HFSSpinBox->value()); 00379 expConf->add<double>("SIGMAGOUZ", stratDiag.HUSSpinBox->value()); 00380 expConf->add<double>("SIGMAAVOBS", stratDiag.HOSSpinBox->value()); 00381 expConf->add<double>("SIGMAAVROB", stratDiag.HRSSpinBox->value()); 00382 expConf->add<double>("SIGMAGOPRE", stratDiag.HPSSpinBox->value()); 00383 expConf->add<double>("SIGMAGOGOAL", stratDiag.HGSSpinBox->value()); 00384 expConf->add<double>("WEIGHTGOFRO", stratDiag.HFASpinBox->value()); 00385 expConf->add<double>("WEIGHTGOUZ", stratDiag.HUASpinBox->value()); 00386 expConf->add<double>("WEIGHTAVOBS", stratDiag.HOASpinBox->value()); 00387 expConf->add<double>("WEIGHTAVROB", stratDiag.HRASpinBox->value()); 00388 expConf->add<double>("WEIGHTGOPRE", stratDiag.HPASpinBox->value()); 00389 expConf->add<double>("WEIGHTGOGOAL", stratDiag.HGASpinBox->value()); 00390 expConf->add<double>("VMAX", stratDiag.HVMAXSpinBox->value()); 00391 expConf->add<double>("WMAX", stratDiag.HWMAXSpinBox->value()); 00392 expConf->add<double>("K1", stratDiag.HK1SpinBox->value()); 00393 expConf->add<double>("K2", stratDiag.HK2SpinBox->value()); 00394 expConf->add<double>("LOCALPOTENTIALWIDTH", stratDiag.HLPWSpinBox->value()); 00395 expConf->add<double>("LOCALPOTENTIALHEIGHT", stratDiag.HLPHSpinBox->value()); 00396 if (stratDiag.HUseESZCB->isChecked()) expConf->add<bool>("USEESZ",true); 00397 else expConf->add<bool>("USEESZ",false); 00398 expConf->add<double>("ESZRADIUS", stratDiag.HESZRadiusSpinBox->value()); 00399 expConf->add<int>("ESZDILATIONRADIUS", stratDiag.HESZDilationSpinBox->value()); 00400 00401 expConf->add<double>("TREERADIUS", stratDiag.HTreeRadiusSpinBox->value()); 00402 expConf->add<int>("TREEDILATIONRADIUS", stratDiag.HTreeDilationSpinBox->value()); 00403 expConf->add<int>("MIN_FRONTIER_LENGTH", stratDiag.HMFLSpinBox->value()); 00404 expConf->add<int>("MIN_GATEWAY_LENGTH", stratDiag.HMGLSpinBox->value()); 00405 expConf->add<double>("UTILITY_RADIUS", stratDiag.HURSpinBox->value()); 00406 00407 } 00408 else if (actionBehaviour_Based->isChecked()){ 00409 expConf->add<double>("SIGMAGOFRO", stratDiag.BBFSSpinBox->value()); 00410 expConf->add<double>("SIGMAGOUZ", stratDiag.BBUSSpinBox->value()); 00411 expConf->add<double>("SIGMAAVOBS", stratDiag.BBOSSpinBox->value()); 00412 expConf->add<double>("SIGMAAVROB", stratDiag.BBRSSpinBox->value()); 00413 expConf->add<double>("SIGMAGOPRE", stratDiag.BBPSSpinBox->value()); 00414 expConf->add<double>("SIGMAGOGOAL", stratDiag.BBGSSpinBox->value()); 00415 expConf->add<double>("WEIGHTGOFRO", stratDiag.BBFASpinBox->value()); 00416 expConf->add<double>("WEIGHTGOUZ", stratDiag.BBUASpinBox->value()); 00417 expConf->add<double>("WEIGHTAVOBS", stratDiag.BBOASpinBox->value()); 00418 expConf->add<double>("WEIGHTAVROB", stratDiag.BBRASpinBox->value()); 00419 expConf->add<double>("WEIGHTGOPRE", stratDiag.BBPASpinBox->value()); 00420 expConf->add<double>("WEIGHTGOGOAL", stratDiag.BBGASpinBox->value()); 00421 expConf->add<double>("VMAX", stratDiag.BBVMAXSpinBox->value()); 00422 expConf->add<double>("WMAX", stratDiag.BBWMAXSpinBox->value()); 00423 expConf->add<double>("K1", stratDiag.BBK1SpinBox->value()); 00424 expConf->add<double>("K2", stratDiag.BBK2SpinBox->value()); 00425 expConf->add<double>("LOCALPOTENTIALWIDTH", stratDiag.BBLPWSpinBox->value()); 00426 expConf->add<double>("LOCALPOTENTIALHEIGHT", stratDiag.BBLPHSpinBox->value()); 00427 if (stratDiag.BBUseESZCB->isChecked()) expConf->add<bool>("USEESZ",true); 00428 else expConf->add<bool>("USEESZ",false); 00429 expConf->add<double>("ESZRADIUS", stratDiag.BBESZRadiusSpinBox->value()); 00430 expConf->add<int>("ESZDILATIONRADIUS", stratDiag.BBESZDilationSpinBox->value()); 00431 00432 if (stratDiag.BBCBEscapeActivated->isChecked()) expConf->add<bool>("ESCAPE_FROM_LOCAL_MINIMA",true); 00433 else expConf->add<bool>("ESCAPE_FROM_LOCAL_MINIMA",false); 00434 if (stratDiag.BBCBIntegrated->isChecked()) expConf->add<bool>("INTEGRATE_SLAM",true); 00435 else expConf->add<bool>("INTEGRATE_SLAM",false); 00436 expConf->add<int>("INFLATE_OBSTACLES", stratDiag.BBIOSpinBox->value()); 00437 00438 } 00439 else if (actionNearest_Frontier->isChecked()){ 00440 expConf->add<double>("SIGMAAVOBS", stratDiag.NFOSSpinBox->value()); 00441 expConf->add<double>("SIGMAGOGOAL", stratDiag.NFGSSpinBox->value()); 00442 expConf->add<double>("WEIGHTAVOBS", stratDiag.NFOASpinBox->value()); 00443 expConf->add<double>("WEIGHTGOGOAL", stratDiag.NFGASpinBox->value()); 00444 expConf->add<double>("VMAX", stratDiag.NFVMAXSpinBox->value()); 00445 expConf->add<double>("WMAX", stratDiag.NFWMAXSpinBox->value()); 00446 expConf->add<double>("K1", stratDiag.NFK1SpinBox->value()); 00447 expConf->add<double>("K2", stratDiag.NFK2SpinBox->value()); 00448 expConf->add<double>("LOCALPOTENTIALWIDTH", stratDiag.NFLPWSpinBox->value()); 00449 expConf->add<double>("LOCALPOTENTIALHEIGHT", stratDiag.NFLPHSpinBox->value()); 00450 00451 expConf->add<double>("REPLANNING_PERIOD", stratDiag.NFRPSpinBox->value()); 00452 expConf->add<int>("INFLATE_OBSTACLES", stratDiag.NFIOSpinBox->value()); 00453 00454 } 00455 else if (actionCost_Utility->isChecked()){ 00456 expConf->add<double>("SIGMAAVOBS", stratDiag.CUOSSpinBox->value()); 00457 expConf->add<double>("SIGMAGOGOAL", stratDiag.CUGSSpinBox->value()); 00458 expConf->add<double>("WEIGHTAVOBS", stratDiag.CUOASpinBox->value()); 00459 expConf->add<double>("WEIGHTGOGOAL", stratDiag.CUGASpinBox->value()); 00460 expConf->add<double>("VMAX", stratDiag.CUVMAXSpinBox->value()); 00461 expConf->add<double>("WMAX", stratDiag.CUWMAXSpinBox->value()); 00462 expConf->add<double>("K1", stratDiag.CUK1SpinBox->value()); 00463 expConf->add<double>("K2", stratDiag.CUK2SpinBox->value()); 00464 expConf->add<double>("LOCALPOTENTIALWIDTH", stratDiag.CULPWSpinBox->value()); 00465 expConf->add<double>("LOCALPOTENTIALHEIGHT", stratDiag.CULPHSpinBox->value()); 00466 00467 expConf->add<double>("REPLANNING_PERIOD", stratDiag.CURPSpinBox->value()); 00468 expConf->add<int>("INFLATE_OBSTACLES", stratDiag.CUIOSpinBox->value()); 00469 expConf->add<double>("UTILITY_RADIUS", stratDiag.CUURSpinBox->value()); 00470 expConf->add<double>("UTILITY_WEIGHT", stratDiag.CUUWSpinBox->value()); 00471 expConf->add<double>("COST_WEIGHT", stratDiag.CUCWSpinBox->value()); 00472 00473 } 00474 00475 else if (actionCoordinated->isChecked()){ 00476 expConf->add<double>("SIGMAAVOBS", stratDiag.COSSpinBox->value()); 00477 expConf->add<double>("SIGMAGOGOAL", stratDiag.CGSSpinBox->value()); 00478 expConf->add<double>("WEIGHTAVOBS", stratDiag.COASpinBox->value()); 00479 expConf->add<double>("WEIGHTGOGOAL", stratDiag.CGASpinBox->value()); 00480 expConf->add<double>("VMAX", stratDiag.CVMAXSpinBox->value()); 00481 expConf->add<double>("WMAX", stratDiag.CWMAXSpinBox->value()); 00482 expConf->add<double>("K1", stratDiag.CK1SpinBox->value()); 00483 expConf->add<double>("K2", stratDiag.CK2SpinBox->value()); 00484 expConf->add<double>("LOCALPOTENTIALWIDTH", stratDiag.CLPWSpinBox->value()); 00485 expConf->add<double>("LOCALPOTENTIALHEIGHT", stratDiag.CLPHSpinBox->value()); 00486 00487 expConf->add<double>("REPLANNING_PERIOD", stratDiag.CRPSpinBox->value()); 00488 expConf->add<int>("INFLATE_OBSTACLES", stratDiag.CIOSpinBox->value()); 00489 expConf->add<double>("INFLUENCE_RADIUS", stratDiag.CIRSpinBox->value()); 00490 } 00491 else if (actionMarket_Based->isChecked()){ 00492 expConf->add<double>("SIGMAAVOBS", stratDiag.MBOSSpinBox->value()); 00493 expConf->add<double>("SIGMAGOGOAL", stratDiag.MBGSSpinBox->value()); 00494 expConf->add<double>("WEIGHTAVOBS", stratDiag.MBOASpinBox->value()); 00495 expConf->add<double>("WEIGHTGOGOAL", stratDiag.MBGASpinBox->value()); 00496 expConf->add<double>("VMAX", stratDiag.MBVMAXSpinBox->value()); 00497 expConf->add<double>("WMAX", stratDiag.MBWMAXSpinBox->value()); 00498 expConf->add<double>("K1", stratDiag.MBK1SpinBox->value()); 00499 expConf->add<double>("K2", stratDiag.MBK2SpinBox->value()); 00500 expConf->add<double>("LOCALPOTENTIALWIDTH", stratDiag.MBLPWSpinBox->value()); 00501 expConf->add<double>("LOCALPOTENTIALHEIGHT", stratDiag.MBLPHSpinBox->value()); 00502 00503 expConf->add<double>("REPLANNING_PERIOD", stratDiag.MBRPSpinBox->value()); 00504 expConf->add<int>("INFLATE_OBSTACLES", stratDiag.MBIOSpinBox->value()); 00505 expConf->add<double>("UTILITY_RADIUS", stratDiag.MBURSpinBox->value()); 00506 expConf->add<double>("UTILITY_WEIGHT", stratDiag.MBUWSpinBox->value()); 00507 expConf->add<double>("COST_WEIGHT", stratDiag.MBCWSpinBox->value()); 00508 } 00509 00510 else if (actionIntegrated->isChecked()){ 00511 expConf->add<double>("SIGMAAVOBS", stratDiag.IOSSpinBox->value()); 00512 expConf->add<double>("SIGMAGOGOAL", stratDiag.IGSSpinBox->value()); 00513 expConf->add<double>("WEIGHTAVOBS", stratDiag.IOASpinBox->value()); 00514 expConf->add<double>("WEIGHTGOGOAL", stratDiag.IGASpinBox->value()); 00515 expConf->add<double>("VMAX", stratDiag.IVMAXSpinBox->value()); 00516 expConf->add<double>("WMAX", stratDiag.IWMAXSpinBox->value()); 00517 expConf->add<double>("K1", stratDiag.IK1SpinBox->value()); 00518 expConf->add<double>("K2", stratDiag.IK2SpinBox->value()); 00519 expConf->add<double>("LOCALPOTENTIALWIDTH", stratDiag.ILPWSpinBox->value()); 00520 expConf->add<double>("LOCALPOTENTIALHEIGHT", stratDiag.ILPHSpinBox->value()); 00521 00522 expConf->add<double>("REPLANNING_PERIOD", stratDiag.IRPSpinBox->value()); 00523 expConf->add<int>("INFLATE_OBSTACLES", stratDiag.IIOSpinBox->value()); 00524 expConf->add<double>("UTILITY_RADIUS", stratDiag.IURSpinBox->value()); 00525 expConf->add<double>("UTILITY_WEIGHT", stratDiag.IUWSpinBox->value()); 00526 expConf->add<double>("COST_WEIGHT", stratDiag.ICWSpinBox->value()); 00527 expConf->add<double>("LOCALIZATION_WEIGHT", stratDiag.ILWSpinBox->value()); 00528 expConf->add<double>("CAMERA_RANGE", stratDiag.ICRSpinBox->value()); 00529 } 00530 00531 } 00532 } 00533 } 00534 00535 // 00536 // About Dialog 00537 // 00538 void MainWindow::openAboutDiag(bool){ 00539 Ui::AboutDialog aboutDiag; 00540 QDialog diag; 00541 aboutDiag.setupUi(&diag); 00542 diag.exec(); 00543 } 00544 00545 // 00546 // Application Configuration Dialog 00547 // 00548 void MainWindow::openAppConfigDiag(bool){ 00549 if (appConf){ 00550 Ui::AppConfigDialog appDiag; 00551 QDialog diag; 00552 appDiag.setupUi(&diag); 00553 00554 appDiag.sampleTimeSpinBox->setValue(appConf->read<double>("SAMPLE_TIME")); 00555 appDiag.groupPosesCheckBox->setChecked(appConf->read<bool>("GROUPINITIALPOSES")); 00556 appDiag.maxIniPoseDistSpinBox->setValue(appConf->read<double>("MAXDISTINIPOSE")); 00557 appDiag.minIniPoseDistSpinBox->setValue(appConf->read<double>("MINDISTINIPOSE")); 00558 00559 if(!appConf->read<bool>("GROUPINITIALPOSES")) appDiag.maxIniPoseDistSpinBox->setEnabled(false); 00560 00561 diag.exec(); 00562 00563 if (diag.result()==QDialog::Accepted){ 00564 appConf->add<double>("SAMPLE_TIME", appDiag.sampleTimeSpinBox->value()); 00565 appConf->add<bool>("GROUPINITIALPOSES", appDiag.groupPosesCheckBox->isChecked()); 00566 appConf->add<double>("MAXDISTINIPOSE", appDiag.maxIniPoseDistSpinBox->value()); 00567 appConf->add<double>("MINDISTINIPOSE", appDiag.minIniPoseDistSpinBox->value()); 00568 } 00569 } 00570 } 00571 00572 // 00573 // SLAM Options Dialog 00574 // 00575 void MainWindow::openSLAMOptSetDiag(bool){ 00576 00577 if (slamConf){ 00578 Ui::SlamOptionsDialog slamDiag; 00579 QDialog diag; 00580 slamDiag.setupUi(&diag); 00581 00582 slamDiag.resolutionSpinBox->setValue(slamConf->read<double>("RESOLUTION")); 00583 slamDiag.xOriginSpinBox->setValue(slamConf->read<double>("XORIGIN")); 00584 slamDiag.yOriginSpinBox->setValue(slamConf->read<double>("YORIGIN")); 00585 slamDiag.widthSpinBox->setValue(slamConf->read<double>("SCENE_WIDTH")); 00586 slamDiag.heightSpinBox->setValue(slamConf->read<double>("SCENE_HEIGHT")); 00587 slamDiag.EKFRadioButton->setChecked((slamConf->read<int>("SLAM")==1)); 00588 slamDiag.RBPFRadioButton->setChecked((slamConf->read<int>("SLAM")==0)); 00589 slamDiag.particlesSpinBox->setValue(slamConf->read<int>("PARTICLESPERROBOT")); 00590 slamDiag.MultiplyPerRobotsCheckBox->setChecked(slamConf->read<bool>("MULTIPLYPARTPERROBOTS")); 00591 slamDiag.MahalanobisThSpinBox->setValue(slamConf->read<double>("MAHALANOBISTH")); 00592 slamDiag.DescriptorThSpinBox->setValue(slamConf->read<double>("DESCRIPTORTH")); 00593 slamDiag.lowThSpinBox->setValue(slamConf->read<double>("THRESHOLD_LOW")); 00594 slamDiag.highThSpinBox->setValue(slamConf->read<double>("THRESHOLD_HIGH")); 00595 slamDiag.MatchByDescriptorRadioButton->setChecked((slamConf->read<int>("NEARESTNEIGHBOURBY")==0)); 00596 slamDiag.MatchByDistanceRadioButton->setChecked((slamConf->read<int>("NEARESTNEIGHBOURBY")==1)); 00597 00598 diag.exec(); 00599 00600 if (diag.result()==QDialog::Accepted){ 00601 slamConf->add<double>("RESOLUTION", slamDiag.resolutionSpinBox->value()); 00602 slamConf->add<double>("XORIGIN", slamDiag.xOriginSpinBox->value()); 00603 slamConf->add<double>("YORIGIN", slamDiag.yOriginSpinBox->value()); 00604 slamConf->add<double>("SCENE_WIDTH", slamDiag.widthSpinBox->value()); 00605 slamConf->add<double>("SCENE_HEIGHT", slamDiag.heightSpinBox->value()); 00606 if (slamDiag.EKFRadioButton->isChecked()){ 00607 slamConf->add<int>("SLAM", 1); 00608 actionEKF->setChecked(true); 00609 } 00610 if (slamDiag.RBPFRadioButton->isChecked()){ 00611 slamConf->add<int>("SLAM", 0); 00612 actionRBPF->setChecked(true); 00613 } 00614 slamConf->add<int>("PARTICLESPERROBOT", slamDiag.particlesSpinBox->value()); 00615 slamConf->add<bool>("MULTIPLYPARTPERROBOTS", slamDiag.MultiplyPerRobotsCheckBox->isChecked()); 00616 slamConf->add<double>("MAHALANOBISTH", slamDiag.MahalanobisThSpinBox->value()); 00617 slamConf->add<double>("DESCRIPTORTH", slamDiag.DescriptorThSpinBox->value()); 00618 slamConf->add<double>("THRESHOLD_LOW", slamDiag.lowThSpinBox->value()); 00619 slamConf->add<double>("THRESHOLD_HIGH", slamDiag.highThSpinBox->value()); 00620 if (slamDiag.MatchByDescriptorRadioButton->isChecked()) 00621 slamConf->add<int>("NEARESTNEIGHBOURBY", 0); 00622 if (slamDiag.MatchByDistanceRadioButton->isChecked()) 00623 slamConf->add<int>("NEARESTNEIGHBOURBY", 1); 00624 } 00625 } 00626 } 00627 00628 // 00629 // Scene Properties Dialog 00630 // 00631 void MainWindow::openScenePropDiag(bool){ 00632 00633 if (mapLoaded){ 00634 Ui::ScenePropertiesDialog sceneDiag; 00635 QDialog diag; 00636 sceneDiag.setupUi(&diag); 00637 sceneDiag.wallsLab->setNum(scenarioConf->read<int>("NUMWALLS")); 00638 sceneDiag.landmarksLab->setNum(scenarioConf->read<int>("NUMFEATURES")); 00639 sceneDiag.descLengthLab->setNum(scenarioConf->read<int>("DESCRIPTORLENGTH")); 00640 00641 diag.exec(); 00642 00643 } 00644 } 00645 00646 // 00647 // Robots Settings Dialog 00648 // 00649 void MainWindow::openRobotSetDiag(bool){ 00650 00651 if (mapLoaded){ 00652 Ui::RobotSettingsDialog robotSetDiag; 00653 QDialog diag; 00654 robotSetDiag.setupUi(&diag); 00655 00656 robotSetDiag.fSpinBox->setValue(scenarioConf->read<float>("f")); 00657 robotSetDiag.bSpinBox->setValue(scenarioConf->read<float>("I")); 00658 robotSetDiag.MaxDistSpinBox->setValue(scenarioConf->read<float>("distMAX")); 00659 robotSetDiag.MinDistSpinBox->setValue(scenarioConf->read<float>("distMIN")); 00660 robotSetDiag.widthSpinBox->setValue(scenarioConf->read<float>("WIDTH")); 00661 robotSetDiag.heightSpinBox->setValue(scenarioConf->read<float>("HEIGHT")); 00662 robotSetDiag.sigmacSpinBox->setValue(scenarioConf->read<float>("sigmac")); 00663 robotSetDiag.sigmarSpinBox->setValue(scenarioConf->read<float>("sigmar")); 00664 robotSetDiag.sigmadSpinBox->setValue(scenarioConf->read<float>("sigmad")); 00665 robotSetDiag.cameraxSpinBox->setValue(scenarioConf->read<float>("camx")); 00666 robotSetDiag.cameraySpinBox->setValue(scenarioConf->read<float>("camy")); 00667 robotSetDiag.camerazSpinBox->setValue(scenarioConf->read<float>("camz")); 00668 00669 robotSetDiag.laserPointsSpinBox->setValue(scenarioConf->read<float>("NUMPOINTS")); 00670 robotSetDiag.laserMinAngleSpinBox->setValue(scenarioConf->read<float>("LASERMINANGLE")); 00671 robotSetDiag.laserMaxAngleSpinBox->setValue(scenarioConf->read<float>("LASERMAXANGLE")); 00672 robotSetDiag.laserMaxDistSpinBox->setValue(scenarioConf->read<float>("LASERMAXDIST")); 00673 robotSetDiag.laserMinDistSpinBox->setValue(scenarioConf->read<float>("LASERMINDIST")); 00674 robotSetDiag.laserSigmaSpinBox->setValue(scenarioConf->read<float>("LASERSIGMA")); 00675 robotSetDiag.laserxSpinBox->setValue(scenarioConf->read<float>("LASERX")); 00676 robotSetDiag.laserySpinBox->setValue(scenarioConf->read<float>("LASERY")); 00677 robotSetDiag.laserzSpinBox->setValue(scenarioConf->read<float>("LASERZ")); 00678 00679 robotSetDiag.alfa1SpinBox->setValue(scenarioConf->read<float>("alfa1")); 00680 robotSetDiag.alfa2SpinBox->setValue(scenarioConf->read<float>("alfa2")); 00681 robotSetDiag.alfa3SpinBox->setValue(scenarioConf->read<float>("alfa3")); 00682 robotSetDiag.alfa4SpinBox->setValue(scenarioConf->read<float>("alfa4")); 00683 robotSetDiag.vmaxSpinBox->setValue(scenarioConf->read<float>("VABSMAX")); 00684 robotSetDiag.wmaxSpinBox->setValue(scenarioConf->read<float>("WABSMAX")); 00685 00686 diag.exec(); 00687 00688 if (diag.result()==QDialog::Accepted){ 00689 scenarioConf->add("f", robotSetDiag.fSpinBox->value()); 00690 scenarioConf->add("I", robotSetDiag.bSpinBox->value()); 00691 scenarioConf->add("distMAX", robotSetDiag.MaxDistSpinBox->value()); 00692 scenarioConf->add("distMIN", robotSetDiag.MinDistSpinBox->value()); 00693 scenarioConf->add("WIDTH", robotSetDiag.widthSpinBox->value()); 00694 scenarioConf->add("HEIGHT", robotSetDiag.heightSpinBox->value()); 00695 scenarioConf->add("sigmac", robotSetDiag.sigmacSpinBox->value()); 00696 scenarioConf->add("sigmar", robotSetDiag.sigmarSpinBox->value()); 00697 scenarioConf->add("sigmad", robotSetDiag.sigmadSpinBox->value()); 00698 scenarioConf->add("camx", robotSetDiag.cameraxSpinBox->value()); 00699 scenarioConf->add("camy", robotSetDiag.cameraySpinBox->value()); 00700 scenarioConf->add("camz", robotSetDiag.camerazSpinBox->value()); 00701 scenarioConf->add("NUMPOINTS", robotSetDiag.laserPointsSpinBox->value()); 00702 scenarioConf->add("LASERMINANGLE", robotSetDiag.laserMinAngleSpinBox->value()); 00703 scenarioConf->add("LASERMAXANGLE", robotSetDiag.laserMaxAngleSpinBox->value()); 00704 scenarioConf->add("LASERMAXDIST", robotSetDiag.laserMaxDistSpinBox->value()); 00705 scenarioConf->add("LASERMINDIST", robotSetDiag.laserMinDistSpinBox->value()); 00706 scenarioConf->add("LASERSIGMA", robotSetDiag.laserSigmaSpinBox->value()); 00707 scenarioConf->add("LASERX", robotSetDiag.laserxSpinBox->value()); 00708 scenarioConf->add("LASERY", robotSetDiag.laserySpinBox->value()); 00709 scenarioConf->add("LASERZ", robotSetDiag.laserzSpinBox->value()); 00710 scenarioConf->add("alfa1", robotSetDiag.alfa1SpinBox->value()); 00711 scenarioConf->add("alfa2", robotSetDiag.alfa2SpinBox->value()); 00712 scenarioConf->add("alfa3", robotSetDiag.alfa3SpinBox->value()); 00713 scenarioConf->add("alfa4", robotSetDiag.alfa4SpinBox->value()); 00714 scenarioConf->add("VABSMAX", robotSetDiag.vmaxSpinBox->value()); 00715 scenarioConf->add("wABSMAX", robotSetDiag.wmaxSpinBox->value()); 00716 00717 scene->initialize(numrobots,*scenarioConf, appConf->read<double>("SAMPLE_TIME")); 00718 } 00719 } 00720 } 00721 00722 ////////////////////////////////////////////////////////////// END DIALOGS /////////////////////////////////////////////////////////////// 00723 00724 00725 ////////////////////////////////////////////////////////////// OTHER OPTIONS //////////////////////////////////////////////// 00726 00727 void MainWindow::changeNumRobots(int n){ 00728 numrobots = n; 00729 if (mapLoaded){ 00730 scene->setNumRobots(n); 00731 drawScene(); 00732 drawRobots(); 00733 } 00734 } 00735 00736 void MainWindow::randomPoses(bool){ 00737 if (mapLoaded){ 00738 scene->reset(); 00739 scene->randomPoses(appConf->read<bool>("GROUPINITIALPOSES") ,appConf->read<double>("MAXDISTINIPOSE"), appConf->read<double>("MINDISTINIPOSE")); 00740 drawScene(); 00741 drawRobots(); 00742 // TODO: change the config with the new poses when saving 00743 } 00744 } 00745 00746 void MainWindow::changeSlam(bool){ 00747 if (actionEKF->isChecked()){ 00748 slamConf->add<int>("SLAM", 1); 00749 } 00750 else if (actionRBPF->isChecked()){ 00751 slamConf->add<int>("SLAM", 0); 00752 } 00753 } 00754 00755 00756 void MainWindow::changeStrategy(bool){ 00757 if (expConf) delete expConf; 00758 if (actionHybrid->isChecked()){ 00759 expConf = new ConfigFile((QDir::homePath() += "/.mrxt/config/tec0.config").toStdString().c_str()); 00760 } 00761 else if (actionBehaviour_Based->isChecked()){ 00762 expConf = new ConfigFile((QDir::homePath() += "/.mrxt/config/tec1.config").toStdString().c_str()); 00763 } 00764 else if (actionNearest_Frontier->isChecked()){ 00765 expConf = new ConfigFile((QDir::homePath() += "/.mrxt/config/tec2.config").toStdString().c_str()); 00766 } 00767 else if (actionCost_Utility->isChecked()){ 00768 expConf = new ConfigFile((QDir::homePath() += "/.mrxt/config/tec3.config").toStdString().c_str()); 00769 } 00770 else if (actionCoordinated->isChecked()){ 00771 expConf = new ConfigFile((QDir::homePath() += "/.mrxt/config/tec4.config").toStdString().c_str()); 00772 } 00773 else if (actionMarket_Based->isChecked()){ 00774 expConf = new ConfigFile((QDir::homePath() += "/.mrxt/config/tec5.config").toStdString().c_str()); 00775 } 00776 else if (actionIntegrated->isChecked()){ 00777 expConf = new ConfigFile((QDir::homePath() += "/.mrxt/config/tec6.config").toStdString().c_str()); 00778 } 00779 else expConf = 0; 00780 } 00781 00782 ///////////////////////////////////////////////////////// END OTHER OPTIONS ////////////////////////////////////////////////////////////// 00783 00784 00785 ////////////////////////////////////////////////////////// SIMULATION CONTROL /////////////////////////////////////////////////////////////// 00786 00787 // 00788 // Start Simulation 00789 // 00790 void MainWindow::startSimulation(bool){ 00791 if (mapLoaded){ 00792 actionPlay->setDisabled(true); 00793 randomPosesButton->setDisabled(true); 00794 actionStop->setEnabled(true); 00795 actionSaveVmap->setEnabled(false); 00796 actionSaveOmap->setEnabled(false); 00797 actionSaveSLAMlog->setEnabled(false); 00798 actionSaveGTlog->setEnabled(false); 00799 spinBox->setDisabled(true); 00800 myteam = new team(numrobots, *scene, *expConf, *slamConf, robots::EXPLORER, "explorers"); 00801 QString tempLogpath = QDir::homePath() += "/.mrxt/outfiles/tempLog"; 00802 myteam->setLogName(tempLogpath.toStdString().c_str()); 00803 slam = myteam->getGlobalSlam(); 00804 connect( slam, SIGNAL(slamUpdated(void)), this, SLOT(updateSlam(void))); 00805 QString tempLogGTpath = QDir::homePath() += "/.mrxt/outfiles/tempLogGT"; 00806 scene->setLogName(tempLogGTpath.toStdString().c_str()); 00807 scene->run(); 00808 myteam->start(); 00809 if (waitTh) delete waitTh; 00810 waitTh = new waitEnding(myteam); 00811 connect( waitTh, SIGNAL(simulationFinished(bool)), this, SLOT(stopSimulation(bool))); 00812 waitTh->start(); 00813 } 00814 } 00815 00816 // 00817 // Stop Simulation 00818 // 00819 void MainWindow::stopSimulation(bool){ 00820 disconnect( waitTh, SIGNAL(simulationFinished(bool)), this, SLOT(stopSimulation(bool))); 00821 printf("[GUI] Stop Simulation Requested\n"); 00822 disconnect( slam, SIGNAL(slamUpdated(void))); 00823 visualMap* vm = slam->getVMap(); 00824 slam = 0; 00825 myteam->stop(); 00826 scene->stop(); 00827 if (myteam){ delete myteam; myteam=0;} 00828 printf("[GUI] Simulation Stopped\n"); 00829 scene->reset(); 00830 actionStop->setDisabled(true); 00831 actionPlay->setEnabled(true); 00832 actionSaveVmap->setEnabled(true); 00833 actionSaveOmap->setEnabled(true); 00834 actionSaveSLAMlog->setEnabled(true); 00835 actionSaveGTlog->setEnabled(true); 00836 randomPosesButton->setEnabled(true); 00837 spinBox->setEnabled(true); 00838 00839 double error = scene->evaluateVMap(*vm); 00840 double time = scene->getTime(); 00841 delete vm; 00842 00843 QString qstr = "- Exploration Time: " + QString::number(time) + " secs\n- Error in landmark based map: " + QString::number(error) + " m"; 00844 QMessageBox::information(this, "Simulation Results", qstr); 00845 } 00846 00847 // 00848 // Update poses SLOT 00849 // 00850 // This slot is trigered by the simulation scene thread each step with the new real poses of the robots 00851 // 00852 void MainWindow::updatePoses(rposes poses){ 00853 00854 for (int r = 0; r< numrobots; r++){ 00855 robotShapes[r]->setX(poses[r].x); 00856 robotShapes[r]->setY(-poses[r].y); 00857 //robotShapes[r]->setTransformOriginPoint(poses[r].x,poses[r].y); 00858 robotShapes[r]->setRotation(-poses[r].th*180.0/PI); 00859 } 00860 00861 int msecs = (int)(1000.0f*scene->getTime()); 00862 QTime ref; 00863 QTime qtime = ref.addMSecs(msecs); 00864 timeDisplay->display(qtime.toString(Qt::TextDate)); 00865 } 00866 00867 // 00868 // Update SLAM SLOT 00869 // 00870 // This slot is trigered by the simulation SLAM thread each step 00871 // 00872 void MainWindow::updateSlam(){ 00873 if (slam){ 00874 QPixmap* mapfig = slam->getPixmap(); 00875 slamMapLabel->setPixmap(*mapfig); 00876 delete mapfig; 00877 } 00878 } 00879 00880 ////////////////////////////////////////////////////////// END SIMULATION CONTROL /////////////////////////////////////////////////////////////// 00881 00882 ////////////////////////////////////////////////////////// VISUALIZATION /////////////////////////////////////////////////////////////// 00883 00884 // 00885 // Refresh Poses 00886 // 00887 // This method updates the visualization of the simulated scene 00888 // 00889 void MainWindow::drawRobots(){ 00890 00891 // QPixmap* mapfig = scene->getPixmap(); 00892 // mapLabel->setPixmap(*mapfig); 00893 // delete mapfig; 00894 00895 // get poses 00896 rposes poses = scene->getPoses(); 00897 // char entrada[50]; 00898 // for (int r = 0; r< numrobots; r++){ 00899 // sprintf(entrada,"R%i",r+1); 00900 // string robotpos = scenarioConf->read<string>(entrada); 00901 // StringTokenizer st(&robotpos[0]); 00902 00903 // float x = atof(st.nextToken()); 00904 // float y = atof(st.nextToken()); 00905 // float th = atof(st.nextToken()); 00906 // pose pos(x,y,th); 00907 00908 // //pose pos = scene->getPos(r); //ESTO NO EXISTE! 00909 // poses.push_back(pos); 00910 // } 00911 00912 // get footprint 00913 std::vector<double> robot_footprint; 00914 string robotpos = scenarioConf->read<string>("FOOTPRINT"); 00915 StringTokenizer st(&robotpos[0]); 00916 robot_footprint.clear(); 00917 for (int t = 0; t < st.countTokens(); t++){ 00918 float pt = atof(st.nextToken()); 00919 robot_footprint.push_back(pt); 00920 } 00921 int total_lines2 = robot_footprint.size(); 00922 00923 // create robot shape 00924 rlines robots_lines; 00925 std::vector<line> newline; 00926 robotShapes.clear(); 00927 QGraphicsItemGroup* newgroup; 00928 for (int r = 0; r< numrobots; r++){ 00929 robots_lines.push_back(newline); 00930 robotShapes.push_back(newgroup); 00931 QList<QGraphicsItem *> list; 00932 QPolygonF polygon; 00933 for (int l = 0; l < total_lines2; l+=2){ 00934 float x1 = robot_footprint[l]; 00935 float y1 = robot_footprint[l+1]; 00936 float x2 = robot_footprint[(l+2)%total_lines2]; 00937 float y2 = robot_footprint[(l+3)%total_lines2]; 00938 line lin( x1*cos(poses[r].th)-y1*sin(poses[r].th), 00939 -(x1*sin(poses[r].th)+y1*cos(poses[r].th)), 00940 x2*cos(poses[r].th)-y2*sin(poses[r].th), 00941 -(x2*sin(poses[r].th)+y2*cos(poses[r].th))); 00942 robots_lines[r].push_back(lin); 00943 polygon << QPointF(x1,y1); 00944 //QGraphicsLineItem* rl = qscene.addLine(lin.x1, lin.y1, lin.x2, lin.y2,QPen(QColor(255,0,0))); 00945 //list.push_back(rl); 00946 } 00947 QGraphicsItem* pol = qscene.addPolygon(polygon,QPen(QColor(0,0,0)),QBrush(QColor(255,0,0),Qt::SolidPattern)); 00948 list.push_back(pol); 00949 QGraphicsLineItem* rlori = qscene.addLine(0, 0, 0.4 , 0,QPen(QColor(0,0,0))); 00950 list.push_back(rlori); 00951 robotShapes[r] = qscene.createItemGroup(list); 00952 } 00953 updatePoses(poses); 00954 } 00955 00956 void MainWindow::sceneZoom(int zoomSlider){ 00957 double newscale = zoomSlider; 00958 double zoomval = newscale/scale; 00959 view.scale(zoomval,zoomval); 00960 scale = newscale; 00961 } 00962 00963 void MainWindow::drawScene(){ 00964 00965 while (qscene.items().size() > 0){ 00966 qscene.removeItem(qscene.items().at(0)); 00967 } 00968 00969 char entrada[20]; 00970 00971 // walls / obstacles 00972 int numwalls = scenarioConf->read<int>("NUMWALLS"); 00973 for (int i = 0; i< numwalls; i++){ 00974 sprintf(entrada,"WALL%i",i+1); 00975 string wallstr = scenarioConf->read<string>(entrada); 00976 StringTokenizer st(&(wallstr[0])); 00977 float x1 = atof(st.nextToken()); 00978 float y1 = -atof(st.nextToken()); 00979 float x2 = atof(st.nextToken()); 00980 float y2 = -atof(st.nextToken()); 00981 qscene.addLine( x1, y1, x2, y2 ); 00982 } 00983 00984 // walls marks 00985 int nummarks = scenarioConf->read<int>("NUMFEATURES"); 00986 for (int i = 0; i< nummarks; i++){ 00987 sprintf(entrada,"FEAT%i",i+1); 00988 string featstr = scenarioConf->read<string>(entrada); 00989 StringTokenizer st(&(featstr[0])); 00990 float x = atof(st.nextToken()); 00991 float y = -atof(st.nextToken()); 00992 float z = atof(st.nextToken()); 00993 float desc = atof(st.nextToken()); 00994 qscene.addRect(x-0.05,y-0.05,0.1,0.1,QPen(QColor(0,100,0)),QBrush(QColor(0,100,0),Qt::SolidPattern)); 00995 } 00996 00997 if (!scaleInitialized){ 00998 view.scale(scale,scale); 00999 scaleInitialized = true; 01000 } 01001 } 01002 01003 ////////////////////////////////////////////////////////// END VISUALIZATION /////////////////////////////////////////////////////////////// 01004 01005 01006 01007 01008
1.7.6.1
