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


stdr_gui
Author(s): Manos Tsardoulias
autogenerated on Tue Feb 7 2017 03:46:43