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