stdr_gui_controller.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002    STDR Simulator - Simple Two DImensional Robot Simulator
00003    Copyright (C) 2013 STDR Simulator
00004    This program is free software; you can redistribute it and/or modify
00005    it under the terms of the GNU General Public License as published by
00006    the Free Software Foundation; either version 3 of the License, or
00007    (at your option) any later version.
00008    This program is distributed in the hope that it will be useful,
00009    but WITHOUT ANY WARRANTY; without even the implied warranty of
00010    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00011    GNU General Public License for more details.
00012    You should have received a copy of the GNU General Public License
00013    along with this program; if not, write to the Free Software Foundation,
00014    Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301  USA
00015    
00016    Authors : 
00017    * Manos Tsardoulias, etsardou@gmail.com
00018    * Aris Thallas, aris.thallas@gmail.com
00019    * Chris Zalidis, zalidis@gmail.com 
00020 ******************************************************************************/
00021 
00022 #include "stdr_gui/stdr_gui_controller.h"
00023 
00024 
00025 namespace stdr_gui
00026 {
00031   void spinThreadFunction(void)
00032   {
00033     ros::spin();
00034   }
00035   
00042   CGuiController::CGuiController(int argc,char **argv):
00043     gui_connector_(argc,argv),
00044     info_connector_(argc,argv),
00045     map_connector_(argc,argv),
00046     argc_(argc),
00047     argv_(argv)
00048   {
00049     setupWidgets();
00050   
00051     robot_following_ = "";
00052   
00053     map_lock_ = false;
00054     map_initialized_ = false;
00055     
00056     icon_move_.addFile(QString::fromUtf8((
00057       stdr_gui_tools::getRosPackagePath("stdr_gui") + 
00058       std::string("/resources/images/arrow_move.png")).c_str()), 
00059       QSize(20,20), QIcon::Normal, QIcon::Off);
00060         
00061     icon_delete_.addFile(QString::fromUtf8((
00062       stdr_gui_tools::getRosPackagePath("stdr_gui")+
00063       std::string("/resources/images/remove_icon.png")).c_str()), 
00064       QSize(20,20), QIcon::Normal, QIcon::Off);
00065   }
00066 
00071   CGuiController::~CGuiController(void)
00072   {
00073     
00074   }
00075   
00080   void CGuiController::initializeCommunications(void)
00081   {
00082     map_subscriber_ = n_.subscribe(
00083       "map", 
00084       1, 
00085       &CGuiController::receiveMap,
00086       this);
00087       
00089     rfids_subscriber_ = n_.subscribe(
00090       "stdr_server/rfid_list", 
00091       1, 
00092       &CGuiController::receiveRfids,
00093       this);
00094     new_rfid_tag_client_ = 
00095       n_.serviceClient<stdr_msgs::AddRfidTag>("stdr_server/add_rfid_tag");
00096     delete_rfid_tag_client_ = 
00097       n_.serviceClient<stdr_msgs::DeleteRfidTag>("stdr_server/delete_rfid_tag");
00098     
00100     co2_sources_subscriber_ = n_.subscribe(
00101       "stdr_server/co2_sources_list", 
00102       1, 
00103       &CGuiController::receiveCO2Sources,
00104       this);
00105     new_co2_source_client_ = 
00106       n_.serviceClient<stdr_msgs::AddCO2Source>("stdr_server/add_co2_source");
00107     delete_co2_source_client_ = 
00108       n_.serviceClient<stdr_msgs::DeleteCO2Source>(
00109         "stdr_server/delete_co2_source");
00110     
00112     thermal_sources_subscriber_ = n_.subscribe(
00113       "stdr_server/thermal_sources_list", 
00114       1, 
00115       &CGuiController::receiveThermalSources,
00116       this);
00117     new_thermal_source_client_ = 
00118       n_.serviceClient<stdr_msgs::AddThermalSource>(
00119         "stdr_server/add_thermal_source");
00120     delete_thermal_source_client_ = 
00121       n_.serviceClient<stdr_msgs::DeleteThermalSource>(
00122         "stdr_server/delete_thermal_source");
00123     
00125     sound_sources_subscriber_ = n_.subscribe(
00126       "stdr_server/sound_sources_list", 
00127       1, 
00128       &CGuiController::receiveSoundSources,
00129       this);
00130     new_sound_source_client_ = 
00131       n_.serviceClient<stdr_msgs::AddSoundSource>(
00132         "stdr_server/add_sound_source");
00133     delete_sound_source_client_ = 
00134       n_.serviceClient<stdr_msgs::DeleteSoundSource>(
00135         "stdr_server/delete_sound_source");
00136     
00137     QObject::connect(
00138       &gui_connector_,SIGNAL(setZoomInCursor(bool)),
00139       &map_connector_, SLOT(setCursorZoomIn(bool)));
00140     
00141     QObject::connect(
00142       &gui_connector_,SIGNAL(setZoomOutCursor(bool)),
00143       &map_connector_, SLOT(setCursorZoomOut(bool)));
00144     
00145     QObject::connect(
00146       &gui_connector_,SIGNAL(setAdjustedCursor(bool)),
00147       &map_connector_, SLOT(setCursorAdjusted(bool)));
00148     
00149     QObject::connect(
00150       &map_connector_,SIGNAL(zoomInPressed(QPoint)),
00151       this, SLOT(zoomInPressed(QPoint)));
00152     
00153     QObject::connect(
00154       &map_connector_,SIGNAL(zoomOutPressed(QPoint)),
00155       this, SLOT(zoomOutPressed(QPoint)));
00156     
00157     QObject::connect(
00158       &map_connector_,SIGNAL(itemClicked(QPoint,Qt::MouseButton)),
00159       this, SLOT(itemClicked(QPoint,Qt::MouseButton)));
00160     
00161     QObject::connect(
00162       &info_connector_,SIGNAL(laserVisualizerClicked(QString,QString)),
00163       this, SLOT(laserVisualizerClicked(QString,QString)));
00164     
00165     QObject::connect(
00166       &info_connector_,SIGNAL(sonarVisualizerClicked(QString,QString)),
00167       this, SLOT(sonarVisualizerClicked(QString,QString)));
00168       
00169     QObject::connect(
00170       &info_connector_,SIGNAL(robotVisualizerClicked(QString)),
00171       this, SLOT(robotVisualizerClicked(QString)));
00172     
00173     QObject::connect(
00174       &(gui_connector_.robotCreatorConn),
00175         SIGNAL(saveRobotPressed(stdr_msgs::RobotMsg,QString)),
00176       this, SLOT(saveRobotPressed(stdr_msgs::RobotMsg,QString)));
00177     
00178     QObject::connect(
00179       &(gui_connector_.robotCreatorConn),
00180         SIGNAL(loadRobotPressed(stdr_msgs::RobotMsg)),
00181       this, SLOT(loadRobotPressed(stdr_msgs::RobotMsg)));
00182       
00183     QObject::connect(
00184       &(gui_connector_),
00185         SIGNAL(robotFromFile(stdr_msgs::RobotMsg)),
00186       this, SLOT(loadRobotFromFilePressed(stdr_msgs::RobotMsg)));  
00187       
00188     QObject::connect(
00189       &(gui_connector_),
00190         SIGNAL(loadRfidPressed()),
00191       this, SLOT(loadRfidPressed()));
00192     
00193     QObject::connect(
00194       &(gui_connector_),
00195         SIGNAL(loadThermalPressed()),
00196       this, SLOT(loadThermalPressed()));
00197     
00198     QObject::connect(
00199       &(gui_connector_),
00200         SIGNAL(loadCo2Pressed()),
00201       this, SLOT(loadCo2Pressed()));
00202       
00203     QObject::connect(
00204       &(gui_connector_),
00205         SIGNAL(loadSoundPressed()),
00206       this, SLOT(loadSoundPressed()));
00207     
00208     QObject::connect(
00209       this,SIGNAL(waitForRobotPose()),
00210       &map_connector_, SLOT(waitForPlace()));
00211     
00212     QObject::connect(
00213       &map_connector_,SIGNAL(robotPlaceSet(QPoint)),
00214       this, SLOT(robotPlaceSet(QPoint)));
00215       
00216     QObject::connect(
00217       this,SIGNAL(replaceRobot(std::string)),
00218       &map_connector_, SLOT(waitForReplace(std::string)));
00219       
00220     QObject::connect(
00221       &map_connector_,SIGNAL(robotReplaceSet(QPoint,std::string)),
00222       this, SLOT(robotReplaceSet(QPoint,std::string)));
00223     
00224     timer_ = new QTimer(this);
00225     connect(
00226       timer_, SIGNAL(timeout()), 
00227       this, SLOT(updateMapInternal()));
00228       
00229     QObject::connect(
00230       this,SIGNAL(waitForRfidPose()),
00231       &map_connector_, SLOT(waitForRfidPlace()));
00232     
00233     QObject::connect(
00234       &map_connector_,SIGNAL(rfidPlaceSet(QPoint)),
00235       this, SLOT(rfidPlaceSet(QPoint)));
00236       
00237     QObject::connect(
00238       this,SIGNAL(waitForThermalPose()),
00239       &map_connector_, SLOT(waitForThermalPlace()));
00240     
00241     QObject::connect(
00242       &map_connector_,SIGNAL(thermalPlaceSet(QPoint)),
00243       this, SLOT(thermalPlaceSet(QPoint)));
00244       
00245     QObject::connect(
00246       this,SIGNAL(waitForCo2Pose()),
00247       &map_connector_, SLOT(waitForCo2Place()));
00248       
00249     QObject::connect(
00250       this,SIGNAL(waitForSoundPose()),
00251       &map_connector_, SLOT(waitForSoundPlace()));
00252     
00253     QObject::connect(
00254       &map_connector_,SIGNAL(soundPlaceSet(QPoint)),
00255       this, SLOT(soundPlaceSet(QPoint)));
00256       
00257     QObject::connect(
00258       &map_connector_,SIGNAL(co2PlaceSet(QPoint)),
00259       this, SLOT(co2PlaceSet(QPoint)));
00260       
00261     QObject::connect(
00262       &info_connector_,SIGNAL(laserVisibilityClicked(QString,QString)),
00263       this, SLOT(laserVisibilityClicked(QString,QString)));
00264       
00265     QObject::connect(
00266       &info_connector_,SIGNAL(sonarVisibilityClicked(QString,QString)),
00267       this, SLOT(sonarVisibilityClicked(QString,QString)));
00268       
00269     QObject::connect(
00270       &info_connector_,SIGNAL(rfidReaderVisibilityClicked(QString,QString)),
00271       this, SLOT(rfidReaderVisibilityClicked(QString,QString)));
00272       
00273     QObject::connect(
00274       &info_connector_,SIGNAL(co2SensorVisibilityClicked(QString,QString)),
00275       this, SLOT(co2SensorVisibilityClicked(QString,QString)));
00276       
00277     QObject::connect(
00278       &info_connector_,SIGNAL(thermalSensorVisibilityClicked(QString,QString)),
00279       this, SLOT(thermalSensorVisibilityClicked(QString,QString)));
00280     
00281     QObject::connect(
00282       &info_connector_,SIGNAL(soundSensorVisibilityClicked(QString,QString)),
00283       this, SLOT(soundSensorVisibilityClicked(QString,QString)));
00284       
00285     QObject::connect(
00286       &info_connector_,SIGNAL(robotVisibilityClicked(QString)),
00287       this, SLOT(robotVisibilityClicked(QString)));
00288     
00289     QObject::connect(
00290       this, SIGNAL(setRobotVisibility(QString,char)),
00291       &info_connector_, SLOT(setRobotVisibility(QString,char)));
00292     
00293     QObject::connect(
00294       this, SIGNAL(setLaserVisibility(QString,QString,char)),
00295       &info_connector_, SLOT(setLaserVisibility(QString,QString,char)));
00296     
00297     QObject::connect(
00298       this, SIGNAL(setSonarVisibility(QString,QString,char)),
00299       &info_connector_, SLOT(setSonarVisibility(QString,QString,char)));
00300       
00301     QObject::connect(
00302       this, SIGNAL(setRfidReaderVisibility(QString,QString,char)),
00303       &info_connector_, SLOT(setRfidReaderVisibility(QString,QString,char)));
00304       
00305     QObject::connect(
00306       this, SIGNAL(setCO2SensorVisibility(QString,QString,char)),
00307       &info_connector_, SLOT(setCO2SensorVisibility(QString,QString,char)));
00308     
00309     QObject::connect(
00310       this, SIGNAL(setThermalSensorVisibility(QString,QString,char)),
00311       &info_connector_, SLOT(setThermalSensorVisibility(QString,QString,char)));
00312     
00313     QObject::connect(
00314       this, SIGNAL(setSoundSensorVisibility(QString,QString,char)),
00315       &info_connector_, SLOT(setSoundSensorVisibility(QString,QString,char)));
00316   }
00317   
00322   void CGuiController::setupWidgets(void)
00323   {
00324     {
00325       gui_connector_.addToGrid(info_connector_.getLoader(),0,0);
00326     }
00327     {
00328       initial_map_ = running_map_ = QImage((
00329         stdr_gui_tools::getRosPackagePath("stdr_gui") + 
00330         std::string("/resources/images/logo.png")).c_str());
00331 
00332       map_msg_.info.width = initial_map_.width();
00333       map_msg_.info.height = initial_map_.height();
00334       
00335       map_connector_.updateImage(&running_map_);
00336       
00337       gui_connector_.addToGrid(map_connector_.getLoader(),0,1);
00338 
00339       gui_connector_.setGridColumnStretch(1,5);
00340       gui_connector_.setGridColumnStretch(0,2);
00341     }
00342   }
00343   
00348   bool CGuiController::init(void)
00349   {
00350     if ( ! ros::master::check() ) 
00351     {
00352       return false;
00353     }
00354     gui_connector_.show();
00355 
00356     initializeCommunications();
00357     boost::thread spinThread(&spinThreadFunction);
00358     return true;
00359   }
00360 
00364   void CGuiController::receiveRfids(const stdr_msgs::RfidTagVector& msg)
00365   {
00366     rfid_tag_pure_ = msg;
00367     rfid_tags_.clear();
00368     for(unsigned int i = 0 ; i < msg.rfid_tags.size() ; i++)
00369     {
00370       QPoint p(msg.rfid_tags[i].pose.x / map_msg_.info.resolution,
00371         msg.rfid_tags[i].pose.y / map_msg_.info.resolution);
00372       
00373       CGuiRfidTag temp_tag(p, msg.rfid_tags[i].tag_id, 
00374         map_msg_.info.resolution);
00375       
00376       temp_tag.setMessage(QString(msg.rfid_tags[i].message.c_str()));
00377       
00378       rfid_tags_.insert(std::pair<QString, CGuiRfidTag>(
00379         QString(temp_tag.getName().c_str()), temp_tag));
00380     }
00381     for(unsigned int i = 0 ; i < registered_robots_.size() ; i++)
00382     {
00383       registered_robots_[i].setEnvironmentalTags(rfid_tag_pure_);
00384     }
00385   }
00386   
00390   void CGuiController::receiveCO2Sources(const stdr_msgs::CO2SourceVector& msg)
00391   {
00392     co2_source_pure_ = msg;
00393     co2_sources_.clear();
00394     for(unsigned int i = 0 ; i < msg.co2_sources.size() ; i++)
00395     {
00396       QPoint p(msg.co2_sources[i].pose.x / map_msg_.info.resolution,
00397         msg.co2_sources[i].pose.y / map_msg_.info.resolution);
00398       
00399       CGuiCo2Source temp_source(p, msg.co2_sources[i].id, 
00400         map_msg_.info.resolution);
00401       
00402       temp_source.setPpm(msg.co2_sources[i].ppm);
00403       
00404       co2_sources_.insert(std::pair<QString, CGuiCo2Source>(
00405         QString(temp_source.getName().c_str()), temp_source));
00406     }
00407     for(unsigned int i = 0 ; i < registered_robots_.size() ; i++)
00408     {
00409       registered_robots_[i].setEnvironmentalCO2Sources(co2_source_pure_);
00410     }
00411   }
00412   
00416   void CGuiController::receiveThermalSources
00417     (const stdr_msgs::ThermalSourceVector& msg)
00418   {
00419     thermal_source_pure_ = msg;
00420     thermal_sources_.clear();
00421     for(unsigned int i = 0 ; i < msg.thermal_sources.size() ; i++)
00422     {
00423       QPoint p(msg.thermal_sources[i].pose.x / map_msg_.info.resolution,
00424         msg.thermal_sources[i].pose.y / map_msg_.info.resolution);
00425       
00426       CGuiThermalSource temp_source(p, msg.thermal_sources[i].id, 
00427         map_msg_.info.resolution);
00428       
00429       temp_source.setDegrees(msg.thermal_sources[i].degrees);
00430       
00431       thermal_sources_.insert(std::pair<QString, CGuiThermalSource>(
00432         QString(temp_source.getName().c_str()), temp_source));
00433     }
00434     for(unsigned int i = 0 ; i < registered_robots_.size() ; i++)
00435     {
00436       registered_robots_[i].setEnvironmentalThermalSources(
00437         thermal_source_pure_);
00438     }
00439   }
00440   
00444   void CGuiController::receiveSoundSources
00445     (const stdr_msgs::SoundSourceVector& msg)
00446   {
00447     sound_source_pure_ = msg;
00448     sound_sources_.clear();
00449     for(unsigned int i = 0 ; i < msg.sound_sources.size() ; i++)
00450     {
00451       QPoint p(msg.sound_sources[i].pose.x / map_msg_.info.resolution,
00452         msg.sound_sources[i].pose.y / map_msg_.info.resolution);
00453       
00454       CGuiSoundSource temp_source(p, msg.sound_sources[i].id, 
00455         map_msg_.info.resolution);
00456       
00457       //~ temp_source.setMessage(QString(msg.rfid_tags[i].message.c_str()));
00458       
00459       sound_sources_.insert(std::pair<QString, CGuiSoundSource>(
00460         QString(temp_source.getName().c_str()), temp_source));
00461     }
00462     for(unsigned int i = 0 ; i < registered_robots_.size() ; i++)
00463     {
00464       registered_robots_[i].setEnvironmentalSoundSources(sound_source_pure_);
00465     }
00466   }
00467 
00474   void CGuiController::receiveMap(const nav_msgs::OccupancyGrid& msg)
00475   {
00476     map_msg_ = msg;
00477     initial_map_ = running_map_ =
00478       QImage(msg.info.width,msg.info.height,QImage::Format_RGB32);
00479     QPainter painter(&running_map_);
00480     int d(0);
00481     QColor c;
00482     for( unsigned int i = 0 ; i < msg.info.width ; i++ )
00483     {
00484       for( unsigned int j = 0 ; j < msg.info.height ; j++ )
00485       {
00486         if( msg.data[j * msg.info.width + i] == -1 )
00487         {
00488           c=QColor(127,127,127);
00489         }
00490         else
00491         {
00492           d = (100.0 - msg.data[j * msg.info.width + i]) / 100.0 * 255.0;
00493           c=QColor(d,d,d);
00494         }
00495         painter.setPen(c);
00496         painter.drawPoint(i,j);
00497       }
00498     }
00499     int originx = msg.info.origin.position.x / msg.info.resolution;
00500     int originy = msg.info.origin.position.y / msg.info.resolution;
00501     painter.setPen(Qt::blue);
00502     painter.drawLine(originx, originy - 20, originx, originy + 20);
00503     painter.drawLine(originx - 20, originy, originx + 20, originy);
00504     
00505     initial_map_ = running_map_;
00506 
00507     info_connector_.updateMapInfo( msg.info.width * msg.info.resolution,
00508                   msg.info.height * msg.info.resolution,
00509                   msg.info.resolution);
00510     map_connector_.setInitialImageSize(
00511       QSize(initial_map_.width(),initial_map_.height()));
00512     
00513     elapsed_time_.start();
00514     
00515     map_initialized_ = true;
00516     map_connector_.setMapInitialized(true);
00517     gui_connector_.setMapInitialized(true);
00518     
00519     timer_->start(50);
00520     
00521     robot_subscriber_ = n_.subscribe(
00522       "stdr_server/active_robots", 
00523       1, 
00524       &CGuiController::receiveRobots,
00525       this);
00526   }
00527   
00534   void CGuiController::saveRobotPressed(stdr_msgs::RobotMsg newRobotMsg,
00535     QString file_name)
00536   {
00537     std::string file_name_str=file_name.toStdString();
00538     
00539     try {
00540       stdr_parser::Parser::saveMessage(newRobotMsg, file_name.toStdString());
00541     }
00542     catch(stdr_parser::ParserException ex)
00543     {
00544       gui_connector_.raiseMessage("STDR Parser - Error", ex.what());
00545     }
00546   }
00547   
00553   void CGuiController::loadRobotPressed(stdr_msgs::RobotMsg newRobotMsg)
00554   {
00555     if ( ! map_initialized_ )
00556     {
00557       return;
00558     }
00559     Q_EMIT waitForRobotPose();
00560   }
00561   
00567   void CGuiController::loadRobotFromFilePressed(stdr_msgs::RobotMsg newRobotMsg)
00568   {
00569     if ( ! map_initialized_ )
00570     {
00571       return;
00572     }
00573     gui_connector_.robotCreatorConn.setNewRobot(
00574       stdr_gui_tools::fixRobotAnglesToDegrees(newRobotMsg));
00575     Q_EMIT waitForRobotPose();
00576   }
00577   
00582   void CGuiController::loadRfidPressed(void)
00583   {
00584     if ( ! map_initialized_ )
00585     {
00586       return;
00587     }
00588     Q_EMIT waitForRfidPose();
00589   }
00590   
00595   void CGuiController::loadThermalPressed(void)
00596   {
00597     if ( ! map_initialized_ )
00598     {
00599       return;
00600     }
00601     Q_EMIT waitForThermalPose();
00602   }
00603   
00608   void CGuiController::loadCo2Pressed(void)
00609   {
00610     if ( ! map_initialized_ )
00611     {
00612       return;
00613     }
00614     Q_EMIT waitForCo2Pose();
00615   }
00616   
00621   void CGuiController::loadSoundPressed(void)
00622   {
00623     if ( ! map_initialized_ )
00624     {
00625       return;
00626     }
00627     Q_EMIT waitForSoundPose();
00628   }
00629   
00635   void CGuiController::zoomInPressed(QPoint p)
00636   {
00637     if ( ! map_initialized_ )
00638     {
00639       return;
00640     }
00641     map_connector_.updateZoom(p,true);
00642   }
00643 
00649   void CGuiController::zoomOutPressed(QPoint p)
00650   {
00651     if ( ! map_initialized_ )
00652     {
00653       return;
00654     }
00655     map_connector_.updateZoom(p,false);
00656   }
00657 
00663   void CGuiController::cleanupVisualizers(
00664     const stdr_msgs::RobotIndexedVectorMsg& msg)
00665   {
00666     std::set<QString> newSensors, erasedSensorsL, erasedSensorsS;
00667     for( unsigned int r = 0 ; r < msg.robots.size() ; r++ )
00668     {
00669       QString baseName = QString(msg.robots[r].name.c_str());
00670       for(unsigned int l = 0; 
00671         l < msg.robots[r].robot.laserSensors.size() ; l++)
00672       {
00673         QString fullName = baseName + QString("/") + 
00674           QString(
00675             msg.robots[r].robot.laserSensors[l].frame_id.c_str() );
00676         newSensors.insert(fullName);
00677       }
00678       for(unsigned int s = 0;
00679         s < msg.robots[r].robot.sonarSensors.size() ; s++ )
00680       {
00681         QString fullName = baseName + QString("/") + 
00682           QString(
00683             msg.robots[r].robot.sonarSensors[s].frame_id.c_str());
00684         newSensors.insert(fullName);
00685       }
00686     }
00687     
00688     for(LaserVisIterator it = laser_visualizers_.begin() ; 
00689       it != laser_visualizers_.end() ; it++ )
00690     {
00691       if( newSensors.find(it->first) == newSensors.end() )
00692       {
00693         erasedSensorsL.insert(it->first);
00694       }
00695     }
00696     for(SonarVisIterator it = sonar_visualizers_.begin() ; 
00697       it != sonar_visualizers_.end() ; it++)
00698     {
00699       if(newSensors.find(it->first) == newSensors.end())
00700       {
00701         erasedSensorsS.insert(it->first);
00702       }
00703     }
00704     
00705     for(std::set<QString>::iterator it = erasedSensorsL.begin() ; 
00706       it != erasedSensorsL.end() ; it++)
00707     {
00708       laser_visualizers_[*it]->destruct();
00709       laser_visualizers_.erase(*it);
00710     }
00711     for(std::set<QString>::iterator it = erasedSensorsS.begin() ; 
00712       it != erasedSensorsS.end() ; it++)
00713     {
00714       sonar_visualizers_[*it]->destruct();
00715       sonar_visualizers_.erase(*it);
00716     }
00717       
00718   }
00719   
00725   void CGuiController::receiveRobots(
00726     const stdr_msgs::RobotIndexedVectorMsg& msg)
00727   {
00728     if ( ! map_initialized_ )
00729     {
00730       return;
00731     }
00732     while(map_lock_)    
00733     {
00734       usleep(100);
00735     }
00736     map_lock_ = true;
00737     
00738     cleanupVisualizers(msg);
00739     
00740     registered_robots_.clear();
00741     all_robots_ = msg;
00742     for(unsigned int i = 0 ; i < msg.robots.size() ; i++)
00743     {
00744       registered_robots_.push_back(CGuiRobot(msg.robots[i]));
00745     }
00746     info_connector_.updateTree(msg);
00747     
00748     for(unsigned int i = 0 ; i < registered_robots_.size() ; i++)
00749     {
00750       registered_robots_[i].setEnvironmentalTags(rfid_tag_pure_);
00751       registered_robots_[i].setEnvironmentalCO2Sources(co2_source_pure_);
00752       registered_robots_[i].setEnvironmentalThermalSources(thermal_source_pure_);
00753       registered_robots_[i].setEnvironmentalSoundSources(sound_source_pure_);
00754     }
00755     map_lock_ = false;
00756   }
00757   
00763   void CGuiController::robotPlaceSet(QPoint p)
00764   {
00765     if ( ! map_initialized_ )
00766     {
00767       return;
00768     }
00769     while(map_lock_)
00770     { 
00771       usleep(100);
00772     }
00773     map_lock_ = true;
00774     
00775     QPoint pnew = map_connector_.getGlobalPoint(p);
00776     gui_connector_.robotCreatorConn.setInitialPose(
00777       pnew.x() * map_msg_.info.resolution,
00778       pnew.y() * map_msg_.info.resolution);
00779       
00780     stdr_msgs::RobotIndexedMsg newRobot;
00781     try 
00782     {
00783       newRobot = robot_handler_.spawnNewRobot(
00784         stdr_gui_tools::fixRobotAnglesToRad(
00785           gui_connector_.robotCreatorConn.getNewRobot()));
00786           
00787       my_robots_.insert(newRobot.name);  
00788     }
00789     catch (stdr_robot::ConnectionException& ex) 
00790     {
00791       map_lock_ = false;
00792       gui_connector_.raiseMessage("STDR Robot Spawn - Error", ex.what());
00793     }
00794     catch (stdr_robot::DoubleFrameIdException& ex) 
00795     {
00796       map_lock_ = false;
00797       gui_connector_.raiseMessage("STDR Robot Spawn - Error", ex.what());
00798     }
00799     map_lock_ = false;
00800   }
00801   
00808   void CGuiController::rfidPlaceSet(QPoint p)
00809   {
00810     if ( ! map_initialized_ )
00811     {
00812       return;
00813     }
00814     
00815     QPoint pnew = map_connector_.getGlobalPoint(p);
00816     QString name=QString("rfid_tag_") + QString().setNum(rfid_tags_.size());
00817     
00818     bool ok;
00819     stdr_msgs::RfidTag new_tag;
00820     
00822     QString rfid_id = QInputDialog::getText(
00823       &(info_connector_.loader), tr("QInputDialog::getText()"),
00824       tr("RFID tag id:"), QLineEdit::Normal,
00825       "", &ok);
00826     if ( ok && !rfid_id.isEmpty() ) {
00827         new_tag.tag_id = rfid_id.toStdString();
00828     }
00830     QString rfid_message = QInputDialog::getText(
00831       &(info_connector_.loader), tr("QInputDialog::getText()"),
00832       tr("RFID tag message (optional):"), QLineEdit::Normal,
00833       "", &ok);
00834     if ( ok && !rfid_message.isEmpty() ) {
00835         new_tag.message = rfid_message.toStdString();
00836     }
00837     
00838     new_tag.pose.x = pnew.x() * map_msg_.info.resolution ;
00839     new_tag.pose.y = pnew.y() * map_msg_.info.resolution ;
00840     new_tag.pose.theta = 0;
00841     
00842     stdr_msgs::AddRfidTag srv;
00843     srv.request.newTag = new_tag;
00844     if (new_rfid_tag_client_.call(srv))
00845     {
00846       gui_connector_.raiseMessage(
00847         "STDR robot - Error", QString(srv.response.message.c_str()));
00848     }
00849     
00850   }
00851   
00858   void CGuiController::co2PlaceSet(QPoint p)
00859   {
00860     if ( ! map_initialized_ )
00861     {
00862       return;
00863     }
00864     
00865     QPoint pnew = map_connector_.getGlobalPoint(p);
00866     QString name=QString("co2_source_") + QString().setNum(co2_sources_.size());
00867     
00868     bool ok;
00869     stdr_msgs::CO2Source new_source;
00870     
00872     QString id = QInputDialog::getText(
00873       &(info_connector_.loader), tr("QInputDialog::getText()"),
00874       tr("CO2 Source id:"), QLineEdit::Normal,
00875       "", &ok);
00876     if ( ok && !id.isEmpty() ) {
00877         new_source.id = id.toStdString();
00878     }
00880     QString ppms = QInputDialog::getText(
00881       &(info_connector_.loader), tr("QInputDialog::getText()"),
00882       tr("PPMs (Part Per Million):"), QLineEdit::Normal,
00883       "", &ok);
00884     if ( ok && !ppms.isEmpty() ) {
00885         new_source.ppm = ppms.toFloat();
00886     }
00887     
00888     new_source.pose.x = pnew.x() * map_msg_.info.resolution ;
00889     new_source.pose.y = pnew.y() * map_msg_.info.resolution ;
00890     new_source.pose.theta = 0;
00891     
00892     stdr_msgs::AddCO2Source srv;
00893     srv.request.newSource = new_source;
00894     if (new_co2_source_client_.call(srv))
00895     {
00896       gui_connector_.raiseMessage(
00897         "STDR robot - Error", QString(srv.response.message.c_str()));
00898     }
00899   }
00900   
00907   void CGuiController::thermalPlaceSet(QPoint p)
00908   {
00909     if ( ! map_initialized_ )
00910     {
00911       return;
00912     }
00913     
00914     QPoint pnew = map_connector_.getGlobalPoint(p);
00915     QString name=QString("thermal_source_") + 
00916       QString().setNum(thermal_sources_.size());
00917     
00918     bool ok;
00919     stdr_msgs::ThermalSource new_source;
00920     
00922     QString id = QInputDialog::getText(
00923       &(info_connector_.loader), tr("QInputDialog::getText()"),
00924       tr("Thermal Source id:"), QLineEdit::Normal,
00925       "", &ok);
00926     if ( ok && !id.isEmpty() ) {
00927         new_source.id = id.toStdString();
00928     }
00930     QString degrees = QInputDialog::getText(
00931       &(info_connector_.loader), tr("QInputDialog::getText()"),
00932       tr("Degrees:"), QLineEdit::Normal,
00933       "", &ok);
00934     if ( ok && !degrees.isEmpty() ) {
00935         new_source.degrees = degrees.toFloat();
00936     }
00937     
00938     new_source.pose.x = pnew.x() * map_msg_.info.resolution ;
00939     new_source.pose.y = pnew.y() * map_msg_.info.resolution ;
00940     new_source.pose.theta = 0;
00941     
00942     stdr_msgs::AddThermalSource srv;
00943     srv.request.newSource = new_source;
00944     if (new_thermal_source_client_.call(srv))
00945     {
00946       gui_connector_.raiseMessage(
00947         "STDR robot - Error", QString(srv.response.message.c_str()));
00948     }
00949   }
00950   
00957   void CGuiController::soundPlaceSet(QPoint p)
00958   {
00959     if ( ! map_initialized_ )
00960     {
00961       return;
00962     }
00963     
00964     QPoint pnew = map_connector_.getGlobalPoint(p);
00965     QString name=QString("sound_source_") + 
00966       QString().setNum(sound_sources_.size());
00967     
00968     bool ok;
00969     stdr_msgs::SoundSource new_source;
00970     
00972     QString id = QInputDialog::getText(
00973       &(info_connector_.loader), tr("QInputDialog::getText()"),
00974       tr("Sound Source id:"), QLineEdit::Normal,
00975       "", &ok);
00976     if ( ok && !id.isEmpty() ) {
00977         new_source.id = id.toStdString();
00978     }
00980     QString dbs = QInputDialog::getText(
00981       &(info_connector_.loader), tr("QInputDialog::getText()"),
00982       tr("DBs (Decibels):"), QLineEdit::Normal,
00983       "", &ok);
00984     if ( ok && !dbs.isEmpty() ) {
00985         new_source.dbs = dbs.toFloat();
00986     }
00987     
00988     new_source.pose.x = pnew.x() * map_msg_.info.resolution ;
00989     new_source.pose.y = pnew.y() * map_msg_.info.resolution ;
00990     new_source.pose.theta = 0;
00991     
00992     stdr_msgs::AddSoundSource srv;
00993     srv.request.newSource = new_source;
00994     if (new_sound_source_client_.call(srv))
00995     {
00996       gui_connector_.raiseMessage(
00997         "STDR robot - Error", QString(srv.response.message.c_str()));
00998     }
00999   }
01000   
01005   void CGuiController::updateMapInternal(void)
01006   {
01007     while(map_lock_)
01008     {
01009       usleep(100);
01010     }
01011     map_lock_ = true;
01012     running_map_ = initial_map_;
01013     
01014     if(gui_connector_.isGridEnabled())
01015       map_connector_.drawGrid(&running_map_, map_msg_.info.resolution);
01016     
01017     for(unsigned int i = 0 ; i < registered_robots_.size() ; i++)
01018     {
01019       registered_robots_[i].draw(
01020         &running_map_,map_msg_.info.resolution,&listener_);
01021     }
01022     running_map_ = running_map_.mirrored(false,true);
01023     for(unsigned int i = 0 ; i < registered_robots_.size() ; i++)
01024     {
01025       if(registered_robots_[i].getShowLabel())
01026         registered_robots_[i].drawLabel(
01027           &running_map_,map_msg_.info.resolution);
01028     }
01029     
01030     for(unsigned int i = 0 ; i < registered_robots_.size() ; i++)
01031     {
01032       if(registered_robots_[i].getFrameId() == robot_following_)
01033       {
01034         map_connector_.updateCenter(
01035           registered_robots_[i].getCurrentPose());
01036       }
01037     }
01038 
01039     for(RfidTagIterator it = rfid_tags_.begin() ; 
01040       it != rfid_tags_.end() ; it++)
01041     {
01042       it->second.draw(&running_map_);
01043     }
01044     
01045     for(Co2SourcesIterator it = co2_sources_.begin() ; 
01046       it != co2_sources_.end() ; it++)
01047     {
01048       it->second.draw(&running_map_);
01049     }
01050     
01051     for(SoundSourcesIterator it = sound_sources_.begin() ; 
01052       it != sound_sources_.end() ; it++)
01053     {
01054       it->second.draw(&running_map_);
01055     }
01056     
01057     for(ThermalSourcesIterator it = thermal_sources_.begin() ; 
01058       it != thermal_sources_.end() ; it++)
01059     {
01060       it->second.draw(&running_map_);
01061     }
01062     
01063     map_connector_.updateImage(&running_map_);
01064     
01065     gui_connector_.setStatusBarMessage(
01066       QString("Time elapsed : ") + 
01067       stdr_gui_tools::getLiteralTime(elapsed_time_.elapsed()));
01068     map_lock_ = false;
01069   
01071     std::vector<QString> toBeErased;
01072     for(LaserVisIterator it = laser_visualizers_.begin() ; 
01073       it != laser_visualizers_.end() ; it++)
01074     {
01075       if( ! it->second->getActive())
01076       {
01077         toBeErased.push_back(it->first);
01078       }
01079       else
01080       {
01081         it->second->paint();
01082       }
01083     }
01084     for(unsigned int i = 0 ; i < toBeErased.size() ; i++)
01085     {
01086       laser_visualizers_.erase(toBeErased[i]);
01087     }
01088     toBeErased.clear();
01089     for(SonarVisIterator it = sonar_visualizers_.begin() ; 
01090       it != sonar_visualizers_.end() ; it++)
01091     {
01092       if( ! it->second->getActive())
01093       {
01094         toBeErased.push_back(it->first);
01095       }
01096       else
01097       {
01098         it->second->paint();
01099       }
01100     }
01101     for(unsigned int i = 0 ; i < toBeErased.size() ; i++)
01102     {
01103       sonar_visualizers_.erase(toBeErased[i]);
01104     }
01105     toBeErased.clear();
01106     for(RobotVisIterator it = robot_visualizers_.begin() ; 
01107       it != robot_visualizers_.end() ; 
01108       it++)
01109     {
01110       if( ! it->second->getActive())
01111       {
01112         toBeErased.push_back(it->first);
01113       }
01114       else
01115       {
01116         QString robotName = it->first;
01117         for(unsigned int r = 0 ; r < registered_robots_.size() ; r++)
01118         {
01119           if(registered_robots_[r].getFrameId() == robotName.toStdString())
01120           {
01121             it->second->setImage(
01122               registered_robots_[r].
01123                 getVisualization(map_msg_.info.resolution));
01124             it->second->setCurrentPose(registered_robots_[r].getCurrentPoseM());
01125             
01126             it->second->setCurrentSpeed(
01127               registered_robots_[r].getSpeeds());
01128             break;
01129           }
01130         }
01131       }
01132     }
01133     for(unsigned int i = 0 ; i < toBeErased.size() ; i++)
01134     {
01135       robot_visualizers_.erase(toBeErased[i]);
01136     }
01137     
01139     if(gui_connector_.closeTriggered())
01140     {
01141       QEvent *e = gui_connector_.getCloseEvent();
01142       
01143       this->exit();
01144       gui_connector_.shutdown();
01145     }
01146   }
01147   
01154   stdr_msgs::LaserSensorMsg CGuiController::getLaserDescription(
01155     QString robotName,
01156     QString laserName)
01157   {
01158     for(unsigned int i = 0 ; i < all_robots_.robots.size() ; i++)
01159     {
01160       if(all_robots_.robots[i].name == robotName.toStdString())
01161       {
01162         for(unsigned int j = 0 ; 
01163           j < all_robots_.robots[i].robot.laserSensors.size() ; 
01164           j++)
01165         {
01166           if(all_robots_.robots[i].robot.laserSensors[j].frame_id
01167                == laserName.toStdString())
01168           {
01169             return all_robots_.robots[i].robot.laserSensors[j];
01170           }
01171         }
01172       }
01173     }
01174     return stdr_msgs::LaserSensorMsg();
01175   }
01176   
01183   stdr_msgs::SonarSensorMsg CGuiController::getSonarDescription(
01184     QString robotName,
01185     QString sonarName)
01186   {
01187     for(unsigned int i = 0 ; i < all_robots_.robots.size() ; i++)
01188     {
01189       if(all_robots_.robots[i].name == robotName.toStdString())
01190       {
01191         for(unsigned int j = 0 ; 
01192           j < all_robots_.robots[i].robot.sonarSensors.size() ; 
01193           j++)
01194         {
01195           if(all_robots_.robots[i].robot.sonarSensors[j].frame_id
01196                == sonarName.toStdString())
01197           {
01198             return all_robots_.robots[i].robot.sonarSensors[j];
01199           }
01200         }
01201       }
01202     }   
01203     return stdr_msgs::SonarSensorMsg();
01204   }
01205   
01212   void CGuiController::laserVisualizerClicked(
01213     QString robotName,
01214     QString laserName)
01215   {
01216     QString name = robotName + QString("/") + laserName;
01217     if(laser_visualizers_.find(name) != laser_visualizers_.end())
01218     {
01219       return;
01220     }
01221     CLaserVisualisation *lv;
01222     lv = new CLaserVisualisation(name,map_msg_.info.resolution);
01223     laser_visualizers_.insert(
01224       std::pair<QString,CLaserVisualisation *>(name,lv));
01225     lv->setWindowFlags(Qt::WindowStaysOnTopHint);
01226 
01227     lv->setLaser(getLaserDescription(robotName,laserName));
01228     
01229     lv->show();
01230   }
01231   
01238   void CGuiController::sonarVisualizerClicked(
01239     QString robotName,
01240     QString sonarName)
01241   {
01242     QString name = robotName + QString("/") + sonarName;
01243     if(sonar_visualizers_.find(name) != sonar_visualizers_.end())
01244     {
01245       return;
01246     }
01247     CSonarVisualisation *sv;
01248     sv = new CSonarVisualisation(name,map_msg_.info.resolution);
01249     sonar_visualizers_.insert(
01250       std::pair<QString,CSonarVisualisation *>(name,sv));
01251     sv->setWindowFlags(Qt::WindowStaysOnTopHint);
01252 
01253     sv->setSonar(getSonarDescription(robotName,sonarName));
01254             
01255     sv->show();
01256   }
01257   
01263   void CGuiController::robotVisualizerClicked(QString robotName)
01264   {
01265     QString name = robotName;
01266     if(robot_visualizers_.find(name) != robot_visualizers_.end())
01267     {
01268       return;
01269     }
01270     CRobotVisualisation *sv;
01271     sv = new CRobotVisualisation(name,map_msg_.info.resolution);
01272     robot_visualizers_.insert(
01273       std::pair<QString,CRobotVisualisation *>(name,sv));
01274     sv->setWindowFlags(Qt::WindowStaysOnTopHint);
01275 
01276     sv->show();
01277   }
01278   
01285   void CGuiController::itemClicked(QPoint p,Qt::MouseButton b)
01286   {
01287     gui_connector_.uncheckZoomButtons();
01288     QPoint pointClicked = map_connector_.getGlobalPoint(p);
01289     for(unsigned int i = 0 ; i < registered_robots_.size() ; i++)
01290     {
01291       if(registered_robots_[i].checkEventProximity(pointClicked))
01292       {
01293         if(b == Qt::RightButton)
01294         {
01295           QMenu myMenu;
01296           
01297           QAction *nothing = myMenu.addAction(
01298             QString("Robot : ") + 
01299             QString(registered_robots_[i].getFrameId().c_str()));
01300           nothing->setCheckable(false);
01301           nothing->setEnabled(false);
01302           
01303           QAction *deleteRobot = myMenu.addAction(icon_delete_,"Delete robot");
01304           QAction *moveRobot = myMenu.addAction(icon_move_,"Move robot");
01305           myMenu.addSeparator();
01306           QAction *showCircle = myMenu.addAction("Show proximity circles");
01307           QAction *followRobot = myMenu.addAction("Follow robot");
01308           
01309           QAction* selectedItem = myMenu.exec(map_connector_.mapToGlobal(p));
01310           if(selectedItem == showCircle)
01311           {
01312             registered_robots_[i].toggleShowCircles();
01313           }
01314           else if(selectedItem == deleteRobot)
01315           {
01316             robot_handler_.deleteRobot(
01317               registered_robots_[i].getFrameId());
01318           }
01319           else if(selectedItem == moveRobot)
01320           {
01321             Q_EMIT replaceRobot(registered_robots_[i].getFrameId());
01322           }
01323           else if(selectedItem == followRobot)
01324           {
01325             if(robot_following_ == registered_robots_[i].getFrameId())
01326             {
01327               robot_following_ = "";
01328             }
01329             else
01330             {
01331               robot_following_ = registered_robots_[i].getFrameId();
01332             }
01333           }
01334         }
01335         else if(b == Qt::LeftButton)
01336         {
01337           registered_robots_[i].toggleShowLabel();
01338         }
01339       }
01340     }
01341     for(RfidTagIterator i = rfid_tags_.begin() ; i != rfid_tags_.end() ; i++)
01342     {
01343       if(i->second.checkProximity(pointClicked))
01344       {
01345         if(b == Qt::RightButton)
01346         {
01347           QMenu myMenu;
01348           
01349           QAction *name = myMenu.addAction(
01350             QString("RFID tag : ") + QString(i->first)
01351             );
01352           name->setCheckable(false);
01353           name->setEnabled(false);
01354           
01355           QAction *message = myMenu.addAction(
01356             QString("Message : ") + QString(i->second.getMessage())
01357             );
01358           message->setCheckable(false);
01359           message->setEnabled(false);
01360           
01361           QAction *deleteTag = myMenu.addAction(icon_delete_,"Delete RFID tag");
01362           
01363           QAction* selectedItem = myMenu.exec(map_connector_.mapToGlobal(p));
01364           if(selectedItem == deleteTag)
01365           {
01366             stdr_msgs::DeleteRfidTag srv;
01367             srv.request.name = i->first.toStdString();
01368             delete_rfid_tag_client_.call(srv);
01369             break; 
01370           }
01371         }
01372       }
01373     }
01374     for(Co2SourcesIterator i = co2_sources_.begin() ; 
01375       i != co2_sources_.end() ; i++)
01376     {
01377       if(i->second.checkProximity(pointClicked))
01378       {
01379         if(b == Qt::RightButton)
01380         {
01381           QMenu myMenu;
01382           
01383           QAction *name = myMenu.addAction(
01384             QString("CO2 Source : ") + QString(i->first)
01385             );
01386           name->setCheckable(false);
01387           name->setEnabled(false);
01388           
01389           QAction *message = myMenu.addAction(
01390             QString("PPMs : ") + QString().setNum(i->second.getPpm())
01391             );
01392           message->setCheckable(false);
01393           message->setEnabled(false);
01394           
01395           QAction *deleteSource = myMenu.addAction(icon_delete_,
01396             "Delete CO2 Source");
01397           
01398           QAction* selectedItem = myMenu.exec(map_connector_.mapToGlobal(p));
01399           if(selectedItem == deleteSource)
01400           {
01401             stdr_msgs::DeleteCO2Source srv;
01402             srv.request.name = i->first.toStdString();
01403             delete_co2_source_client_.call(srv);
01404             break; 
01405           }
01406         }
01407       }
01408     }
01409     for(ThermalSourcesIterator i = thermal_sources_.begin() ; 
01410       i != thermal_sources_.end() ; i++)
01411     {
01412       if(i->second.checkProximity(pointClicked))
01413       {
01414         if(b == Qt::RightButton)
01415         {
01416           QMenu myMenu;
01417           
01418           QAction *name = myMenu.addAction(
01419             QString("Thermal Source : ") + QString(i->first)
01420             );
01421           name->setCheckable(false);
01422           name->setEnabled(false);
01423           
01424           QAction *message = myMenu.addAction(
01425             QString("Degrees : ") + QString().setNum(i->second.getDegrees())
01426             );
01427           message->setCheckable(false);
01428           message->setEnabled(false);
01429           
01430           QAction *deleteSource = myMenu.addAction(icon_delete_,
01431             "Delete Thermal Source");
01432           
01433           QAction* selectedItem = myMenu.exec(map_connector_.mapToGlobal(p));
01434           if(selectedItem == deleteSource)
01435           {
01436             stdr_msgs::DeleteThermalSource srv;
01437             srv.request.name = i->first.toStdString();
01438             delete_thermal_source_client_.call(srv);
01439             break; 
01440           }
01441         }
01442       }
01443     }
01444     for(SoundSourcesIterator i = sound_sources_.begin() ; 
01445       i != sound_sources_.end() ; i++)
01446     {
01447       if(i->second.checkProximity(pointClicked))
01448       {
01449         if(b == Qt::RightButton)
01450         {
01451           QMenu myMenu;
01452           
01453           QAction *name = myMenu.addAction(
01454             QString("Sound Source : ") + QString(i->first)
01455             );
01456           name->setCheckable(false);
01457           name->setEnabled(false);
01458           
01459           QAction *message = myMenu.addAction(
01460             QString("DBs : ") + QString().setNum(i->second.getDb())
01461             );
01462           message->setCheckable(false);
01463           message->setEnabled(false);
01464           
01465           QAction *deleteSource = myMenu.addAction(icon_delete_,
01466             "Delete Sound Source");
01467           
01468           QAction* selectedItem = myMenu.exec(map_connector_.mapToGlobal(p));
01469           if(selectedItem == deleteSource)
01470           {
01471             stdr_msgs::DeleteSoundSource srv;
01472             srv.request.name = i->first.toStdString();
01473             delete_sound_source_client_.call(srv);
01474             break; 
01475           }
01476         }
01477       }
01478     }
01479   }
01480   
01487   void CGuiController::robotReplaceSet(QPoint p,std::string robotName)
01488   {
01489     if ( ! map_initialized_ )
01490     {
01491       return;
01492     }
01493     QPoint pnew = map_connector_.getGlobalPoint(p);
01494     
01495     geometry_msgs::Pose2D newPose;
01496     newPose.x = pnew.x() * map_msg_.info.resolution;
01497     newPose.y = pnew.y() * map_msg_.info.resolution;
01498     
01499     for(unsigned int i = 0 ; i < registered_robots_.size() ; i++)
01500     {
01501       if(registered_robots_[i].getFrameId() == robotName)
01502       {
01503         newPose.theta = registered_robots_[i].getCurrentPoseM().theta;
01504         break;  
01505       }
01506     }
01507     
01508     bool success = robot_handler_.moveRobot(robotName,newPose);
01509     if(!success)
01510     {
01511       gui_connector_.raiseMessage(
01512         "STDR robot - Error", "Unable to relocate the robot");
01513     }
01514   }
01515   
01522   void CGuiController::laserVisibilityClicked(
01523     QString robotName,QString laserName)
01524   {
01525     for(unsigned int i = 0 ; i < registered_robots_.size() ; i++)
01526     {
01527       if(registered_robots_[i].getFrameId() == robotName.toStdString())
01528       {
01529         char vs = registered_robots_[i].getLaserVisualizationStatus(
01530           laserName.toStdString());
01531         Q_EMIT setLaserVisibility(robotName,laserName,(vs + 1) % 3);
01532         registered_robots_[i].toggleLaserVisualizationStatus(
01533           laserName.toStdString());
01534         break;
01535       }
01536     }
01537   }
01538   
01545   void CGuiController::sonarVisibilityClicked(
01546     QString robotName,QString sonarName)
01547   {
01548     for(unsigned int i = 0 ; i < registered_robots_.size() ; i++)
01549     {
01550       if(registered_robots_[i].getFrameId() == robotName.toStdString())
01551       {
01552         char vs = registered_robots_[i].getSonarVisualizationStatus(
01553           sonarName.toStdString());
01554         Q_EMIT setSonarVisibility(robotName,sonarName,(vs + 1) % 3);
01555         registered_robots_[i].toggleSonarVisualizationStatus(
01556           sonarName.toStdString());
01557         break;
01558       }
01559     }
01560   }
01561   
01567   void CGuiController::rfidReaderVisibilityClicked
01568     (QString robotName,QString rfidReaderName)
01569   {
01570     for(unsigned int i = 0 ; i < registered_robots_.size() ; i++)
01571     {
01572       if(registered_robots_[i].getFrameId() == robotName.toStdString())
01573       {
01574         char vs = registered_robots_[i].getRfidReaderVisualizationStatus(
01575           rfidReaderName.toStdString());
01576         Q_EMIT setRfidReaderVisibility(robotName,rfidReaderName,(vs + 1) % 3);
01577         registered_robots_[i].toggleRfidReaderVisualizationStatus(
01578           rfidReaderName.toStdString());
01579         break;
01580       }
01581     }
01582   }
01588   void CGuiController::co2SensorVisibilityClicked
01589     (QString robotName,QString co2SensorName)
01590   {
01591     for(unsigned int i = 0 ; i < registered_robots_.size() ; i++)
01592     {
01593       if(registered_robots_[i].getFrameId() == robotName.toStdString())
01594       {
01595         char vs = registered_robots_[i].getCO2SensorVisualizationStatus(
01596           co2SensorName.toStdString());
01597         Q_EMIT setCO2SensorVisibility(robotName, co2SensorName, (vs + 1) % 3);
01598         registered_robots_[i].toggleCO2SensorVisualizationStatus(
01599           co2SensorName.toStdString());
01600         break;
01601       }
01602     }
01603   }
01609   void CGuiController::thermalSensorVisibilityClicked
01610     (QString robotName,QString thermalSensorName)
01611   {
01612     for(unsigned int i = 0 ; i < registered_robots_.size() ; i++)
01613     {
01614       if(registered_robots_[i].getFrameId() == robotName.toStdString())
01615       {
01616         char vs = registered_robots_[i].getThermalSensorVisualizationStatus(
01617           thermalSensorName.toStdString());
01618         Q_EMIT setThermalSensorVisibility(robotName, 
01619           thermalSensorName, (vs + 1) % 3);
01620         registered_robots_[i].toggleThermalSensorVisualizationStatus(
01621           thermalSensorName.toStdString());
01622         break;
01623       }
01624     }
01625   }
01631   void CGuiController::soundSensorVisibilityClicked
01632     (QString robotName,QString soundSensorName)
01633   {
01634     for(unsigned int i = 0 ; i < registered_robots_.size() ; i++)
01635     {
01636       if(registered_robots_[i].getFrameId() == robotName.toStdString())
01637       {
01638         char vs = registered_robots_[i].getSoundSensorVisualizationStatus(
01639           soundSensorName.toStdString());
01640         Q_EMIT setSoundSensorVisibility(robotName, 
01641           soundSensorName, (vs + 1) % 3);
01642         registered_robots_[i].toggleSoundSensorVisualizationStatus(
01643           soundSensorName.toStdString());
01644         break;
01645       }
01646     }
01647   }
01648   
01654   void CGuiController::robotVisibilityClicked(QString robotName)
01655   {
01656     for(unsigned int i = 0 ; i < registered_robots_.size() ; i++)
01657     {
01658       if(registered_robots_[i].getFrameId() == robotName.toStdString())
01659       {
01660         char vs = registered_robots_[i].getVisualizationStatus();
01661         Q_EMIT setRobotVisibility(robotName,(vs + 1) % 3);
01662         registered_robots_[i].toggleVisualizationStatus();
01663       }
01664     }
01665   }
01666 }
01667 
01668 


stdr_gui
Author(s): Manos Tsardoulias
autogenerated on Wed Sep 2 2015 03:36:44