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       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     // x
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     // y
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     // z
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     // search for points
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