stdr_parser: stdr_parser_msg_creator.cpp Source File

Go to the documentation of this file.
00001 /******************************************************************************
00002    STDR Simulator - Simple Two DImensional Robot Simulator
00003    Copyright (C) 2013 STDR Simulator
00004    This program is free software; you can redistribute it and/or modify
00005    it under the terms of the GNU General Public License as published by
00006    the Free Software Foundation; either version 3 of the License, or
00007    (at your option) any later version.
00008    This program is distributed in the hope that it will be useful,
00009    but WITHOUT ANY WARRANTY; without even the implied warranty of
00010    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00011    GNU General Public License for more details.
00012    You should have received a copy of the GNU General Public License
00013    along with this program; if not, write to the Free Software Foundation,
00014    Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301  USA
00015    
00016    Authors : 
00017    * Manos Tsardoulias, etsardou@gmail.com
00018    * Aris Thallas, aris.thallas@gmail.com
00019    * Chris Zalidis, zalidis@gmail.com 
00020 ******************************************************************************/
00021 
00022 #include "stdr_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       
00060       msg.x = stdr_parser::MessageCreator::stringToType<float>(
00061         Specs::specs["x"].default_value.c_str());
00062     }
00063     else
00064     {
00065 
00066       msg.x = stdr_parser::MessageCreator::stringToType<float>(
00067         n->elements[indexes[0]]->elements[0]->value.c_str());
00068     }
00070     indexes = n->getTag("y");
00071     if(indexes.size() == 0)
00072     {
00073       msg.y = stdr_parser::MessageCreator::stringToType<float>(
00074         Specs::specs["y"].default_value.c_str());
00075     }
00076     else
00077     {
00078       msg.y =  stdr_parser::MessageCreator::stringToType<float>(
00079         n->elements[indexes[0]]->elements[0]->value.c_str());
00080     }
00082     indexes = n->getTag("theta");
00083     if(indexes.size() == 0)
00084     {
00085       msg.theta =  stdr_parser::MessageCreator::stringToType<float>(
00086         Specs::specs["theta"].default_value.c_str());
00087     }
00088     else
00089     {
00090       msg.theta =  stdr_parser::MessageCreator::stringToType<float>(
00091         n->elements[indexes[0]]->elements[0]->value.c_str());
00092     }
00093     return msg;
00094   }
00095   
00101   template <>
00102   geometry_msgs::Point MessageCreator::createMessage(Node *n, unsigned int id)
00103   {
00104     geometry_msgs::Point msg;
00105     std::vector<int> indexes;
00106     // x
00107     indexes = n->getTag("x");
00108     if( indexes.size() == 0) {
00109       msg.x =  stdr_parser::MessageCreator::stringToType<float>(
00110         Specs::specs["x"].default_value.c_str());
00111     } else {
00112       msg.x =  stdr_parser::MessageCreator::stringToType<float>(
00113         n->elements[indexes[0]]->elements[0]->value.c_str());
00114     }
00115     // y
00116     indexes = n->getTag("y");
00117     if( indexes.size() == 0) {
00118       msg.y =  stdr_parser::MessageCreator::stringToType<float>(
00119         Specs::specs["y"].default_value.c_str());
00120     } else {
00121       msg.y =  stdr_parser::MessageCreator::stringToType<float>(
00122         n->elements[indexes[0]]->elements[0]->value.c_str());
00123     }
00124     // z
00125     indexes = n->getTag("z");
00126     if( indexes.size() == 0) {
00127       msg.z =  stdr_parser::MessageCreator::stringToType<float>(
00128         Specs::specs["z"].default_value.c_str());
00129     } else {
00130       msg.z =  stdr_parser::MessageCreator::stringToType<float>(
00131         n->elements[indexes[0]]->elements[0]->value.c_str());
00132     }
00133     return msg;
00134   }
00135 
00141   template <> stdr_msgs::Noise MessageCreator::createMessage(
00142     Node *n,unsigned int id)
00143   {
00144     stdr_msgs::Noise msg;
00145     Node* specs = n->elements[0];
00146     if(specs->tag == "noise")
00147     {
00148       specs = specs->elements[0];
00149     }
00150     std::vector<int> indexes;
00152     indexes = specs->getTag("noise_mean");
00153     if(indexes.size() == 0)
00154     {
00155       msg.noiseMean =  stdr_parser::MessageCreator::stringToType<float>(
00156         Specs::specs["noise_mean"].default_value.c_str());
00157     }
00158     else
00159     {
00160       msg.noiseMean =  stdr_parser::MessageCreator::stringToType<float>(
00161         specs->elements[indexes[0]]->elements[0]->value.c_str());
00162     }
00164     indexes = specs->getTag("noise_std");
00165     if(indexes.size() == 0)
00166     {
00167       msg.noiseStd = stdr_parser::MessageCreator::stringToType<float>(
00168         Specs::specs["noise_std"].default_value.c_str());
00169     }
00170     else
00171     {
00172       msg.noiseStd = stdr_parser::MessageCreator::stringToType<float>(
00173         specs->elements[indexes[0]]->elements[0]->value.c_str());
00174     }
00175     return msg;
00176   }
00177   
00183   template <> 
00184   stdr_msgs::FootprintMsg MessageCreator::createMessage(
00185     Node *n,unsigned int id)
00186   {
00187     stdr_msgs::FootprintMsg msg;
00188     Node* specs = n->elements[0];
00189     if(specs->tag == "footprint")
00190     {
00191       specs = specs->elements[0];
00192     }
00193     else if(specs->tag == "footprint_specifications")
00194     {
00195       // specs must remain specs 
00196     }
00197 
00198     std::vector<int> indexes;
00200     indexes = specs->getTag("radius");
00201     if(indexes.size() == 0)
00202     {
00203       msg.radius = stdr_parser::MessageCreator::stringToType<float>(
00204         Specs::specs["radius"].default_value.c_str());
00205     }
00206     else
00207     {
00208       msg.radius = stdr_parser::MessageCreator::stringToType<float>(
00209         specs->elements[indexes[0]]->elements[0]->value);
00210     }
00211     // search for points
00212     indexes = specs->getTag("points");
00213     if( indexes.size() != 0 ) {
00214       specs = specs->elements[indexes[0]];
00215       std::vector<int> points = specs->getTag("point");
00216       for( unsigned int i = 0; i < points.size(); i++ ) {
00217         msg.points.push_back(createMessage<geometry_msgs::Point>(
00218               specs->elements[points[i]], i));
00219       }
00220     }
00221     return msg;
00222   }
00223   
00229   template <> 
00230   stdr_msgs::LaserSensorMsg MessageCreator::createMessage(
00231     Node *n,unsigned int id)
00232   {
00233     stdr_msgs::LaserSensorMsg msg;
00234     Node* specs = n->elements[0];
00235     if(specs->tag == "laser")
00236     {
00237       specs = specs->elements[0];
00238     }
00239     std::vector<int> indexes;
00240     
00242     indexes = specs->getTag("max_angle");
00243     if(indexes.size() == 0)
00244     {
00245       msg.maxAngle = stdr_parser::MessageCreator::stringToType<float>(
00246         Specs::specs["max_angle"].default_value.c_str());
00247     }
00248     else
00249     {
00250       msg.maxAngle =  stdr_parser::MessageCreator::stringToType<float>(
00251         specs->elements[indexes[0]]->elements[0]->value.c_str());
00252     }
00253     
00255     indexes = specs->getTag("min_angle");
00256     if(indexes.size() == 0)
00257     {
00258       msg.minAngle = stdr_parser::MessageCreator::stringToType<float>(
00259         Specs::specs["min_angle"].default_value.c_str());
00260     }
00261     else
00262     {
00263       msg.minAngle = stdr_parser::MessageCreator::stringToType<float>(
00264         specs->elements[indexes[0]]->elements[0]->value.c_str());
00265     }
00266     
00268     indexes = specs->getTag("max_range");
00269     if(indexes.size() == 0)
00270     {
00271       msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
00272         Specs::specs["max_range"].default_value.c_str());
00273     }
00274     else
00275     {
00276       msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
00277         specs->elements[indexes[0]]->elements[0]->value.c_str());
00278     }
00279     
00281     indexes = specs->getTag("min_range");
00282     if(indexes.size() == 0)
00283     {
00284       msg.minRange = stdr_parser::MessageCreator::stringToType<float>(
00285         Specs::specs["min_range"].default_value.c_str());
00286     }
00287     else
00288     {
00289       msg.minRange = stdr_parser::MessageCreator::stringToType<float>(
00290         specs->elements[indexes[0]]->elements[0]->value.c_str());
00291     }
00292     
00294     indexes = specs->getTag("num_rays");
00295     if(indexes.size() == 0)
00296     {
00297       msg.numRays = stdr_parser::MessageCreator::stringToType<int>(
00298         Specs::specs["num_rays"].default_value.c_str());
00299     }
00300     else
00301     {
00302       msg.numRays = stdr_parser::MessageCreator::stringToType<int>(
00303         specs->elements[indexes[0]]->elements[0]->value.c_str());
00304     }
00305     
00307     indexes = specs->getTag("noise");
00308     if(indexes.size() != 0)
00309     {
00310       msg.noise = 
00311         createMessage<stdr_msgs::Noise>(specs->elements[indexes[0]],0);
00312     }
00313     
00315     indexes = specs->getTag("frequency");
00316     if(indexes.size() == 0)
00317     {
00318       msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
00319         Specs::specs["frequency"].default_value.c_str());
00320     }
00321     else
00322     {
00323       msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
00324         specs->elements[indexes[0]]->elements[0]->value.c_str());
00325     }
00326     
00328     indexes = specs->getTag("frame_id");
00329     if(indexes.size() == 0)
00330     {
00331       msg.frame_id = std::string("laser_") + SSTR(id);
00332     }
00333     else
00334     {
00335       msg.frame_id = specs->elements[indexes[0]]->elements[0]->value;
00336     }
00337     
00339     indexes = specs->getTag("pose");
00340     if(indexes.size() != 0)
00341     {
00342       msg.pose = 
00343         createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
00344     }
00345     else
00346     {
00347       msg.pose.x =  stdr_parser::MessageCreator::stringToType<float>(
00348         Specs::specs["x"].default_value.c_str());
00349       msg.pose.y = stdr_parser::MessageCreator::stringToType<float>(
00350         Specs::specs["y"].default_value.c_str());
00351       msg.pose.theta = stdr_parser::MessageCreator::stringToType<float>(
00352         Specs::specs["theta"].default_value.c_str());
00353     }
00354     return msg;
00355   }
00356   
00362   template <> stdr_msgs::SonarSensorMsg MessageCreator::createMessage(
00363     Node *n,unsigned int id)
00364   {
00365     stdr_msgs::SonarSensorMsg msg;
00366     Node* specs = n->elements[0];
00367     if(specs->tag == "sonar")
00368     {
00369       specs = specs->elements[0];
00370     }
00371     std::vector<int> indexes;
00372     
00374     indexes = specs->getTag("max_range");
00375     if(indexes.size() == 0)
00376     {
00377       msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
00378         Specs::specs["max_range"].default_value.c_str());
00379     }
00380     else
00381     {
00382       msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
00383         specs->elements[indexes[0]]->elements[0]->value.c_str());
00384     }
00385     
00387     indexes = specs->getTag("min_range");
00388     if(indexes.size() == 0)
00389     {
00390       msg.minRange = stdr_parser::MessageCreator::stringToType<float>(
00391         Specs::specs["min_range"].default_value.c_str());
00392     }
00393     else
00394     {
00395       msg.minRange = stdr_parser::MessageCreator::stringToType<float>(
00396         specs->elements[indexes[0]]->elements[0]->value.c_str());
00397     }
00398     
00400     indexes = specs->getTag("cone_angle");
00401     if(indexes.size() == 0)
00402     {
00403       msg.coneAngle =  stdr_parser::MessageCreator::stringToType<float>(
00404         Specs::specs["cone_angle"].default_value.c_str());
00405     }
00406     else
00407     {
00408       msg.coneAngle = stdr_parser::MessageCreator::stringToType<float>(
00409         specs->elements[indexes[0]]->elements[0]->value.c_str());
00410     }
00411     
00413     indexes = specs->getTag("noise");
00414     if(indexes.size() != 0)
00415     {
00416       msg.noise =   
00417         createMessage<stdr_msgs::Noise>(specs->elements[indexes[0]],0);
00418     }
00419     
00421     indexes = specs->getTag("frequency");
00422     if(indexes.size() == 0)
00423     {
00424       msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
00425         Specs::specs["frequency"].default_value.c_str());
00426     }
00427     else
00428     {
00429       msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
00430         specs->elements[indexes[0]]->elements[0]->value.c_str());
00431     }
00432     
00434     indexes = specs->getTag("frame_id");
00435     if(indexes.size() == 0)
00436     {
00437       msg.frame_id = std::string("sonar_") + SSTR(id);
00438     }
00439     else
00440     {
00441       msg.frame_id = specs->elements[indexes[0]]->elements[0]->value;
00442     }
00443     
00445     indexes = specs->getTag("pose");
00446     if(indexes.size() != 0)
00447     {
00448       msg.pose = 
00449         createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
00450     }
00451     else
00452     {
00453       msg.pose.x = stdr_parser::MessageCreator::stringToType<float>(
00454         Specs::specs["x"].default_value.c_str());
00455       msg.pose.y = stdr_parser::MessageCreator::stringToType<float>(
00456         Specs::specs["y"].default_value.c_str());
00457       msg.pose.theta = stdr_parser::MessageCreator::stringToType<float>(
00458         Specs::specs["theta"].default_value.c_str());
00459     }
00460     return msg;
00461   }
00462   
00469   template <> stdr_msgs::RfidSensorMsg MessageCreator::createMessage(
00470     Node *n,unsigned int id)
00471   {
00472     stdr_msgs::RfidSensorMsg msg;
00473     Node* specs = n->elements[0];
00474     if(specs->tag == "rfid_reader")
00475     {
00476       specs = specs->elements[0];
00477     }
00478     std::vector<int> indexes;
00479     
00481     indexes = specs->getTag("angle_span");
00482     if(indexes.size() == 0)
00483     {
00484       msg.angleSpan = stdr_parser::MessageCreator::stringToType<float>(
00485         Specs::specs["angle_span"].default_value.c_str());
00486     }
00487     else
00488     {
00489       msg.angleSpan = stdr_parser::MessageCreator::stringToType<float>(
00490         specs->elements[indexes[0]]->elements[0]->value.c_str());
00491     }
00492     
00494     indexes = specs->getTag("max_range");
00495     if(indexes.size() == 0)
00496     {
00497       msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
00498         Specs::specs["max_range"].default_value.c_str());
00499     }
00500     else
00501     {
00502       msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
00503         specs->elements[indexes[0]]->elements[0]->value.c_str());
00504     }
00505     
00507     indexes = specs->getTag("signal_cutoff");
00508     if(indexes.size() == 0)
00509     {
00510       msg.signalCutoff = stdr_parser::MessageCreator::stringToType<float>(
00511           Specs::specs["signal_cutoff"].default_value.c_str());
00512     }
00513     else
00514     {
00515       msg.signalCutoff =  stdr_parser::MessageCreator::stringToType<float>(
00516         specs->elements[indexes[0]]->elements[0]->value.c_str());
00517     }
00518     
00520     indexes = specs->getTag("frequency");
00521     if(indexes.size() == 0)
00522     {
00523       msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
00524         Specs::specs["frequency"].default_value.c_str());
00525     }
00526     else
00527     {
00528       msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
00529         specs->elements[indexes[0]]->elements[0]->value.c_str());
00530     }
00531 
00533     indexes = specs->getTag("frame_id");
00534     if(indexes.size() == 0)
00535     {
00536       msg.frame_id = std::string("rfid_reader_") + SSTR(id);
00537     }
00538     else
00539     {
00540       msg.frame_id = specs->elements[indexes[0]]->elements[0]->value;
00541     }
00542     
00544     indexes = specs->getTag("pose");
00545     if(indexes.size() != 0)
00546     {
00547       msg.pose = 
00548         createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
00549     }
00550     else
00551     {
00552       msg.pose.x = stdr_parser::MessageCreator::stringToType<float>(
00553         Specs::specs["x"].default_value.c_str());
00554       msg.pose.y = stdr_parser::MessageCreator::stringToType<float>(
00555         Specs::specs["y"].default_value.c_str());
00556       msg.pose.theta = stdr_parser::MessageCreator::stringToType<float>(
00557         Specs::specs["theta"].default_value.c_str());
00558     }
00559     return msg;
00560   }
00561   
00568   template <> stdr_msgs::CO2SensorMsg MessageCreator::createMessage(
00569     Node *n,unsigned int id)
00570   {
00571     stdr_msgs::CO2SensorMsg msg;
00572     Node* specs = n->elements[0];
00573     if(specs->tag == "co2_sensor")
00574     {
00575       specs = specs->elements[0];
00576     }
00577     std::vector<int> indexes;
00578     
00580     indexes = specs->getTag("max_range");
00581     if(indexes.size() == 0)
00582     {
00583       msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
00584         Specs::specs["max_range"].default_value.c_str());
00585     }
00586     else
00587     {
00588       msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
00589         specs->elements[indexes[0]]->elements[0]->value.c_str());
00590     }
00591     
00593     indexes = specs->getTag("frequency");
00594     if(indexes.size() == 0)
00595     {
00596       msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
00597         Specs::specs["frequency"].default_value.c_str());
00598     }
00599     else
00600     {
00601       msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
00602         specs->elements[indexes[0]]->elements[0]->value.c_str());
00603     }
00604 
00606     indexes = specs->getTag("frame_id");
00607     if(indexes.size() == 0)
00608     {
00609       msg.frame_id = std::string("co2_sensor_") + SSTR(id);
00610     }
00611     else
00612     {
00613       msg.frame_id = specs->elements[indexes[0]]->elements[0]->value;
00614     }
00615     
00617     indexes = specs->getTag("pose");
00618     if(indexes.size() != 0)
00619     {
00620       msg.pose = 
00621         createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
00622     }
00623     else
00624     {
00625       msg.pose.x = stdr_parser::MessageCreator::stringToType<float>(
00626         Specs::specs["x"].default_value.c_str());
00627       msg.pose.y = stdr_parser::MessageCreator::stringToType<float>(
00628         Specs::specs["y"].default_value.c_str());
00629       msg.pose.theta = stdr_parser::MessageCreator::stringToType<float>(
00630         Specs::specs["theta"].default_value.c_str());
00631     }
00632     return msg;
00633   }
00634   
00641   template <> stdr_msgs::ThermalSensorMsg MessageCreator::createMessage(
00642     Node *n,unsigned int id)
00643   {
00644     stdr_msgs::ThermalSensorMsg msg;
00645     Node* specs = n->elements[0];
00646     if(specs->tag == "thermal_sensor")
00647     {
00648       specs = specs->elements[0];
00649     }
00650     std::vector<int> indexes;
00651     
00653     indexes = specs->getTag("max_range");
00654     if(indexes.size() == 0)
00655     {
00656       msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
00657         Specs::specs["max_range"].default_value.c_str());
00658     }
00659     else
00660     {
00661       msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
00662         specs->elements[indexes[0]]->elements[0]->value.c_str());
00663     }
00664     
00666     indexes = specs->getTag("angle_span");
00667     if(indexes.size() == 0)
00668     {
00669       msg.angleSpan = stdr_parser::MessageCreator::stringToType<float>(
00670         Specs::specs["angle_span"].default_value.c_str());
00671     }
00672     else
00673     {
00674       msg.angleSpan = stdr_parser::MessageCreator::stringToType<float>(
00675         specs->elements[indexes[0]]->elements[0]->value.c_str());
00676     }
00677     
00679     indexes = specs->getTag("frequency");
00680     if(indexes.size() == 0)
00681     {
00682       msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
00683         Specs::specs["frequency"].default_value.c_str());
00684     }
00685     else
00686     {
00687       msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
00688         specs->elements[indexes[0]]->elements[0]->value.c_str());
00689     }
00690 
00692     indexes = specs->getTag("frame_id");
00693     if(indexes.size() == 0)
00694     {
00695       msg.frame_id = std::string("thermal_sensor_") + SSTR(id);
00696     }
00697     else
00698     {
00699       msg.frame_id = specs->elements[indexes[0]]->elements[0]->value;
00700     }
00701     
00703     indexes = specs->getTag("pose");
00704     if(indexes.size() != 0)
00705     {
00706       msg.pose = 
00707         createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
00708     }
00709     else
00710     {
00711       msg.pose.x = stdr_parser::MessageCreator::stringToType<float>(
00712         Specs::specs["x"].default_value.c_str());
00713       msg.pose.y = stdr_parser::MessageCreator::stringToType<float>(
00714         Specs::specs["y"].default_value.c_str());
00715       msg.pose.theta = stdr_parser::MessageCreator::stringToType<float>(
00716         Specs::specs["theta"].default_value.c_str());
00717     }
00718     return msg;
00719   }
00720   
00727   template <> stdr_msgs::SoundSensorMsg MessageCreator::createMessage(
00728     Node *n,unsigned int id)
00729   {
00730     stdr_msgs::SoundSensorMsg msg;
00731     Node* specs = n->elements[0];
00732     if(specs->tag == "sound_sensor")
00733     {
00734       specs = specs->elements[0];
00735     }
00736     std::vector<int> indexes;
00737     
00739     indexes = specs->getTag("max_range");
00740     if(indexes.size() == 0)
00741     {
00742       msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
00743         Specs::specs["max_range"].default_value.c_str());
00744     }
00745     else
00746     {
00747       msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
00748         specs->elements[indexes[0]]->elements[0]->value.c_str());
00749     }
00750     
00752     indexes = specs->getTag("angle_span");
00753     if(indexes.size() == 0)
00754     {
00755       msg.angleSpan = stdr_parser::MessageCreator::stringToType<float>(
00756         Specs::specs["angle_span"].default_value.c_str());
00757     }
00758     else
00759     {
00760       msg.angleSpan = stdr_parser::MessageCreator::stringToType<float>(
00761         specs->elements[indexes[0]]->elements[0]->value.c_str());
00762     }
00763     
00765     indexes = specs->getTag("frequency");
00766     if(indexes.size() == 0)
00767     {
00768       msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
00769         Specs::specs["frequency"].default_value.c_str());
00770     }
00771     else
00772     {
00773       msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
00774         specs->elements[indexes[0]]->elements[0]->value.c_str());
00775     }
00776 
00778     indexes = specs->getTag("frame_id");
00779     if(indexes.size() == 0)
00780     {
00781       msg.frame_id = std::string("sound_sensor_") + SSTR(id);
00782     }
00783     else
00784     {
00785       msg.frame_id = specs->elements[indexes[0]]->elements[0]->value;
00786     }
00787     
00789     indexes = specs->getTag("pose");
00790     if(indexes.size() != 0)
00791     {
00792       msg.pose = 
00793         createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
00794     }
00795     else
00796     {
00797       msg.pose.x = stdr_parser::MessageCreator::stringToType<float>(
00798         Specs::specs["x"].default_value.c_str());
00799       msg.pose.y = stdr_parser::MessageCreator::stringToType<float>(
00800         Specs::specs["y"].default_value.c_str());
00801       msg.pose.theta = stdr_parser::MessageCreator::stringToType<float>(
00802         Specs::specs["theta"].default_value.c_str());
00803     }
00804     return msg;
00805   }
00806 
00813   template <> 
00814   stdr_msgs::KinematicMsg MessageCreator::createMessage(
00815     Node *n,unsigned int id)
00816   {
00817     stdr_msgs::KinematicMsg msg;
00818     Node* specs = n->elements[0];
00819     if(specs->tag == "kinematic")
00820     {
00821       specs = specs->elements[0];
00822     }
00823     std::vector<int> indexes;
00824     
00826     indexes = specs->getTag("kinematic_model");
00827     if(indexes.size() == 0)
00828     {
00829       msg.type = Specs::specs["kinematic_model"].default_value.c_str();
00830     }
00831     else
00832     {
00833       msg.type = specs->elements[indexes[0]]->elements[0]->
00834         value.c_str();
00835     }
00836 
00838     indexes = specs->getTag("kinematic_parameters");
00839     if(indexes.size() == 0)
00840     {
00841       msg.a_ux_ux = stdr_parser::MessageCreator::stringToType<float>(
00842         Specs::specs["a_ux_ux"].default_value.c_str());
00843       msg.a_ux_uy = stdr_parser::MessageCreator::stringToType<float>(
00844         Specs::specs["a_ux_uy"].default_value.c_str());
00845       msg.a_ux_w = stdr_parser::MessageCreator::stringToType<float>(
00846         Specs::specs["a_ux_w"].default_value.c_str());
00847 
00848       msg.a_uy_ux = stdr_parser::MessageCreator::stringToType<float>(
00849         Specs::specs["a_uy_ux"].default_value.c_str());
00850       msg.a_uy_uy = stdr_parser::MessageCreator::stringToType<float>(
00851         Specs::specs["a_uy_uy"].default_value.c_str());
00852       msg.a_uy_w = stdr_parser::MessageCreator::stringToType<float>(
00853         Specs::specs["a_uy_w"].default_value.c_str());
00854 
00855       msg.a_w_ux = stdr_parser::MessageCreator::stringToType<float>(
00856         Specs::specs["a_w_ux"].default_value.c_str());
00857       msg.a_w_uy = stdr_parser::MessageCreator::stringToType<float>(
00858         Specs::specs["a_w_uy"].default_value.c_str());
00859       msg.a_w_w = stdr_parser::MessageCreator::stringToType<float>(
00860         Specs::specs["a_w_w"].default_value.c_str());
00861 
00862       msg.a_g_ux = stdr_parser::MessageCreator::stringToType<float>(
00863         Specs::specs["a_g_ux"].default_value.c_str());
00864       msg.a_g_uy = stdr_parser::MessageCreator::stringToType<float>(
00865         Specs::specs["a_g_uy"].default_value.c_str());
00866       msg.a_g_w = stdr_parser::MessageCreator::stringToType<float>(
00867         Specs::specs["a_g_w"].default_value.c_str());
00868     }
00869     else
00870     {
00871       specs = specs->elements[indexes[0]];
00872 
00873       indexes = specs->getTag("a_ux_ux");
00874       msg.a_ux_ux = stdr_parser::MessageCreator::stringToType<float>(
00875         specs->elements[indexes[0]]->elements[0]->value.c_str());
00876       indexes = specs->getTag("a_ux_uy");
00877       msg.a_ux_uy = stdr_parser::MessageCreator::stringToType<float>(
00878           specs->elements[indexes[0]]->elements[0]->value.c_str());
00879       indexes = specs->getTag("a_ux_w");
00880       msg.a_ux_w = stdr_parser::MessageCreator::stringToType<float>(
00881         specs->elements[indexes[0]]->elements[0]->value.c_str());
00882 
00883       indexes = specs->getTag("a_uy_ux");
00884       msg.a_uy_ux = stdr_parser::MessageCreator::stringToType<float>(
00885         specs->elements[indexes[0]]->elements[0]->value.c_str());
00886       indexes = specs->getTag("a_uy_uy");
00887       msg.a_uy_uy = stdr_parser::MessageCreator::stringToType<float>(
00888         specs->elements[indexes[0]]->elements[0]->value.c_str());
00889       indexes = specs->getTag("a_uy_w");
00890       msg.a_uy_w = stdr_parser::MessageCreator::stringToType<float>(
00891         specs->elements[indexes[0]]->elements[0]->value.c_str());
00892 
00893       indexes = specs->getTag("a_w_ux");
00894       msg.a_w_ux = stdr_parser::MessageCreator::stringToType<float>(
00895         specs->elements[indexes[0]]->elements[0]->value.c_str());
00896       indexes = specs->getTag("a_w_uy");
00897       msg.a_w_uy = stdr_parser::MessageCreator::stringToType<float>(
00898         specs->elements[indexes[0]]->elements[0]->value.c_str());
00899       indexes = specs->getTag("a_w_w");
00900       msg.a_w_w =  stdr_parser::MessageCreator::stringToType<float>(
00901         specs->elements[indexes[0]]->elements[0]->value.c_str());
00902 
00903       indexes = specs->getTag("a_g_ux");
00904       msg.a_g_ux = stdr_parser::MessageCreator::stringToType<float>(
00905         specs->elements[indexes[0]]->elements[0]->value.c_str());
00906       indexes = specs->getTag("a_g_uy");
00907       msg.a_g_uy = stdr_parser::MessageCreator::stringToType<float>(
00908         specs->elements[indexes[0]]->elements[0]->value.c_str());
00909       indexes = specs->getTag("a_g_w");
00910       msg.a_g_w = stdr_parser::MessageCreator::stringToType<float>(
00911         specs->elements[indexes[0]]->elements[0]->value.c_str());
00912     }
00913     return msg;
00914   }
00915 
00916   
00922   template <> 
00923   stdr_msgs::RobotMsg MessageCreator::createMessage(Node *n,unsigned int id)
00924   {
00925     stdr_msgs::RobotMsg msg;
00926     Node* specs = n->elements[0];
00927     if(specs->tag == "robot")
00928     {
00929       specs = specs->elements[0];
00930     }
00931     std::vector<int> indexes;
00932     
00934     indexes = specs->getTag("initial_pose");
00935     if(indexes.size() != 0)
00936     {
00937       msg.initialPose = 
00938         createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
00939     }
00940     else
00941     {
00942       msg.initialPose.x = stdr_parser::MessageCreator::stringToType<float>(
00943         Specs::specs["x"].default_value.c_str());
00944       msg.initialPose.y = stdr_parser::MessageCreator::stringToType<float>(
00945         Specs::specs["y"].default_value.c_str());
00946       msg.initialPose.theta = stdr_parser::MessageCreator::stringToType<float>(
00947         Specs::specs["theta"].default_value.c_str());
00948     }
00949     
00951     indexes = specs->getTag("footprint");
00952     if(indexes.size() != 0)
00953     {
00954       msg.footprint = 
00955         createMessage<stdr_msgs::FootprintMsg>(specs->elements[indexes[0]],0);
00956     }
00957     else
00958     {
00959       msg.footprint.radius = stdr_parser::MessageCreator::stringToType<float>(
00960         Specs::specs["radius"].default_value.c_str());
00961     }
00962     
00964     indexes = specs->getTag("laser");
00965     if(indexes.size() != 0)
00966     {
00967       for(unsigned int i = 0 ; i < indexes.size() ; i++)
00968       {
00969         msg.laserSensors.push_back(
00970           createMessage<stdr_msgs::LaserSensorMsg>(
00971             specs->elements[indexes[i]] , i));
00972       }
00973     }
00974     
00976     indexes = specs->getTag("sonar");
00977     if(indexes.size() != 0)
00978     {
00979       for(unsigned int i = 0 ; i < indexes.size() ; i++)
00980       {
00981         msg.sonarSensors.push_back(
00982           createMessage<stdr_msgs::SonarSensorMsg>(
00983             specs->elements[indexes[i]] , i));
00984       }
00985     }
00986     
00988     indexes = specs->getTag("rfid_reader");
00989     if(indexes.size() != 0)
00990     {
00991       for(unsigned int i = 0 ; i < indexes.size() ; i++)
00992       {
00993         msg.rfidSensors.push_back(
00994           createMessage<stdr_msgs::RfidSensorMsg>(
00995             specs->elements[indexes[i]] , i));
00996       }
00997     }
00998     
01000     indexes = specs->getTag("co2_sensor");
01001     if(indexes.size() != 0)
01002     {
01003       for(unsigned int i = 0 ; i < indexes.size() ; i++)
01004       {
01005         msg.co2Sensors.push_back(
01006           createMessage<stdr_msgs::CO2SensorMsg>(
01007             specs->elements[indexes[i]] , i));
01008       }
01009     }
01010     
01012     indexes = specs->getTag("thermal_sensor");
01013     if(indexes.size() != 0)
01014     {
01015       for(unsigned int i = 0 ; i < indexes.size() ; i++)
01016       {
01017         msg.thermalSensors.push_back(
01018           createMessage<stdr_msgs::ThermalSensorMsg>(
01019             specs->elements[indexes[i]] , i));
01020       }
01021     }
01022     
01024     indexes = specs->getTag("sound_sensor");
01025     if(indexes.size() != 0)
01026     {
01027       for(unsigned int i = 0 ; i < indexes.size() ; i++)
01028       {
01029         msg.soundSensors.push_back(
01030           createMessage<stdr_msgs::SoundSensorMsg>(
01031             specs->elements[indexes[i]] , i));
01032       }
01033     }
01034 
01036     indexes = specs->getTag("kinematic");
01037     if(indexes.size() != 0)
01038     {
01039       msg.kinematicModel = createMessage<stdr_msgs::KinematicMsg>(
01040         specs->elements[indexes[0]], 0);
01041     }
01042     
01043     return msg;
01044   }
01045   
01046  
01047 
01048 }
01049