Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include "stdr_parser/stdr_parser_msg_creator.h"
00023
00024 namespace stdr_parser
00025 {
00030 MessageCreator::MessageCreator(void)
00031 {
00032
00033 }
00034
00040 template <typename T>
00041 T MessageCreator::createMessage(Node *n,unsigned int id)
00042 {
00043 }
00044
00050 template <> geometry_msgs::Pose2D MessageCreator::createMessage(
00051 Node *n,unsigned int id)
00052 {
00053 geometry_msgs::Pose2D msg;
00054 std::vector<int> indexes;
00056 indexes = n->getTag("x");
00057 if(indexes.size() == 0)
00058 {
00059 msg.x = atof(Specs::specs["x"].default_value.c_str());
00060 }
00061 else
00062 {
00063 msg.x = atof(n->elements[indexes[0]]->elements[0]->value.c_str());
00064 }
00066 indexes = n->getTag("y");
00067 if(indexes.size() == 0)
00068 {
00069 msg.y = atof(Specs::specs["y"].default_value.c_str());
00070 }
00071 else
00072 {
00073 msg.y = atof(n->elements[indexes[0]]->elements[0]->value.c_str());
00074 }
00076 indexes = n->getTag("theta");
00077 if(indexes.size() == 0)
00078 {
00079 msg.theta = atof(Specs::specs["theta"].default_value.c_str());
00080 }
00081 else
00082 {
00083 msg.theta = atof(n->elements[indexes[0]]->elements[0]->value.c_str());
00084 }
00085 return msg;
00086 }
00087
00093 template <>
00094 geometry_msgs::Point MessageCreator::createMessage(Node *n, unsigned int id)
00095 {
00096 geometry_msgs::Point msg;
00097 std::vector<int> indexes;
00098
00099 indexes = n->getTag("x");
00100 if( indexes.size() == 0) {
00101 msg.x = atof(Specs::specs["x"].default_value.c_str());
00102 } else {
00103 msg.x = atof(n->elements[indexes[0]]->elements[0]->value.c_str());
00104 }
00105
00106 indexes = n->getTag("y");
00107 if( indexes.size() == 0) {
00108 msg.y = atof(Specs::specs["y"].default_value.c_str());
00109 } else {
00110 msg.y = atof(n->elements[indexes[0]]->elements[0]->value.c_str());
00111 }
00112
00113 indexes = n->getTag("z");
00114 if( indexes.size() == 0) {
00115 msg.z = atof(Specs::specs["z"].default_value.c_str());
00116 } else {
00117 msg.z = atof(n->elements[indexes[0]]->elements[0]->value.c_str());
00118 }
00119 return msg;
00120 }
00121
00127 template <> stdr_msgs::Noise MessageCreator::createMessage(
00128 Node *n,unsigned int id)
00129 {
00130 stdr_msgs::Noise msg;
00131 Node* specs = n->elements[0];
00132 if(specs->tag == "noise")
00133 {
00134 specs = specs->elements[0];
00135 }
00136 std::vector<int> indexes;
00138 indexes = specs->getTag("noise_mean");
00139 if(indexes.size() == 0)
00140 {
00141 msg.noiseMean = atof(Specs::specs["noise_mean"].default_value.c_str());
00142 }
00143 else
00144 {
00145 msg.noiseMean = atof(specs->elements[indexes[0]]->elements[0]->
00146 value.c_str());
00147 }
00149 indexes = specs->getTag("noise_std");
00150 if(indexes.size() == 0)
00151 {
00152 msg.noiseStd = atof(Specs::specs["noise_std"].default_value.c_str());
00153 }
00154 else
00155 {
00156 msg.noiseStd = atof(specs->elements[indexes[0]]->elements[0]->
00157 value.c_str());
00158 }
00159 return msg;
00160 }
00161
00167 template <>
00168 stdr_msgs::FootprintMsg MessageCreator::createMessage(
00169 Node *n,unsigned int id)
00170 {
00171 stdr_msgs::FootprintMsg msg;
00172 Node* specs = n->elements[0];
00173 if(specs->tag == "footprint")
00174 {
00175 specs = specs->elements[0];
00176 }
00177 std::vector<int> indexes;
00179 indexes = specs->getTag("radius");
00180 if(indexes.size() == 0)
00181 {
00182 msg.radius = atof(Specs::specs["radius"].default_value.c_str());
00183 }
00184 else
00185 {
00186 msg.radius = atof(specs->elements[indexes[0]]->elements[0]->
00187 value.c_str());
00188 }
00189
00190 indexes = specs->getTag("points");
00191 if( indexes.size() != 0 ) {
00192 specs = specs->elements[indexes[0]];
00193 std::vector<int> points = specs->getTag("point");
00194 for( unsigned int i = 0; i < points.size(); i++ ) {
00195 msg.points.push_back(createMessage<geometry_msgs::Point>(
00196 specs->elements[points[i]], i));
00197 }
00198 }
00199 return msg;
00200 }
00201
00207 template <>
00208 stdr_msgs::LaserSensorMsg MessageCreator::createMessage(
00209 Node *n,unsigned int id)
00210 {
00211 stdr_msgs::LaserSensorMsg msg;
00212 Node* specs = n->elements[0];
00213 if(specs->tag == "laser")
00214 {
00215 specs = specs->elements[0];
00216 }
00217 std::vector<int> indexes;
00218
00220 indexes = specs->getTag("max_angle");
00221 if(indexes.size() == 0)
00222 {
00223 msg.maxAngle = atof(Specs::specs["max_angle"].default_value.c_str());
00224 }
00225 else
00226 {
00227 msg.maxAngle = atof(specs->elements[indexes[0]]->elements[0]->
00228 value.c_str());
00229 }
00230
00232 indexes = specs->getTag("min_angle");
00233 if(indexes.size() == 0)
00234 {
00235 msg.minAngle = atof(Specs::specs["min_angle"].default_value.c_str());
00236 }
00237 else
00238 {
00239 msg.minAngle = atof(specs->elements[indexes[0]]->elements[0]->
00240 value.c_str());
00241 }
00242
00244 indexes = specs->getTag("max_range");
00245 if(indexes.size() == 0)
00246 {
00247 msg.maxRange = atof(Specs::specs["max_range"].default_value.c_str());
00248 }
00249 else
00250 {
00251 msg.maxRange = atof(specs->elements[indexes[0]]->elements[0]->
00252 value.c_str());
00253 }
00254
00256 indexes = specs->getTag("min_range");
00257 if(indexes.size() == 0)
00258 {
00259 msg.minRange = atof(Specs::specs["min_range"].default_value.c_str());
00260 }
00261 else
00262 {
00263 msg.minRange = atof(specs->elements[indexes[0]]->elements[0]->
00264 value.c_str());
00265 }
00266
00268 indexes = specs->getTag("num_rays");
00269 if(indexes.size() == 0)
00270 {
00271 msg.numRays = atoi(Specs::specs["num_rays"].default_value.c_str());
00272 }
00273 else
00274 {
00275 msg.numRays = atoi(specs->elements[indexes[0]]->elements[0]->
00276 value.c_str());
00277 }
00278
00280 indexes = specs->getTag("noise");
00281 if(indexes.size() != 0)
00282 {
00283 msg.noise =
00284 createMessage<stdr_msgs::Noise>(specs->elements[indexes[0]],0);
00285 }
00286
00288 indexes = specs->getTag("frequency");
00289 if(indexes.size() == 0)
00290 {
00291 msg.frequency = atof(Specs::specs["frequency"].default_value.c_str());
00292 }
00293 else
00294 {
00295 msg.frequency = atof(specs->elements[indexes[0]]->elements[0]->
00296 value.c_str());
00297 }
00298
00300 indexes = specs->getTag("frame_id");
00301 if(indexes.size() == 0)
00302 {
00303 msg.frame_id = std::string("laser_") + SSTR(id);
00304 }
00305 else
00306 {
00307 msg.frame_id = specs->elements[indexes[0]]->elements[0]->value;
00308 }
00309
00311 indexes = specs->getTag("pose");
00312 if(indexes.size() != 0)
00313 {
00314 msg.pose =
00315 createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
00316 }
00317 else
00318 {
00319 msg.pose.x = atof(Specs::specs["x"].default_value.c_str());
00320 msg.pose.y = atof(Specs::specs["y"].default_value.c_str());
00321 msg.pose.theta = atof(Specs::specs["theta"].default_value.c_str());
00322 }
00323 return msg;
00324 }
00325
00331 template <> stdr_msgs::SonarSensorMsg MessageCreator::createMessage(
00332 Node *n,unsigned int id)
00333 {
00334 stdr_msgs::SonarSensorMsg msg;
00335 Node* specs = n->elements[0];
00336 if(specs->tag == "sonar")
00337 {
00338 specs = specs->elements[0];
00339 }
00340 std::vector<int> indexes;
00341
00343 indexes = specs->getTag("max_range");
00344 if(indexes.size() == 0)
00345 {
00346 msg.maxRange = atof(Specs::specs["max_range"].default_value.c_str());
00347 }
00348 else
00349 {
00350 msg.maxRange = atof(specs->elements[indexes[0]]->elements[0]->
00351 value.c_str());
00352 }
00353
00355 indexes = specs->getTag("min_range");
00356 if(indexes.size() == 0)
00357 {
00358 msg.minRange = atof(Specs::specs["min_range"].default_value.c_str());
00359 }
00360 else
00361 {
00362 msg.minRange = atof(specs->elements[indexes[0]]->elements[0]->
00363 value.c_str());
00364 }
00365
00367 indexes = specs->getTag("cone_angle");
00368 if(indexes.size() == 0)
00369 {
00370 msg.coneAngle = atof(Specs::specs["cone_angle"].default_value.c_str());
00371 }
00372 else
00373 {
00374 msg.coneAngle = atof(specs->elements[indexes[0]]->elements[0]->
00375 value.c_str());
00376 }
00377
00379 indexes = specs->getTag("noise");
00380 if(indexes.size() != 0)
00381 {
00382 msg.noise =
00383 createMessage<stdr_msgs::Noise>(specs->elements[indexes[0]],0);
00384 }
00385
00387 indexes = specs->getTag("frequency");
00388 if(indexes.size() == 0)
00389 {
00390 msg.frequency = atof(Specs::specs["frequency"].default_value.c_str());
00391 }
00392 else
00393 {
00394 msg.frequency = atof(specs->elements[indexes[0]]->elements[0]->
00395 value.c_str());
00396 }
00397
00399 indexes = specs->getTag("frame_id");
00400 if(indexes.size() == 0)
00401 {
00402 msg.frame_id = std::string("sonar_") + SSTR(id);
00403 }
00404 else
00405 {
00406 msg.frame_id = specs->elements[indexes[0]]->elements[0]->value;
00407 }
00408
00410 indexes = specs->getTag("pose");
00411 if(indexes.size() != 0)
00412 {
00413 msg.pose =
00414 createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
00415 }
00416 else
00417 {
00418 msg.pose.x = atof(Specs::specs["x"].default_value.c_str());
00419 msg.pose.y = atof(Specs::specs["y"].default_value.c_str());
00420 msg.pose.theta = atof(Specs::specs["theta"].default_value.c_str());
00421 }
00422 return msg;
00423 }
00424
00431 template <> stdr_msgs::RfidSensorMsg MessageCreator::createMessage(
00432 Node *n,unsigned int id)
00433 {
00434 stdr_msgs::RfidSensorMsg msg;
00435 Node* specs = n->elements[0];
00436 if(specs->tag == "rfid_reader")
00437 {
00438 specs = specs->elements[0];
00439 }
00440 std::vector<int> indexes;
00441
00443 indexes = specs->getTag("angle_span");
00444 if(indexes.size() == 0)
00445 {
00446 msg.angleSpan = atof(Specs::specs["angle_span"].default_value.c_str());
00447 }
00448 else
00449 {
00450 msg.angleSpan = atof(specs->elements[indexes[0]]->elements[0]->
00451 value.c_str());
00452 }
00453
00455 indexes = specs->getTag("max_range");
00456 if(indexes.size() == 0)
00457 {
00458 msg.maxRange = atof(Specs::specs["max_range"].default_value.c_str());
00459 }
00460 else
00461 {
00462 msg.maxRange = atof(specs->elements[indexes[0]]->elements[0]->
00463 value.c_str());
00464 }
00465
00467 indexes = specs->getTag("signal_cutoff");
00468 if(indexes.size() == 0)
00469 {
00470 msg.signalCutoff =
00471 atof(Specs::specs["signal_cutoff"].default_value.c_str());
00472 }
00473 else
00474 {
00475 msg.signalCutoff = atof(specs->elements[indexes[0]]->elements[0]->
00476 value.c_str());
00477 }
00478
00480 indexes = specs->getTag("frequency");
00481 if(indexes.size() == 0)
00482 {
00483 msg.frequency = atof(Specs::specs["frequency"].default_value.c_str());
00484 }
00485 else
00486 {
00487 msg.frequency = atof(specs->elements[indexes[0]]->elements[0]->
00488 value.c_str());
00489 }
00490
00492 indexes = specs->getTag("frame_id");
00493 if(indexes.size() == 0)
00494 {
00495 msg.frame_id = std::string("rfid_reader_") + SSTR(id);
00496 }
00497 else
00498 {
00499 msg.frame_id = specs->elements[indexes[0]]->elements[0]->value;
00500 }
00501
00503 indexes = specs->getTag("pose");
00504 if(indexes.size() != 0)
00505 {
00506 msg.pose =
00507 createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
00508 }
00509 else
00510 {
00511 msg.pose.x = atof(Specs::specs["x"].default_value.c_str());
00512 msg.pose.y = atof(Specs::specs["y"].default_value.c_str());
00513 msg.pose.theta = atof(Specs::specs["theta"].default_value.c_str());
00514 }
00515 return msg;
00516 }
00517
00524 template <> stdr_msgs::CO2SensorMsg MessageCreator::createMessage(
00525 Node *n,unsigned int id)
00526 {
00527 stdr_msgs::CO2SensorMsg msg;
00528 Node* specs = n->elements[0];
00529 if(specs->tag == "co2_sensor")
00530 {
00531 specs = specs->elements[0];
00532 }
00533 std::vector<int> indexes;
00534
00536 indexes = specs->getTag("max_range");
00537 if(indexes.size() == 0)
00538 {
00539 msg.maxRange = atof(Specs::specs["max_range"].default_value.c_str());
00540 }
00541 else
00542 {
00543 msg.maxRange = atof(specs->elements[indexes[0]]->elements[0]->
00544 value.c_str());
00545 }
00546
00548 indexes = specs->getTag("frequency");
00549 if(indexes.size() == 0)
00550 {
00551 msg.frequency = atof(Specs::specs["frequency"].default_value.c_str());
00552 }
00553 else
00554 {
00555 msg.frequency = atof(specs->elements[indexes[0]]->elements[0]->
00556 value.c_str());
00557 }
00558
00560 indexes = specs->getTag("frame_id");
00561 if(indexes.size() == 0)
00562 {
00563 msg.frame_id = std::string("co2_sensor_") + SSTR(id);
00564 }
00565 else
00566 {
00567 msg.frame_id = specs->elements[indexes[0]]->elements[0]->value;
00568 }
00569
00571 indexes = specs->getTag("pose");
00572 if(indexes.size() != 0)
00573 {
00574 msg.pose =
00575 createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
00576 }
00577 else
00578 {
00579 msg.pose.x = atof(Specs::specs["x"].default_value.c_str());
00580 msg.pose.y = atof(Specs::specs["y"].default_value.c_str());
00581 msg.pose.theta = atof(Specs::specs["theta"].default_value.c_str());
00582 }
00583 return msg;
00584 }
00585
00592 template <> stdr_msgs::ThermalSensorMsg MessageCreator::createMessage(
00593 Node *n,unsigned int id)
00594 {
00595 stdr_msgs::ThermalSensorMsg msg;
00596 Node* specs = n->elements[0];
00597 if(specs->tag == "thermal_sensor")
00598 {
00599 specs = specs->elements[0];
00600 }
00601 std::vector<int> indexes;
00602
00604 indexes = specs->getTag("max_range");
00605 if(indexes.size() == 0)
00606 {
00607 msg.maxRange = atof(Specs::specs["max_range"].default_value.c_str());
00608 }
00609 else
00610 {
00611 msg.maxRange = atof(specs->elements[indexes[0]]->elements[0]->
00612 value.c_str());
00613 }
00614
00616 indexes = specs->getTag("angle_span");
00617 if(indexes.size() == 0)
00618 {
00619 msg.angleSpan = atof(Specs::specs["angle_span"].default_value.c_str());
00620 }
00621 else
00622 {
00623 msg.angleSpan = atof(specs->elements[indexes[0]]->elements[0]->
00624 value.c_str());
00625 }
00626
00628 indexes = specs->getTag("frequency");
00629 if(indexes.size() == 0)
00630 {
00631 msg.frequency = atof(Specs::specs["frequency"].default_value.c_str());
00632 }
00633 else
00634 {
00635 msg.frequency = atof(specs->elements[indexes[0]]->elements[0]->
00636 value.c_str());
00637 }
00638
00640 indexes = specs->getTag("frame_id");
00641 if(indexes.size() == 0)
00642 {
00643 msg.frame_id = std::string("thermal_sensor_") + SSTR(id);
00644 }
00645 else
00646 {
00647 msg.frame_id = specs->elements[indexes[0]]->elements[0]->value;
00648 }
00649
00651 indexes = specs->getTag("pose");
00652 if(indexes.size() != 0)
00653 {
00654 msg.pose =
00655 createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
00656 }
00657 else
00658 {
00659 msg.pose.x = atof(Specs::specs["x"].default_value.c_str());
00660 msg.pose.y = atof(Specs::specs["y"].default_value.c_str());
00661 msg.pose.theta = atof(Specs::specs["theta"].default_value.c_str());
00662 }
00663 return msg;
00664 }
00665
00672 template <> stdr_msgs::SoundSensorMsg MessageCreator::createMessage(
00673 Node *n,unsigned int id)
00674 {
00675 stdr_msgs::SoundSensorMsg msg;
00676 Node* specs = n->elements[0];
00677 if(specs->tag == "sound_sensor")
00678 {
00679 specs = specs->elements[0];
00680 }
00681 std::vector<int> indexes;
00682
00684 indexes = specs->getTag("max_range");
00685 if(indexes.size() == 0)
00686 {
00687 msg.maxRange = atof(Specs::specs["max_range"].default_value.c_str());
00688 }
00689 else
00690 {
00691 msg.maxRange = atof(specs->elements[indexes[0]]->elements[0]->
00692 value.c_str());
00693 }
00694
00696 indexes = specs->getTag("angle_span");
00697 if(indexes.size() == 0)
00698 {
00699 msg.angleSpan = atof(Specs::specs["angle_span"].default_value.c_str());
00700 }
00701 else
00702 {
00703 msg.angleSpan = atof(specs->elements[indexes[0]]->elements[0]->
00704 value.c_str());
00705 }
00706
00708 indexes = specs->getTag("frequency");
00709 if(indexes.size() == 0)
00710 {
00711 msg.frequency = atof(Specs::specs["frequency"].default_value.c_str());
00712 }
00713 else
00714 {
00715 msg.frequency = atof(specs->elements[indexes[0]]->elements[0]->
00716 value.c_str());
00717 }
00718
00720 indexes = specs->getTag("frame_id");
00721 if(indexes.size() == 0)
00722 {
00723 msg.frame_id = std::string("sound_sensor_") + SSTR(id);
00724 }
00725 else
00726 {
00727 msg.frame_id = specs->elements[indexes[0]]->elements[0]->value;
00728 }
00729
00731 indexes = specs->getTag("pose");
00732 if(indexes.size() != 0)
00733 {
00734 msg.pose =
00735 createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
00736 }
00737 else
00738 {
00739 msg.pose.x = atof(Specs::specs["x"].default_value.c_str());
00740 msg.pose.y = atof(Specs::specs["y"].default_value.c_str());
00741 msg.pose.theta = atof(Specs::specs["theta"].default_value.c_str());
00742 }
00743 return msg;
00744 }
00745
00751 template <>
00752 stdr_msgs::RobotMsg MessageCreator::createMessage(Node *n,unsigned int id)
00753 {
00754 stdr_msgs::RobotMsg msg;
00755 Node* specs = n->elements[0];
00756 if(specs->tag == "robot")
00757 {
00758 specs = specs->elements[0];
00759 }
00760 std::vector<int> indexes;
00761
00763 indexes = specs->getTag("initial_pose");
00764 if(indexes.size() != 0)
00765 {
00766 msg.initialPose =
00767 createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
00768 }
00769 else
00770 {
00771 msg.initialPose.x = atof(Specs::specs["x"].default_value.c_str());
00772 msg.initialPose.y = atof(Specs::specs["y"].default_value.c_str());
00773 msg.initialPose.theta = atof(Specs::specs["theta"].default_value.c_str());
00774 }
00775
00777 indexes = specs->getTag("footprint");
00778 if(indexes.size() != 0)
00779 {
00780 msg.footprint =
00781 createMessage<stdr_msgs::FootprintMsg>(specs->elements[indexes[0]],0);
00782 }
00783 else
00784 {
00785 msg.footprint.radius = atof(Specs::specs["radius"].default_value.c_str());
00786 }
00787
00789 indexes = specs->getTag("laser");
00790 if(indexes.size() != 0)
00791 {
00792 for(unsigned int i = 0 ; i < indexes.size() ; i++)
00793 {
00794 msg.laserSensors.push_back(
00795 createMessage<stdr_msgs::LaserSensorMsg>(
00796 specs->elements[indexes[i]] , i));
00797 }
00798 }
00799
00801 indexes = specs->getTag("sonar");
00802 if(indexes.size() != 0)
00803 {
00804 for(unsigned int i = 0 ; i < indexes.size() ; i++)
00805 {
00806 msg.sonarSensors.push_back(
00807 createMessage<stdr_msgs::SonarSensorMsg>(
00808 specs->elements[indexes[i]] , i));
00809 }
00810 }
00811
00813 indexes = specs->getTag("rfid_reader");
00814 if(indexes.size() != 0)
00815 {
00816 for(unsigned int i = 0 ; i < indexes.size() ; i++)
00817 {
00818 msg.rfidSensors.push_back(
00819 createMessage<stdr_msgs::RfidSensorMsg>(
00820 specs->elements[indexes[i]] , i));
00821 }
00822 }
00823
00825 indexes = specs->getTag("co2_sensor");
00826 if(indexes.size() != 0)
00827 {
00828 for(unsigned int i = 0 ; i < indexes.size() ; i++)
00829 {
00830 msg.co2Sensors.push_back(
00831 createMessage<stdr_msgs::CO2SensorMsg>(
00832 specs->elements[indexes[i]] , i));
00833 }
00834 }
00835
00837 indexes = specs->getTag("thermal_sensor");
00838 if(indexes.size() != 0)
00839 {
00840 for(unsigned int i = 0 ; i < indexes.size() ; i++)
00841 {
00842 msg.thermalSensors.push_back(
00843 createMessage<stdr_msgs::ThermalSensorMsg>(
00844 specs->elements[indexes[i]] , i));
00845 }
00846 }
00847
00849 indexes = specs->getTag("sound_sensor");
00850 if(indexes.size() != 0)
00851 {
00852 for(unsigned int i = 0 ; i < indexes.size() ; i++)
00853 {
00854 msg.soundSensors.push_back(
00855 createMessage<stdr_msgs::SoundSensorMsg>(
00856 specs->elements[indexes[i]] , i));
00857 }
00858 }
00859
00860 return msg;
00861 }
00862
00863
00864
00865 }
00866