stdr_parser_msg_creator.cpp
Go to the documentation of this file.
1 /******************************************************************************
2  STDR Simulator - Simple Two DImensional Robot Simulator
3  Copyright (C) 2013 STDR Simulator
4  This program is free software; you can redistribute it and/or modify
5  it under the terms of the GNU General Public License as published by
6  the Free Software Foundation; either version 3 of the License, or
7  (at your option) any later version.
8  This program is distributed in the hope that it will be useful,
9  but WITHOUT ANY WARRANTY; without even the implied warranty of
10  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  GNU General Public License for more details.
12  You should have received a copy of the GNU General Public License
13  along with this program; if not, write to the Free Software Foundation,
14  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
15 
16  Authors :
17  * Manos Tsardoulias, etsardou@gmail.com
18  * Aris Thallas, aris.thallas@gmail.com
19  * Chris Zalidis, zalidis@gmail.com
20 ******************************************************************************/
21 
22 #include "stdr_parser/stdr_parser_msg_creator.h"
23 
24 namespace stdr_parser
25 {
30  MessageCreator::MessageCreator(void)
31  {
32 
33  }
34 
40  template <typename T>
41  T MessageCreator::createMessage(Node *n,unsigned int id)
42  {
43  }
44 
50  template <> geometry_msgs::Pose2D MessageCreator::createMessage(
51  Node *n,unsigned int id)
52  {
53  geometry_msgs::Pose2D msg;
54  std::vector<int> indexes;
56  indexes = n->getTag("x");
57  if(indexes.size() == 0)
58  {
59 
60  msg.x = stdr_parser::MessageCreator::stringToType<float>(
61  Specs::specs["x"].default_value.c_str());
62  }
63  else
64  {
65 
66  msg.x = stdr_parser::MessageCreator::stringToType<float>(
67  n->elements[indexes[0]]->elements[0]->value.c_str());
68  }
70  indexes = n->getTag("y");
71  if(indexes.size() == 0)
72  {
73  msg.y = stdr_parser::MessageCreator::stringToType<float>(
74  Specs::specs["y"].default_value.c_str());
75  }
76  else
77  {
78  msg.y = stdr_parser::MessageCreator::stringToType<float>(
79  n->elements[indexes[0]]->elements[0]->value.c_str());
80  }
82  indexes = n->getTag("theta");
83  if(indexes.size() == 0)
84  {
85  msg.theta = stdr_parser::MessageCreator::stringToType<float>(
86  Specs::specs["theta"].default_value.c_str());
87  }
88  else
89  {
90  msg.theta = stdr_parser::MessageCreator::stringToType<float>(
91  n->elements[indexes[0]]->elements[0]->value.c_str());
92  }
93  return msg;
94  }
95 
101  template <>
102  geometry_msgs::Point MessageCreator::createMessage(Node *n, unsigned int id)
103  {
104  geometry_msgs::Point msg;
105  std::vector<int> indexes;
106  // x
107  indexes = n->getTag("x");
108  if( indexes.size() == 0) {
109  msg.x = stdr_parser::MessageCreator::stringToType<float>(
110  Specs::specs["x"].default_value.c_str());
111  } else {
112  msg.x = stdr_parser::MessageCreator::stringToType<float>(
113  n->elements[indexes[0]]->elements[0]->value.c_str());
114  }
115  // y
116  indexes = n->getTag("y");
117  if( indexes.size() == 0) {
118  msg.y = stdr_parser::MessageCreator::stringToType<float>(
119  Specs::specs["y"].default_value.c_str());
120  } else {
121  msg.y = stdr_parser::MessageCreator::stringToType<float>(
122  n->elements[indexes[0]]->elements[0]->value.c_str());
123  }
124  // z
125  indexes = n->getTag("z");
126  if( indexes.size() == 0) {
127  msg.z = stdr_parser::MessageCreator::stringToType<float>(
128  Specs::specs["z"].default_value.c_str());
129  } else {
130  msg.z = stdr_parser::MessageCreator::stringToType<float>(
131  n->elements[indexes[0]]->elements[0]->value.c_str());
132  }
133  return msg;
134  }
135 
141  template <> stdr_msgs::Noise MessageCreator::createMessage(
142  Node *n,unsigned int id)
143  {
144  stdr_msgs::Noise msg;
145  Node* specs = n->elements[0];
146  if(specs->tag == "noise")
147  {
148  specs = specs->elements[0];
149  }
150  std::vector<int> indexes;
152  indexes = specs->getTag("noise_mean");
153  if(indexes.size() == 0)
154  {
155  msg.noiseMean = stdr_parser::MessageCreator::stringToType<float>(
156  Specs::specs["noise_mean"].default_value.c_str());
157  }
158  else
159  {
160  msg.noiseMean = stdr_parser::MessageCreator::stringToType<float>(
161  specs->elements[indexes[0]]->elements[0]->value.c_str());
162  }
164  indexes = specs->getTag("noise_std");
165  if(indexes.size() == 0)
166  {
167  msg.noiseStd = stdr_parser::MessageCreator::stringToType<float>(
168  Specs::specs["noise_std"].default_value.c_str());
169  }
170  else
171  {
172  msg.noiseStd = stdr_parser::MessageCreator::stringToType<float>(
173  specs->elements[indexes[0]]->elements[0]->value.c_str());
174  }
175  return msg;
176  }
177 
183  template <>
184  stdr_msgs::FootprintMsg MessageCreator::createMessage(
185  Node *n,unsigned int id)
186  {
187  stdr_msgs::FootprintMsg msg;
188  Node* specs = n->elements[0];
189  if(specs->tag == "footprint")
190  {
191  specs = specs->elements[0];
192  }
193  else if(specs->tag == "footprint_specifications")
194  {
195  // specs must remain specs
196  }
197 
198  std::vector<int> indexes;
200  indexes = specs->getTag("radius");
201  if(indexes.size() == 0)
202  {
203  msg.radius = stdr_parser::MessageCreator::stringToType<float>(
204  Specs::specs["radius"].default_value.c_str());
205  }
206  else
207  {
208  msg.radius = stdr_parser::MessageCreator::stringToType<float>(
209  specs->elements[indexes[0]]->elements[0]->value);
210  }
211  // search for points
212  indexes = specs->getTag("points");
213  if( indexes.size() != 0 ) {
214  specs = specs->elements[indexes[0]];
215  std::vector<int> points = specs->getTag("point");
216  for( unsigned int i = 0; i < points.size(); i++ ) {
217  msg.points.push_back(createMessage<geometry_msgs::Point>(
218  specs->elements[points[i]], i));
219  }
220  }
221  return msg;
222  }
223 
229  template <>
230  stdr_msgs::LaserSensorMsg MessageCreator::createMessage(
231  Node *n,unsigned int id)
232  {
233  stdr_msgs::LaserSensorMsg msg;
234  Node* specs = n->elements[0];
235  if(specs->tag == "laser")
236  {
237  specs = specs->elements[0];
238  }
239  std::vector<int> indexes;
240 
242  indexes = specs->getTag("max_angle");
243  if(indexes.size() == 0)
244  {
245  msg.maxAngle = stdr_parser::MessageCreator::stringToType<float>(
246  Specs::specs["max_angle"].default_value.c_str());
247  }
248  else
249  {
250  msg.maxAngle = stdr_parser::MessageCreator::stringToType<float>(
251  specs->elements[indexes[0]]->elements[0]->value.c_str());
252  }
253 
255  indexes = specs->getTag("min_angle");
256  if(indexes.size() == 0)
257  {
258  msg.minAngle = stdr_parser::MessageCreator::stringToType<float>(
259  Specs::specs["min_angle"].default_value.c_str());
260  }
261  else
262  {
263  msg.minAngle = stdr_parser::MessageCreator::stringToType<float>(
264  specs->elements[indexes[0]]->elements[0]->value.c_str());
265  }
266 
268  indexes = specs->getTag("max_range");
269  if(indexes.size() == 0)
270  {
271  msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
272  Specs::specs["max_range"].default_value.c_str());
273  }
274  else
275  {
276  msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
277  specs->elements[indexes[0]]->elements[0]->value.c_str());
278  }
279 
281  indexes = specs->getTag("min_range");
282  if(indexes.size() == 0)
283  {
284  msg.minRange = stdr_parser::MessageCreator::stringToType<float>(
285  Specs::specs["min_range"].default_value.c_str());
286  }
287  else
288  {
289  msg.minRange = stdr_parser::MessageCreator::stringToType<float>(
290  specs->elements[indexes[0]]->elements[0]->value.c_str());
291  }
292 
294  indexes = specs->getTag("num_rays");
295  if(indexes.size() == 0)
296  {
297  msg.numRays = stdr_parser::MessageCreator::stringToType<int>(
298  Specs::specs["num_rays"].default_value.c_str());
299  }
300  else
301  {
302  msg.numRays = stdr_parser::MessageCreator::stringToType<int>(
303  specs->elements[indexes[0]]->elements[0]->value.c_str());
304  }
305 
307  indexes = specs->getTag("noise");
308  if(indexes.size() != 0)
309  {
310  msg.noise =
311  createMessage<stdr_msgs::Noise>(specs->elements[indexes[0]],0);
312  }
313 
315  indexes = specs->getTag("frequency");
316  if(indexes.size() == 0)
317  {
318  msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
319  Specs::specs["frequency"].default_value.c_str());
320  }
321  else
322  {
323  msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
324  specs->elements[indexes[0]]->elements[0]->value.c_str());
325  }
326 
328  indexes = specs->getTag("frame_id");
329  if(indexes.size() == 0)
330  {
331  msg.frame_id = std::string("laser_") + SSTR(id);
332  }
333  else
334  {
335  msg.frame_id = specs->elements[indexes[0]]->elements[0]->value;
336  }
337 
339  indexes = specs->getTag("pose");
340  if(indexes.size() != 0)
341  {
342  msg.pose =
343  createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
344  }
345  else
346  {
347  msg.pose.x = stdr_parser::MessageCreator::stringToType<float>(
348  Specs::specs["x"].default_value.c_str());
349  msg.pose.y = stdr_parser::MessageCreator::stringToType<float>(
350  Specs::specs["y"].default_value.c_str());
351  msg.pose.theta = stdr_parser::MessageCreator::stringToType<float>(
352  Specs::specs["theta"].default_value.c_str());
353  }
354  return msg;
355  }
356 
362  template <> stdr_msgs::SonarSensorMsg MessageCreator::createMessage(
363  Node *n,unsigned int id)
364  {
365  stdr_msgs::SonarSensorMsg msg;
366  Node* specs = n->elements[0];
367  if(specs->tag == "sonar")
368  {
369  specs = specs->elements[0];
370  }
371  std::vector<int> indexes;
372 
374  indexes = specs->getTag("max_range");
375  if(indexes.size() == 0)
376  {
377  msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
378  Specs::specs["max_range"].default_value.c_str());
379  }
380  else
381  {
382  msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
383  specs->elements[indexes[0]]->elements[0]->value.c_str());
384  }
385 
387  indexes = specs->getTag("min_range");
388  if(indexes.size() == 0)
389  {
390  msg.minRange = stdr_parser::MessageCreator::stringToType<float>(
391  Specs::specs["min_range"].default_value.c_str());
392  }
393  else
394  {
395  msg.minRange = stdr_parser::MessageCreator::stringToType<float>(
396  specs->elements[indexes[0]]->elements[0]->value.c_str());
397  }
398 
400  indexes = specs->getTag("cone_angle");
401  if(indexes.size() == 0)
402  {
403  msg.coneAngle = stdr_parser::MessageCreator::stringToType<float>(
404  Specs::specs["cone_angle"].default_value.c_str());
405  }
406  else
407  {
408  msg.coneAngle = stdr_parser::MessageCreator::stringToType<float>(
409  specs->elements[indexes[0]]->elements[0]->value.c_str());
410  }
411 
413  indexes = specs->getTag("noise");
414  if(indexes.size() != 0)
415  {
416  msg.noise =
417  createMessage<stdr_msgs::Noise>(specs->elements[indexes[0]],0);
418  }
419 
421  indexes = specs->getTag("frequency");
422  if(indexes.size() == 0)
423  {
424  msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
425  Specs::specs["frequency"].default_value.c_str());
426  }
427  else
428  {
429  msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
430  specs->elements[indexes[0]]->elements[0]->value.c_str());
431  }
432 
434  indexes = specs->getTag("frame_id");
435  if(indexes.size() == 0)
436  {
437  msg.frame_id = std::string("sonar_") + SSTR(id);
438  }
439  else
440  {
441  msg.frame_id = specs->elements[indexes[0]]->elements[0]->value;
442  }
443 
445  indexes = specs->getTag("pose");
446  if(indexes.size() != 0)
447  {
448  msg.pose =
449  createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
450  }
451  else
452  {
453  msg.pose.x = stdr_parser::MessageCreator::stringToType<float>(
454  Specs::specs["x"].default_value.c_str());
455  msg.pose.y = stdr_parser::MessageCreator::stringToType<float>(
456  Specs::specs["y"].default_value.c_str());
457  msg.pose.theta = stdr_parser::MessageCreator::stringToType<float>(
458  Specs::specs["theta"].default_value.c_str());
459  }
460  return msg;
461  }
462 
469  template <> stdr_msgs::RfidSensorMsg MessageCreator::createMessage(
470  Node *n,unsigned int id)
471  {
472  stdr_msgs::RfidSensorMsg msg;
473  Node* specs = n->elements[0];
474  if(specs->tag == "rfid_reader")
475  {
476  specs = specs->elements[0];
477  }
478  std::vector<int> indexes;
479 
481  indexes = specs->getTag("angle_span");
482  if(indexes.size() == 0)
483  {
484  msg.angleSpan = stdr_parser::MessageCreator::stringToType<float>(
485  Specs::specs["angle_span"].default_value.c_str());
486  }
487  else
488  {
489  msg.angleSpan = stdr_parser::MessageCreator::stringToType<float>(
490  specs->elements[indexes[0]]->elements[0]->value.c_str());
491  }
492 
494  indexes = specs->getTag("max_range");
495  if(indexes.size() == 0)
496  {
497  msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
498  Specs::specs["max_range"].default_value.c_str());
499  }
500  else
501  {
502  msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
503  specs->elements[indexes[0]]->elements[0]->value.c_str());
504  }
505 
507  indexes = specs->getTag("signal_cutoff");
508  if(indexes.size() == 0)
509  {
510  msg.signalCutoff = stdr_parser::MessageCreator::stringToType<float>(
511  Specs::specs["signal_cutoff"].default_value.c_str());
512  }
513  else
514  {
515  msg.signalCutoff = stdr_parser::MessageCreator::stringToType<float>(
516  specs->elements[indexes[0]]->elements[0]->value.c_str());
517  }
518 
520  indexes = specs->getTag("frequency");
521  if(indexes.size() == 0)
522  {
523  msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
524  Specs::specs["frequency"].default_value.c_str());
525  }
526  else
527  {
528  msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
529  specs->elements[indexes[0]]->elements[0]->value.c_str());
530  }
531 
533  indexes = specs->getTag("frame_id");
534  if(indexes.size() == 0)
535  {
536  msg.frame_id = std::string("rfid_reader_") + SSTR(id);
537  }
538  else
539  {
540  msg.frame_id = specs->elements[indexes[0]]->elements[0]->value;
541  }
542 
544  indexes = specs->getTag("pose");
545  if(indexes.size() != 0)
546  {
547  msg.pose =
548  createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
549  }
550  else
551  {
552  msg.pose.x = stdr_parser::MessageCreator::stringToType<float>(
553  Specs::specs["x"].default_value.c_str());
554  msg.pose.y = stdr_parser::MessageCreator::stringToType<float>(
555  Specs::specs["y"].default_value.c_str());
556  msg.pose.theta = stdr_parser::MessageCreator::stringToType<float>(
557  Specs::specs["theta"].default_value.c_str());
558  }
559  return msg;
560  }
561 
568  template <> stdr_msgs::CO2SensorMsg MessageCreator::createMessage(
569  Node *n,unsigned int id)
570  {
571  stdr_msgs::CO2SensorMsg msg;
572  Node* specs = n->elements[0];
573  if(specs->tag == "co2_sensor")
574  {
575  specs = specs->elements[0];
576  }
577  std::vector<int> indexes;
578 
580  indexes = specs->getTag("max_range");
581  if(indexes.size() == 0)
582  {
583  msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
584  Specs::specs["max_range"].default_value.c_str());
585  }
586  else
587  {
588  msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
589  specs->elements[indexes[0]]->elements[0]->value.c_str());
590  }
591 
593  indexes = specs->getTag("frequency");
594  if(indexes.size() == 0)
595  {
596  msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
597  Specs::specs["frequency"].default_value.c_str());
598  }
599  else
600  {
601  msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
602  specs->elements[indexes[0]]->elements[0]->value.c_str());
603  }
604 
606  indexes = specs->getTag("frame_id");
607  if(indexes.size() == 0)
608  {
609  msg.frame_id = std::string("co2_sensor_") + SSTR(id);
610  }
611  else
612  {
613  msg.frame_id = specs->elements[indexes[0]]->elements[0]->value;
614  }
615 
617  indexes = specs->getTag("pose");
618  if(indexes.size() != 0)
619  {
620  msg.pose =
621  createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
622  }
623  else
624  {
625  msg.pose.x = stdr_parser::MessageCreator::stringToType<float>(
626  Specs::specs["x"].default_value.c_str());
627  msg.pose.y = stdr_parser::MessageCreator::stringToType<float>(
628  Specs::specs["y"].default_value.c_str());
629  msg.pose.theta = stdr_parser::MessageCreator::stringToType<float>(
630  Specs::specs["theta"].default_value.c_str());
631  }
632  return msg;
633  }
634 
641  template <> stdr_msgs::ThermalSensorMsg MessageCreator::createMessage(
642  Node *n,unsigned int id)
643  {
644  stdr_msgs::ThermalSensorMsg msg;
645  Node* specs = n->elements[0];
646  if(specs->tag == "thermal_sensor")
647  {
648  specs = specs->elements[0];
649  }
650  std::vector<int> indexes;
651 
653  indexes = specs->getTag("max_range");
654  if(indexes.size() == 0)
655  {
656  msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
657  Specs::specs["max_range"].default_value.c_str());
658  }
659  else
660  {
661  msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
662  specs->elements[indexes[0]]->elements[0]->value.c_str());
663  }
664 
666  indexes = specs->getTag("angle_span");
667  if(indexes.size() == 0)
668  {
669  msg.angleSpan = stdr_parser::MessageCreator::stringToType<float>(
670  Specs::specs["angle_span"].default_value.c_str());
671  }
672  else
673  {
674  msg.angleSpan = stdr_parser::MessageCreator::stringToType<float>(
675  specs->elements[indexes[0]]->elements[0]->value.c_str());
676  }
677 
679  indexes = specs->getTag("frequency");
680  if(indexes.size() == 0)
681  {
682  msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
683  Specs::specs["frequency"].default_value.c_str());
684  }
685  else
686  {
687  msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
688  specs->elements[indexes[0]]->elements[0]->value.c_str());
689  }
690 
692  indexes = specs->getTag("frame_id");
693  if(indexes.size() == 0)
694  {
695  msg.frame_id = std::string("thermal_sensor_") + SSTR(id);
696  }
697  else
698  {
699  msg.frame_id = specs->elements[indexes[0]]->elements[0]->value;
700  }
701 
703  indexes = specs->getTag("pose");
704  if(indexes.size() != 0)
705  {
706  msg.pose =
707  createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
708  }
709  else
710  {
711  msg.pose.x = stdr_parser::MessageCreator::stringToType<float>(
712  Specs::specs["x"].default_value.c_str());
713  msg.pose.y = stdr_parser::MessageCreator::stringToType<float>(
714  Specs::specs["y"].default_value.c_str());
715  msg.pose.theta = stdr_parser::MessageCreator::stringToType<float>(
716  Specs::specs["theta"].default_value.c_str());
717  }
718  return msg;
719  }
720 
727  template <> stdr_msgs::SoundSensorMsg MessageCreator::createMessage(
728  Node *n,unsigned int id)
729  {
730  stdr_msgs::SoundSensorMsg msg;
731  Node* specs = n->elements[0];
732  if(specs->tag == "sound_sensor")
733  {
734  specs = specs->elements[0];
735  }
736  std::vector<int> indexes;
737 
739  indexes = specs->getTag("max_range");
740  if(indexes.size() == 0)
741  {
742  msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
743  Specs::specs["max_range"].default_value.c_str());
744  }
745  else
746  {
747  msg.maxRange = stdr_parser::MessageCreator::stringToType<float>(
748  specs->elements[indexes[0]]->elements[0]->value.c_str());
749  }
750 
752  indexes = specs->getTag("angle_span");
753  if(indexes.size() == 0)
754  {
755  msg.angleSpan = stdr_parser::MessageCreator::stringToType<float>(
756  Specs::specs["angle_span"].default_value.c_str());
757  }
758  else
759  {
760  msg.angleSpan = stdr_parser::MessageCreator::stringToType<float>(
761  specs->elements[indexes[0]]->elements[0]->value.c_str());
762  }
763 
765  indexes = specs->getTag("frequency");
766  if(indexes.size() == 0)
767  {
768  msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
769  Specs::specs["frequency"].default_value.c_str());
770  }
771  else
772  {
773  msg.frequency = stdr_parser::MessageCreator::stringToType<float>(
774  specs->elements[indexes[0]]->elements[0]->value.c_str());
775  }
776 
778  indexes = specs->getTag("frame_id");
779  if(indexes.size() == 0)
780  {
781  msg.frame_id = std::string("sound_sensor_") + SSTR(id);
782  }
783  else
784  {
785  msg.frame_id = specs->elements[indexes[0]]->elements[0]->value;
786  }
787 
789  indexes = specs->getTag("pose");
790  if(indexes.size() != 0)
791  {
792  msg.pose =
793  createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
794  }
795  else
796  {
797  msg.pose.x = stdr_parser::MessageCreator::stringToType<float>(
798  Specs::specs["x"].default_value.c_str());
799  msg.pose.y = stdr_parser::MessageCreator::stringToType<float>(
800  Specs::specs["y"].default_value.c_str());
801  msg.pose.theta = stdr_parser::MessageCreator::stringToType<float>(
802  Specs::specs["theta"].default_value.c_str());
803  }
804  return msg;
805  }
806 
813  template <>
814  stdr_msgs::KinematicMsg MessageCreator::createMessage(
815  Node *n,unsigned int id)
816  {
817  stdr_msgs::KinematicMsg msg;
818  Node* specs = n->elements[0];
819  if(specs->tag == "kinematic")
820  {
821  specs = specs->elements[0];
822  }
823  std::vector<int> indexes;
824 
826  indexes = specs->getTag("kinematic_model");
827  if(indexes.size() == 0)
828  {
829  msg.type = Specs::specs["kinematic_model"].default_value.c_str();
830  }
831  else
832  {
833  msg.type = specs->elements[indexes[0]]->elements[0]->
834  value.c_str();
835  }
836 
838  indexes = specs->getTag("kinematic_parameters");
839  if(indexes.size() == 0)
840  {
841  msg.a_ux_ux = stdr_parser::MessageCreator::stringToType<float>(
842  Specs::specs["a_ux_ux"].default_value.c_str());
843  msg.a_ux_uy = stdr_parser::MessageCreator::stringToType<float>(
844  Specs::specs["a_ux_uy"].default_value.c_str());
845  msg.a_ux_w = stdr_parser::MessageCreator::stringToType<float>(
846  Specs::specs["a_ux_w"].default_value.c_str());
847 
848  msg.a_uy_ux = stdr_parser::MessageCreator::stringToType<float>(
849  Specs::specs["a_uy_ux"].default_value.c_str());
850  msg.a_uy_uy = stdr_parser::MessageCreator::stringToType<float>(
851  Specs::specs["a_uy_uy"].default_value.c_str());
852  msg.a_uy_w = stdr_parser::MessageCreator::stringToType<float>(
853  Specs::specs["a_uy_w"].default_value.c_str());
854 
855  msg.a_w_ux = stdr_parser::MessageCreator::stringToType<float>(
856  Specs::specs["a_w_ux"].default_value.c_str());
857  msg.a_w_uy = stdr_parser::MessageCreator::stringToType<float>(
858  Specs::specs["a_w_uy"].default_value.c_str());
859  msg.a_w_w = stdr_parser::MessageCreator::stringToType<float>(
860  Specs::specs["a_w_w"].default_value.c_str());
861 
862  msg.a_g_ux = stdr_parser::MessageCreator::stringToType<float>(
863  Specs::specs["a_g_ux"].default_value.c_str());
864  msg.a_g_uy = stdr_parser::MessageCreator::stringToType<float>(
865  Specs::specs["a_g_uy"].default_value.c_str());
866  msg.a_g_w = stdr_parser::MessageCreator::stringToType<float>(
867  Specs::specs["a_g_w"].default_value.c_str());
868  }
869  else
870  {
871  specs = specs->elements[indexes[0]];
872 
873  indexes = specs->getTag("a_ux_ux");
874  msg.a_ux_ux = stdr_parser::MessageCreator::stringToType<float>(
875  specs->elements[indexes[0]]->elements[0]->value.c_str());
876  indexes = specs->getTag("a_ux_uy");
877  msg.a_ux_uy = stdr_parser::MessageCreator::stringToType<float>(
878  specs->elements[indexes[0]]->elements[0]->value.c_str());
879  indexes = specs->getTag("a_ux_w");
880  msg.a_ux_w = stdr_parser::MessageCreator::stringToType<float>(
881  specs->elements[indexes[0]]->elements[0]->value.c_str());
882 
883  indexes = specs->getTag("a_uy_ux");
884  msg.a_uy_ux = stdr_parser::MessageCreator::stringToType<float>(
885  specs->elements[indexes[0]]->elements[0]->value.c_str());
886  indexes = specs->getTag("a_uy_uy");
887  msg.a_uy_uy = stdr_parser::MessageCreator::stringToType<float>(
888  specs->elements[indexes[0]]->elements[0]->value.c_str());
889  indexes = specs->getTag("a_uy_w");
890  msg.a_uy_w = stdr_parser::MessageCreator::stringToType<float>(
891  specs->elements[indexes[0]]->elements[0]->value.c_str());
892 
893  indexes = specs->getTag("a_w_ux");
894  msg.a_w_ux = stdr_parser::MessageCreator::stringToType<float>(
895  specs->elements[indexes[0]]->elements[0]->value.c_str());
896  indexes = specs->getTag("a_w_uy");
897  msg.a_w_uy = stdr_parser::MessageCreator::stringToType<float>(
898  specs->elements[indexes[0]]->elements[0]->value.c_str());
899  indexes = specs->getTag("a_w_w");
900  msg.a_w_w = stdr_parser::MessageCreator::stringToType<float>(
901  specs->elements[indexes[0]]->elements[0]->value.c_str());
902 
903  indexes = specs->getTag("a_g_ux");
904  msg.a_g_ux = stdr_parser::MessageCreator::stringToType<float>(
905  specs->elements[indexes[0]]->elements[0]->value.c_str());
906  indexes = specs->getTag("a_g_uy");
907  msg.a_g_uy = stdr_parser::MessageCreator::stringToType<float>(
908  specs->elements[indexes[0]]->elements[0]->value.c_str());
909  indexes = specs->getTag("a_g_w");
910  msg.a_g_w = stdr_parser::MessageCreator::stringToType<float>(
911  specs->elements[indexes[0]]->elements[0]->value.c_str());
912  }
913  return msg;
914  }
915 
916 
922  template <>
923  stdr_msgs::RobotMsg MessageCreator::createMessage(Node *n,unsigned int id)
924  {
925  stdr_msgs::RobotMsg msg;
926  Node* specs = n->elements[0];
927  if(specs->tag == "robot")
928  {
929  specs = specs->elements[0];
930  }
931  std::vector<int> indexes;
932 
934  indexes = specs->getTag("initial_pose");
935  if(indexes.size() != 0)
936  {
937  msg.initialPose =
938  createMessage<geometry_msgs::Pose2D>(specs->elements[indexes[0]],0);
939  }
940  else
941  {
942  msg.initialPose.x = stdr_parser::MessageCreator::stringToType<float>(
943  Specs::specs["x"].default_value.c_str());
944  msg.initialPose.y = stdr_parser::MessageCreator::stringToType<float>(
945  Specs::specs["y"].default_value.c_str());
946  msg.initialPose.theta = stdr_parser::MessageCreator::stringToType<float>(
947  Specs::specs["theta"].default_value.c_str());
948  }
949 
951  indexes = specs->getTag("footprint");
952  if(indexes.size() != 0)
953  {
954  msg.footprint =
955  createMessage<stdr_msgs::FootprintMsg>(specs->elements[indexes[0]],0);
956  }
957  else
958  {
959  msg.footprint.radius = stdr_parser::MessageCreator::stringToType<float>(
960  Specs::specs["radius"].default_value.c_str());
961  }
962 
964  indexes = specs->getTag("laser");
965  if(indexes.size() != 0)
966  {
967  for(unsigned int i = 0 ; i < indexes.size() ; i++)
968  {
969  msg.laserSensors.push_back(
970  createMessage<stdr_msgs::LaserSensorMsg>(
971  specs->elements[indexes[i]] , i));
972  }
973  }
974 
976  indexes = specs->getTag("sonar");
977  if(indexes.size() != 0)
978  {
979  for(unsigned int i = 0 ; i < indexes.size() ; i++)
980  {
981  msg.sonarSensors.push_back(
982  createMessage<stdr_msgs::SonarSensorMsg>(
983  specs->elements[indexes[i]] , i));
984  }
985  }
986 
988  indexes = specs->getTag("rfid_reader");
989  if(indexes.size() != 0)
990  {
991  for(unsigned int i = 0 ; i < indexes.size() ; i++)
992  {
993  msg.rfidSensors.push_back(
994  createMessage<stdr_msgs::RfidSensorMsg>(
995  specs->elements[indexes[i]] , i));
996  }
997  }
998 
1000  indexes = specs->getTag("co2_sensor");
1001  if(indexes.size() != 0)
1002  {
1003  for(unsigned int i = 0 ; i < indexes.size() ; i++)
1004  {
1005  msg.co2Sensors.push_back(
1006  createMessage<stdr_msgs::CO2SensorMsg>(
1007  specs->elements[indexes[i]] , i));
1008  }
1009  }
1010 
1012  indexes = specs->getTag("thermal_sensor");
1013  if(indexes.size() != 0)
1014  {
1015  for(unsigned int i = 0 ; i < indexes.size() ; i++)
1016  {
1017  msg.thermalSensors.push_back(
1018  createMessage<stdr_msgs::ThermalSensorMsg>(
1019  specs->elements[indexes[i]] , i));
1020  }
1021  }
1022 
1024  indexes = specs->getTag("sound_sensor");
1025  if(indexes.size() != 0)
1026  {
1027  for(unsigned int i = 0 ; i < indexes.size() ; i++)
1028  {
1029  msg.soundSensors.push_back(
1030  createMessage<stdr_msgs::SoundSensorMsg>(
1031  specs->elements[indexes[i]] , i));
1032  }
1033  }
1034 
1036  indexes = specs->getTag("kinematic");
1037  if(indexes.size() != 0)
1038  {
1039  msg.kinematicModel = createMessage<stdr_msgs::KinematicMsg>(
1040  specs->elements[indexes[0]], 0);
1041  }
1042 
1043  return msg;
1044  }
1045 
1046 
1047 
1048 }
1049 
stdr_parser::Node::tag
std::string tag
The node value (if it not a tag node)
Definition: stdr_parser_node.h:84
stdr_parser::MessageCreator::createMessage
static T createMessage(Node *n, unsigned int id)
Creates a pose message from a parsed file.
Definition: stdr_parser_msg_creator.cpp:41
stdr_parser::Node::getTag
std::vector< int > getTag(std::string tag)
Searches for a tag in the specific node.
Definition: stdr_parser_node.cpp:61
stdr_parser
The main namespace for STDR GUI XML parser.
stdr_parser_msg_creator.h
SSTR
#define SSTR(x)
< Transforms a float number to string
Definition: stdr_parser_tools.h:59
stdr_parser::Specs::specs
static std::map< std::string, ElSpecs > specs
List of non-mergable tags. Read from stdr_multiple_allowed.xml.
Definition: stdr_parser_specs.h:66
stdr_parser::MessageCreator::MessageCreator
MessageCreator(void)
Default constructor.
Definition: stdr_parser_msg_creator.cpp:30
stdr_parser::Node
Implements the main functionalities of the stdr parser tree.
Definition: stdr_parser_node.h:37
stdr_parser::Node::elements
std::vector< Node * > elements
File it was into.
Definition: stdr_parser_node.h:89

stdr_parser
Author(s): Manos Tsardoulias, Chris Zalidis, Aris Thallas
autogenerated on Mon Jun 10 2019 15:14:54