stdr_robot_creator_connector.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_robot_creator/stdr_robot_creator_connector.h"
00023 
00024 namespace stdr_gui
00025 {
00026   
00027   int CRobotCreatorConnector::laser_number = -1;
00028   int CRobotCreatorConnector::sonar_number = -1;
00029   int CRobotCreatorConnector::rfid_number = -1;
00030   int CRobotCreatorConnector::co2_sensors_number = -1;
00031   int CRobotCreatorConnector::thermal_sensors_number = -1;
00032   int CRobotCreatorConnector::sound_sensors_number = -1;
00033   
00040   CRobotCreatorConnector::CRobotCreatorConnector(int argc, char **argv):
00041     QObject(),
00042     loader_(argc,argv),
00043     argc_(argc),
00044     argv_(argv)
00045   {
00046     QObject::connect(
00047       loader_.robotTreeWidget,SIGNAL(itemClicked(QTreeWidgetItem *, int)),
00048       this,SLOT(treeItemClicked(QTreeWidgetItem *, int)));
00049       
00050     QObject::connect(
00051       loader_.laserPropLoader.laserUpdateButton,SIGNAL(clicked(bool)),
00052       this,SLOT(updateLaser()));
00053     QObject::connect(
00054       loader_.laserPropLoader.refresh_laser,SIGNAL(clicked(bool)),
00055       this,SLOT(updateLaserOpen()));
00056     
00057     QObject::connect(
00058       loader_.robotPropLoader.updateButton,SIGNAL(clicked(bool)),
00059       this,SLOT(updateRobot()));
00060     QObject::connect(
00061       loader_.robotPropLoader.refresh_robot,SIGNAL(clicked(bool)),
00062       this,SLOT(updateRobotOpen()));
00063     
00064     QObject::connect(
00065       loader_.sonarPropLoader.pushButton,SIGNAL(clicked(bool)),
00066       this,SLOT(updateSonar()));
00067     QObject::connect(
00068       loader_.sonarPropLoader.refresh_sonar,SIGNAL(clicked(bool)),
00069       this,SLOT(updateSonarOpen()));
00070       
00071     QObject::connect(
00072       loader_.rfidAntennaPropLoader.pushButton,SIGNAL(clicked(bool)),
00073       this,SLOT(updateRfidAntenna()));
00074     QObject::connect(
00075       loader_.rfidAntennaPropLoader.refreshRfid,SIGNAL(clicked(bool)),
00076       this,SLOT(updateRfidAntennaOpen()));
00077       
00078     QObject::connect(
00079       loader_.co2SensorPropLoader.pushButton,SIGNAL(clicked(bool)),
00080       this,SLOT(updateCO2Sensor()));
00081     QObject::connect(
00082       loader_.co2SensorPropLoader.refreshButton,SIGNAL(clicked(bool)),
00083       this,SLOT(updateCO2SensorOpen()));
00084 
00085     QObject::connect(
00086       loader_.thermalSensorPropLoader.pushButton,SIGNAL(clicked(bool)),
00087       this,SLOT(updateThermalSensor()));
00088     QObject::connect(
00089       loader_.thermalSensorPropLoader.refreshButton,SIGNAL(clicked(bool)),
00090       this,SLOT(updateThermalSensorOpen()));
00091     
00092     QObject::connect(
00093       loader_.soundSensorPropLoader.pushButton,SIGNAL(clicked(bool)),
00094       this,SLOT(updateSoundSensor()));
00095     QObject::connect(
00096       loader_.soundSensorPropLoader.refreshButton,SIGNAL(clicked(bool)),
00097       this,SLOT(updateSoundSensorOpen()));
00098       
00099     QObject::connect(
00100       loader_.robotFootLoader.updateButton,SIGNAL(clicked(bool)),
00101       this,SLOT(updateFootprintPoint()));
00102     QObject::connect(
00103       loader_.robotFootLoader.refresh_robot,SIGNAL(clicked(bool)),
00104       this,SLOT(updateFootprintPointOpen()));
00105 
00106     QObject::connect(
00107       loader_.kinematicPropLoader.updateButton,SIGNAL(clicked(bool)),
00108       this,SLOT(updateKinematicModel()));
00109     
00110     QObject::connect(
00111       loader_.loadRobotButton,SIGNAL(clicked(bool)),
00112       this,SLOT(loadRobot()));
00113     
00114 
00115     climax_ = - 1;
00116     sonar_hightlight_id_ = -1;
00117     laser_hightlight_id_ = -1;
00118     rfid_antenna_hightlight_id_ = -1;
00119     co2_sensor_hightlight_id_ = -1;
00120     thermal_sensor_hightlight_id_ = -1;
00121     sound_sensor_hightlight_id_ = -1;
00122   }
00123   
00128   CRobotCreatorConnector::~CRobotCreatorConnector(void)
00129   {
00130     
00131   }
00132   
00137   void CRobotCreatorConnector::initialise(void)
00138   {
00139     new_robot_msg_ = stdr_msgs::RobotMsg();
00140 
00141     loader_.robotInfoRadius.setText(0,"Radius");
00142     loader_.robotInfoRadius.setText(1,"0.15");
00143     loader_.robotInfoOrientation.setText(0,"Orientation");
00144     loader_.robotInfoOrientation.setText(1,"0");
00145     
00146     new_robot_msg_.footprint.radius = 0.15;
00147     
00148     unsigned int laserCount = loader_.lasersNode.childCount();
00149     unsigned int sonarCount = loader_.sonarsNode.childCount();
00150     unsigned int rfidCount = loader_.rfidAntennasNode.childCount();
00151     unsigned int co2SensorsCount = loader_.co2SensorsNode.childCount();
00152     unsigned int thermalSensorsCount = loader_.thermalSensorsNode.childCount();
00153     unsigned int soundSensorsCount = loader_.soundSensorsNode.childCount();
00154     unsigned int footprintCount = loader_.robotInfoFootprint.childCount();
00155     
00156     for(int i = laserCount - 1 ; i >= 0 ; i--)
00157       deleteTreeNode(loader_.lasersNode.child(i));
00158     for(int i = sonarCount - 1 ; i >= 0 ; i--)
00159       deleteTreeNode(loader_.sonarsNode.child(i));
00160     for(int i = rfidCount - 1 ; i >= 0 ; i--)
00161       deleteTreeNode(loader_.rfidAntennasNode.child(i));
00162     for(int i = co2SensorsCount - 1 ; i >= 0 ; i--)
00163       deleteTreeNode(loader_.co2SensorsNode.child(i));
00164     for(int i = thermalSensorsCount - 1 ; i >= 0 ; i--)
00165       deleteTreeNode(loader_.thermalSensorsNode.child(i));
00166     for(int i = soundSensorsCount - 1 ; i >= 0 ; i--)
00167       deleteTreeNode(loader_.soundSensorsNode.child(i));
00168     for(int i = footprintCount - 1 ; i >= 0 ; i--)
00169       deleteTreeNode(loader_.robotInfoFootprint.child(i));
00170     
00171     CRobotCreatorConnector::laser_number = 0;
00172     CRobotCreatorConnector::sonar_number = 0;
00173     CRobotCreatorConnector::rfid_number = 0;
00174     CRobotCreatorConnector::co2_sensors_number = 0;
00175     CRobotCreatorConnector::thermal_sensors_number = 0;
00176     CRobotCreatorConnector::sound_sensors_number = 0;
00177     
00178     updateRobotPreview();
00179     
00180     loader_.show();
00181   }
00182 
00189   void CRobotCreatorConnector::treeItemClicked( 
00190     QTreeWidgetItem * item, 
00191     int column)
00192   {
00193     
00195     if(item->parent() == &loader_.lasersNode)
00196     {
00197       unsigned int laserFrameId = searchLaser(item->text(0));
00198       if(laserFrameId == -1) 
00199       {
00200         ROS_ERROR("Something went terribly wrong...");
00201       }
00202       laser_hightlight_id_ = laserFrameId;
00203       sonar_hightlight_id_ = -1;
00204       rfid_antenna_hightlight_id_ = -1;
00205       co2_sensor_hightlight_id_ = -1;
00206       thermal_sensor_hightlight_id_ = -1;
00207       sound_sensor_hightlight_id_ = -1;
00208     }  
00210     if(item->parent() == &loader_.sonarsNode)
00211     {
00212       unsigned int sonarFrameId = searchSonar(item->text(0));
00213       if(sonarFrameId == -1) 
00214       {
00215         ROS_ERROR("Something went terribly wrong...");
00216       }
00217       sonar_hightlight_id_ = sonarFrameId;
00218       laser_hightlight_id_ = -1;
00219       rfid_antenna_hightlight_id_ = -1;
00220       co2_sensor_hightlight_id_ = -1;
00221       thermal_sensor_hightlight_id_ = -1;
00222       sound_sensor_hightlight_id_ = -1;
00223     }  
00225     if(item->parent() == &loader_.rfidAntennasNode)
00226     {
00227       unsigned int frameId = searchRfid(item->text(0));
00228       if(frameId == -1) 
00229       {
00230         ROS_ERROR("Something went terribly wrong...");
00231       }
00232       rfid_antenna_hightlight_id_ = frameId;
00233       laser_hightlight_id_ = -1;
00234       sonar_hightlight_id_ = -1;
00235       co2_sensor_hightlight_id_ = -1;
00236       thermal_sensor_hightlight_id_ = -1;
00237       sound_sensor_hightlight_id_ = -1;
00238     }  
00240     if(item->parent() == &loader_.co2SensorsNode)
00241     {
00242       unsigned int frameId = searchCO2Sensor(item->text(0));
00243       if(frameId == -1) 
00244       {
00245         ROS_ERROR("Something went terribly wrong...");
00246       }
00247       rfid_antenna_hightlight_id_ = -1;
00248       laser_hightlight_id_ = -1;
00249       sonar_hightlight_id_ = -1;
00250       co2_sensor_hightlight_id_ = frameId;
00251       thermal_sensor_hightlight_id_ = -1;
00252       sound_sensor_hightlight_id_ = -1;
00253     }  
00255     if(item->parent() == &loader_.thermalSensorsNode)
00256     {
00257       unsigned int frameId = searchThermalSensor(item->text(0));
00258       if(frameId == -1) 
00259       {
00260         ROS_ERROR("Something went terribly wrong...");
00261       }
00262       rfid_antenna_hightlight_id_ = -1;
00263       laser_hightlight_id_ = -1;
00264       sonar_hightlight_id_ = -1;
00265       co2_sensor_hightlight_id_ = -1;
00266       thermal_sensor_hightlight_id_ = frameId;
00267       sound_sensor_hightlight_id_ = -1;
00268     }  
00270     if(item->parent() == &loader_.soundSensorsNode)
00271     {
00272       unsigned int frameId = searchSoundSensor(item->text(0));
00273       if(frameId == -1) 
00274       {
00275         ROS_ERROR("Something went terribly wrong...");
00276       }
00277       rfid_antenna_hightlight_id_ = -1;
00278       laser_hightlight_id_ = -1;
00279       sonar_hightlight_id_ = -1;
00280       co2_sensor_hightlight_id_ = -1;
00281       thermal_sensor_hightlight_id_ = -1;
00282       sound_sensor_hightlight_id_ = frameId;
00283     }  
00284     
00285     updateRobotPreview();
00286     
00288     if(item == &loader_.robotNode && column == 2)
00289     {    
00290       editRobot();
00291     }  
00293     if(item == &loader_.robotNode && column == 3)
00294     {    
00295       saveRobot();
00296     }  
00298     if(item == &loader_.robotNode && column == 4)
00299     {    
00300       getRobotFromYaml();
00301     }  
00303     if(item == &loader_.kinematicNode && column == 2)
00304     {  
00305       loader_.kinematicPropLoader.show();
00306     }
00308     if(item == &loader_.lasersNode && column == 2)
00309     {
00310       addLaser();
00311     }  
00313     if(item->parent() == &loader_.lasersNode && column == 2)
00314     {
00315       eraseLaser(item);
00316     }  
00318     if(item->parent() == &loader_.lasersNode && column == 1)
00319     {
00320       editLaser(item);
00321     }  
00323     if(item == &loader_.sonarsNode && column == 2)
00324     {
00325       addSonar();
00326     }  
00328     if(item == &loader_.rfidAntennasNode && column == 2)
00329     {
00330       addRfidAntenna();
00331     }  
00333     if(item == &loader_.co2SensorsNode && column == 2)
00334     {
00335       addCO2Sensor();
00336     }  
00338     if(item == &loader_.thermalSensorsNode && column == 2)
00339     {
00340       addThermalSensor();
00341     }  
00343     if(item == &loader_.soundSensorsNode && column == 2)
00344     {
00345       addSoundSensor();
00346     }  
00348     if(item->parent() == &loader_.sonarsNode && column == 2)
00349     {
00350       eraseSonar(item);
00351     }  
00353     if(item->parent() == &loader_.rfidAntennasNode && column == 2)
00354     {
00355       eraseRfid(item);
00356     }  
00358     if(item->parent() == &loader_.co2SensorsNode && column == 2)
00359     {
00360       eraseCO2Sensor(item);
00361     }  
00363     if(item->parent() == &loader_.thermalSensorsNode && column == 2)
00364     {
00365       eraseThermalSensor(item);
00366     }  
00368     if(item->parent() == &loader_.soundSensorsNode && column == 2)
00369     {
00370       eraseSoundSensor(item);
00371     }  
00373     if(item->parent() == &loader_.sonarsNode && column == 1)
00374     {
00375       editSonar(item);
00376     }  
00378     if(item->parent() == &loader_.rfidAntennasNode && column == 1)
00379     {
00380       editRfid(item);
00381     }
00383     if(item->parent() == &loader_.co2SensorsNode && column == 1)
00384     {
00385       editCO2Sensor(item);
00386     }
00388     if(item->parent() == &loader_.thermalSensorsNode && column == 1)
00389     {
00390       editThermalSensor(item);
00391     }
00393     if(item->parent() == &loader_.soundSensorsNode && column == 1)
00394     {
00395       editSoundSensor(item);
00396     }
00398     if(item->parent() == &loader_.lasersNode && column == 3)
00399     {
00400       saveLaser(item);
00401     }
00403     if(item->parent() == &loader_.lasersNode && column == 4)
00404     {
00405       loadLaser(item);
00406     }
00408     if(item->parent() == &loader_.rfidAntennasNode && column == 3)
00409     {
00410       saveRfidAntenna(item);
00411     }
00413     if(item->parent() == &loader_.co2SensorsNode && column == 3)
00414     {
00415       saveCO2Sensor(item);
00416     }
00418     if(item->parent() == &loader_.thermalSensorsNode && column == 3)
00419     {
00420       saveThermalSensor(item);
00421     }
00423     if(item->parent() == &loader_.soundSensorsNode && column == 3)
00424     {
00425       saveSoundSensor(item);
00426     }
00428     if(item->parent() == &loader_.rfidAntennasNode && column == 4)
00429     {
00430       loadRfidAntenna(item);
00431     }
00433     if(item->parent() == &loader_.co2SensorsNode && column == 4)
00434     {
00435       loadCO2Sensor(item);
00436     }
00438     if(item->parent() == &loader_.thermalSensorsNode && column == 4)
00439     {
00440       loadThermalSensor(item);
00441     }
00443     if(item->parent() == &loader_.soundSensorsNode && column == 4)
00444     {
00445       loadSoundSensor(item);
00446     }
00448     if(item->parent() == &loader_.sonarsNode && column == 3)
00449     {
00450       saveSonar(item);
00451     }
00453     if(item->parent() == &loader_.sonarsNode && column == 4)
00454     {
00455       loadSonar(item);
00456     }
00458     if(item == &loader_.robotInfoFootprint && column == 2)
00459     {
00460       addFootprintPoint();
00461     }  
00463     if(item->parent() == &loader_.robotInfoFootprint && column == 2)
00464     {
00465       eraseFootprintPoint(item);
00466     } 
00468     if(item->parent() == &loader_.robotInfoFootprint && column == 1)
00469     {
00470       editFootprintPoint(item);
00471     }  
00472   }
00473 
00478   void CRobotCreatorConnector::addFootprintPoint(geometry_msgs::Point pt)
00479   {
00480     QTreeWidgetItem  *new_point;
00481       
00482     new_point = new QTreeWidgetItem();
00483     
00484     new_point->setText(0,QString("[") + QString().setNum(pt.x) + QString(",") + 
00485       QString().setNum(pt.y) + QString("]"));
00486     new_point->setIcon(1,loader_.editIcon);
00487     new_point->setIcon(2,loader_.removeIcon);
00488     
00489     loader_.robotInfoFootprint.addChild(new_point);
00490 
00491     loader_.robotInfoFootprint.setExpanded(true);
00492     updateRobotPreview();
00493   }
00494 
00499   void CRobotCreatorConnector::addFootprintPoint(void)
00500   {
00501     geometry_msgs::Point pt;
00502     pt.x = 0;
00503     pt.y = 0;
00504     
00505     new_robot_msg_.footprint.points.push_back(pt);
00506 
00507     QTreeWidgetItem  *new_point;
00508       
00509     new_point = new QTreeWidgetItem();
00510     
00511     new_point->setText(0,QString("[0,0]"));
00512     new_point->setIcon(1,loader_.editIcon);
00513     new_point->setIcon(2,loader_.removeIcon);
00514     
00515     loader_.robotInfoFootprint.addChild(new_point);
00516 
00517     loader_.robotInfoFootprint.setExpanded(true);
00518     updateRobotPreview();
00519   }
00520   
00525   void CRobotCreatorConnector::eraseFootprintPoint(QTreeWidgetItem *item)
00526   {
00527     
00528     for(unsigned int i = 0 ; i < loader_.robotInfoFootprint.childCount() ; i++)
00529     {
00530       if(loader_.robotInfoFootprint.child(i) == item)
00531       {
00532         delete item;
00533         new_robot_msg_.footprint.points.erase(
00534           new_robot_msg_.footprint.points.begin() + i);
00535       }
00536     }
00537     updateRobotPreview();
00538   }
00539   
00544   void CRobotCreatorConnector::updateFootprintPoint(void)
00545   {
00546     QString xstr = loader_.robotFootLoader.robotFootprintX->text();
00547     QString ystr = loader_.robotFootLoader.robotFootprintY->text();
00548     float x = xstr.toFloat();
00549     float y = ystr.toFloat();
00550     
00551     int index = -1;
00552     for(unsigned int i = 0 ; i < loader_.robotInfoFootprint.childCount() ; i++)
00553     {
00554       if(loader_.robotInfoFootprint.child(i) == current_footprint_point_)
00555       {
00556         index = i;
00557         break;
00558       }
00559     }
00560     if( index == -1 )
00561     {
00562       return;
00563     }
00564     
00565     new_robot_msg_.footprint.points[index].x = x;
00566     new_robot_msg_.footprint.points[index].y = y;
00567     
00568     current_footprint_point_->setText(0,QString("[") + xstr + QString(",") +
00569       ystr + QString("]"));
00570 
00571     loader_.robotFootLoader.hide();
00572     
00573     updateRobotPreview();
00574   }
00575 
00580   void CRobotCreatorConnector::updateKinematicModel(void)
00581   {
00582     QString model_lit = loader_.kinematicPropLoader.comboBox->currentText();
00583     int model_ind = loader_.kinematicPropLoader.comboBox->currentIndex();
00584    
00585     switch(model_ind){
00586       case 0:
00587         new_robot_msg_.kinematicModel.type = "ideal";
00588         break;
00589       case 1:
00590         new_robot_msg_.kinematicModel.type = "omni";
00591         break;
00592     }
00593 
00594     loader_.kinematicPropLoader.hide();
00595     loader_.kinematicNode.setText(0,"Kinematic model: " \
00596       + QString(new_robot_msg_.kinematicModel.type.c_str()));
00597 
00598     
00599     updateRobotPreview();
00600   }
00601 
00606   void CRobotCreatorConnector::updateFootprintPointOpen(void)
00607   {
00608     QString xstr = loader_.robotFootLoader.robotFootprintX->text();
00609     QString ystr = loader_.robotFootLoader.robotFootprintY->text();
00610     float x = xstr.toFloat();
00611     float y = ystr.toFloat();
00612 
00613     int index = -1;
00614     for(unsigned int i = 0 ; i < loader_.robotInfoFootprint.childCount() ; i++)
00615     {
00616       if(loader_.robotInfoFootprint.child(i) == current_footprint_point_)
00617       {
00618         index = i;
00619         break;
00620       }
00621     }
00622     if( index == -1 )
00623     {
00624       return;
00625     }
00626     
00627     new_robot_msg_.footprint.points[index].x = x;
00628     new_robot_msg_.footprint.points[index].y = y;
00629     
00630     current_footprint_point_->setText(0,QString("[") + xstr + QString(",") +
00631       ystr + QString("]"));
00632 
00633     updateRobotPreview();
00634   }
00635 
00640   void CRobotCreatorConnector::addLaser(void)
00641   {
00642     QString laserFrameId=QString("laser_") + 
00643       QString().setNum(++CRobotCreatorConnector::laser_number);
00644     
00645     stdr_msgs::LaserSensorMsg lmsg;
00646     lmsg.frame_id = laserFrameId.toStdString();
00647     lmsg.numRays = 270;
00648     lmsg.maxAngle = 135;
00649     lmsg.minAngle = - 135;
00650     lmsg.maxRange = 4.0;
00651     lmsg.minRange = 0.0;
00652     lmsg.pose.x = 0;
00653     lmsg.pose.y = 0;
00654     lmsg.pose.theta = 0;
00655     lmsg.noise.noiseMean = 0;
00656     lmsg.noise.noiseStd = 0;
00657     lmsg.frequency = 10.0;
00658     
00659     new_robot_msg_.laserSensors.push_back(lmsg);
00660     
00661     QTreeWidgetItem  *lnode;
00662     lnode = new QTreeWidgetItem();
00663     lnode->setText(0,laserFrameId);
00664     lnode->setIcon(1,loader_.editIcon);
00665     lnode->setIcon(2,loader_.removeIcon);
00666     lnode->setIcon(3,loader_.saveIcon);
00667     lnode->setIcon(4,loader_.loadIcon);
00668 
00669     QTreeWidgetItem 
00670       *angleSpan,
00671       *orientation,
00672       *maxRange,
00673       *minRange,
00674       *noiseMean,
00675       *noiseStd,
00676       *poseX,
00677       *poseY,
00678       *frequency,
00679       *rays;
00680       
00681     rays = new QTreeWidgetItem();
00682     angleSpan = new QTreeWidgetItem();
00683     orientation = new QTreeWidgetItem();
00684     maxRange = new QTreeWidgetItem();
00685     minRange = new QTreeWidgetItem();
00686     noiseMean = new QTreeWidgetItem();
00687     noiseStd = new QTreeWidgetItem();
00688     poseX = new QTreeWidgetItem();
00689     poseY = new QTreeWidgetItem();
00690     frequency = new QTreeWidgetItem();
00691     
00692     rays->setText(0,QString("Number of rays"));
00693     angleSpan->setText(0,QString("Angle span"));
00694     orientation->setText(0,QString("Orientation"));
00695     maxRange->setText(0,QString("Max range"));
00696     minRange->setText(0,QString("Min range"));
00697     noiseMean->setText(0,QString("Noise mean"));
00698     noiseStd->setText(0,QString("Noise std"));
00699     poseX->setText(0,QString("Pose - x"));
00700     poseY->setText(0,QString("Pose - y"));
00701     frequency->setText(0,QString("Frequency"));
00702     
00703     rays->setText(1,QString().setNum(lmsg.numRays));
00704     angleSpan->setText(1,QString().setNum(lmsg.maxAngle - lmsg.minAngle));
00705     orientation->setText(1,QString().setNum(lmsg.maxAngle + lmsg.minAngle));
00706     maxRange->setText(1,QString().setNum(lmsg.maxRange));
00707     minRange->setText(1,QString().setNum(lmsg.minRange));
00708     noiseMean->setText(1,QString().setNum(lmsg.noise.noiseMean));
00709     noiseStd->setText(1,QString().setNum(lmsg.noise.noiseStd));
00710     poseX->setText(1,QString().setNum(lmsg.pose.x));
00711     poseY->setText(1,QString().setNum(lmsg.pose.y));
00712     frequency->setText(1,QString().setNum(lmsg.frequency));
00713     
00714     lnode->addChild(rays);
00715     lnode->addChild(angleSpan);
00716     lnode->addChild(orientation);
00717     lnode->addChild(maxRange);
00718     lnode->addChild(minRange);
00719     lnode->addChild(noiseMean);
00720     lnode->addChild(noiseStd);
00721     lnode->addChild(poseX);
00722     lnode->addChild(poseY);
00723     lnode->addChild(frequency);
00724     
00725     loader_.lasersNode.addChild(lnode);
00726     
00727     lnode->setExpanded(false);
00728     loader_.lasersNode.setExpanded(true);
00729     updateRobotPreview();
00730   }
00731   
00737   void CRobotCreatorConnector::addLaser(stdr_msgs::LaserSensorMsg lmsg)
00738   {
00739     CRobotCreatorConnector::laser_number++;
00740     QString laserFrameId=QString(lmsg.frame_id.c_str());
00741     
00742     QTreeWidgetItem  *lnode;
00743     lnode = new QTreeWidgetItem();
00744     lnode->setText(0,laserFrameId);
00745     lnode->setIcon(1,loader_.editIcon);
00746     lnode->setIcon(2,loader_.removeIcon);
00747     lnode->setIcon(3,loader_.saveIcon);
00748     lnode->setIcon(4,loader_.loadIcon);
00749 
00750     QTreeWidgetItem 
00751       *angleSpan,
00752       *orientation,
00753       *maxRange,
00754       *minRange,
00755       *noiseMean,
00756       *noiseStd,
00757       *poseX,
00758       *poseY,
00759       *frequency,
00760       *rays;
00761       
00762     rays = new QTreeWidgetItem();
00763     angleSpan = new QTreeWidgetItem();
00764     orientation = new QTreeWidgetItem();
00765     maxRange = new QTreeWidgetItem();
00766     minRange = new QTreeWidgetItem();
00767     noiseMean = new QTreeWidgetItem();
00768     noiseStd = new QTreeWidgetItem();
00769     poseX = new QTreeWidgetItem();
00770     poseY = new QTreeWidgetItem();
00771     frequency = new QTreeWidgetItem();
00772     
00773     rays->setText(0,QString("Number of rays"));
00774     angleSpan->setText(0,QString("Angle span"));
00775     orientation->setText(0,QString("Orientation"));
00776     maxRange->setText(0,QString("Max range"));
00777     minRange->setText(0,QString("Min range"));
00778     noiseMean->setText(0,QString("Noise mean"));
00779     noiseStd->setText(0,QString("Noise std"));
00780     poseX->setText(0,QString("Pose - x"));
00781     poseY->setText(0,QString("Pose - y"));
00782     frequency->setText(0,QString("Frequency"));
00783     
00784     rays->setText(1,QString().setNum(lmsg.numRays));
00785     angleSpan->setText(1,QString().setNum(lmsg.maxAngle - lmsg.minAngle));
00786     orientation->setText(1,QString().setNum(lmsg.pose.theta));
00787     maxRange->setText(1,QString().setNum(lmsg.maxRange));
00788     minRange->setText(1,QString().setNum(lmsg.minRange));
00789     noiseMean->setText(1,QString().setNum(lmsg.noise.noiseMean));
00790     noiseStd->setText(1,QString().setNum(lmsg.noise.noiseStd));
00791     poseX->setText(1,QString().setNum(lmsg.pose.x));
00792     poseY->setText(1,QString().setNum(lmsg.pose.y));
00793     frequency->setText(1,QString().setNum(lmsg.frequency));
00794     
00795     lnode->addChild(rays);
00796     lnode->addChild(angleSpan);
00797     lnode->addChild(orientation);
00798     lnode->addChild(maxRange);
00799     lnode->addChild(minRange);
00800     lnode->addChild(noiseMean);
00801     lnode->addChild(noiseStd);
00802     lnode->addChild(poseX);
00803     lnode->addChild(poseY);
00804     lnode->addChild(frequency);
00805     
00806     loader_.lasersNode.addChild(lnode);
00807     
00808     lnode->setExpanded(false);
00809     loader_.lasersNode.setExpanded(true);
00810   }
00811   
00816   void CRobotCreatorConnector::addSonar(void)
00817   {
00818     QString sonarFrameId = 
00819       QString("sonar_") + 
00820       QString().setNum(++CRobotCreatorConnector::sonar_number);
00821     
00822     stdr_msgs::SonarSensorMsg smsg;
00823     smsg.frame_id = sonarFrameId.toStdString();
00824     smsg.maxRange = 3.0;
00825     smsg.minRange = 0.3;
00826     smsg.coneAngle = 50.0;
00827     smsg.pose.x = 0;
00828     smsg.pose.y = 0;
00829     smsg.pose.theta = 0;
00830     smsg.noise.noiseMean = 0;
00831     smsg.noise.noiseStd = 0;
00832     smsg.frequency = 10;
00833     
00834     new_robot_msg_.sonarSensors.push_back(smsg);
00835     
00836     QTreeWidgetItem  *snode;
00837     snode = new QTreeWidgetItem();
00838     snode->setText(0,sonarFrameId);
00839     snode->setIcon(1,loader_.editIcon);
00840     snode->setIcon(2,loader_.removeIcon);
00841     snode->setIcon(3,loader_.saveIcon);
00842     snode->setIcon(4,loader_.loadIcon);
00843 
00844     QTreeWidgetItem 
00845       *coneAngle,
00846       *orientation,
00847       *maxRange,
00848       *minRange,
00849       *noiseMean,
00850       *noiseStd,
00851       *poseX,
00852       *poseY,
00853       *frequency;
00854       
00855     coneAngle = new QTreeWidgetItem();
00856     orientation = new QTreeWidgetItem();
00857     maxRange = new QTreeWidgetItem();
00858     minRange = new QTreeWidgetItem();
00859     noiseMean = new QTreeWidgetItem();
00860     noiseStd = new QTreeWidgetItem();
00861     poseX = new QTreeWidgetItem();
00862     poseY = new QTreeWidgetItem();
00863     frequency = new QTreeWidgetItem();
00864     
00865     coneAngle->setText(0,QString("Cone span"));
00866     orientation->setText(0,QString("Orientation"));
00867     maxRange->setText(0,QString("Max range"));
00868     minRange->setText(0,QString("Min range"));
00869     noiseMean->setText(0,QString("Noise mean"));
00870     noiseStd->setText(0,QString("Noise std"));
00871     poseX->setText(0,QString("Pose - x"));
00872     poseY->setText(0,QString("Pose - y"));
00873     frequency->setText(0,QString("Frequency"));
00874     
00875     coneAngle->setText(1,QString().setNum(smsg.coneAngle));
00876     orientation->setText(1,QString().setNum(smsg.pose.theta));
00877     maxRange->setText(1,QString().setNum(smsg.maxRange));
00878     minRange->setText(1,QString().setNum(smsg.minRange));
00879     noiseMean->setText(1,QString().setNum(smsg.noise.noiseMean));
00880     noiseStd->setText(1,QString().setNum(smsg.noise.noiseStd));
00881     poseX->setText(1,QString().setNum(smsg.pose.x));
00882     poseY->setText(1,QString().setNum(smsg.pose.y));
00883     frequency->setText(1,QString().setNum(smsg.frequency));
00884     
00885     snode->addChild(coneAngle);
00886     snode->addChild(orientation);
00887     snode->addChild(maxRange);
00888     snode->addChild(minRange);
00889     snode->addChild(noiseMean);
00890     snode->addChild(noiseStd);
00891     snode->addChild(poseX);
00892     snode->addChild(poseY);
00893     snode->addChild(frequency);
00894     
00895     loader_.sonarsNode.addChild(snode);
00896     
00897     snode->setExpanded(false);
00898     loader_.sonarsNode.setExpanded(true);
00899     updateRobotPreview();
00900   }
00901   
00907   void CRobotCreatorConnector::addSonar(stdr_msgs::SonarSensorMsg smsg)
00908   {
00909     CRobotCreatorConnector::sonar_number++;
00910     QString sonarFrameId = QString(smsg.frame_id.c_str());
00911 
00912     QTreeWidgetItem  *snode;
00913     snode = new QTreeWidgetItem();
00914     snode->setText(0,sonarFrameId);
00915     snode->setIcon(1,loader_.editIcon);
00916     snode->setIcon(2,loader_.removeIcon);
00917     snode->setIcon(3,loader_.saveIcon);
00918     snode->setIcon(4,loader_.loadIcon);
00919 
00920     QTreeWidgetItem 
00921       *coneAngle,
00922       *orientation,
00923       *maxRange,
00924       *minRange,
00925       *noiseMean,
00926       *noiseStd,
00927       *poseX,
00928       *poseY,
00929       *frequency;
00930       
00931     coneAngle = new QTreeWidgetItem();
00932     orientation = new QTreeWidgetItem();
00933     maxRange = new QTreeWidgetItem();
00934     minRange = new QTreeWidgetItem();
00935     noiseMean = new QTreeWidgetItem();
00936     noiseStd = new QTreeWidgetItem();
00937     poseX = new QTreeWidgetItem();
00938     poseY = new QTreeWidgetItem();
00939     frequency = new QTreeWidgetItem();
00940     
00941     coneAngle->setText(0,QString("Cone span"));
00942     orientation->setText(0,QString("Orientation"));
00943     maxRange->setText(0,QString("Max range"));
00944     minRange->setText(0,QString("Min range"));
00945     noiseMean->setText(0,QString("Noise mean"));
00946     noiseStd->setText(0,QString("Noise std"));
00947     poseX->setText(0,QString("Pose - x"));
00948     poseY->setText(0,QString("Pose - y"));
00949     frequency->setText(0,QString("Frequency"));
00950     
00951     coneAngle->setText(1,QString().setNum(smsg.coneAngle));
00952     orientation->setText(1,QString().setNum(smsg.pose.theta));
00953     maxRange->setText(1,QString().setNum(smsg.maxRange));
00954     minRange->setText(1,QString().setNum(smsg.minRange));
00955     noiseMean->setText(1,QString().setNum(smsg.noise.noiseMean));
00956     noiseStd->setText(1,QString().setNum(smsg.noise.noiseStd));
00957     poseX->setText(1,QString().setNum(smsg.pose.x));
00958     poseY->setText(1,QString().setNum(smsg.pose.y));
00959     frequency->setText(1,QString().setNum(smsg.frequency));
00960     
00961     snode->addChild(coneAngle);
00962     snode->addChild(orientation);
00963     snode->addChild(maxRange);
00964     snode->addChild(minRange);
00965     snode->addChild(noiseMean);
00966     snode->addChild(noiseStd);
00967     snode->addChild(poseX);
00968     snode->addChild(poseY);
00969     snode->addChild(frequency);
00970     
00971     loader_.sonarsNode.addChild(snode);
00972     
00973     snode->setExpanded(false);
00974     loader_.sonarsNode.setExpanded(true);
00975   }
00976   
00981   void CRobotCreatorConnector::addRfidAntenna(void)
00982   {
00983     QString rfidFrameId=QString("rfid_reader_") + 
00984       QString().setNum(CRobotCreatorConnector::rfid_number++);
00985     
00986     stdr_msgs::RfidSensorMsg smsg;
00987     smsg.frame_id = rfidFrameId.toStdString();
00988     smsg.maxRange = 3.0;
00989     smsg.angleSpan = 360.0;
00990     smsg.pose.x = 0;
00991     smsg.pose.y = 0;
00992     smsg.pose.theta = 0;
00993     smsg.signalCutoff = 0;
00994     smsg.frequency = 10;
00995     
00996     new_robot_msg_.rfidSensors.push_back(smsg);
00997     
00998     QTreeWidgetItem  *snode;
00999     snode = new QTreeWidgetItem();
01000     snode->setText(0,rfidFrameId);
01001     snode->setIcon(1,loader_.editIcon);
01002     snode->setIcon(2,loader_.removeIcon);
01003     snode->setIcon(3,loader_.saveIcon);
01004     snode->setIcon(4,loader_.loadIcon);
01005 
01006     QTreeWidgetItem 
01007       *angleSpan,
01008       *orientation,
01009       *maxRange,
01010       *poseX,
01011       *poseY,
01012       *signalCutoff,
01013       *frequency;
01014       
01015     angleSpan = new QTreeWidgetItem();
01016     orientation = new QTreeWidgetItem();
01017     maxRange = new QTreeWidgetItem();
01018     poseX = new QTreeWidgetItem();
01019     poseY = new QTreeWidgetItem();
01020     signalCutoff = new QTreeWidgetItem();
01021     frequency = new QTreeWidgetItem();
01022     
01023     angleSpan->setText(0,QString("Angle span"));
01024     orientation->setText(0,QString("Orientation"));
01025     maxRange->setText(0,QString("Max range"));
01026     poseX->setText(0,QString("Pose - x"));
01027     poseY->setText(0,QString("Pose - y"));
01028     signalCutoff->setText(0,QString("Signal cutoff"));
01029     frequency->setText(0,QString("Frequency"));
01030     
01031     angleSpan->setText(1,QString().setNum(smsg.angleSpan));
01032     orientation->setText(1,QString().setNum(smsg.pose.theta));
01033     maxRange->setText(1,QString().setNum(smsg.maxRange));
01034     poseX->setText(1,QString().setNum(smsg.pose.x));
01035     poseY->setText(1,QString().setNum(smsg.pose.y));
01036     signalCutoff->setText(1,QString().setNum(smsg.signalCutoff));
01037     frequency->setText(1,QString().setNum(smsg.frequency));
01038     
01039     snode->addChild(angleSpan);
01040     snode->addChild(orientation);
01041     snode->addChild(maxRange);
01042     snode->addChild(poseX);
01043     snode->addChild(poseY);
01044     snode->addChild(signalCutoff);
01045     snode->addChild(frequency);
01046     
01047     loader_.rfidAntennasNode.addChild(snode);
01048     
01049     snode->setExpanded(false);
01050     loader_.rfidAntennasNode.setExpanded(true);
01051     updateRobotPreview();
01052   }
01056   void CRobotCreatorConnector::addCO2Sensor(void)
01057   {
01058     QString co2SensorFrameId=QString("co2_sensor_") + 
01059       QString().setNum(CRobotCreatorConnector::co2_sensors_number++);
01060     
01061     stdr_msgs::CO2SensorMsg smsg;
01062     smsg.frame_id = co2SensorFrameId.toStdString();
01063     smsg.maxRange = 3.0;
01064     //~ smsg.angleSpan = 360.0;
01065     smsg.pose.x = 0;
01066     smsg.pose.y = 0;
01067     smsg.pose.theta = 0;
01068     //~ smsg.signalCutoff = 0;
01069     smsg.frequency = 10;
01070     
01071     new_robot_msg_.co2Sensors.push_back(smsg);
01072     
01073     QTreeWidgetItem  *snode;
01074     snode = new QTreeWidgetItem();
01075     snode->setText(0,co2SensorFrameId);
01076     snode->setIcon(1,loader_.editIcon);
01077     snode->setIcon(2,loader_.removeIcon);
01078     snode->setIcon(3,loader_.saveIcon);
01079     snode->setIcon(4,loader_.loadIcon);
01080 
01081     QTreeWidgetItem 
01082       //~ *angleSpan,
01083       *orientation,
01084       *maxRange,
01085       *poseX,
01086       *poseY,
01087       //~ *signalCutoff,
01088       *frequency;
01089       
01090     //~ angleSpan = new QTreeWidgetItem();
01091     orientation = new QTreeWidgetItem();
01092     maxRange = new QTreeWidgetItem();
01093     poseX = new QTreeWidgetItem();
01094     poseY = new QTreeWidgetItem();
01095     //~ signalCutoff = new QTreeWidgetItem();
01096     frequency = new QTreeWidgetItem();
01097     
01098     //~ angleSpan->setText(0,QString("Angle span"));
01099     orientation->setText(0,QString("Orientation"));
01100     maxRange->setText(0,QString("Max range"));
01101     poseX->setText(0,QString("Pose - x"));
01102     poseY->setText(0,QString("Pose - y"));
01103     //~ signalCutoff->setText(0,QString("Signal cutoff"));
01104     frequency->setText(0,QString("Frequency"));
01105     
01106     //~ angleSpan->setText(1,QString().setNum(smsg.angleSpan));
01107     orientation->setText(1,QString().setNum(smsg.pose.theta));
01108     maxRange->setText(1,QString().setNum(smsg.maxRange));
01109     poseX->setText(1,QString().setNum(smsg.pose.x));
01110     poseY->setText(1,QString().setNum(smsg.pose.y));
01111     //~ signalCutoff->setText(1,QString().setNum(smsg.signalCutoff));
01112     frequency->setText(1,QString().setNum(smsg.frequency));
01113     
01114     //~ snode->addChild(angleSpan);
01115     snode->addChild(orientation);
01116     snode->addChild(maxRange);
01117     snode->addChild(poseX);
01118     snode->addChild(poseY);
01119     //~ snode->addChild(signalCutoff);
01120     snode->addChild(frequency);
01121     
01122     loader_.co2SensorsNode.addChild(snode);
01123     
01124     snode->setExpanded(false);
01125     loader_.co2SensorsNode.setExpanded(true);
01126     updateRobotPreview();
01127   }
01131   void CRobotCreatorConnector::addThermalSensor(void)
01132   {
01133     QString thermalSensorFrameId=QString("thermal_sensor_") + 
01134       QString().setNum(CRobotCreatorConnector::thermal_sensors_number++);
01135     
01136     stdr_msgs::ThermalSensorMsg smsg;
01137     smsg.frame_id = thermalSensorFrameId.toStdString();
01138     smsg.maxRange = 3.0;
01139     smsg.angleSpan = 20.0;
01140     smsg.pose.x = 0;
01141     smsg.pose.y = 0;
01142     smsg.pose.theta = 0;
01143     //~ smsg.signalCutoff = 0;
01144     smsg.frequency = 10;
01145     
01146     new_robot_msg_.thermalSensors.push_back(smsg);
01147     
01148     QTreeWidgetItem  *snode;
01149     snode = new QTreeWidgetItem();
01150     snode->setText(0,thermalSensorFrameId);
01151     snode->setIcon(1,loader_.editIcon);
01152     snode->setIcon(2,loader_.removeIcon);
01153     snode->setIcon(3,loader_.saveIcon);
01154     snode->setIcon(4,loader_.loadIcon);
01155 
01156     QTreeWidgetItem 
01157       *angleSpan,
01158       *orientation,
01159       *maxRange,
01160       *poseX,
01161       *poseY,
01162       //~ *signalCutoff,
01163       *frequency;
01164       
01165     angleSpan = new QTreeWidgetItem();
01166     orientation = new QTreeWidgetItem();
01167     maxRange = new QTreeWidgetItem();
01168     poseX = new QTreeWidgetItem();
01169     poseY = new QTreeWidgetItem();
01170     //~ signalCutoff = new QTreeWidgetItem();
01171     frequency = new QTreeWidgetItem();
01172     
01173     angleSpan->setText(0,QString("Angle span"));
01174     orientation->setText(0,QString("Orientation"));
01175     maxRange->setText(0,QString("Max range"));
01176     poseX->setText(0,QString("Pose - x"));
01177     poseY->setText(0,QString("Pose - y"));
01178     //~ signalCutoff->setText(0,QString("Signal cutoff"));
01179     frequency->setText(0,QString("Frequency"));
01180     
01181     angleSpan->setText(1,QString().setNum(smsg.angleSpan));
01182     orientation->setText(1,QString().setNum(smsg.pose.theta));
01183     maxRange->setText(1,QString().setNum(smsg.maxRange));
01184     poseX->setText(1,QString().setNum(smsg.pose.x));
01185     poseY->setText(1,QString().setNum(smsg.pose.y));
01186     //~ signalCutoff->setText(1,QString().setNum(smsg.signalCutoff));
01187     frequency->setText(1,QString().setNum(smsg.frequency));
01188     
01189     snode->addChild(angleSpan);
01190     snode->addChild(orientation);
01191     snode->addChild(maxRange);
01192     snode->addChild(poseX);
01193     snode->addChild(poseY);
01194     //~ snode->addChild(signalCutoff);
01195     snode->addChild(frequency);
01196     
01197     loader_.thermalSensorsNode.addChild(snode);
01198     
01199     snode->setExpanded(false);
01200     loader_.thermalSensorsNode.setExpanded(true);
01201     updateRobotPreview();
01202   }
01206   void CRobotCreatorConnector::addSoundSensor(void)
01207   {
01208     QString soundSensorFrameId=QString("sound_sensor_") + 
01209       QString().setNum(CRobotCreatorConnector::sound_sensors_number++);
01210     
01211     stdr_msgs::SoundSensorMsg smsg;
01212     smsg.frame_id = soundSensorFrameId.toStdString();
01213     smsg.maxRange = 3.0;
01214     smsg.angleSpan = 180.0;
01215     smsg.pose.x = 0;
01216     smsg.pose.y = 0;
01217     smsg.pose.theta = 0;
01218     //~ smsg.signalCutoff = 0;
01219     smsg.frequency = 10;
01220     
01221     new_robot_msg_.soundSensors.push_back(smsg);
01222     
01223     QTreeWidgetItem  *snode;
01224     snode = new QTreeWidgetItem();
01225     snode->setText(0,soundSensorFrameId);
01226     snode->setIcon(1,loader_.editIcon);
01227     snode->setIcon(2,loader_.removeIcon);
01228     snode->setIcon(3,loader_.saveIcon);
01229     snode->setIcon(4,loader_.loadIcon);
01230 
01231     QTreeWidgetItem 
01232       *angleSpan,
01233       *orientation,
01234       *maxRange,
01235       *poseX,
01236       *poseY,
01237       //~ *signalCutoff,
01238       *frequency;
01239       
01240     angleSpan = new QTreeWidgetItem();
01241     orientation = new QTreeWidgetItem();
01242     maxRange = new QTreeWidgetItem();
01243     poseX = new QTreeWidgetItem();
01244     poseY = new QTreeWidgetItem();
01245     //~ signalCutoff = new QTreeWidgetItem();
01246     frequency = new QTreeWidgetItem();
01247     
01248     angleSpan->setText(0,QString("Angle span"));
01249     orientation->setText(0,QString("Orientation"));
01250     maxRange->setText(0,QString("Max range"));
01251     poseX->setText(0,QString("Pose - x"));
01252     poseY->setText(0,QString("Pose - y"));
01253     //~ signalCutoff->setText(0,QString("Signal cutoff"));
01254     frequency->setText(0,QString("Frequency"));
01255     
01256     angleSpan->setText(1,QString().setNum(smsg.angleSpan));
01257     orientation->setText(1,QString().setNum(smsg.pose.theta));
01258     maxRange->setText(1,QString().setNum(smsg.maxRange));
01259     poseX->setText(1,QString().setNum(smsg.pose.x));
01260     poseY->setText(1,QString().setNum(smsg.pose.y));
01261     //~ signalCutoff->setText(1,QString().setNum(smsg.signalCutoff));
01262     frequency->setText(1,QString().setNum(smsg.frequency));
01263     
01264     snode->addChild(angleSpan);
01265     snode->addChild(orientation);
01266     snode->addChild(maxRange);
01267     snode->addChild(poseX);
01268     snode->addChild(poseY);
01269     //~ snode->addChild(signalCutoff);
01270     snode->addChild(frequency);
01271     
01272     loader_.soundSensorsNode.addChild(snode);
01273     
01274     snode->setExpanded(false);
01275     loader_.soundSensorsNode.setExpanded(true);
01276     updateRobotPreview();
01277   }
01278   
01283   void CRobotCreatorConnector::addRfidAntenna(stdr_msgs::RfidSensorMsg smsg)
01284   {
01285     CRobotCreatorConnector::rfid_number++;
01286     QString rfidFrameId=QString(smsg.frame_id.c_str());
01287 
01288     QTreeWidgetItem  *snode;
01289     snode = new QTreeWidgetItem();
01290     snode->setText(0,rfidFrameId);
01291     snode->setIcon(1,loader_.editIcon);
01292     snode->setIcon(2,loader_.removeIcon);
01293     snode->setIcon(3,loader_.saveIcon);
01294     snode->setIcon(4,loader_.loadIcon);
01295 
01296     QTreeWidgetItem 
01297       *angleSpan,
01298       *orientation,
01299       *maxRange,
01300       *poseX,
01301       *poseY,
01302       *signalCutoff,
01303       *frequency;
01304       
01305     angleSpan = new QTreeWidgetItem();
01306     orientation = new QTreeWidgetItem();
01307     maxRange = new QTreeWidgetItem();
01308     poseX = new QTreeWidgetItem();
01309     poseY = new QTreeWidgetItem();
01310     signalCutoff = new QTreeWidgetItem();
01311     frequency = new QTreeWidgetItem();
01312     
01313     angleSpan->setText(0,QString("Angle span"));
01314     orientation->setText(0,QString("Orientation"));
01315     maxRange->setText(0,QString("Max range"));
01316     poseX->setText(0,QString("Pose - x"));
01317     poseY->setText(0,QString("Pose - y"));
01318     signalCutoff->setText(0,QString("Signal cutoff"));
01319     frequency->setText(0,QString("Frequency"));
01320     
01321     angleSpan->setText(1,QString().setNum(smsg.angleSpan));
01322     orientation->setText(1,QString().setNum(smsg.pose.theta));
01323     maxRange->setText(1,QString().setNum(smsg.maxRange));
01324     poseX->setText(1,QString().setNum(smsg.pose.x));
01325     poseY->setText(1,QString().setNum(smsg.pose.y));
01326     signalCutoff->setText(1,QString().setNum(smsg.signalCutoff));
01327     frequency->setText(1,QString().setNum(smsg.frequency));
01328     
01329     snode->addChild(angleSpan);
01330     snode->addChild(orientation);
01331     snode->addChild(maxRange);
01332     snode->addChild(poseX);
01333     snode->addChild(poseY);
01334     snode->addChild(signalCutoff);
01335     snode->addChild(frequency);
01336     
01337     loader_.rfidAntennasNode.addChild(snode);
01338     
01339     snode->setExpanded(false);
01340     loader_.rfidAntennasNode.setExpanded(true);
01341     updateRobotPreview();
01342   }
01347   void CRobotCreatorConnector::addCO2Sensor(stdr_msgs::CO2SensorMsg smsg)
01348   {
01349     CRobotCreatorConnector::co2_sensors_number++;
01350     QString co2SensorFrameId=QString(smsg.frame_id.c_str());
01351 
01352     QTreeWidgetItem  *snode;
01353     snode = new QTreeWidgetItem();
01354     snode->setText(0,co2SensorFrameId);
01355     snode->setIcon(1,loader_.editIcon);
01356     snode->setIcon(2,loader_.removeIcon);
01357     snode->setIcon(3,loader_.saveIcon);
01358     snode->setIcon(4,loader_.loadIcon);
01359 
01360     QTreeWidgetItem 
01361       //~ *angleSpan,
01362       *orientation,
01363       *maxRange,
01364       *poseX,
01365       *poseY,
01366       //~ *signalCutoff,
01367       *frequency;
01368       
01369     //~ angleSpan = new QTreeWidgetItem();
01370     orientation = new QTreeWidgetItem();
01371     maxRange = new QTreeWidgetItem();
01372     poseX = new QTreeWidgetItem();
01373     poseY = new QTreeWidgetItem();
01374     //~ signalCutoff = new QTreeWidgetItem();
01375     frequency = new QTreeWidgetItem();
01376     
01377     //~ angleSpan->setText(0,QString("Angle span"));
01378     orientation->setText(0,QString("Orientation"));
01379     maxRange->setText(0,QString("Max range"));
01380     poseX->setText(0,QString("Pose - x"));
01381     poseY->setText(0,QString("Pose - y"));
01382     //~ signalCutoff->setText(0,QString("Signal cutoff"));
01383     frequency->setText(0,QString("Frequency"));
01384     
01385     //~ angleSpan->setText(1,QString().setNum(smsg.angleSpan));
01386     orientation->setText(1,QString().setNum(smsg.pose.theta));
01387     maxRange->setText(1,QString().setNum(smsg.maxRange));
01388     poseX->setText(1,QString().setNum(smsg.pose.x));
01389     poseY->setText(1,QString().setNum(smsg.pose.y));
01390     //~ signalCutoff->setText(1,QString().setNum(smsg.signalCutoff));
01391     frequency->setText(1,QString().setNum(smsg.frequency));
01392     
01393     //~ snode->addChild(angleSpan);
01394     snode->addChild(orientation);
01395     snode->addChild(maxRange);
01396     snode->addChild(poseX);
01397     snode->addChild(poseY);
01398     //~ snode->addChild(signalCutoff);
01399     snode->addChild(frequency);
01400     
01401     loader_.co2SensorsNode.addChild(snode);
01402     
01403     snode->setExpanded(false);
01404     loader_.co2SensorsNode.setExpanded(true);
01405     updateRobotPreview();
01406   }
01411   void CRobotCreatorConnector::addThermalSensor(stdr_msgs::ThermalSensorMsg smsg)
01412   {
01413     CRobotCreatorConnector::thermal_sensors_number++;
01414     QString thermalSensorFrameId=QString(smsg.frame_id.c_str());
01415 
01416     QTreeWidgetItem  *snode;
01417     snode = new QTreeWidgetItem();
01418     snode->setText(0,thermalSensorFrameId);
01419     snode->setIcon(1,loader_.editIcon);
01420     snode->setIcon(2,loader_.removeIcon);
01421     snode->setIcon(3,loader_.saveIcon);
01422     snode->setIcon(4,loader_.loadIcon);
01423 
01424     QTreeWidgetItem 
01425       *angleSpan,
01426       *orientation,
01427       *maxRange,
01428       *poseX,
01429       *poseY,
01430       //~ *signalCutoff,
01431       *frequency;
01432       
01433     angleSpan = new QTreeWidgetItem();
01434     orientation = new QTreeWidgetItem();
01435     maxRange = new QTreeWidgetItem();
01436     poseX = new QTreeWidgetItem();
01437     poseY = new QTreeWidgetItem();
01438     //~ signalCutoff = new QTreeWidgetItem();
01439     frequency = new QTreeWidgetItem();
01440     
01441     angleSpan->setText(0,QString("Angle span"));
01442     orientation->setText(0,QString("Orientation"));
01443     maxRange->setText(0,QString("Max range"));
01444     poseX->setText(0,QString("Pose - x"));
01445     poseY->setText(0,QString("Pose - y"));
01446     //~ signalCutoff->setText(0,QString("Signal cutoff"));
01447     frequency->setText(0,QString("Frequency"));
01448     
01449     angleSpan->setText(1,QString().setNum(smsg.angleSpan));
01450     orientation->setText(1,QString().setNum(smsg.pose.theta));
01451     maxRange->setText(1,QString().setNum(smsg.maxRange));
01452     poseX->setText(1,QString().setNum(smsg.pose.x));
01453     poseY->setText(1,QString().setNum(smsg.pose.y));
01454     //~ signalCutoff->setText(1,QString().setNum(smsg.signalCutoff));
01455     frequency->setText(1,QString().setNum(smsg.frequency));
01456     
01457     snode->addChild(angleSpan);
01458     snode->addChild(orientation);
01459     snode->addChild(maxRange);
01460     snode->addChild(poseX);
01461     snode->addChild(poseY);
01462     //~ snode->addChild(signalCutoff);
01463     snode->addChild(frequency);
01464     
01465     loader_.thermalSensorsNode.addChild(snode);
01466     
01467     snode->setExpanded(false);
01468     loader_.thermalSensorsNode.setExpanded(true);
01469     updateRobotPreview();
01470   }
01475   void CRobotCreatorConnector::addSoundSensor(stdr_msgs::SoundSensorMsg smsg)
01476   {
01477     CRobotCreatorConnector::sound_sensors_number++;
01478     QString soundSensorFrameId=QString(smsg.frame_id.c_str());
01479 
01480     QTreeWidgetItem  *snode;
01481     snode = new QTreeWidgetItem();
01482     snode->setText(0,soundSensorFrameId);
01483     snode->setIcon(1,loader_.editIcon);
01484     snode->setIcon(2,loader_.removeIcon);
01485     snode->setIcon(3,loader_.saveIcon);
01486     snode->setIcon(4,loader_.loadIcon);
01487 
01488     QTreeWidgetItem 
01489       *angleSpan,
01490       *orientation,
01491       *maxRange,
01492       *poseX,
01493       *poseY,
01494       //~ *signalCutoff,
01495       *frequency;
01496       
01497     angleSpan = new QTreeWidgetItem();
01498     orientation = new QTreeWidgetItem();
01499     maxRange = new QTreeWidgetItem();
01500     poseX = new QTreeWidgetItem();
01501     poseY = new QTreeWidgetItem();
01502     //~ signalCutoff = new QTreeWidgetItem();
01503     frequency = new QTreeWidgetItem();
01504     
01505     angleSpan->setText(0,QString("Angle span"));
01506     orientation->setText(0,QString("Orientation"));
01507     maxRange->setText(0,QString("Max range"));
01508     poseX->setText(0,QString("Pose - x"));
01509     poseY->setText(0,QString("Pose - y"));
01510     //~ signalCutoff->setText(0,QString("Signal cutoff"));
01511     frequency->setText(0,QString("Frequency"));
01512     
01513     angleSpan->setText(1,QString().setNum(smsg.angleSpan));
01514     orientation->setText(1,QString().setNum(smsg.pose.theta));
01515     maxRange->setText(1,QString().setNum(smsg.maxRange));
01516     poseX->setText(1,QString().setNum(smsg.pose.x));
01517     poseY->setText(1,QString().setNum(smsg.pose.y));
01518     //~ signalCutoff->setText(1,QString().setNum(smsg.signalCutoff));
01519     frequency->setText(1,QString().setNum(smsg.frequency));
01520     
01521     snode->addChild(angleSpan);
01522     snode->addChild(orientation);
01523     snode->addChild(maxRange);
01524     snode->addChild(poseX);
01525     snode->addChild(poseY);
01526     //~ snode->addChild(signalCutoff);
01527     snode->addChild(frequency);
01528     
01529     loader_.soundSensorsNode.addChild(snode);
01530     
01531     snode->setExpanded(false);
01532     loader_.soundSensorsNode.setExpanded(true);
01533     updateRobotPreview();
01534   }
01535   
01541   void CRobotCreatorConnector::eraseLaser(QTreeWidgetItem *item)
01542   {
01543     unsigned int laserFrameId = searchLaser(item->text(0));
01544     if(laserFrameId == -1) 
01545     {
01546       return;
01547     }
01548     new_robot_msg_.laserSensors.erase(
01549       new_robot_msg_.laserSensors.begin() + laserFrameId);
01550     deleteTreeNode(item);
01551     updateRobotPreview();
01552   }
01553   
01559   void CRobotCreatorConnector::eraseSonar(QTreeWidgetItem *item)
01560   {
01561     unsigned int sonarFrameId = searchSonar(item->text(0));
01562     if(sonarFrameId == -1) 
01563     {
01564       return;
01565     }
01566     new_robot_msg_.sonarSensors.erase(
01567       new_robot_msg_.sonarSensors.begin() + sonarFrameId);
01568     deleteTreeNode(item);
01569     updateRobotPreview();
01570   }
01571   
01577   void CRobotCreatorConnector::eraseRfid(QTreeWidgetItem *item)
01578   {
01579     unsigned int rfidFrameId = searchRfid(item->text(0));
01580     if(rfidFrameId == -1) 
01581     {
01582       return;
01583     }
01584     new_robot_msg_.rfidSensors.erase(
01585       new_robot_msg_.rfidSensors.begin() + rfidFrameId);
01586     deleteTreeNode(item);
01587     updateRobotPreview();
01588   }
01592   void CRobotCreatorConnector::eraseCO2Sensor(QTreeWidgetItem *item)
01593   {
01594     unsigned int frameId = searchCO2Sensor(item->text(0));
01595     if(frameId == -1) 
01596     {
01597       return;
01598     }
01599     new_robot_msg_.co2Sensors.erase(
01600       new_robot_msg_.co2Sensors.begin() + frameId);
01601     deleteTreeNode(item);
01602     updateRobotPreview();
01603   }
01607   void CRobotCreatorConnector::eraseThermalSensor(QTreeWidgetItem *item)
01608   {
01609     unsigned int frameId = searchThermalSensor(item->text(0));
01610     if(frameId == -1) 
01611     {
01612       return;
01613     }
01614     new_robot_msg_.thermalSensors.erase(
01615       new_robot_msg_.thermalSensors.begin() + frameId);
01616     deleteTreeNode(item);
01617     updateRobotPreview();
01618   }
01622   void CRobotCreatorConnector::eraseSoundSensor(QTreeWidgetItem *item)
01623   {
01624     unsigned int frameId = searchSoundSensor(item->text(0));
01625     if(frameId == -1) 
01626     {
01627       return;
01628     }
01629     new_robot_msg_.soundSensors.erase(
01630       new_robot_msg_.soundSensors.begin() + frameId);
01631     deleteTreeNode(item);
01632     updateRobotPreview();
01633   }
01634   
01640   void CRobotCreatorConnector::editFootprintPoint(QTreeWidgetItem *item)
01641   {
01642     int index = -1;
01643     for(unsigned int i = 0 ; i < loader_.robotInfoFootprint.childCount() ; i++)
01644     {
01645       if(loader_.robotInfoFootprint.child(i) == item)
01646       {
01647         index = i;
01648         break;
01649       }
01650     }
01651     if( index == -1 )
01652     {
01653       return;
01654     }
01655     
01656     loader_.robotFootLoader.robotFootprintX->setText(
01657       QString().setNum(new_robot_msg_.footprint.points[index].x));
01658     loader_.robotFootLoader.robotFootprintY->setText(
01659       QString().setNum(new_robot_msg_.footprint.points[index].y));
01660     
01661     loader_.robotFootLoader.setWindowTitle(
01662       QApplication::translate(
01663         "Footprint point", 
01664         item->text(0).toStdString().c_str(), 
01665         0, 
01666         QApplication::UnicodeUTF8));
01667     
01668     current_footprint_point_ = item;
01669     
01670     loader_.robotFootLoader.show();
01671   }
01672   
01678   void CRobotCreatorConnector::editLaser(QTreeWidgetItem *item)
01679   {
01680     unsigned int laserFrameId = searchLaser(item->text(0));
01681     if(laserFrameId == -1) 
01682     {
01683       return;
01684     }  
01685     loader_.laserPropLoader.laserRays->setText(
01686       QString().setNum(new_robot_msg_.laserSensors[laserFrameId].numRays));
01687         
01688     loader_.laserPropLoader.laserMaxDistance->setText(
01689       QString().setNum(new_robot_msg_.laserSensors[laserFrameId].maxRange));
01690     
01691     loader_.laserPropLoader.laserMinDistance->setText(
01692       QString().setNum(new_robot_msg_.laserSensors[laserFrameId].minRange));
01693     
01694     loader_.laserPropLoader.laserAngleSpan->setText(
01695       QString().setNum(
01696         new_robot_msg_.laserSensors[laserFrameId].maxAngle - 
01697         new_robot_msg_.laserSensors[laserFrameId].minAngle));
01698     
01699     loader_.laserPropLoader.laserOrientation->setText(
01700       QString().setNum(new_robot_msg_.laserSensors[laserFrameId].pose.theta));
01701     
01702     loader_.laserPropLoader.laserNoiseMean->setText(
01703       QString().setNum(
01704         new_robot_msg_.laserSensors[laserFrameId].noise.noiseMean));
01705     
01706     loader_.laserPropLoader.laserNoiseStd->setText(
01707       QString().setNum(
01708         new_robot_msg_.laserSensors[laserFrameId].noise.noiseStd));
01709     
01710     loader_.laserPropLoader.laserTranslationX->setText(
01711       QString().setNum(new_robot_msg_.laserSensors[laserFrameId].pose.x));
01712     
01713     loader_.laserPropLoader.laserTranslationY->setText(
01714       QString().setNum(new_robot_msg_.laserSensors[laserFrameId].pose.y));
01715     
01716     loader_.laserPropLoader.laserFrequency->setText(
01717       QString().setNum(new_robot_msg_.laserSensors[laserFrameId].frequency));
01718     
01719     loader_.laserPropLoader.setWindowTitle(
01720       QApplication::translate(
01721         "LaserProperties", 
01722         item->text(0).toStdString().c_str(), 
01723         0, 
01724         QApplication::UnicodeUTF8));
01725     
01726     current_laser_ = item;
01727     
01728     loader_.laserPropLoader.show();
01729   }
01730   
01736   void CRobotCreatorConnector::saveLaser(QTreeWidgetItem *item)
01737   {
01738     unsigned int laserFrameId = searchLaser(item->text(0));
01739     if(laserFrameId == -1) 
01740     {
01741       return;
01742     }  
01743     QString file_name = QFileDialog::getSaveFileName(&loader_, 
01744       tr("Save laser sensor"),
01745         QString().fromStdString(
01746         stdr_gui_tools::getRosPackagePath("stdr_resources")) + 
01747         QString("/resources/"),
01748         tr("Yaml files (*.yaml)"));
01749     
01750     std::string file_name_str=file_name.toStdString();
01751     stdr_msgs::LaserSensorMsg lmsg = new_robot_msg_.laserSensors[laserFrameId];
01752 
01753     try {
01754       stdr_parser::Parser::saveMessage(
01755         stdr_gui_tools::fixLaserAnglesToRad(lmsg), file_name_str);
01756     }
01757     catch(stdr_parser::ParserException ex)
01758     {
01759       QMessageBox msg;
01760       msg.setWindowTitle(QString("STDR Parser - Error"));
01761       msg.setText(QString(ex.what()));
01762       msg.exec();
01763       return;
01764     }
01765   }
01766   
01772   void CRobotCreatorConnector::saveSonar(QTreeWidgetItem *item)
01773   {
01774     unsigned int sonarFrameId = searchSonar(item->text(0));
01775     if(sonarFrameId == -1) 
01776     {
01777       return;
01778     }  
01779     QString file_name = QFileDialog::getSaveFileName(&loader_, 
01780       tr("Save sonar sensor"),
01781         QString().fromStdString(
01782         stdr_gui_tools::getRosPackagePath("stdr_resources")) + 
01783         QString("/resources/"),
01784         tr("Resource files (*.yaml *.xml)"));
01785     
01786     std::string file_name_str=file_name.toStdString();
01787     stdr_msgs::SonarSensorMsg smsg = new_robot_msg_.sonarSensors[sonarFrameId];
01788 
01789     try {
01790       stdr_parser::Parser::saveMessage(
01791         stdr_gui_tools::fixSonarAnglesToRad(smsg), file_name_str);
01792     }
01793     catch(stdr_parser::ParserException ex)
01794     {
01795       QMessageBox msg;
01796       msg.setWindowTitle(QString("STDR Parser - Error"));
01797       msg.setText(QString(ex.what()));
01798       msg.exec();
01799       return;
01800     }
01801   }
01802   
01808   void CRobotCreatorConnector::saveRfidAntenna(QTreeWidgetItem *item)
01809   {
01810     unsigned int frameId = searchRfid(item->text(0));
01811     if(frameId == -1) 
01812     {
01813       return;
01814     }  
01815     QString file_name = QFileDialog::getSaveFileName(&loader_, 
01816       tr("Save RFID reader sensor"),
01817         QString().fromStdString(
01818         stdr_gui_tools::getRosPackagePath("stdr_resources")) + 
01819         QString("/resources/"),
01820         tr("Resource files (*.yaml *.xml)"));
01821     
01822     std::string file_name_str=file_name.toStdString();
01823     stdr_msgs::RfidSensorMsg smsg = new_robot_msg_.rfidSensors[frameId];
01824 
01825     try {
01826       stdr_parser::Parser::saveMessage(
01827         stdr_gui_tools::fixRfidAnglesToRad(smsg), file_name_str);
01828     }
01829     catch(stdr_parser::ParserException ex)
01830     {
01831       QMessageBox msg;
01832       msg.setWindowTitle(QString("STDR Parser - Error"));
01833       msg.setText(QString(ex.what()));
01834       msg.exec();
01835       return;
01836     }
01837   }
01838   
01842   void CRobotCreatorConnector::saveCO2Sensor(QTreeWidgetItem *item)
01843   {
01844     unsigned int frameId = searchCO2Sensor(item->text(0));
01845     if(frameId == -1) 
01846     {
01847       return;
01848     }  
01849     QString file_name = QFileDialog::getSaveFileName(&loader_, 
01850       tr("Save CO2 sensor"),
01851         QString().fromStdString(
01852         stdr_gui_tools::getRosPackagePath("stdr_resources")) + 
01853         QString("/resources/"),
01854         tr("Resource files (*.yaml *.xml)"));
01855     
01856     std::string file_name_str=file_name.toStdString();
01857     stdr_msgs::CO2SensorMsg smsg = new_robot_msg_.co2Sensors[frameId];
01858 
01859     try {
01860       stdr_parser::Parser::saveMessage(
01861         stdr_gui_tools::fixCO2AnglesToRad(smsg), file_name_str);
01862     }
01863     catch(stdr_parser::ParserException ex)
01864     {
01865       QMessageBox msg;
01866       msg.setWindowTitle(QString("STDR Parser - Error"));
01867       msg.setText(QString(ex.what()));
01868       msg.exec();
01869       return;
01870     }
01871   }
01875   void CRobotCreatorConnector::saveThermalSensor(QTreeWidgetItem *item)
01876   {
01877     unsigned int frameId = searchThermalSensor(item->text(0));
01878     if(frameId == -1) 
01879     {
01880       return;
01881     }  
01882     QString file_name = QFileDialog::getSaveFileName(&loader_, 
01883       tr("Save Thermal sensor"),
01884         QString().fromStdString(
01885         stdr_gui_tools::getRosPackagePath("stdr_resources")) + 
01886         QString("/resources/"),
01887         tr("Resource files (*.yaml *.xml)"));
01888     
01889     std::string file_name_str=file_name.toStdString();
01890     stdr_msgs::ThermalSensorMsg smsg = new_robot_msg_.thermalSensors[frameId];
01891 
01892     try {
01893       stdr_parser::Parser::saveMessage(
01894         stdr_gui_tools::fixThermalAnglesToRad(smsg), file_name_str);
01895     }
01896     catch(stdr_parser::ParserException ex)
01897     {
01898       QMessageBox msg;
01899       msg.setWindowTitle(QString("STDR Parser - Error"));
01900       msg.setText(QString(ex.what()));
01901       msg.exec();
01902       return;
01903     }
01904   }
01908   void CRobotCreatorConnector::saveSoundSensor(QTreeWidgetItem *item)
01909   {
01910     unsigned int frameId = searchSoundSensor(item->text(0));
01911     if(frameId == -1) 
01912     {
01913       return;
01914     }  
01915     QString file_name = QFileDialog::getSaveFileName(&loader_, 
01916       tr("Save Sound sensor"),
01917         QString().fromStdString(
01918         stdr_gui_tools::getRosPackagePath("stdr_resources")) + 
01919         QString("/resources/"),
01920         tr("Resource files (*.yaml *.xml)"));
01921     
01922     std::string file_name_str=file_name.toStdString();
01923     stdr_msgs::SoundSensorMsg smsg = new_robot_msg_.soundSensors[frameId];
01924 
01925     try {
01926       stdr_parser::Parser::saveMessage(
01927         stdr_gui_tools::fixSoundAnglesToRad(smsg), file_name_str);
01928     }
01929     catch(stdr_parser::ParserException ex)
01930     {
01931       QMessageBox msg;
01932       msg.setWindowTitle(QString("STDR Parser - Error"));
01933       msg.setText(QString(ex.what()));
01934       msg.exec();
01935       return;
01936     }
01937   }
01938   
01944   void CRobotCreatorConnector::loadLaser(QTreeWidgetItem *item)
01945   {
01946     unsigned int laserFrameId = searchLaser(item->text(0));
01947     if(laserFrameId == -1) 
01948     {
01949       return;
01950     }  
01951     QString file_name = QFileDialog::getOpenFileName(
01952       &loader_,
01953       tr("Load laser sensor"), 
01954       QString().fromStdString(
01955         stdr_gui_tools::getRosPackagePath("stdr_resources")) + 
01956         QString("/resources/"), 
01957         tr("Resource Files (*.yaml *.xml)"));
01958     
01959     if (file_name.isEmpty()) {
01960       return;
01961     }
01962     std::string old_frame_id = item->text(0).toStdString();
01963     stdr_msgs::LaserSensorMsg lmsg;
01964     try {
01965       lmsg = 
01966         stdr_parser::Parser::createMessage<stdr_msgs::LaserSensorMsg>
01967           (file_name.toStdString());
01968     }
01969     catch(stdr_parser::ParserException ex)
01970     {
01971       QMessageBox msg;
01972       msg.setWindowTitle(QString("STDR Parser - Error"));
01973       msg.setText(QString(ex.what()));
01974       msg.exec();
01975       return;
01976     }
01977     lmsg = stdr_gui_tools::fixLaserAnglesToDegrees(lmsg);
01978     lmsg.frame_id = old_frame_id;
01979     new_robot_msg_.laserSensors[laserFrameId]=lmsg;
01980     updateLaserTree(item,lmsg);
01981     updateRobotPreview(); 
01982   }
01983   
01989   void CRobotCreatorConnector::loadSonar(QTreeWidgetItem *item)
01990   {
01991     unsigned int sonarFrameId = searchSonar(item->text(0));
01992     if(sonarFrameId == -1) 
01993     {
01994       return;
01995     }  
01996     QString file_name = QFileDialog::getOpenFileName(
01997       &loader_,
01998       tr("Load sonar sensor"), 
01999       QString().fromStdString(
02000         stdr_gui_tools::getRosPackagePath("stdr_resources")) + 
02001         QString("/resources/"), 
02002         tr("Resource Files (*.yaml *.xml)"));
02003     
02004     if (file_name.isEmpty()) {
02005       return;
02006     }
02007     stdr_msgs::SonarSensorMsg smsg;
02008     std::string old_frame_id = item->text(0).toStdString();
02009     try {
02010       smsg = 
02011         stdr_parser::Parser::createMessage<stdr_msgs::SonarSensorMsg>
02012           (file_name.toStdString());
02013     }
02014     catch(stdr_parser::ParserException ex)
02015     {
02016       QMessageBox msg;
02017       msg.setWindowTitle(QString("STDR Parser - Error"));
02018       msg.setText(QString(ex.what()));
02019       msg.exec();
02020       return;
02021     }
02022     smsg = stdr_gui_tools::fixSonarAnglesToDegrees(smsg);
02023     smsg.frame_id = old_frame_id;
02024     new_robot_msg_.sonarSensors[sonarFrameId]=smsg;
02025     updateSonarTree(item,smsg);
02026     updateRobotPreview(); 
02027   }
02028   
02034   void CRobotCreatorConnector::loadRfidAntenna(QTreeWidgetItem *item)
02035   {
02036     unsigned int frameId = searchRfid(item->text(0));
02037     if(frameId == -1) 
02038     {
02039       return;
02040     }  
02041     QString file_name = QFileDialog::getOpenFileName(
02042       &loader_,
02043       tr("Load RFID reader sensor"), 
02044       QString().fromStdString(
02045         stdr_gui_tools::getRosPackagePath("stdr_resources")) + 
02046         QString("/resources/"), 
02047         tr("Resource Files (*.yaml *.xml)"));
02048     
02049     if (file_name.isEmpty()) {
02050       return;
02051     }
02052     stdr_msgs::RfidSensorMsg smsg;
02053     std::string old_frame_id = item->text(0).toStdString();
02054     try {
02055       smsg = 
02056         stdr_parser::Parser::createMessage<stdr_msgs::RfidSensorMsg>
02057           (file_name.toStdString());
02058     }
02059     catch(stdr_parser::ParserException ex)
02060     {
02061       QMessageBox msg;
02062       msg.setWindowTitle(QString("STDR Parser - Error"));
02063       msg.setText(QString(ex.what()));
02064       msg.exec();
02065       return;
02066     }
02067     smsg = stdr_gui_tools::fixRfidAnglesToDegrees(smsg);
02068     smsg.frame_id = old_frame_id;
02069     new_robot_msg_.rfidSensors[frameId]=smsg;
02070     updateRfidTree(item,smsg);
02071     updateRobotPreview(); 
02072   }
02076   void CRobotCreatorConnector::loadCO2Sensor(QTreeWidgetItem *item)
02077   {
02078     unsigned int frameId = searchCO2Sensor(item->text(0));
02079     if(frameId == -1) 
02080     {
02081       return;
02082     }  
02083     QString file_name = QFileDialog::getOpenFileName(
02084       &loader_,
02085       tr("Load CO2 sensor"), 
02086       QString().fromStdString(
02087         stdr_gui_tools::getRosPackagePath("stdr_resources")) + 
02088         QString("/resources/"), 
02089         tr("Resource Files (*.yaml *.xml)"));
02090     
02091     if (file_name.isEmpty()) {
02092       return;
02093     }
02094     stdr_msgs::CO2SensorMsg smsg;
02095     std::string old_frame_id = item->text(0).toStdString();
02096     try {
02097       smsg = 
02098         stdr_parser::Parser::createMessage<stdr_msgs::CO2SensorMsg>
02099           (file_name.toStdString());
02100     }
02101     catch(stdr_parser::ParserException ex)
02102     {
02103       QMessageBox msg;
02104       msg.setWindowTitle(QString("STDR Parser - Error"));
02105       msg.setText(QString(ex.what()));
02106       msg.exec();
02107       return;
02108     }
02109     smsg = stdr_gui_tools::fixCO2AnglesToDegrees(smsg);
02110     smsg.frame_id = old_frame_id;
02111     new_robot_msg_.co2Sensors[frameId]=smsg;
02112     updateCO2SensorTree(item,smsg);
02113     updateRobotPreview(); 
02114   }
02118   void CRobotCreatorConnector::loadThermalSensor(QTreeWidgetItem *item)
02119   {
02120     unsigned int frameId = searchThermalSensor(item->text(0));
02121     if(frameId == -1) 
02122     {
02123       return;
02124     }  
02125     QString file_name = QFileDialog::getOpenFileName(
02126       &loader_,
02127       tr("Load thermal sensor"), 
02128       QString().fromStdString(
02129         stdr_gui_tools::getRosPackagePath("stdr_resources")) + 
02130         QString("/resources/"), 
02131         tr("Resource Files (*.yaml *.xml)"));
02132     
02133     if (file_name.isEmpty()) {
02134       return;
02135     }
02136     stdr_msgs::ThermalSensorMsg smsg;
02137     std::string old_frame_id = item->text(0).toStdString();
02138     try {
02139       smsg = 
02140         stdr_parser::Parser::createMessage<stdr_msgs::ThermalSensorMsg>
02141           (file_name.toStdString());
02142     }
02143     catch(stdr_parser::ParserException ex)
02144     {
02145       QMessageBox msg;
02146       msg.setWindowTitle(QString("STDR Parser - Error"));
02147       msg.setText(QString(ex.what()));
02148       msg.exec();
02149       return;
02150     }
02151     smsg = stdr_gui_tools::fixThermalAnglesToDegrees(smsg);
02152     smsg.frame_id = old_frame_id;
02153     new_robot_msg_.thermalSensors[frameId]=smsg;
02154     updateThermalSensorTree(item,smsg);
02155     updateRobotPreview(); 
02156   }
02160   void CRobotCreatorConnector::loadSoundSensor(QTreeWidgetItem *item)
02161   {
02162     unsigned int frameId = searchSoundSensor(item->text(0));
02163     if(frameId == -1) 
02164     {
02165       return;
02166     }  
02167     QString file_name = QFileDialog::getOpenFileName(
02168       &loader_,
02169       tr("Load sound sensor"), 
02170       QString().fromStdString(
02171         stdr_gui_tools::getRosPackagePath("stdr_resources")) + 
02172         QString("/resources/"), 
02173         tr("Resource Files (*.yaml *.xml)"));
02174     
02175     if (file_name.isEmpty()) {
02176       return;
02177     }
02178     stdr_msgs::SoundSensorMsg smsg;
02179     std::string old_frame_id = item->text(0).toStdString();
02180     try {
02181       smsg = 
02182         stdr_parser::Parser::createMessage<stdr_msgs::SoundSensorMsg>
02183           (file_name.toStdString());
02184     }
02185     catch(stdr_parser::ParserException ex)
02186     {
02187       QMessageBox msg;
02188       msg.setWindowTitle(QString("STDR Parser - Error"));
02189       msg.setText(QString(ex.what()));
02190       msg.exec();
02191       return;
02192     }
02193     smsg = stdr_gui_tools::fixSoundAnglesToDegrees(smsg);
02194     smsg.frame_id = old_frame_id;
02195     new_robot_msg_.soundSensors[frameId]=smsg;
02196     updateSoundSensorTree(item,smsg);
02197     updateRobotPreview(); 
02198   }
02199   
02205   void CRobotCreatorConnector::editSonar(QTreeWidgetItem *item)
02206   {
02207     unsigned int sonarFrameId = searchSonar(item->text(0));
02208     if(sonarFrameId == -1) 
02209     {
02210       return;
02211     }  
02212     loader_.sonarPropLoader.sonarMaxDistance->setText(
02213       QString().setNum(new_robot_msg_.sonarSensors[sonarFrameId].maxRange));
02214     
02215     loader_.sonarPropLoader.sonarMinDistance->setText(
02216       QString().setNum(new_robot_msg_.sonarSensors[sonarFrameId].minRange));
02217     
02218     loader_.sonarPropLoader.sonarX->setText(
02219       QString().setNum(new_robot_msg_.sonarSensors[sonarFrameId].pose.x));
02220     
02221     loader_.sonarPropLoader.sonarY->setText(
02222       QString().setNum(new_robot_msg_.sonarSensors[sonarFrameId].pose.y));
02223     
02224     loader_.sonarPropLoader.sonarConeSpan->setText(
02225       QString().setNum(new_robot_msg_.sonarSensors[sonarFrameId].coneAngle));
02226     
02227     loader_.sonarPropLoader.sonarNoiseMean->setText(
02228       QString().setNum(
02229         new_robot_msg_.sonarSensors[sonarFrameId].noise.noiseMean));
02230     
02231     loader_.sonarPropLoader.sonarNoiseStd->setText(
02232       QString().setNum(
02233         new_robot_msg_.sonarSensors[sonarFrameId].noise.noiseStd));
02234     
02235     loader_.sonarPropLoader.sonarOrientation->setText(
02236       QString().setNum(new_robot_msg_.sonarSensors[sonarFrameId].pose.theta));
02237     
02238     loader_.sonarPropLoader.sonarFrequency->setText(
02239       QString().setNum(new_robot_msg_.sonarSensors[sonarFrameId].frequency));
02240     
02241     loader_.sonarPropLoader.setWindowTitle(
02242       QApplication::translate(
02243         "SonarProperties", 
02244         item->text(0).toStdString().c_str(), 
02245         0, 
02246         QApplication::UnicodeUTF8));
02247     
02248     current_sonar_ = item;
02249     
02250     loader_.sonarPropLoader.show();
02251   }
02252   
02258   void CRobotCreatorConnector::editRfid(QTreeWidgetItem *item)
02259   {
02260     unsigned int frameId = searchRfid(item->text(0));
02261     if(frameId == -1) 
02262     {
02263       return;
02264     }  
02265     loader_.rfidAntennaPropLoader.rfidMaxDistance->setText(
02266       QString().setNum(new_robot_msg_.rfidSensors[frameId].maxRange));
02267   
02268     loader_.rfidAntennaPropLoader.rfidX->setText(
02269       QString().setNum(new_robot_msg_.rfidSensors[frameId].pose.x));
02270   
02271     loader_.rfidAntennaPropLoader.rfidY->setText(
02272       QString().setNum(new_robot_msg_.rfidSensors[frameId].pose.y));
02273   
02274     loader_.rfidAntennaPropLoader.rfidAngleSpan->setText(
02275       QString().setNum(new_robot_msg_.rfidSensors[frameId].angleSpan));
02276   
02277     loader_.rfidAntennaPropLoader.rfidOrientation->setText(
02278       QString().setNum(new_robot_msg_.rfidSensors[frameId].pose.theta));
02279   
02280     loader_.rfidAntennaPropLoader.rfidSignalCutoff->setText(
02281       QString().setNum(new_robot_msg_.rfidSensors[frameId].signalCutoff));
02282   
02283     loader_.rfidAntennaPropLoader.rfidFrequency->setText(
02284       QString().setNum(new_robot_msg_.rfidSensors[frameId].frequency));
02285     
02286     loader_.rfidAntennaPropLoader.setWindowTitle(
02287       QApplication::translate(
02288         "RfidAntennaProperties", 
02289         item->text(0).toStdString().c_str(), 
02290         0, 
02291         QApplication::UnicodeUTF8));
02292     
02293     current_rfid_ = item;
02294     
02295     loader_.rfidAntennaPropLoader.show();
02296   }
02301   void CRobotCreatorConnector::editCO2Sensor(QTreeWidgetItem *item)
02302   {
02303     unsigned int frameId = searchCO2Sensor(item->text(0));
02304     if(frameId == -1) 
02305     {
02306       return;
02307     }  
02308     loader_.co2SensorPropLoader.maxDistance->setText(
02309       QString().setNum(new_robot_msg_.co2Sensors[frameId].maxRange));
02310   
02311     loader_.co2SensorPropLoader.x_->setText(
02312       QString().setNum(new_robot_msg_.co2Sensors[frameId].pose.x));
02313   
02314     loader_.co2SensorPropLoader.y_->setText(
02315       QString().setNum(new_robot_msg_.co2Sensors[frameId].pose.y));
02316   
02317     //~ loader_.co2SensorPropLoader.angleSpan->setText(
02318       //~ QString().setNum(new_robot_msg_.co2Sensors[frameId].angleSpan));
02319   
02320     loader_.co2SensorPropLoader.orientation->setText(
02321       QString().setNum(new_robot_msg_.co2Sensors[frameId].pose.theta));
02322   
02323     //~ loader_.co2SensorPropLoader.signalCutoff->setText(
02324       //~ QString().setNum(new_robot_msg_.co2Sensors[frameId].signalCutoff));
02325   
02326     loader_.co2SensorPropLoader.frequency->setText(
02327       QString().setNum(new_robot_msg_.co2Sensors[frameId].frequency));
02328     
02329     loader_.co2SensorPropLoader.setWindowTitle(
02330       QApplication::translate(
02331         "CO2SensorProperties", 
02332         item->text(0).toStdString().c_str(), 
02333         0, 
02334         QApplication::UnicodeUTF8));
02335     
02336     current_co2_sensor_ = item;
02337     
02338     loader_.co2SensorPropLoader.show();
02339   }
02344   void CRobotCreatorConnector::editThermalSensor(QTreeWidgetItem *item)
02345   {
02346     unsigned int frameId = searchThermalSensor(item->text(0));
02347     if(frameId == -1) 
02348     {
02349       return;
02350     }  
02351     loader_.thermalSensorPropLoader.maxDistance->setText(
02352       QString().setNum(new_robot_msg_.thermalSensors[frameId].maxRange));
02353   
02354     loader_.thermalSensorPropLoader.x_->setText(
02355       QString().setNum(new_robot_msg_.thermalSensors[frameId].pose.x));
02356   
02357     loader_.thermalSensorPropLoader.y_->setText(
02358       QString().setNum(new_robot_msg_.thermalSensors[frameId].pose.y));
02359   
02360     loader_.thermalSensorPropLoader.angleSpan->setText(
02361       QString().setNum(new_robot_msg_.thermalSensors[frameId].angleSpan));
02362   
02363     loader_.thermalSensorPropLoader.orientation->setText(
02364       QString().setNum(new_robot_msg_.thermalSensors[frameId].pose.theta));
02365 
02366     loader_.thermalSensorPropLoader.frequency->setText(
02367       QString().setNum(new_robot_msg_.thermalSensors[frameId].frequency));
02368     
02369     loader_.thermalSensorPropLoader.setWindowTitle(
02370       QApplication::translate(
02371         "ThermalSensorProperties", 
02372         item->text(0).toStdString().c_str(), 
02373         0, 
02374         QApplication::UnicodeUTF8));
02375     
02376     current_thermal_sensor_ = item;
02377     
02378     loader_.thermalSensorPropLoader.show();
02379   }
02384   void CRobotCreatorConnector::editSoundSensor(QTreeWidgetItem *item)
02385   {
02386     unsigned int frameId = searchSoundSensor(item->text(0));
02387     if(frameId == -1) 
02388     {
02389       return;
02390     }  
02391     loader_.soundSensorPropLoader.maxDistance->setText(
02392       QString().setNum(new_robot_msg_.soundSensors[frameId].maxRange));
02393   
02394     loader_.soundSensorPropLoader.x_->setText(
02395       QString().setNum(new_robot_msg_.soundSensors[frameId].pose.x));
02396   
02397     loader_.soundSensorPropLoader.y_->setText(
02398       QString().setNum(new_robot_msg_.soundSensors[frameId].pose.y));
02399   
02400     loader_.soundSensorPropLoader.angleSpan->setText(
02401       QString().setNum(new_robot_msg_.soundSensors[frameId].angleSpan));
02402   
02403     loader_.soundSensorPropLoader.orientation->setText(
02404       QString().setNum(new_robot_msg_.soundSensors[frameId].pose.theta));
02405 
02406     loader_.soundSensorPropLoader.frequency->setText(
02407       QString().setNum(new_robot_msg_.soundSensors[frameId].frequency));
02408     
02409     loader_.soundSensorPropLoader.setWindowTitle(
02410       QApplication::translate(
02411         "SoundSensorProperties", 
02412         item->text(0).toStdString().c_str(), 
02413         0, 
02414         QApplication::UnicodeUTF8));
02415     
02416     current_sound_sensor_ = item;
02417     
02418     loader_.soundSensorPropLoader.show();
02419   }
02420   
02426   int CRobotCreatorConnector::searchLaser(QString frameId)
02427   {
02428     for(unsigned int i = 0 ; i < new_robot_msg_.laserSensors.size() ; i++)
02429     {
02430       if(new_robot_msg_.laserSensors[i].frame_id == frameId.toStdString())
02431       {
02432         return i;
02433       }
02434     }
02435     return -1;
02436   }
02437   
02443   int CRobotCreatorConnector::searchSonar(QString frameId)
02444   {
02445     for(unsigned int i = 0 ; i < new_robot_msg_.sonarSensors.size() ; i++)
02446     {
02447       if(new_robot_msg_.sonarSensors[i].frame_id == frameId.toStdString())
02448       {
02449         return i;
02450       }
02451     }
02452     return -1;
02453   }
02454   
02460   int CRobotCreatorConnector::searchRfid(QString frameId)
02461   {
02462     for(unsigned int i = 0 ; i < new_robot_msg_.rfidSensors.size() ; i++)
02463     {
02464       if(new_robot_msg_.rfidSensors[i].frame_id == frameId.toStdString())
02465       {
02466         return i;
02467       }
02468     }
02469     return -1;
02470   }
02476   int CRobotCreatorConnector::searchCO2Sensor(QString frameId)
02477   {
02478     for(unsigned int i = 0 ; i < new_robot_msg_.co2Sensors.size() ; i++)
02479     {
02480       if(new_robot_msg_.co2Sensors[i].frame_id == frameId.toStdString())
02481       {
02482         return i;
02483       }
02484     }
02485     return -1;
02486   }
02492   int CRobotCreatorConnector::searchThermalSensor(QString frameId)
02493   {
02494     for(unsigned int i = 0 ; i < new_robot_msg_.thermalSensors.size() ; i++)
02495     {
02496       if(new_robot_msg_.thermalSensors[i].frame_id == frameId.toStdString())
02497       {
02498         return i;
02499       }
02500     }
02501     return -1;
02502   }
02508   int CRobotCreatorConnector::searchSoundSensor(QString frameId)
02509   {
02510     for(unsigned int i = 0 ; i < new_robot_msg_.soundSensors.size() ; i++)
02511     {
02512       if(new_robot_msg_.soundSensors[i].frame_id == frameId.toStdString())
02513       {
02514         return i;
02515       }
02516     }
02517     return -1;
02518   }
02519   
02524   void CRobotCreatorConnector::updateRobotTree(void)
02525   {
02526     for(unsigned int i = 0 ; i < loader_.robotNode.childCount() ; i++)
02527     {
02528       if(loader_.robotNode.child(i)->text(0) == QString("Orientation"))
02529       {
02530         loader_.robotNode.child(i)->
02531           setText(1,QString().setNum(new_robot_msg_.initialPose.theta));
02532       }
02533       if(loader_.robotNode.child(i)->text(0) == QString("Radius"))
02534       {
02535         loader_.robotNode.child(i)->
02536           setText(1,QString().setNum(new_robot_msg_.footprint.radius));
02537       }
02538     }
02539     int count = 0;
02540     
02541     count = loader_.lasersNode.childCount();
02542     for(int i = count - 1 ; i >= 0 ; i--)
02543     {
02544       QTreeWidgetItem *child = loader_.lasersNode.child(i);
02545       loader_.lasersNode.removeChild(loader_.lasersNode.child(i));
02546     }
02547     for(unsigned int i = 0 ; i < new_robot_msg_.laserSensors.size() ; i++)
02548     {
02549       addLaser(new_robot_msg_.laserSensors[i]);
02550     }
02551     
02552     count = loader_.rfidAntennasNode.childCount();
02553     for(int i = count - 1 ; i >= 0 ; i--)
02554     {
02555       QTreeWidgetItem *child = loader_.rfidAntennasNode.child(i);
02556       loader_.rfidAntennasNode.removeChild(loader_.rfidAntennasNode.child(i));
02557     }
02558     for(unsigned int i = 0 ; i < new_robot_msg_.rfidSensors.size() ; i++)
02559     {
02560       addRfidAntenna(new_robot_msg_.rfidSensors[i]);
02561     }
02562     
02563     count = loader_.thermalSensorsNode.childCount();
02564     for(int i = count - 1 ; i >= 0 ; i--)
02565     {
02566       QTreeWidgetItem *child = loader_.thermalSensorsNode.child(i);
02567       loader_.thermalSensorsNode.removeChild(loader_.thermalSensorsNode.child(i));
02568     }
02569     for(unsigned int i = 0 ; i < new_robot_msg_.thermalSensors.size() ; i++)
02570     {
02571       addThermalSensor(new_robot_msg_.thermalSensors[i]);
02572     }
02573     
02574     count = loader_.soundSensorsNode.childCount();
02575     for(int i = count - 1 ; i >= 0 ; i--)
02576     {
02577       QTreeWidgetItem *child = loader_.soundSensorsNode.child(i);
02578       loader_.soundSensorsNode.removeChild(loader_.soundSensorsNode.child(i));
02579     }
02580     for(unsigned int i = 0 ; i < new_robot_msg_.soundSensors.size() ; i++)
02581     {
02582       addSoundSensor(new_robot_msg_.soundSensors[i]);
02583     }
02584     
02585     count = loader_.sonarsNode.childCount();
02586     for(int i = count - 1 ; i >= 0 ; i--)
02587     {
02588       QTreeWidgetItem *child = loader_.sonarsNode.child(i);
02589       loader_.sonarsNode.removeChild(loader_.sonarsNode.child(i));
02590     }
02591     for(unsigned int i = 0 ; i < new_robot_msg_.sonarSensors.size() ; i++)
02592     {
02593       addSonar(new_robot_msg_.sonarSensors[i]);
02594     }
02595     
02596     for(unsigned int i = 0 ; i < new_robot_msg_.footprint.points.size() ; i++)
02597     {
02598       addFootprintPoint(new_robot_msg_.footprint.points[i]);
02599     }
02600   }
02601   
02608   void CRobotCreatorConnector::updateLaserTree(
02609     QTreeWidgetItem *item,
02610     stdr_msgs::LaserSensorMsg l)
02611   {
02612     for(unsigned int i = 0 ; i < item->childCount() ; i++)
02613     {
02614       if(item->child(i)->text(0) == QString("Angle span"))
02615       {
02616         item->child(i)->setText(1,QString().setNum(l.maxAngle - l.minAngle));
02617       }
02618       else if(item->child(i)->text(0) == QString("Orientation"))
02619       {
02620         item->child(i)->setText(1,QString().setNum(l.pose.theta));
02621       }
02622       else if(item->child(i)->text(0) == QString("Max range"))
02623       {
02624         item->child(i)->setText(1,QString().setNum(l.maxRange));
02625       }
02626       else if(item->child(i)->text(0) == QString("Number of rays"))
02627       {
02628         item->child(i)->setText(1,QString().setNum(l.numRays));
02629       }
02630       else if(item->child(i)->text(0) == QString("Min range"))
02631       {
02632         item->child(i)->setText(1,QString().setNum(l.minRange));
02633       }
02634       else if(item->child(i)->text(0) == QString("Noise mean"))
02635       {
02636         item->child(i)->setText(1,QString().setNum(l.noise.noiseMean));
02637       }
02638       else if(item->child(i)->text(0) == QString("Noise std"))
02639       {
02640         item->child(i)->setText(1,QString().setNum(l.noise.noiseStd));
02641       }
02642       else if(item->child(i)->text(0) == QString("Pose - x"))
02643       {
02644         item->child(i)->setText(1,QString().setNum(l.pose.x));
02645       }
02646       else if(item->child(i)->text(0) == QString("Pose - y"))
02647       {
02648         item->child(i)->setText(1,QString().setNum(l.pose.y));
02649       }
02650       else if(item->child(i)->text(0) == QString("Frequency"))
02651       {
02652         item->child(i)->setText(1,QString().setNum(l.frequency));
02653       }
02654     }
02655   }
02656   
02661   void CRobotCreatorConnector::updateLaser(void)
02662   {
02663     unsigned int laserFrameId = searchLaser(current_laser_->text(0));
02664     if(laserFrameId == -1) 
02665     {
02666       return;
02667     }
02668     float max_range = 1000.0;
02669     float min_range = 0.0;
02670     for(unsigned int i = 0 ; i < current_laser_->childCount() ; i++)
02671     {
02672       
02674       if(current_laser_->child(i)->text(0) == QString("Angle span"))
02675       {
02676         current_laser_->child(i)->setText(
02677           1,loader_.laserPropLoader.laserAngleSpan->text());
02678         float angleSpan = loader_.laserPropLoader.laserAngleSpan->
02679           text().toFloat() / 2.0;
02680         if( angleSpan <= 0 )
02681         {
02682           showMessage(QString("Laser angle span invalid :") + 
02683             QString().setNum(angleSpan * 2.0));
02684           return;
02685         }
02686         new_robot_msg_.laserSensors[laserFrameId].minAngle = - angleSpan;
02687         new_robot_msg_.laserSensors[laserFrameId].maxAngle = angleSpan;
02688       }
02689       
02691       else if(current_laser_->child(i)->text(0) == QString("Orientation"))
02692       {
02693         float orientation = loader_.laserPropLoader.laserOrientation->
02694           text().toFloat();
02695         current_laser_->child(i)->setText(
02696           1,QString().setNum(orientation));
02697         //~ new_robot_msg_.laserSensors[laserFrameId].minAngle += orientation;
02698         //~ new_robot_msg_.laserSensors[laserFrameId].maxAngle += orientation;
02699         new_robot_msg_.laserSensors[laserFrameId].pose.theta = orientation;
02700       }
02701       
02703       else if(current_laser_->child(i)->text(0) == QString("Max range"))
02704       {
02705         max_range = loader_.laserPropLoader.laserMaxDistance->
02706           text().toFloat();
02707         if( max_range <= 0 )
02708         {
02709           showMessage(QString("Laser maximum range invalid :") + 
02710             QString().setNum(max_range));
02711           return;
02712         }
02713         if( max_range < min_range )
02714         {
02715           showMessage(QString("Laser maximum range lower than minimum range"));
02716           return;
02717         }
02718         current_laser_->child(i)->setText(1,QString().setNum(max_range));
02719         new_robot_msg_.laserSensors[laserFrameId].maxRange = max_range;
02720       }
02721       
02723       else if(current_laser_->child(i)->text(0) == QString("Number of rays"))
02724       {
02725         int rays = loader_.laserPropLoader.laserRays->text().toFloat();
02726         if( rays <= 0 )
02727         {
02728           showMessage(QString("Laser rays number invalid :") + 
02729             QString().setNum(rays));
02730           return;
02731         }
02732         current_laser_->child(i)->setText(1,QString().setNum(rays));
02733         new_robot_msg_.laserSensors[laserFrameId].numRays = rays;
02734       }
02735       
02737       else if(current_laser_->child(i)->text(0) == QString("Min range"))
02738       {
02739         min_range = loader_.laserPropLoader.laserMinDistance->
02740           text().toFloat();
02741         if( min_range < 0 )
02742         {
02743           showMessage(QString("Laser minimum range invalid :") + 
02744             QString().setNum(min_range));
02745           return;
02746         }
02747         if( min_range > max_range )
02748         {
02749           showMessage(QString("Laser minimum range higher than maximum range"));
02750           return;
02751         }
02752         current_laser_->child(i)->setText(1,QString().setNum(min_range));
02753         new_robot_msg_.laserSensors[laserFrameId].minRange = min_range;
02754       }
02755       
02757       else if(current_laser_->child(i)->text(0) == QString("Noise mean"))
02758       {
02759         float noiseMean = loader_.laserPropLoader.laserNoiseMean->
02760           text().toFloat();
02761         current_laser_->child(i)->setText(1,QString().setNum(noiseMean));
02762         new_robot_msg_.laserSensors[laserFrameId].noise.noiseMean = 
02763           noiseMean;
02764       }
02765       
02767       else if(current_laser_->child(i)->text(0) == QString("Noise std"))
02768       {
02769         float noiseStd = 
02770           loader_.laserPropLoader.laserNoiseStd->text().toFloat();
02771         if( noiseStd < 0 )
02772         {
02773           showMessage(QString("Laser standard deviation of noise invalid :") + 
02774             QString().setNum(noiseStd));
02775           return;
02776         }
02777         current_laser_->child(i)->setText(1,QString().setNum(noiseStd));
02778         new_robot_msg_.laserSensors[laserFrameId].noise.noiseStd = 
02779           noiseStd;
02780       }
02781       
02783       else if(current_laser_->child(i)->text(0) == QString("Pose - x"))
02784       {
02785         float dx = loader_.laserPropLoader.laserTranslationX->
02786           text().toFloat();
02787         current_laser_->child(i)->setText(1,QString().setNum(dx));
02788         new_robot_msg_.laserSensors[laserFrameId].pose.x = dx;
02789       }
02790       
02792       else if(current_laser_->child(i)->text(0) == QString("Pose - y"))
02793       {
02794         float dy = loader_.laserPropLoader.laserTranslationY->
02795           text().toFloat();
02796         current_laser_->child(i)->setText(1,QString().setNum(dy));
02797         new_robot_msg_.laserSensors[laserFrameId].pose.y = dy;
02798       }
02799       
02801       else if(current_laser_->child(i)->text(0) == QString("Frequency"))
02802       {
02803         float frequency = loader_.laserPropLoader.laserFrequency->
02804           text().toFloat();
02805         if( frequency <= 0 )
02806         {
02807           showMessage(QString("Laser publishing frequency invalid :") + 
02808             QString().setNum(frequency));
02809           return;
02810         }
02811         current_laser_->child(i)->setText(1,QString().setNum(frequency));
02812         new_robot_msg_.laserSensors[laserFrameId].frequency = frequency;
02813       }
02814     }
02815 
02816     loader_.laserPropLoader.hide();
02817     
02818     updateRobotPreview();
02819   }
02820   
02825   void CRobotCreatorConnector::updateLaserOpen(void)
02826   {
02827     unsigned int laserFrameId = searchLaser(current_laser_->text(0));
02828     if(laserFrameId == -1) 
02829     {
02830       return;
02831     }
02832     float max_range = 1000.0;
02833     float min_range = 0.0;
02834     for(unsigned int i = 0 ; i < current_laser_->childCount() ; i++)
02835     {
02836       
02838       if(current_laser_->child(i)->text(0) == QString("Angle span"))
02839       {
02840         current_laser_->child(i)->setText(
02841           1,loader_.laserPropLoader.laserAngleSpan->text());
02842         float angleSpan = loader_.laserPropLoader.laserAngleSpan->
02843           text().toFloat() / 2.0;
02844         if( angleSpan <= 0 )
02845         {
02846           showMessage(QString("Laser angle span invalid :") + 
02847             QString().setNum(angleSpan * 2.0));
02848           return;
02849         }
02850         new_robot_msg_.laserSensors[laserFrameId].minAngle = - angleSpan;
02851         new_robot_msg_.laserSensors[laserFrameId].maxAngle = angleSpan;
02852       }
02853       
02855       else if(current_laser_->child(i)->text(0) == QString("Orientation"))
02856       {
02857         float orientation = loader_.laserPropLoader.laserOrientation->
02858           text().toFloat();
02859         current_laser_->child(i)->setText(
02860           1,QString().setNum(orientation));
02861         //~ new_robot_msg_.laserSensors[laserFrameId].minAngle += orientation;
02862         //~ new_robot_msg_.laserSensors[laserFrameId].maxAngle += orientation;
02863         new_robot_msg_.laserSensors[laserFrameId].pose.theta = orientation;
02864       }
02865       
02867       else if(current_laser_->child(i)->text(0) == QString("Max range"))
02868       {
02869         max_range = loader_.laserPropLoader.laserMaxDistance->
02870           text().toFloat();
02871         if( max_range <= 0 )
02872         {
02873           showMessage(QString("Laser maximum range invalid :") + 
02874             QString().setNum(max_range));
02875           return;
02876         }
02877         if( max_range < min_range )
02878         {
02879           showMessage(QString("Laser maximum range lower than minimum range"));
02880           return;
02881         }
02882         current_laser_->child(i)->setText(1,QString().setNum(max_range));
02883         new_robot_msg_.laserSensors[laserFrameId].maxRange = max_range;
02884       }
02885       
02887       else if(current_laser_->child(i)->text(0) == QString("Number of rays"))
02888       {
02889         int rays = loader_.laserPropLoader.laserRays->text().toFloat();
02890         if( rays <= 0 )
02891         {
02892           showMessage(QString("Laser rays number invalid :") + 
02893             QString().setNum(rays));
02894           return;
02895         }
02896         current_laser_->child(i)->setText(1,QString().setNum(rays));
02897         new_robot_msg_.laserSensors[laserFrameId].numRays = rays;
02898       }
02899       
02901       else if(current_laser_->child(i)->text(0) == QString("Min range"))
02902       {
02903         min_range = loader_.laserPropLoader.laserMinDistance->
02904           text().toFloat();
02905         if( min_range < 0 )
02906         {
02907           showMessage(QString("Laser minimum range invalid :") + 
02908             QString().setNum(min_range));
02909           return;
02910         }
02911         if( min_range > max_range )
02912         {
02913           showMessage(QString("Laser minimum range higher than maximum range"));
02914           return;
02915         }
02916         current_laser_->child(i)->setText(1,QString().setNum(min_range));
02917         new_robot_msg_.laserSensors[laserFrameId].minRange = min_range;
02918       }
02919       
02921       else if(current_laser_->child(i)->text(0) == QString("Noise mean"))
02922       {
02923         float noiseMean = loader_.laserPropLoader.laserNoiseMean->
02924           text().toFloat();
02925         current_laser_->child(i)->setText(1,QString().setNum(noiseMean));
02926         new_robot_msg_.laserSensors[laserFrameId].noise.noiseMean = 
02927           noiseMean;
02928       }
02929       
02931       else if(current_laser_->child(i)->text(0) == QString("Noise std"))
02932       {
02933         float noiseStd = 
02934           loader_.laserPropLoader.laserNoiseStd->text().toFloat();
02935         if( noiseStd < 0 )
02936         {
02937           showMessage(QString("Laser standard deviation of noise invalid :") + 
02938             QString().setNum(noiseStd));
02939           return;
02940         }
02941         current_laser_->child(i)->setText(1,QString().setNum(noiseStd));
02942         new_robot_msg_.laserSensors[laserFrameId].noise.noiseStd = 
02943           noiseStd;
02944       }
02945       
02947       else if(current_laser_->child(i)->text(0) == QString("Pose - x"))
02948       {
02949         float dx = loader_.laserPropLoader.laserTranslationX->
02950           text().toFloat();
02951         current_laser_->child(i)->setText(1,QString().setNum(dx));
02952         new_robot_msg_.laserSensors[laserFrameId].pose.x = dx;
02953       }
02954       
02956       else if(current_laser_->child(i)->text(0) == QString("Pose - y"))
02957       {
02958         float dy = loader_.laserPropLoader.laserTranslationY->
02959           text().toFloat();
02960         current_laser_->child(i)->setText(1,QString().setNum(dy));
02961         new_robot_msg_.laserSensors[laserFrameId].pose.y = dy;
02962       }
02963       
02965       else if(current_laser_->child(i)->text(0) == QString("Frequency"))
02966       {
02967         float frequency = loader_.laserPropLoader.laserFrequency->
02968           text().toFloat();
02969         if( frequency <= 0 )
02970         {
02971           showMessage(QString("Laser publishing frequency invalid :") + 
02972             QString().setNum(frequency));
02973           return;
02974         }
02975         current_laser_->child(i)->setText(1,QString().setNum(frequency));
02976         new_robot_msg_.laserSensors[laserFrameId].frequency = frequency;
02977       }
02978     }
02979 
02980     updateRobotPreview();
02981   }
02982   
02983   
02988   void CRobotCreatorConnector::updateSonar(void)
02989   {
02990     unsigned int frameId=searchSonar(current_sonar_->text(0));
02991     if(frameId == -1)
02992     {
02993       return;
02994     }
02995     float min_range = 0;
02996     float max_range = 10000.0;
02997     for(unsigned int i = 0 ; i < current_sonar_->childCount() ; i++)
02998     {
03000       if(current_sonar_->child(i)->text(0) == QString("Cone span"))
03001       {
03002         float cone_span = 
03003           loader_.sonarPropLoader.sonarConeSpan->text().toFloat();
03004         if( cone_span <= 0 )
03005         {
03006           showMessage(QString("Sonar cone span invalid :") + 
03007             QString().setNum(cone_span));
03008           return;
03009         }
03010         current_sonar_->child(i)->setText(1,QString().setNum(cone_span));
03011         new_robot_msg_.sonarSensors[frameId].coneAngle = cone_span;
03012       }
03013       
03015       else if(current_sonar_->child(i)->text(0) == QString("Orientation"))
03016       {
03017         float orientation = 
03018           loader_.sonarPropLoader.sonarOrientation->text().toFloat();
03019         current_sonar_->child(i)->
03020           setText(1,QString().setNum(orientation));
03021         new_robot_msg_.sonarSensors[frameId].pose.theta = orientation;
03022       }
03023       
03025       else if(current_sonar_->child(i)->text(0) == QString("Max range"))
03026       {
03027         max_range = loader_.sonarPropLoader.sonarMaxDistance->text().toFloat();
03028         if( max_range <= 0 )
03029         {
03030           showMessage(QString("Sonar maximum range invalid :") + 
03031             QString().setNum(max_range));
03032           return;
03033         }
03034         if( max_range < min_range )
03035         {
03036           showMessage(QString("Sonar maximum range lower than minimum range."));
03037           return;
03038         }
03039         current_sonar_->child(i)->
03040           setText(1,QString().setNum(max_range));
03041         new_robot_msg_.sonarSensors[frameId].maxRange = max_range;
03042       }
03043       
03045       else if(current_sonar_->child(i)->text(0) == QString("Min range"))
03046       {
03047         min_range = loader_.sonarPropLoader.sonarMinDistance->text().toFloat();
03048         if( min_range < 0 )
03049         {
03050           showMessage(QString("Sonar minimum range invalid :") + 
03051             QString().setNum(min_range));
03052           return;
03053         }
03054         if( max_range < min_range )
03055         {
03056           showMessage(
03057             QString("Sonar minimum range higher than maximum range."));
03058           return;
03059         }
03060         current_sonar_->child(i)->setText(1,QString().setNum(min_range));
03061         new_robot_msg_.sonarSensors[frameId].minRange = min_range;
03062       }
03063       
03065       else if(current_sonar_->child(i)->text(0) == QString("Noise mean"))
03066       {
03067         float noiseMean = 
03068           loader_.sonarPropLoader.sonarNoiseMean->text().toFloat();
03069         current_sonar_->child(i)->
03070           setText(1,QString().setNum(noiseMean));
03071         new_robot_msg_.sonarSensors[frameId].noise.noiseMean = noiseMean;
03072       }
03073       
03075       else if(current_sonar_->child(i)->text(0) == QString("Noise std"))
03076       {
03077         float noiseStd = 
03078           loader_.sonarPropLoader.sonarNoiseStd->text().toFloat();
03079         if( noiseStd < 0 )
03080         {
03081           showMessage(QString("Sonar noise standard deviation invalid :") + 
03082             QString().setNum(noiseStd));
03083           return;
03084         }
03085         current_sonar_->
03086           child(i)->setText(1,QString().setNum(noiseStd));
03087         new_robot_msg_.sonarSensors[frameId].noise.noiseStd = noiseStd;
03088       }
03089       
03091       else if(current_sonar_->child(i)->text(0) == QString("Pose - x"))
03092       {
03093         float dx = loader_.sonarPropLoader.sonarX->text().toFloat();
03094         current_sonar_->child(i)->setText(1,QString().setNum(dx));
03095         new_robot_msg_.sonarSensors[frameId].pose.x = dx;
03096       }
03097       
03099       else if(current_sonar_->child(i)->text(0) == QString("Pose - y"))
03100       {
03101         float dy = loader_.sonarPropLoader.sonarY->text().toFloat();
03102         current_sonar_->child(i)->setText(1,QString().setNum(dy));
03103         new_robot_msg_.sonarSensors[frameId].pose.y = dy;
03104       }
03105       
03107       else if(current_sonar_->child(i)->text(0) == QString("Frequency"))
03108       {
03109         float frequency = 
03110           loader_.sonarPropLoader.sonarFrequency->text().toFloat();
03111         if( frequency <= 0 )
03112         {
03113           showMessage(QString("Sonar publishing frequency invalid :") + 
03114             QString().setNum(frequency));
03115           return;
03116         }
03117         current_sonar_->child(i)->setText(1,QString().setNum(frequency));
03118         new_robot_msg_.sonarSensors[frameId].frequency = frequency;
03119       }
03120     }
03121     
03122     loader_.sonarPropLoader.hide();
03123     
03124     updateRobotPreview();
03125   }
03126   
03131   void CRobotCreatorConnector::updateSonarOpen(void)
03132   {
03133     unsigned int frameId=searchSonar(current_sonar_->text(0));
03134     if(frameId == -1)
03135     {
03136       return;
03137     }
03138     float min_range = 0;
03139     float max_range = 10000.0;
03140     for(unsigned int i = 0 ; i < current_sonar_->childCount() ; i++)
03141     {
03143       if(current_sonar_->child(i)->text(0) == QString("Cone span"))
03144       {
03145         float cone_span = 
03146           loader_.sonarPropLoader.sonarConeSpan->text().toFloat();
03147         if( cone_span <= 0 )
03148         {
03149           showMessage(QString("Sonar cone span invalid :") + 
03150             QString().setNum(cone_span));
03151           return;
03152         }
03153         current_sonar_->child(i)->setText(1,QString().setNum(cone_span));
03154         new_robot_msg_.sonarSensors[frameId].coneAngle = cone_span;
03155       }
03156       
03158       else if(current_sonar_->child(i)->text(0) == QString("Orientation"))
03159       {
03160         float orientation = 
03161           loader_.sonarPropLoader.sonarOrientation->text().toFloat();
03162         current_sonar_->child(i)->
03163           setText(1,QString().setNum(orientation));
03164         new_robot_msg_.sonarSensors[frameId].pose.theta = orientation;
03165       }
03166       
03168       else if(current_sonar_->child(i)->text(0) == QString("Max range"))
03169       {
03170         max_range = loader_.sonarPropLoader.sonarMaxDistance->text().toFloat();
03171         if( max_range <= 0 )
03172         {
03173           showMessage(QString("Sonar maximum range invalid :") + 
03174             QString().setNum(max_range));
03175           return;
03176         }
03177         if( max_range < min_range )
03178         {
03179           showMessage(QString("Sonar maximum range lower than minimum range."));
03180           return;
03181         }
03182         current_sonar_->child(i)->
03183           setText(1,QString().setNum(max_range));
03184         new_robot_msg_.sonarSensors[frameId].maxRange = max_range;
03185       }
03186       
03188       else if(current_sonar_->child(i)->text(0) == QString("Min range"))
03189       {
03190         min_range = loader_.sonarPropLoader.sonarMinDistance->text().toFloat();
03191         if( min_range < 0 )
03192         {
03193           showMessage(QString("Sonar minimum range invalid :") + 
03194             QString().setNum(min_range));
03195           return;
03196         }
03197         if( max_range < min_range )
03198         {
03199           showMessage(
03200             QString("Sonar minimum range higher than maximum range."));
03201           return;
03202         }
03203         current_sonar_->child(i)->setText(1,QString().setNum(min_range));
03204         new_robot_msg_.sonarSensors[frameId].minRange = min_range;
03205       }
03206       
03208       else if(current_sonar_->child(i)->text(0) == QString("Noise mean"))
03209       {
03210         float noiseMean = 
03211           loader_.sonarPropLoader.sonarNoiseMean->text().toFloat();
03212         current_sonar_->child(i)->
03213           setText(1,QString().setNum(noiseMean));
03214         new_robot_msg_.sonarSensors[frameId].noise.noiseMean = noiseMean;
03215       }
03216       
03218       else if(current_sonar_->child(i)->text(0) == QString("Noise std"))
03219       {
03220         float noiseStd = 
03221           loader_.sonarPropLoader.sonarNoiseStd->text().toFloat();
03222         if( noiseStd < 0 )
03223         {
03224           showMessage(QString("Sonar noise standard deviation invalid :") + 
03225             QString().setNum(noiseStd));
03226           return;
03227         }
03228         current_sonar_->
03229           child(i)->setText(1,QString().setNum(noiseStd));
03230         new_robot_msg_.sonarSensors[frameId].noise.noiseStd = noiseStd;
03231       }
03232       
03234       else if(current_sonar_->child(i)->text(0) == QString("Pose - x"))
03235       {
03236         float dx = loader_.sonarPropLoader.sonarX->text().toFloat();
03237         current_sonar_->child(i)->setText(1,QString().setNum(dx));
03238         new_robot_msg_.sonarSensors[frameId].pose.x = dx;
03239       }
03240       
03242       else if(current_sonar_->child(i)->text(0) == QString("Pose - y"))
03243       {
03244         float dy = loader_.sonarPropLoader.sonarY->text().toFloat();
03245         current_sonar_->child(i)->setText(1,QString().setNum(dy));
03246         new_robot_msg_.sonarSensors[frameId].pose.y = dy;
03247       }
03248       
03250       else if(current_sonar_->child(i)->text(0) == QString("Frequency"))
03251       {
03252         float frequency = 
03253           loader_.sonarPropLoader.sonarFrequency->text().toFloat();
03254         if( frequency <= 0 )
03255         {
03256           showMessage(QString("Sonar publishing frequency invalid :") + 
03257             QString().setNum(frequency));
03258           return;
03259         }
03260         current_sonar_->child(i)->setText(1,QString().setNum(frequency));
03261         new_robot_msg_.sonarSensors[frameId].frequency = frequency;
03262       }
03263     }
03264 
03265     updateRobotPreview();
03266   }
03267   
03268   
03275   void CRobotCreatorConnector::updateSonarTree(
03276     QTreeWidgetItem *item,
03277     stdr_msgs::SonarSensorMsg l)
03278   {
03279     unsigned int frameId=searchSonar(item->text(0));
03280     if(frameId == -1)
03281     {
03282       return;
03283     }
03284     for(unsigned int i = 0 ; i < item->childCount() ; i++)
03285     {
03286       if(item->child(i)->text(0) == QString("Cone span"))
03287       {
03288         item->child(i)->setText(1,QString().setNum(l.coneAngle));
03289       }
03290       else if(item->child(i)->text(0) == QString("Orientation"))
03291       {
03292         item->child(i)->setText(1,QString().setNum(l.pose.theta));
03293       }
03294       else if(item->child(i)->text(0) == QString("Max range"))
03295       {
03296         item->child(i)->setText(1,QString().setNum(l.maxRange));
03297       }
03298       else if(item->child(i)->text(0) == QString("Min range"))
03299       {
03300         item->child(i)->setText(1,QString().setNum(l.minRange));
03301       }
03302       else if(item->child(i)->text(0) == QString("Noise mean"))
03303       {
03304         item->child(i)->setText(1,QString().setNum(l.noise.noiseMean));
03305       }
03306       else if(item->child(i)->text(0) == QString("Noise std"))
03307       {
03308         item->child(i)->setText(1,QString().setNum(l.noise.noiseStd));
03309       }
03310       else if(item->child(i)->text(0) == QString("Pose - x"))
03311       {
03312         item->child(i)->setText(1,QString().setNum(l.pose.x));
03313       }
03314       else if(item->child(i)->text(0) == QString("Pose - y"))
03315       {
03316         item->child(i)->setText(1,QString().setNum(l.pose.y));
03317       }
03318       else if(item->child(i)->text(0) == QString("Frequency"))
03319       {
03320         item->child(i)->setText(1,QString().setNum(l.frequency));
03321       }
03322     }
03323   }
03324   
03331   void CRobotCreatorConnector::updateRfidTree(
03332     QTreeWidgetItem *item,
03333     stdr_msgs::RfidSensorMsg l)
03334   {
03335     unsigned int frameId=searchRfid(item->text(0));
03336     if(frameId == -1)
03337     {
03338       return;
03339     }
03340     for(unsigned int i = 0 ; i < item->childCount() ; i++)
03341     {
03342       if(item->child(i)->text(0) == QString("Angle span"))
03343       {
03344         item->child(i)->setText(1,QString().setNum(l.angleSpan));
03345       }
03346       else if(item->child(i)->text(0) == QString("Orientation"))
03347       {
03348         item->child(i)->setText(1,QString().setNum(l.pose.theta));
03349       }
03350       else if(item->child(i)->text(0) == QString("Max range"))
03351       {
03352         item->child(i)->setText(1,QString().setNum(l.maxRange));
03353       }
03354       else if(item->child(i)->text(0) == QString("Signal cutoff"))
03355       {
03356         item->child(i)->setText(1,QString().setNum(l.signalCutoff));
03357       }
03358       else if(item->child(i)->text(0) == QString("Pose - x"))
03359       {
03360         item->child(i)->setText(1,QString().setNum(l.pose.x));
03361       }
03362       else if(item->child(i)->text(0) == QString("Pose - y"))
03363       {
03364         item->child(i)->setText(1,QString().setNum(l.pose.y));
03365       }
03366       else if(item->child(i)->text(0) == QString("Frequency"))
03367       {
03368         item->child(i)->setText(1,QString().setNum(l.frequency));
03369       }
03370     }
03371   }
03375   void CRobotCreatorConnector::updateCO2SensorTree(
03376     QTreeWidgetItem *item,
03377     stdr_msgs::CO2SensorMsg l)
03378   {
03379     unsigned int frameId=searchCO2Sensor(item->text(0));
03380     if(frameId == -1)
03381     {
03382       return;
03383     }
03384     for(unsigned int i = 0 ; i < item->childCount() ; i++)
03385     {
03386       //~ if(item->child(i)->text(0) == QString("Angle span"))
03387       //~ {
03388         //~ item->child(i)->setText(1,QString().setNum(l.angleSpan));
03389       //~ }
03390       if(item->child(i)->text(0) == QString("Orientation"))
03391       {
03392         item->child(i)->setText(1,QString().setNum(l.pose.theta));
03393       }
03394       else if(item->child(i)->text(0) == QString("Max range"))
03395       {
03396         item->child(i)->setText(1,QString().setNum(l.maxRange));
03397       }
03398       //~ else if(item->child(i)->text(0) == QString("Signal cutoff"))
03399       //~ {
03400         //~ item->child(i)->setText(1,QString().setNum(l.signalCutoff));
03401       //~ }
03402       else if(item->child(i)->text(0) == QString("Pose - x"))
03403       {
03404         item->child(i)->setText(1,QString().setNum(l.pose.x));
03405       }
03406       else if(item->child(i)->text(0) == QString("Pose - y"))
03407       {
03408         item->child(i)->setText(1,QString().setNum(l.pose.y));
03409       }
03410       else if(item->child(i)->text(0) == QString("Frequency"))
03411       {
03412         item->child(i)->setText(1,QString().setNum(l.frequency));
03413       }
03414     }
03415   }
03419   void CRobotCreatorConnector::updateThermalSensorTree(
03420     QTreeWidgetItem *item,
03421     stdr_msgs::ThermalSensorMsg l)
03422   {
03423     unsigned int frameId=searchThermalSensor(item->text(0));
03424     if(frameId == -1)
03425     {
03426       return;
03427     }
03428     for(unsigned int i = 0 ; i < item->childCount() ; i++)
03429     {
03430       if(item->child(i)->text(0) == QString("Angle span"))
03431       {
03432         item->child(i)->setText(1,QString().setNum(l.angleSpan));
03433       }
03434       else if(item->child(i)->text(0) == QString("Orientation"))
03435       {
03436         item->child(i)->setText(1,QString().setNum(l.pose.theta));
03437       }
03438       else if(item->child(i)->text(0) == QString("Max range"))
03439       {
03440         item->child(i)->setText(1,QString().setNum(l.maxRange));
03441       }
03442       //~ else if(item->child(i)->text(0) == QString("Signal cutoff"))
03443       //~ {
03444         //~ item->child(i)->setText(1,QString().setNum(l.signalCutoff));
03445       //~ }
03446       else if(item->child(i)->text(0) == QString("Pose - x"))
03447       {
03448         item->child(i)->setText(1,QString().setNum(l.pose.x));
03449       }
03450       else if(item->child(i)->text(0) == QString("Pose - y"))
03451       {
03452         item->child(i)->setText(1,QString().setNum(l.pose.y));
03453       }
03454       else if(item->child(i)->text(0) == QString("Frequency"))
03455       {
03456         item->child(i)->setText(1,QString().setNum(l.frequency));
03457       }
03458     }
03459   }
03463   void CRobotCreatorConnector::updateSoundSensorTree(
03464     QTreeWidgetItem *item,
03465     stdr_msgs::SoundSensorMsg l)
03466   {
03467     unsigned int frameId=searchSoundSensor(item->text(0));
03468     if(frameId == -1)
03469     {
03470       return;
03471     }
03472     for(unsigned int i = 0 ; i < item->childCount() ; i++)
03473     {
03474       if(item->child(i)->text(0) == QString("Angle span"))
03475       {
03476         item->child(i)->setText(1,QString().setNum(l.angleSpan));
03477       }
03478       else if(item->child(i)->text(0) == QString("Orientation"))
03479       {
03480         item->child(i)->setText(1,QString().setNum(l.pose.theta));
03481       }
03482       else if(item->child(i)->text(0) == QString("Max range"))
03483       {
03484         item->child(i)->setText(1,QString().setNum(l.maxRange));
03485       }
03486       //~ else if(item->child(i)->text(0) == QString("Signal cutoff"))
03487       //~ {
03488         //~ item->child(i)->setText(1,QString().setNum(l.signalCutoff));
03489       //~ }
03490       else if(item->child(i)->text(0) == QString("Pose - x"))
03491       {
03492         item->child(i)->setText(1,QString().setNum(l.pose.x));
03493       }
03494       else if(item->child(i)->text(0) == QString("Pose - y"))
03495       {
03496         item->child(i)->setText(1,QString().setNum(l.pose.y));
03497       }
03498       else if(item->child(i)->text(0) == QString("Frequency"))
03499       {
03500         item->child(i)->setText(1,QString().setNum(l.frequency));
03501       }
03502     }
03503   }
03504   
03509   void CRobotCreatorConnector::updateRfidAntenna(void)
03510   {
03511     unsigned int frameId = searchRfid(current_rfid_->text(0));
03512     if(frameId == -1)
03513     {
03514       return;
03515     }
03516     for(unsigned int i = 0 ; i < current_rfid_->childCount() ; i++)
03517     {
03518       
03520       if(current_rfid_->child(i)->text(0) == QString("Angle span"))
03521       {
03522         float angle_span = 
03523           loader_.rfidAntennaPropLoader.rfidAngleSpan->text().toFloat();
03524         if( angle_span <= 0 )
03525         {
03526           showMessage(QString("Rfid antenna angle span invalid :") + 
03527             QString().setNum(angle_span));
03528           return;
03529         }
03530         current_rfid_->child(i)->setText(1,QString().setNum(angle_span));
03531         new_robot_msg_.rfidSensors[frameId].angleSpan = angle_span;
03532       }
03533       
03535       else if(current_rfid_->child(i)->text(0) == QString("Orientation"))
03536       {
03537         float orientation = 
03538           loader_.rfidAntennaPropLoader.rfidOrientation->
03539             text().toFloat();
03540         current_rfid_->child(i)->
03541           setText(1,QString().setNum(orientation));
03542         new_robot_msg_.rfidSensors[frameId].pose.theta = orientation;
03543       }
03544       
03546       else if(current_rfid_->child(i)->text(0) == QString("Max range"))
03547       {
03548         float maxRange = 
03549           loader_.rfidAntennaPropLoader.rfidMaxDistance->
03550             text().toFloat();
03551         if( maxRange <= 0 )
03552         {
03553           showMessage(QString("Rfid antenna maximum range invalid :") + 
03554             QString().setNum(maxRange));
03555           return;
03556         }
03557         current_rfid_->child(i)->setText(1,QString().setNum(maxRange));
03558         new_robot_msg_.rfidSensors[frameId].maxRange = maxRange;
03559       }
03560       
03562       else if(current_rfid_->child(i)->text(0) == QString("Pose - x"))
03563       {
03564         float dx = loader_.rfidAntennaPropLoader.rfidX->text().toFloat();
03565         current_rfid_->child(i)->setText(1,QString().setNum(dx));
03566         new_robot_msg_.rfidSensors[frameId].pose.x = dx;
03567       }
03568       
03570       else if(current_rfid_->child(i)->text(0) == QString("Pose - y"))
03571       {
03572         float dy = loader_.rfidAntennaPropLoader.rfidY->text().toFloat();
03573         current_rfid_->child(i)->setText(1,QString().setNum(dy));
03574         new_robot_msg_.rfidSensors[frameId].pose.y = dy;
03575       }
03576       
03578       else if(current_rfid_->child(i)->text(0) == QString("Signal cutoff"))
03579       {
03580         float signal = 
03581           loader_.rfidAntennaPropLoader.rfidSignalCutoff->
03582             text().toFloat();
03583         current_rfid_->child(i)->setText(1,QString().setNum(signal));
03584         new_robot_msg_.rfidSensors[frameId].signalCutoff = signal;
03585       }
03586       
03588       else if(current_rfid_->child(i)->text(0) == QString("Frequency"))
03589       {
03590         float frequency = 
03591           loader_.rfidAntennaPropLoader.rfidFrequency->
03592             text().toFloat();
03593         if( frequency <= 0 )
03594         {
03595           showMessage(QString("Rfid antenna publishing frequency invalid :") + 
03596             QString().setNum(frequency));
03597           return;
03598         }
03599         current_rfid_->child(i)->setText(1,QString().setNum(frequency));
03600         new_robot_msg_.rfidSensors[frameId].frequency = frequency;
03601       }
03602     }
03603 
03604     loader_.rfidAntennaPropLoader.hide();
03605     
03606     updateRobotPreview();
03607   }
03611   void CRobotCreatorConnector::updateCO2Sensor(void)
03612   {
03613     unsigned int frameId = searchCO2Sensor(current_co2_sensor_->text(0));
03614     if(frameId == -1)
03615     {
03616       return;
03617     }
03618     for(unsigned int i = 0 ; i < current_co2_sensor_->childCount() ; i++)
03619     {
03620       
03622       //~ if(current_rfid_->child(i)->text(0) == QString("Angle span"))
03623       //~ {
03624         //~ float angle_span = 
03625           //~ loader_.rfidAntennaPropLoader.rfidAngleSpan->text().toFloat();
03626         //~ if( angle_span <= 0 )
03627         //~ {
03628           //~ showMessage(QString("Rfid antenna angle span invalid :") + 
03629             //~ QString().setNum(angle_span));
03630           //~ return;
03631         //~ }
03632         //~ current_rfid_->child(i)->setText(1,QString().setNum(angle_span));
03633         //~ new_robot_msg_.rfidSensors[frameId].angleSpan = angle_span;
03634       //~ }
03635       
03637       if(current_co2_sensor_->child(i)->text(0) == QString("Orientation"))
03638       {
03639         float orientation = 
03640           loader_.co2SensorPropLoader.orientation->
03641             text().toFloat();
03642         current_co2_sensor_->child(i)->
03643           setText(1,QString().setNum(orientation));
03644         new_robot_msg_.co2Sensors[frameId].pose.theta = orientation;
03645       }
03646       
03648       else if(current_co2_sensor_->child(i)->text(0) == QString("Max range"))
03649       {
03650         float maxRange = 
03651           loader_.co2SensorPropLoader.maxDistance->
03652             text().toFloat();
03653         if( maxRange <= 0 )
03654         {
03655           showMessage(QString("CO2 sensor maximum range invalid :") + 
03656             QString().setNum(maxRange));
03657           return;
03658         }
03659         current_co2_sensor_->child(i)->setText(1,QString().setNum(maxRange));
03660         new_robot_msg_.co2Sensors[frameId].maxRange = maxRange;
03661       }
03662       
03664       else if(current_co2_sensor_->child(i)->text(0) == QString("Pose - x"))
03665       {
03666         float dx = loader_.co2SensorPropLoader.x_->text().toFloat();
03667         current_co2_sensor_->child(i)->setText(1,QString().setNum(dx));
03668         new_robot_msg_.co2Sensors[frameId].pose.x = dx;
03669       }
03670       
03672       else if(current_co2_sensor_->child(i)->text(0) == QString("Pose - y"))
03673       {
03674         float dy = loader_.co2SensorPropLoader.y_->text().toFloat();
03675         current_co2_sensor_->child(i)->setText(1,QString().setNum(dy));
03676         new_robot_msg_.co2Sensors[frameId].pose.y = dy;
03677       }
03678 
03680       else if(current_co2_sensor_->child(i)->text(0) == QString("Frequency"))
03681       {
03682         float frequency = 
03683           loader_.co2SensorPropLoader.frequency->
03684             text().toFloat();
03685         if( frequency <= 0 )
03686         {
03687           showMessage(QString("CO2 sensor publishing frequency invalid :") + 
03688             QString().setNum(frequency));
03689           return;
03690         }
03691         current_co2_sensor_->child(i)->setText(1,QString().setNum(frequency));
03692         new_robot_msg_.co2Sensors[frameId].frequency = frequency;
03693       }
03694     }
03695 
03696     loader_.co2SensorPropLoader.hide();
03697     
03698     updateRobotPreview();
03699   }
03703   void CRobotCreatorConnector::updateThermalSensor(void)
03704   {
03705     unsigned int frameId = searchThermalSensor(current_thermal_sensor_->text(0));
03706     if(frameId == -1)
03707     {
03708       return;
03709     }
03710     for(unsigned int i = 0 ; i < current_thermal_sensor_->childCount() ; i++)
03711     {
03712       
03713       //~ Sensor angle span
03714       if(current_thermal_sensor_->child(i)->text(0) == QString("Angle span"))
03715       {
03716         float angle_span = 
03717           loader_.thermalSensorPropLoader.angleSpan->text().toFloat();
03718         if( angle_span <= 0 )
03719         {
03720           showMessage(QString("Thermal sensor angle span invalid :") + 
03721             QString().setNum(angle_span));
03722           return;
03723         }
03724         current_thermal_sensor_->child(i)->setText(1,QString().setNum(angle_span));
03725         new_robot_msg_.thermalSensors[frameId].angleSpan = angle_span;
03726       }
03727       
03729       else if(current_thermal_sensor_->child(i)->text(0) == QString("Orientation"))
03730       {
03731         float orientation = 
03732           loader_.thermalSensorPropLoader.orientation->
03733             text().toFloat();
03734         current_thermal_sensor_->child(i)->
03735           setText(1,QString().setNum(orientation));
03736         new_robot_msg_.thermalSensors[frameId].pose.theta = orientation;
03737       }
03738       
03740       else if(current_thermal_sensor_->child(i)->text(0) == QString("Max range"))
03741       {
03742         float maxRange = 
03743           loader_.thermalSensorPropLoader.maxDistance->
03744             text().toFloat();
03745         if( maxRange <= 0 )
03746         {
03747           showMessage(QString("Thermal sensor maximum range invalid :") + 
03748             QString().setNum(maxRange));
03749           return;
03750         }
03751         current_thermal_sensor_->child(i)->setText(1,QString().setNum(maxRange));
03752         new_robot_msg_.thermalSensors[frameId].maxRange = maxRange;
03753       }
03754       
03756       else if(current_thermal_sensor_->child(i)->text(0) == QString("Pose - x"))
03757       {
03758         float dx = loader_.thermalSensorPropLoader.x_->text().toFloat();
03759         current_thermal_sensor_->child(i)->setText(1,QString().setNum(dx));
03760         new_robot_msg_.thermalSensors[frameId].pose.x = dx;
03761       }
03762       
03764       else if(current_thermal_sensor_->child(i)->text(0) == QString("Pose - y"))
03765       {
03766         float dy = loader_.thermalSensorPropLoader.y_->text().toFloat();
03767         current_thermal_sensor_->child(i)->setText(1,QString().setNum(dy));
03768         new_robot_msg_.thermalSensors[frameId].pose.y = dy;
03769       }
03770 
03772       else if(current_thermal_sensor_->child(i)->text(0) == QString("Frequency"))
03773       {
03774         float frequency = 
03775           loader_.thermalSensorPropLoader.frequency->
03776             text().toFloat();
03777         if( frequency <= 0 )
03778         {
03779           showMessage(QString("Thermal sensor publishing frequency invalid :") + 
03780             QString().setNum(frequency));
03781           return;
03782         }
03783         current_thermal_sensor_->child(i)->setText(1,QString().setNum(frequency));
03784         new_robot_msg_.thermalSensors[frameId].frequency = frequency;
03785       }
03786     }
03787 
03788     loader_.thermalSensorPropLoader.hide();
03789     
03790     updateRobotPreview();
03791   }
03795   void CRobotCreatorConnector::updateSoundSensor(void)
03796   {
03797     unsigned int frameId = searchSoundSensor(current_sound_sensor_->text(0));
03798     if(frameId == -1)
03799     {
03800       return;
03801     }
03802     for(unsigned int i = 0 ; i < current_sound_sensor_->childCount() ; i++)
03803     {
03804       
03805       //~ Sensor angle span
03806       if(current_sound_sensor_->child(i)->text(0) == QString("Angle span"))
03807       {
03808         float angle_span = 
03809           loader_.soundSensorPropLoader.angleSpan->text().toFloat();
03810         if( angle_span <= 0 )
03811         {
03812           showMessage(QString("Sound sensor angle span invalid :") + 
03813             QString().setNum(angle_span));
03814           return;
03815         }
03816         current_sound_sensor_->child(i)->setText(1,QString().setNum(angle_span));
03817         new_robot_msg_.soundSensors[frameId].angleSpan = angle_span;
03818       }
03819       
03821       else if(current_sound_sensor_->child(i)->text(0) == QString("Orientation"))
03822       {
03823         float orientation = 
03824           loader_.soundSensorPropLoader.orientation->
03825             text().toFloat();
03826         current_sound_sensor_->child(i)->
03827           setText(1,QString().setNum(orientation));
03828         new_robot_msg_.soundSensors[frameId].pose.theta = orientation;
03829       }
03830       
03832       else if(current_sound_sensor_->child(i)->text(0) == QString("Max range"))
03833       {
03834         float maxRange = 
03835           loader_.soundSensorPropLoader.maxDistance->
03836             text().toFloat();
03837         if( maxRange <= 0 )
03838         {
03839           showMessage(QString("Sound sensor maximum range invalid :") + 
03840             QString().setNum(maxRange));
03841           return;
03842         }
03843         current_sound_sensor_->child(i)->setText(1,QString().setNum(maxRange));
03844         new_robot_msg_.soundSensors[frameId].maxRange = maxRange;
03845       }
03846       
03848       else if(current_sound_sensor_->child(i)->text(0) == QString("Pose - x"))
03849       {
03850         float dx = loader_.soundSensorPropLoader.x_->text().toFloat();
03851         current_sound_sensor_->child(i)->setText(1,QString().setNum(dx));
03852         new_robot_msg_.soundSensors[frameId].pose.x = dx;
03853       }
03854       
03856       else if(current_sound_sensor_->child(i)->text(0) == QString("Pose - y"))
03857       {
03858         float dy = loader_.soundSensorPropLoader.y_->text().toFloat();
03859         current_sound_sensor_->child(i)->setText(1,QString().setNum(dy));
03860         new_robot_msg_.soundSensors[frameId].pose.y = dy;
03861       }
03862 
03864       else if(current_sound_sensor_->child(i)->text(0) == QString("Frequency"))
03865       {
03866         float frequency = 
03867           loader_.soundSensorPropLoader.frequency->
03868             text().toFloat();
03869         if( frequency <= 0 )
03870         {
03871           showMessage(QString("Sound sensor publishing frequency invalid :") + 
03872             QString().setNum(frequency));
03873           return;
03874         }
03875         current_sound_sensor_->child(i)->setText(1,QString().setNum(frequency));
03876         new_robot_msg_.soundSensors[frameId].frequency = frequency;
03877       }
03878     }
03879 
03880     loader_.soundSensorPropLoader.hide();
03881     
03882     updateRobotPreview();
03883   }
03884   
03889   void CRobotCreatorConnector::updateRfidAntennaOpen(void)
03890   {
03891     unsigned int frameId = searchRfid(current_rfid_->text(0));
03892     if(frameId == -1)
03893     {
03894       return;
03895     }
03896     for(unsigned int i = 0 ; i < current_rfid_->childCount() ; i++)
03897     {
03898       
03900       if(current_rfid_->child(i)->text(0) == QString("Angle span"))
03901       {
03902         float angle_span = 
03903           loader_.rfidAntennaPropLoader.rfidAngleSpan->text().toFloat();
03904         if( angle_span <= 0 )
03905         {
03906           showMessage(QString("Rfid antenna angle span invalid :") + 
03907             QString().setNum(angle_span));
03908           return;
03909         }
03910         current_rfid_->child(i)->setText(1,QString().setNum(angle_span));
03911         new_robot_msg_.rfidSensors[frameId].angleSpan = angle_span;
03912       }
03913       
03915       else if(current_rfid_->child(i)->text(0) == QString("Orientation"))
03916       {
03917         float orientation = 
03918           loader_.rfidAntennaPropLoader.rfidOrientation->
03919             text().toFloat();
03920         current_rfid_->child(i)->
03921           setText(1,QString().setNum(orientation));
03922         new_robot_msg_.rfidSensors[frameId].pose.theta = orientation;
03923       }
03924       
03926       else if(current_rfid_->child(i)->text(0) == QString("Max range"))
03927       {
03928         float maxRange = 
03929           loader_.rfidAntennaPropLoader.rfidMaxDistance->
03930             text().toFloat();
03931         if( maxRange <= 0 )
03932         {
03933           showMessage(QString("Rfid antenna maximum range invalid :") + 
03934             QString().setNum(maxRange));
03935           return;
03936         }
03937         current_rfid_->child(i)->setText(1,QString().setNum(maxRange));
03938         new_robot_msg_.rfidSensors[frameId].maxRange = maxRange;
03939       }
03940       
03942       else if(current_rfid_->child(i)->text(0) == QString("Pose - x"))
03943       {
03944         float dx = loader_.rfidAntennaPropLoader.rfidX->text().toFloat();
03945         current_rfid_->child(i)->setText(1,QString().setNum(dx));
03946         new_robot_msg_.rfidSensors[frameId].pose.x = dx;
03947       }
03948       
03950       else if(current_rfid_->child(i)->text(0) == QString("Pose - y"))
03951       {
03952         float dy = loader_.rfidAntennaPropLoader.rfidY->text().toFloat();
03953         current_rfid_->child(i)->setText(1,QString().setNum(dy));
03954         new_robot_msg_.rfidSensors[frameId].pose.y = dy;
03955       }
03956       
03958       else if(current_rfid_->child(i)->text(0) == QString("Signal cutoff"))
03959       {
03960         float signal = 
03961           loader_.rfidAntennaPropLoader.rfidSignalCutoff->
03962             text().toFloat();
03963         current_rfid_->child(i)->setText(1,QString().setNum(signal));
03964         new_robot_msg_.rfidSensors[frameId].signalCutoff = signal;
03965       }
03966       
03968       else if(current_rfid_->child(i)->text(0) == QString("Frequency"))
03969       {
03970         float frequency = 
03971           loader_.rfidAntennaPropLoader.rfidFrequency->
03972             text().toFloat();
03973         if( frequency <= 0 )
03974         {
03975           showMessage(QString("Rfid antenna publishing frequency invalid :") + 
03976             QString().setNum(frequency));
03977           return;
03978         }
03979         current_rfid_->child(i)->setText(1,QString().setNum(frequency));
03980         new_robot_msg_.rfidSensors[frameId].frequency = frequency;
03981       }
03982     }
03983     
03984     updateRobotPreview();
03985   }
03989   void CRobotCreatorConnector::updateCO2SensorOpen(void)
03990   {
03991     unsigned int frameId = searchCO2Sensor(current_co2_sensor_->text(0));
03992     if(frameId == -1)
03993     {
03994       return;
03995     }
03996     for(unsigned int i = 0 ; i < current_co2_sensor_->childCount() ; i++)
03997     {
03998       
03999       //~ //!< Angle span
04000       //~ if(current_co2_sensor_->child(i)->text(0) == QString("Angle span"))
04001       //~ {
04002         //~ float angle_span = 
04003           //~ loader_.co2SensorPropLoader.angleSpan->text().toFloat();
04004         //~ if( angle_span <= 0 )
04005         //~ {
04006           //~ showMessage(QString("Rfid antenna angle span invalid :") + 
04007             //~ QString().setNum(angle_span));
04008           //~ return;
04009         //~ }
04010         //~ current_rfid_->child(i)->setText(1,QString().setNum(angle_span));
04011         //~ new_robot_msg_.rfidSensors[frameId].angleSpan = angle_span;
04012       //~ }
04013       
04015       if(current_co2_sensor_->child(i)->text(0) == QString("Orientation"))
04016       {
04017         float orientation = 
04018           loader_.co2SensorPropLoader.orientation->
04019             text().toFloat();
04020         current_co2_sensor_->child(i)->
04021           setText(1,QString().setNum(orientation));
04022         new_robot_msg_.co2Sensors[frameId].pose.theta = orientation;
04023       }
04024       
04026       else if(current_co2_sensor_->child(i)->text(0) == QString("Max range"))
04027       {
04028         float maxRange = 
04029           loader_.co2SensorPropLoader.maxDistance->
04030             text().toFloat();
04031         if( maxRange <= 0 )
04032         {
04033           showMessage(QString("CO2 source maximum range invalid :") + 
04034             QString().setNum(maxRange));
04035           return;
04036         }
04037         current_co2_sensor_->child(i)->setText(1,QString().setNum(maxRange));
04038         new_robot_msg_.co2Sensors[frameId].maxRange = maxRange;
04039       }
04040       
04042       else if(current_co2_sensor_->child(i)->text(0) == QString("Pose - x"))
04043       {
04044         float dx = loader_.co2SensorPropLoader.x_->text().toFloat();
04045         current_co2_sensor_->child(i)->setText(1,QString().setNum(dx));
04046         new_robot_msg_.co2Sensors[frameId].pose.x = dx;
04047       }
04048       
04050       else if(current_co2_sensor_->child(i)->text(0) == QString("Pose - y"))
04051       {
04052         float dy = loader_.co2SensorPropLoader.y_->text().toFloat();
04053         current_co2_sensor_->child(i)->setText(1,QString().setNum(dy));
04054         new_robot_msg_.co2Sensors[frameId].pose.y = dy;
04055       }
04056       
04058       else if(current_co2_sensor_->child(i)->text(0) == QString("Frequency"))
04059       {
04060         float frequency = 
04061           loader_.co2SensorPropLoader.frequency->
04062             text().toFloat();
04063         if( frequency <= 0 )
04064         {
04065           showMessage(QString("CO2 sensor publishing frequency invalid :") + 
04066             QString().setNum(frequency));
04067           return;
04068         }
04069         current_co2_sensor_->child(i)->setText(1,QString().setNum(frequency));
04070         new_robot_msg_.co2Sensors[frameId].frequency = frequency;
04071       }
04072     }
04073     
04074     updateRobotPreview();
04075   }
04079   void CRobotCreatorConnector::updateThermalSensorOpen(void)
04080   {
04081     unsigned int frameId = searchThermalSensor(current_thermal_sensor_->text(0));
04082     if(frameId == -1)
04083     {
04084       return;
04085     }
04086     for(unsigned int i = 0 ; i < current_thermal_sensor_->childCount() ; i++)
04087     {
04088       
04090       if(current_thermal_sensor_->child(i)->text(0) == QString("Angle span"))
04091       {
04092         float angle_span = 
04093           loader_.thermalSensorPropLoader.angleSpan->text().toFloat();
04094         if( angle_span <= 0 )
04095         {
04096           showMessage(QString("Thermal sensor angle span invalid :") + 
04097             QString().setNum(angle_span));
04098           return;
04099         }
04100         current_thermal_sensor_->child(i)->setText(1,QString().setNum(angle_span));
04101         new_robot_msg_.thermalSensors[frameId].angleSpan = angle_span;
04102       }
04103       
04105       else if(current_thermal_sensor_->child(i)->text(0) == QString("Orientation"))
04106       {
04107         float orientation = 
04108           loader_.thermalSensorPropLoader.orientation->
04109             text().toFloat();
04110         current_thermal_sensor_->child(i)->
04111           setText(1,QString().setNum(orientation));
04112         new_robot_msg_.thermalSensors[frameId].pose.theta = orientation;
04113       }
04114       
04116       else if(current_thermal_sensor_->child(i)->text(0) == QString("Max range"))
04117       {
04118         float maxRange = 
04119           loader_.thermalSensorPropLoader.maxDistance->
04120             text().toFloat();
04121         if( maxRange <= 0 )
04122         {
04123           showMessage(QString("Thermal sensor maximum range invalid :") + 
04124             QString().setNum(maxRange));
04125           return;
04126         }
04127         current_thermal_sensor_->child(i)->setText(1,QString().setNum(maxRange));
04128         new_robot_msg_.thermalSensors[frameId].maxRange = maxRange;
04129       }
04130       
04132       else if(current_thermal_sensor_->child(i)->text(0) == QString("Pose - x"))
04133       {
04134         float dx = loader_.thermalSensorPropLoader.x_->text().toFloat();
04135         current_thermal_sensor_->child(i)->setText(1,QString().setNum(dx));
04136         new_robot_msg_.thermalSensors[frameId].pose.x = dx;
04137       }
04138       
04140       else if(current_thermal_sensor_->child(i)->text(0) == QString("Pose - y"))
04141       {
04142         float dy = loader_.thermalSensorPropLoader.y_->text().toFloat();
04143         current_thermal_sensor_->child(i)->setText(1,QString().setNum(dy));
04144         new_robot_msg_.thermalSensors[frameId].pose.y = dy;
04145       }
04146       
04148       else if(current_thermal_sensor_->child(i)->text(0) == QString("Frequency"))
04149       {
04150         float frequency = 
04151           loader_.thermalSensorPropLoader.frequency->
04152             text().toFloat();
04153         if( frequency <= 0 )
04154         {
04155           showMessage(QString("Thermal sensor publishing frequency invalid :") + 
04156             QString().setNum(frequency));
04157           return;
04158         }
04159         current_thermal_sensor_->child(i)->setText(1,QString().setNum(frequency));
04160         new_robot_msg_.thermalSensors[frameId].frequency = frequency;
04161       }
04162     }
04163     
04164     updateRobotPreview();
04165   }
04169   void CRobotCreatorConnector::updateSoundSensorOpen(void)
04170   {
04171     unsigned int frameId = searchSoundSensor(current_sound_sensor_->text(0));
04172     if(frameId == -1)
04173     {
04174       return;
04175     }
04176     for(unsigned int i = 0 ; i < current_sound_sensor_->childCount() ; i++)
04177     {
04178       
04180       if(current_sound_sensor_->child(i)->text(0) == QString("Angle span"))
04181       {
04182         float angle_span = 
04183           loader_.soundSensorPropLoader.angleSpan->text().toFloat();
04184         if( angle_span <= 0 )
04185         {
04186           showMessage(QString("Sound sensor angle span invalid :") + 
04187             QString().setNum(angle_span));
04188           return;
04189         }
04190         current_sound_sensor_->child(i)->setText(1,QString().setNum(angle_span));
04191         new_robot_msg_.soundSensors[frameId].angleSpan = angle_span;
04192       }
04193       
04195       else if(current_sound_sensor_->child(i)->text(0) == QString("Orientation"))
04196       {
04197         float orientation = 
04198           loader_.soundSensorPropLoader.orientation->
04199             text().toFloat();
04200         current_sound_sensor_->child(i)->
04201           setText(1,QString().setNum(orientation));
04202         new_robot_msg_.soundSensors[frameId].pose.theta = orientation;
04203       }
04204       
04206       else if(current_sound_sensor_->child(i)->text(0) == QString("Max range"))
04207       {
04208         float maxRange = 
04209           loader_.soundSensorPropLoader.maxDistance->
04210             text().toFloat();
04211         if( maxRange <= 0 )
04212         {
04213           showMessage(QString("Sound sensor maximum range invalid :") + 
04214             QString().setNum(maxRange));
04215           return;
04216         }
04217         current_sound_sensor_->child(i)->setText(1,QString().setNum(maxRange));
04218         new_robot_msg_.soundSensors[frameId].maxRange = maxRange;
04219       }
04220       
04222       else if(current_sound_sensor_->child(i)->text(0) == QString("Pose - x"))
04223       {
04224         float dx = loader_.soundSensorPropLoader.x_->text().toFloat();
04225         current_sound_sensor_->child(i)->setText(1,QString().setNum(dx));
04226         new_robot_msg_.soundSensors[frameId].pose.x = dx;
04227       }
04228       
04230       else if(current_sound_sensor_->child(i)->text(0) == QString("Pose - y"))
04231       {
04232         float dy = loader_.soundSensorPropLoader.y_->text().toFloat();
04233         current_sound_sensor_->child(i)->setText(1,QString().setNum(dy));
04234         new_robot_msg_.soundSensors[frameId].pose.y = dy;
04235       }
04236       
04238       else if(current_sound_sensor_->child(i)->text(0) == QString("Frequency"))
04239       {
04240         float frequency = 
04241           loader_.soundSensorPropLoader.frequency->
04242             text().toFloat();
04243         if( frequency <= 0 )
04244         {
04245           showMessage(QString("Sound sensor publishing frequency invalid :") + 
04246             QString().setNum(frequency));
04247           return;
04248         }
04249         current_sound_sensor_->child(i)->setText(1,QString().setNum(frequency));
04250         new_robot_msg_.soundSensors[frameId].frequency = frequency;
04251       }
04252     }
04253     
04254     updateRobotPreview();
04255   }
04256   
04261   void CRobotCreatorConnector::editRobot(void)
04262   {
04263     loader_.robotPropLoader.robotOrientation->
04264       setText(QString().setNum(new_robot_msg_.initialPose.theta));
04265     loader_.robotPropLoader.robotRadius->
04266       setText(QString().setNum(new_robot_msg_.footprint.radius));
04267     loader_.robotPropLoader.show();
04268   }
04269   
04274   void CRobotCreatorConnector::updateRobot(void)
04275   {
04276     for(unsigned int i = 0 ; i < loader_.robotNode.childCount() ; i++)
04277     {
04278       if(loader_.robotNode.child(i)->text(0) == QString("Orientation"))
04279       {
04280         loader_.robotNode.child(i)->
04281           setText(1,loader_.robotPropLoader.robotOrientation->text());
04282         new_robot_msg_.initialPose.theta = 
04283           loader_.robotPropLoader.robotOrientation->text().toFloat();
04284       }
04285       if(loader_.robotNode.child(i)->text(0) == QString("Radius"))
04286       {
04287         loader_.robotNode.child(i)->
04288           setText(1,loader_.robotPropLoader.robotRadius->text());
04289         new_robot_msg_.footprint.radius = 
04290           loader_.robotPropLoader.robotRadius->text().toFloat();
04291       }
04292     }
04293 
04294     loader_.robotPropLoader.hide();
04295     
04296     updateRobotPreview();
04297   }
04298   
04303   void CRobotCreatorConnector::updateRobotOpen(void)
04304   {
04305     for(unsigned int i = 0 ; i < loader_.robotNode.childCount() ; i++)
04306     {
04307       if(loader_.robotNode.child(i)->text(0) == QString("Orientation"))
04308       {
04309         loader_.robotNode.child(i)->
04310           setText(1,loader_.robotPropLoader.robotOrientation->text());
04311         new_robot_msg_.initialPose.theta = 
04312           loader_.robotPropLoader.robotOrientation->text().toFloat();
04313       }
04314       if(loader_.robotNode.child(i)->text(0) == QString("Radius"))
04315       {
04316         loader_.robotNode.child(i)->
04317           setText(1,loader_.robotPropLoader.robotRadius->text());
04318         new_robot_msg_.footprint.radius = 
04319           loader_.robotPropLoader.robotRadius->text().toFloat();
04320       }
04321     }
04322     
04323     updateRobotPreview();
04324   }
04325 
04331   void CRobotCreatorConnector::deleteTreeNode(QTreeWidgetItem *item)
04332   {
04333     int count = item->childCount();
04334     for(int i = count - 1 ; i >= 0 ; i--)
04335     {
04336       deleteTreeNode(item->child(i));
04337     }
04338     delete item;
04339   }
04340     
04345   void CRobotCreatorConnector::updateRobotPreview(void)
04346   {
04347     
04348     loader_.robotPreviewImage.fill(QColor(220,220,220,1));
04349     
04350     climax_ = -1;
04351     if(climax_ < new_robot_msg_.footprint.radius) 
04352     {
04353       climax_ = new_robot_msg_.footprint.radius;
04354     }
04355     for(unsigned int i = 0 ; i < new_robot_msg_.footprint.points.size() ; i++)
04356     {
04357       if(climax_ < new_robot_msg_.footprint.points[i].x)
04358       {
04359         climax_ = new_robot_msg_.footprint.points[i].x;
04360       }
04361       if(climax_ < new_robot_msg_.footprint.points[i].y)
04362       {
04363         climax_ = new_robot_msg_.footprint.points[i].y;
04364       }
04365     }
04366     for(unsigned int i = 0 ; i < new_robot_msg_.laserSensors.size() ; i++)
04367     {
04368       if(climax_ < (new_robot_msg_.laserSensors[i].maxRange + 
04369           new_robot_msg_.laserSensors[i].pose.x))
04370       { 
04371         climax_ = new_robot_msg_.laserSensors[i].maxRange + 
04372           new_robot_msg_.laserSensors[i].pose.x;
04373       }
04374       if(climax_ < (new_robot_msg_.laserSensors[i].maxRange + 
04375           new_robot_msg_.laserSensors[i].pose.y) )
04376       {
04377         climax_ = new_robot_msg_.laserSensors[i].maxRange + 
04378           new_robot_msg_.laserSensors[i].pose.y;  
04379       }
04380       if(climax_ < (new_robot_msg_.laserSensors[i].maxRange - 
04381           new_robot_msg_.laserSensors[i].pose.x) )
04382       {
04383         climax_ = new_robot_msg_.laserSensors[i].maxRange - 
04384           new_robot_msg_.laserSensors[i].pose.x;
04385       }
04386       if(climax_ < (new_robot_msg_.laserSensors[i].maxRange - 
04387           new_robot_msg_.laserSensors[i].pose.y) )
04388       {
04389         climax_ = new_robot_msg_.laserSensors[i].maxRange - 
04390           new_robot_msg_.laserSensors[i].pose.y;  
04391       }
04392     }
04393     for(unsigned int i = 0 ; i < new_robot_msg_.sonarSensors.size() ; i++)
04394     {
04395       if(climax_ < (new_robot_msg_.sonarSensors[i].maxRange + 
04396           new_robot_msg_.sonarSensors[i].pose.x) )
04397       {
04398         climax_ = new_robot_msg_.sonarSensors[i].maxRange + 
04399           new_robot_msg_.sonarSensors[i].pose.x;
04400       }
04401       if(climax_ < (new_robot_msg_.sonarSensors[i].maxRange + 
04402           new_robot_msg_.sonarSensors[i].pose.y) )
04403       {
04404         climax_ = new_robot_msg_.sonarSensors[i].maxRange + 
04405           new_robot_msg_.sonarSensors[i].pose.y;  
04406       }
04407       if(climax_ < (new_robot_msg_.sonarSensors[i].maxRange - 
04408           new_robot_msg_.sonarSensors[i].pose.x) )
04409       {  
04410         climax_ = new_robot_msg_.sonarSensors[i].maxRange - 
04411           new_robot_msg_.sonarSensors[i].pose.x;
04412       }
04413       if(climax_ < (new_robot_msg_.sonarSensors[i].maxRange - 
04414           new_robot_msg_.sonarSensors[i].pose.y) )
04415       {  
04416         climax_ = new_robot_msg_.sonarSensors[i].maxRange - 
04417           new_robot_msg_.sonarSensors[i].pose.y;  
04418       }
04419     }
04420     for(unsigned int i = 0 ; i < new_robot_msg_.rfidSensors.size() ; i++)
04421     {
04422       if(climax_ < (new_robot_msg_.rfidSensors[i].maxRange + 
04423           new_robot_msg_.rfidSensors[i].pose.x) )
04424       {
04425         climax_ = new_robot_msg_.rfidSensors[i].maxRange + 
04426           new_robot_msg_.rfidSensors[i].pose.x;
04427       }
04428       if(climax_ < (new_robot_msg_.rfidSensors[i].maxRange + 
04429           new_robot_msg_.rfidSensors[i].pose.y) )
04430       {
04431         climax_ = new_robot_msg_.rfidSensors[i].maxRange + 
04432           new_robot_msg_.rfidSensors[i].pose.y;  
04433       }
04434       if(climax_ < (new_robot_msg_.rfidSensors[i].maxRange - 
04435           new_robot_msg_.rfidSensors[i].pose.x) )
04436       {
04437         climax_ = new_robot_msg_.rfidSensors[i].maxRange - 
04438           new_robot_msg_.rfidSensors[i].pose.x;
04439       }
04440       if(climax_ < (new_robot_msg_.rfidSensors[i].maxRange - 
04441           new_robot_msg_.rfidSensors[i].pose.y) )
04442       {
04443         climax_ = new_robot_msg_.rfidSensors[i].maxRange - 
04444           new_robot_msg_.rfidSensors[i].pose.y;  
04445       }
04446     }
04447     for(unsigned int i = 0 ; i < new_robot_msg_.co2Sensors.size() ; i++)
04448     {
04449       if(climax_ < (new_robot_msg_.co2Sensors[i].maxRange + 
04450           new_robot_msg_.co2Sensors[i].pose.x) )
04451       {
04452         climax_ = new_robot_msg_.co2Sensors[i].maxRange + 
04453           new_robot_msg_.co2Sensors[i].pose.x;
04454       }
04455       if(climax_ < (new_robot_msg_.co2Sensors[i].maxRange + 
04456           new_robot_msg_.co2Sensors[i].pose.y) )
04457       {
04458         climax_ = new_robot_msg_.co2Sensors[i].maxRange + 
04459           new_robot_msg_.co2Sensors[i].pose.y;  
04460       }
04461       if(climax_ < (new_robot_msg_.co2Sensors[i].maxRange - 
04462           new_robot_msg_.co2Sensors[i].pose.x) )
04463       {
04464         climax_ = new_robot_msg_.co2Sensors[i].maxRange - 
04465           new_robot_msg_.co2Sensors[i].pose.x;
04466       }
04467       if(climax_ < (new_robot_msg_.co2Sensors[i].maxRange - 
04468           new_robot_msg_.co2Sensors[i].pose.y) )
04469       {
04470         climax_ = new_robot_msg_.co2Sensors[i].maxRange - 
04471           new_robot_msg_.co2Sensors[i].pose.y;  
04472       }
04473     }
04474     for(unsigned int i = 0 ; i < new_robot_msg_.thermalSensors.size() ; i++)
04475     {
04476       if(climax_ < (new_robot_msg_.thermalSensors[i].maxRange + 
04477           new_robot_msg_.thermalSensors[i].pose.x) )
04478       {
04479         climax_ = new_robot_msg_.thermalSensors[i].maxRange + 
04480           new_robot_msg_.thermalSensors[i].pose.x;
04481       }
04482       if(climax_ < (new_robot_msg_.thermalSensors[i].maxRange + 
04483           new_robot_msg_.thermalSensors[i].pose.y) )
04484       {
04485         climax_ = new_robot_msg_.thermalSensors[i].maxRange + 
04486           new_robot_msg_.thermalSensors[i].pose.y;  
04487       }
04488       if(climax_ < (new_robot_msg_.thermalSensors[i].maxRange - 
04489           new_robot_msg_.thermalSensors[i].pose.x) )
04490       {
04491         climax_ = new_robot_msg_.thermalSensors[i].maxRange - 
04492           new_robot_msg_.thermalSensors[i].pose.x;
04493       }
04494       if(climax_ < (new_robot_msg_.thermalSensors[i].maxRange - 
04495           new_robot_msg_.thermalSensors[i].pose.y) )
04496       {
04497         climax_ = new_robot_msg_.thermalSensors[i].maxRange - 
04498           new_robot_msg_.thermalSensors[i].pose.y;  
04499       }
04500     }
04501     for(unsigned int i = 0 ; i < new_robot_msg_.soundSensors.size() ; i++)
04502     {
04503       if(climax_ < (new_robot_msg_.soundSensors[i].maxRange + 
04504           new_robot_msg_.soundSensors[i].pose.x) )
04505       {
04506         climax_ = new_robot_msg_.soundSensors[i].maxRange + 
04507           new_robot_msg_.soundSensors[i].pose.x;
04508       }
04509       if(climax_ < (new_robot_msg_.soundSensors[i].maxRange + 
04510           new_robot_msg_.soundSensors[i].pose.y) )
04511       {
04512         climax_ = new_robot_msg_.soundSensors[i].maxRange + 
04513           new_robot_msg_.soundSensors[i].pose.y;  
04514       }
04515       if(climax_ < (new_robot_msg_.soundSensors[i].maxRange - 
04516           new_robot_msg_.soundSensors[i].pose.x) )
04517       {
04518         climax_ = new_robot_msg_.soundSensors[i].maxRange - 
04519           new_robot_msg_.soundSensors[i].pose.x;
04520       }
04521       if(climax_ < (new_robot_msg_.soundSensors[i].maxRange - 
04522           new_robot_msg_.soundSensors[i].pose.y) )
04523       {
04524         climax_ = new_robot_msg_.soundSensors[i].maxRange - 
04525           new_robot_msg_.soundSensors[i].pose.y;  
04526       }
04527     }
04528     
04529     climax_ = 230.0 / climax_;
04530     drawRobot();
04531     drawLasers();
04532     drawSonars();
04533     drawRfidAntennas();
04534     drawCO2Sensors();
04535     drawThermalSensors();
04536     drawSoundSensors();
04537     
04538     loader_.robotTreeWidget->resizeColumnToContents(0);
04539     loader_.robotTreeWidget->resizeColumnToContents(1);
04540     loader_.robotTreeWidget->resizeColumnToContents(2);
04541     loader_.robotTreeWidget->resizeColumnToContents(3);
04542     loader_.robotTreeWidget->resizeColumnToContents(4);
04543   }
04544   
04549   void CRobotCreatorConnector::drawRobot(void)
04550   {
04551     QPainter painter(&loader_.robotPreviewImage);
04552     painter.setPen(Qt::blue);
04553     
04554     if(new_robot_msg_.footprint.points.size() == 0)
04555     {
04556       painter.drawEllipse(
04557         250 - new_robot_msg_.footprint.radius * climax_,
04558         250 - new_robot_msg_.footprint.radius * climax_,
04559         new_robot_msg_.footprint.radius * climax_ * 2,
04560         new_robot_msg_.footprint.radius * climax_ * 2);
04561         
04562       painter.setPen(Qt::red);
04563       
04564       painter.drawLine(  
04565         250,
04566         250,
04567         250 + new_robot_msg_.footprint.radius * climax_ * 1.05 * cos(
04568           new_robot_msg_.initialPose.theta / 180.0 * STDR_PI),
04569         250 - new_robot_msg_.footprint.radius * climax_ * 1.05 * sin(
04570           new_robot_msg_.initialPose.theta / 180.0 * STDR_PI));
04571     }
04572     else
04573     {
04574       QPointF *points = new QPointF[new_robot_msg_.footprint.points.size() + 1];
04575       
04576       float max_val = 0;
04577       
04578       for(unsigned int i = 0 ; 
04579         i < new_robot_msg_.footprint.points.size() + 1 ; i++)
04580       {
04581         float x = new_robot_msg_.footprint.points
04582           [i % new_robot_msg_.footprint.points.size()].x;
04583         float y = - new_robot_msg_.footprint.points
04584           [i % new_robot_msg_.footprint.points.size()].y;
04585         
04586         if( pow(x,2) + pow(y,2) > max_val )
04587         {
04588           max_val = pow(x,2) + pow(y,2);
04589         }
04590         
04591         points[i] = QPointF(
04592           250 + 
04593             x * climax_ * 
04594               cos(-new_robot_msg_.initialPose.theta / 180.0 * STDR_PI) 
04595             - y * climax_ * 
04596               sin(-new_robot_msg_.initialPose.theta / 180.0 * STDR_PI),
04597       
04598           250 + 
04599             x * climax_ * 
04600               sin(-new_robot_msg_.initialPose.theta / 180.0 * STDR_PI) 
04601             + y * climax_ * 
04602               cos(-new_robot_msg_.initialPose.theta / 180.0 * STDR_PI));
04603       }
04604       painter.drawPolyline(points, new_robot_msg_.footprint.points.size() + 1);
04605       
04606       painter.setPen(Qt::red);
04607       
04608       max_val = sqrt(max_val);
04609       
04610       painter.drawLine(  
04611         250,
04612         250,
04613         250 + max_val * climax_ * 1.05 * cos(
04614           new_robot_msg_.initialPose.theta / 180.0 * STDR_PI),
04615         250 - max_val * climax_ * 1.05 * sin(
04616           new_robot_msg_.initialPose.theta / 180.0 * STDR_PI));
04617     }
04618     
04619     loader_.robotPreviewLabel->setPixmap(
04620       QPixmap().fromImage(loader_.robotPreviewImage));
04621   }
04622 
04627   void CRobotCreatorConnector::drawLasers(void)
04628   {
04629     QPainter painter(&loader_.robotPreviewImage);
04630     QBrush brush(QColor(0,200,0,50));
04631     painter.setBrush(brush);
04632     float robotOrientation = 
04633       new_robot_msg_.initialPose.theta / 180.0 * STDR_PI;
04634     
04635     for(unsigned int i = 0 ; i < new_robot_msg_.laserSensors.size() ; i++)
04636     {
04637       if(laser_hightlight_id_ == i)
04638       {
04639         brush = QBrush(QColor(0,200,0,150));
04640       }
04641       else
04642       {
04643         brush = QBrush(QColor(0,200,0,50));
04644       }
04645       painter.setBrush(brush);
04646       
04647       float laserx = new_robot_msg_.laserSensors[i].pose.x;
04648       float lasery = new_robot_msg_.laserSensors[i].pose.y;
04649       float newx = laserx * cos(robotOrientation) - 
04650         lasery * sin(robotOrientation);
04651       float newy = laserx * sin(robotOrientation) + 
04652         lasery * cos(robotOrientation);
04653       
04654       painter.drawPie(
04655         250 - new_robot_msg_.laserSensors[i].maxRange * climax_ +   
04656           newx * climax_,
04657           
04658         250 - new_robot_msg_.laserSensors[i].maxRange * climax_ - 
04659           newy * climax_,
04660           
04661         new_robot_msg_.laserSensors[i].maxRange * climax_ * 2,
04662         new_robot_msg_.laserSensors[i].maxRange * climax_ * 2,
04663         
04664         (new_robot_msg_.laserSensors[i].minAngle + 
04665           new_robot_msg_.initialPose.theta + 
04666           new_robot_msg_.laserSensors[i].pose.theta) * 16,
04667         
04668         new_robot_msg_.laserSensors[i].maxAngle * 16 - 
04669           new_robot_msg_.laserSensors[i].minAngle * 16);
04670     }
04671     loader_.robotPreviewLabel->setPixmap(
04672       QPixmap().fromImage(loader_.robotPreviewImage));
04673   }
04674   
04679   void CRobotCreatorConnector::drawSonars(void)
04680   {
04681     QPainter painter(&loader_.robotPreviewImage);
04682     QBrush brush(QColor(200,0,0,50));
04683     painter.setBrush(brush);
04684     float robotOrientation =  
04685       new_robot_msg_.initialPose.theta / 180.0 * STDR_PI;
04686     
04687     for(unsigned int i = 0 ; i < new_robot_msg_.sonarSensors.size() ; i++)
04688     {
04689       
04690       if(sonar_hightlight_id_ == i)
04691       {
04692         brush = QBrush(QColor(200,0,0,150));
04693       }
04694       else
04695       {
04696         brush = QBrush(QColor(200,0,0,50));
04697       }
04698       painter.setBrush(brush);
04699       
04700       float sonarx = new_robot_msg_.sonarSensors[i].pose.x;
04701       float sonary = new_robot_msg_.sonarSensors[i].pose.y;
04702       float newx = sonarx * cos(robotOrientation) - 
04703             sonary * sin(robotOrientation);
04704       float newy = sonarx * sin(robotOrientation) + 
04705             sonary * cos(robotOrientation);
04706       
04707       painter.drawPie(  
04708         250 - new_robot_msg_.sonarSensors[i].maxRange * climax_ + 
04709           newx * climax_,
04710         250 - new_robot_msg_.sonarSensors[i].maxRange * climax_ - 
04711           newy * climax_,
04712         new_robot_msg_.sonarSensors[i].maxRange * climax_ * 2,
04713         new_robot_msg_.sonarSensors[i].maxRange * climax_ * 2,
04714         (new_robot_msg_.sonarSensors[i].pose.theta - 
04715           new_robot_msg_.sonarSensors[i].coneAngle / 2.0 + 
04716           new_robot_msg_.initialPose.theta) * 16,
04717         (new_robot_msg_.sonarSensors[i].coneAngle) * 16);
04718     }
04719     loader_.robotPreviewLabel->setPixmap(
04720       QPixmap().fromImage(loader_.robotPreviewImage));
04721   }
04722   
04727   void CRobotCreatorConnector::drawRfidAntennas(void)
04728   {
04729     QPainter painter(&loader_.robotPreviewImage);
04730     QBrush brush(QColor(0,0,200,20));
04731     painter.setBrush(brush);
04732     float robotOrientation = 
04733       new_robot_msg_.initialPose.theta / 180.0 * STDR_PI;
04734     
04735     for(unsigned int i = 0 ; i < new_robot_msg_.rfidSensors.size() ; i++)
04736     {
04737       
04738       if(rfid_antenna_hightlight_id_ == i)
04739       {
04740         brush = QBrush(QColor(0,0,200,30));
04741       }
04742       else
04743       {
04744         brush = QBrush(QColor(0,0,200,10));
04745       }
04746       
04747       float rfidx = new_robot_msg_.rfidSensors[i].pose.x;
04748       float rfidy = new_robot_msg_.rfidSensors[i].pose.y;
04749       float newx = 
04750         rfidx * cos(robotOrientation) - rfidy * sin(robotOrientation);
04751       float newy = 
04752         rfidx * sin(robotOrientation) + rfidy * cos(robotOrientation);
04753       
04754       painter.drawPie(  
04755         250 - new_robot_msg_.rfidSensors[i].maxRange * climax_ + newx * climax_,
04756         250 - new_robot_msg_.rfidSensors[i].maxRange * climax_ - newy * climax_,
04757         new_robot_msg_.rfidSensors[i].maxRange * climax_ * 2,
04758         new_robot_msg_.rfidSensors[i].maxRange * climax_ * 2,
04759         (new_robot_msg_.rfidSensors[i].pose.theta - 
04760           new_robot_msg_.rfidSensors[i].angleSpan / 2.0+
04761           new_robot_msg_.initialPose.theta) * 16,
04762         (new_robot_msg_.rfidSensors[i].angleSpan) * 16);
04763     }
04764     loader_.robotPreviewLabel->setPixmap(
04765       QPixmap().fromImage(loader_.robotPreviewImage));
04766   }
04770   void CRobotCreatorConnector::drawCO2Sensors(void)
04771   {
04772     QPainter painter(&loader_.robotPreviewImage);
04773     QBrush brush(QColor(100,0,200,20));
04774     painter.setBrush(brush);
04775     float robotOrientation = 
04776       new_robot_msg_.initialPose.theta / 180.0 * STDR_PI;
04777     
04778     for(unsigned int i = 0 ; i < new_robot_msg_.co2Sensors.size() ; i++)
04779     {
04780       
04781       if(co2_sensor_hightlight_id_ == i)
04782       {
04783         brush = QBrush(QColor(100,0,200,30));
04784       }
04785       else
04786       {
04787         brush = QBrush(QColor(100,0,200,10));
04788       }
04789       
04790       float x = new_robot_msg_.co2Sensors[i].pose.x;
04791       float y = new_robot_msg_.co2Sensors[i].pose.y;
04792       float newx = 
04793         x * cos(robotOrientation) - y * sin(robotOrientation);
04794       float newy = 
04795         x * sin(robotOrientation) + y * cos(robotOrientation);
04796       
04797       painter.drawPie(  
04798         250 - new_robot_msg_.co2Sensors[i].maxRange * climax_ + newx * climax_,
04799         250 - new_robot_msg_.co2Sensors[i].maxRange * climax_ - newy * climax_,
04800         new_robot_msg_.co2Sensors[i].maxRange * climax_ * 2,
04801         new_robot_msg_.co2Sensors[i].maxRange * climax_ * 2,
04802         0,
04803         (2 * 180.0) * 16);
04804     }
04805     loader_.robotPreviewLabel->setPixmap(
04806       QPixmap().fromImage(loader_.robotPreviewImage));
04807   }
04812   void CRobotCreatorConnector::drawThermalSensors(void)
04813   {
04814     QPainter painter(&loader_.robotPreviewImage);
04815     QBrush brush(QColor(200,0,0,10));
04816     painter.setBrush(brush);
04817     float robotOrientation = 
04818       new_robot_msg_.initialPose.theta / 180.0 * STDR_PI;
04819     
04820     for(unsigned int i = 0 ; i < new_robot_msg_.thermalSensors.size() ; i++)
04821     {
04822       
04823       if(thermal_sensor_hightlight_id_ == i)
04824       {
04825         brush = QBrush(QColor(200,0,0,30));
04826       }
04827       else
04828       {
04829         brush = QBrush(QColor(200,0,0,10));
04830       }
04831       
04832       float x = new_robot_msg_.thermalSensors[i].pose.x;
04833       float y = new_robot_msg_.thermalSensors[i].pose.y;
04834       float newx = 
04835         x * cos(robotOrientation) - y * sin(robotOrientation);
04836       float newy = 
04837         x * sin(robotOrientation) + y * cos(robotOrientation);
04838       
04839       painter.drawPie(  
04840         250 - new_robot_msg_.thermalSensors[i].maxRange * climax_ + newx * climax_,
04841         250 - new_robot_msg_.thermalSensors[i].maxRange * climax_ - newy * climax_,
04842         new_robot_msg_.thermalSensors[i].maxRange * climax_ * 2,
04843         new_robot_msg_.thermalSensors[i].maxRange * climax_ * 2,
04844         (new_robot_msg_.thermalSensors[i].pose.theta - 
04845           new_robot_msg_.thermalSensors[i].angleSpan / 2.0+
04846           new_robot_msg_.initialPose.theta) * 16,
04847         (new_robot_msg_.thermalSensors[i].angleSpan) * 16);
04848     }
04849     loader_.robotPreviewLabel->setPixmap(
04850       QPixmap().fromImage(loader_.robotPreviewImage));
04851   }
04856   void CRobotCreatorConnector::drawSoundSensors(void)
04857   {
04858     QPainter painter(&loader_.robotPreviewImage);
04859     QBrush brush(QColor(0,50,200,20));
04860     painter.setBrush(brush);
04861     float robotOrientation = 
04862       new_robot_msg_.initialPose.theta / 180.0 * STDR_PI;
04863     
04864     for(unsigned int i = 0 ; i < new_robot_msg_.soundSensors.size() ; i++)
04865     {
04866       
04867       if(sound_sensor_hightlight_id_ == i)
04868       {
04869         brush = QBrush(QColor(0,50,200,30));
04870       }
04871       else
04872       {
04873         brush = QBrush(QColor(0,50,200,10));
04874       }
04875       
04876       float x = new_robot_msg_.soundSensors[i].pose.x;
04877       float y = new_robot_msg_.soundSensors[i].pose.y;
04878       float newx = 
04879         x * cos(robotOrientation) - y * sin(robotOrientation);
04880       float newy = 
04881         x * sin(robotOrientation) + y * cos(robotOrientation);
04882       
04883       painter.drawPie(  
04884         250 - new_robot_msg_.soundSensors[i].maxRange * climax_ + newx * climax_,
04885         250 - new_robot_msg_.soundSensors[i].maxRange * climax_ - newy * climax_,
04886         new_robot_msg_.soundSensors[i].maxRange * climax_ * 2,
04887         new_robot_msg_.soundSensors[i].maxRange * climax_ * 2,
04888         (new_robot_msg_.soundSensors[i].pose.theta - 
04889           new_robot_msg_.soundSensors[i].angleSpan / 2.0+
04890           new_robot_msg_.initialPose.theta) * 16,
04891         (new_robot_msg_.soundSensors[i].angleSpan) * 16);
04892     }
04893     loader_.robotPreviewLabel->setPixmap(
04894       QPixmap().fromImage(loader_.robotPreviewImage));
04895   }
04896 
04901   void CRobotCreatorConnector::saveRobot(void)
04902   {
04903     QString file_name = QFileDialog::getSaveFileName(&loader_, 
04904       tr("Save File"),
04905         QString().fromStdString(
04906         stdr_gui_tools::getRosPackagePath("stdr_resources")) + 
04907         QString("/resources/"),
04908         tr("Resource files (*.yaml *.xml)"));
04909     
04910     Q_EMIT saveRobotPressed(
04911       stdr_gui_tools::fixRobotAnglesToRad(new_robot_msg_),file_name);
04912   }
04913   
04918   void CRobotCreatorConnector::getRobotFromYaml(void)
04919   {
04920     QString file_name = QFileDialog::getOpenFileName(
04921       &loader_,
04922       tr("Load robot"), 
04923       QString().fromStdString(
04924         stdr_gui_tools::getRosPackagePath("stdr_resources")) + 
04925         QString("/resources/"), 
04926         tr("Resource Files (*.yaml *.xml)"));
04927     
04929     if (file_name.isEmpty()) { 
04930       return;
04931     }
04932 
04933     try {
04934       new_robot_msg_ = 
04935         stdr_parser::Parser::createMessage<stdr_msgs::RobotMsg>
04936           (file_name.toStdString());
04937     }
04938     catch(stdr_parser::ParserException ex)
04939     {
04940       QMessageBox msg;
04941       msg.setWindowTitle(QString("STDR Parser - Error"));
04942       msg.setText(QString(ex.what()));
04943       msg.exec();
04944       return;
04945     }
04946 
04947     new_robot_msg_ = stdr_gui_tools::fixRobotAnglesToDegrees(new_robot_msg_);
04948 
04949     CRobotCreatorConnector::laser_number = -1;
04950     CRobotCreatorConnector::sonar_number = -1;
04951     CRobotCreatorConnector::rfid_number = -1;
04952     CRobotCreatorConnector::co2_sensors_number = -1;
04953     CRobotCreatorConnector::thermal_sensors_number = -1;
04954     CRobotCreatorConnector::sound_sensors_number = -1;
04955     
04956     for(int i = loader_.robotInfoFootprint.childCount() - 1 ; i >=0 ; i--)
04957     {
04958       delete loader_.robotInfoFootprint.child(i);
04959     }
04960     
04961     updateRobotTree();
04962 
04963     updateRobotPreview(); 
04964   }
04965   
04970   void CRobotCreatorConnector::loadRobot(void)
04971   {
04972     Q_EMIT loadRobotPressed(new_robot_msg_);
04973     loader_.hide();
04974   }
04975   
04982   void CRobotCreatorConnector::setInitialPose(float x, float y)
04983   {
04984     new_robot_msg_.initialPose.x = x;
04985     new_robot_msg_.initialPose.y = y;
04986   }
04987   
04992   stdr_msgs::RobotMsg CRobotCreatorConnector::getNewRobot(void)
04993   {
04994     return new_robot_msg_;
04995   }
04996   
05002   void CRobotCreatorConnector::setNewRobot(stdr_msgs::RobotMsg msg)
05003   {
05004     new_robot_msg_=msg;
05005   }
05006   
05012   void CRobotCreatorConnector::showMessage(QString msg)
05013   {
05014     QMessageBox msgBox;
05015     msgBox.setText(msg);
05016     msgBox.exec();
05017   }
05018 }


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