00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
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
01065 smsg.pose.x = 0;
01066 smsg.pose.y = 0;
01067 smsg.pose.theta = 0;
01068
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
01083 *orientation,
01084 *maxRange,
01085 *poseX,
01086 *poseY,
01087
01088 *frequency;
01089
01090
01091 orientation = new QTreeWidgetItem();
01092 maxRange = new QTreeWidgetItem();
01093 poseX = new QTreeWidgetItem();
01094 poseY = new QTreeWidgetItem();
01095
01096 frequency = new QTreeWidgetItem();
01097
01098
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
01104 frequency->setText(0,QString("Frequency"));
01105
01106
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
01112 frequency->setText(1,QString().setNum(smsg.frequency));
01113
01114
01115 snode->addChild(orientation);
01116 snode->addChild(maxRange);
01117 snode->addChild(poseX);
01118 snode->addChild(poseY);
01119
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
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
01163 *frequency;
01164
01165 angleSpan = new QTreeWidgetItem();
01166 orientation = new QTreeWidgetItem();
01167 maxRange = new QTreeWidgetItem();
01168 poseX = new QTreeWidgetItem();
01169 poseY = new QTreeWidgetItem();
01170
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
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
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
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
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
01238 *frequency;
01239
01240 angleSpan = new QTreeWidgetItem();
01241 orientation = new QTreeWidgetItem();
01242 maxRange = new QTreeWidgetItem();
01243 poseX = new QTreeWidgetItem();
01244 poseY = new QTreeWidgetItem();
01245
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
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
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
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
01362 *orientation,
01363 *maxRange,
01364 *poseX,
01365 *poseY,
01366
01367 *frequency;
01368
01369
01370 orientation = new QTreeWidgetItem();
01371 maxRange = new QTreeWidgetItem();
01372 poseX = new QTreeWidgetItem();
01373 poseY = new QTreeWidgetItem();
01374
01375 frequency = new QTreeWidgetItem();
01376
01377
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
01383 frequency->setText(0,QString("Frequency"));
01384
01385
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
01391 frequency->setText(1,QString().setNum(smsg.frequency));
01392
01393
01394 snode->addChild(orientation);
01395 snode->addChild(maxRange);
01396 snode->addChild(poseX);
01397 snode->addChild(poseY);
01398
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
01431 *frequency;
01432
01433 angleSpan = new QTreeWidgetItem();
01434 orientation = new QTreeWidgetItem();
01435 maxRange = new QTreeWidgetItem();
01436 poseX = new QTreeWidgetItem();
01437 poseY = new QTreeWidgetItem();
01438
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
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
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
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
01495 *frequency;
01496
01497 angleSpan = new QTreeWidgetItem();
01498 orientation = new QTreeWidgetItem();
01499 maxRange = new QTreeWidgetItem();
01500 poseX = new QTreeWidgetItem();
01501 poseY = new QTreeWidgetItem();
01502
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
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
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
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
02318
02319
02320 loader_.co2SensorPropLoader.orientation->setText(
02321 QString().setNum(new_robot_msg_.co2Sensors[frameId].pose.theta));
02322
02323
02324
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
02698
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
02862
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
03387
03388
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
03399
03400
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
03443
03444
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
03487
03488
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
03623
03624
03625
03626
03627
03628
03629
03630
03631
03632
03633
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
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
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
04000
04001
04002
04003
04004
04005
04006
04007
04008
04009
04010
04011
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 }