39 (stdr_msgs::Noise msg,std::string file_name);
43 YAML::Emitter& operator << <stdr_msgs::Noise>
44 (YAML::Emitter&
out,
const stdr_msgs::Noise& msg)
46 out << YAML::BeginMap;
47 out << YAML::Key <<
"noise";
49 out << YAML::BeginMap;
50 out << YAML::Key <<
"noise_specifications";
52 out << YAML::BeginMap;
53 out << YAML::Key <<
"noise_mean" << YAML::Value << msg.noiseMean;
54 out << YAML::Key <<
"noise_std" << YAML::Value << msg.noiseStd;
64 (stdr_msgs::FootprintMsg msg,std::string file_name);
68 YAML::Emitter& operator << <stdr_msgs::FootprintMsg>
69 (YAML::Emitter&
out,
const stdr_msgs::FootprintMsg& msg)
71 out << YAML::BeginMap;
72 out << YAML::Key <<
"footprint";
74 out << YAML::BeginMap;
75 out << YAML::Key <<
"footprint_specifications";
77 out << YAML::BeginMap;
78 out << YAML::Key <<
"radius" << YAML::Value << msg.radius;
79 out << YAML::Key <<
"points";
81 out << YAML::BeginSeq;
82 for(
unsigned int i = 0 ; i < msg.points.size() ; i++)
84 out << YAML::BeginMap;
85 out << YAML::Key <<
"point";
87 out << YAML::BeginMap;
88 out << YAML::Key <<
"x" << YAML::Value << msg.points[i].x;
89 out << YAML::Key <<
"y" << YAML::Value << msg.points[i].y;
103 (geometry_msgs::Pose2D msg,std::string file_name);
107 YAML::Emitter& operator << <geometry_msgs::Pose2D>
108 (YAML::Emitter&
out,
const geometry_msgs::Pose2D& msg)
110 out << YAML::BeginMap;
111 out << YAML::Key <<
"pose";
113 out << YAML::BeginMap;
114 out << YAML::Key <<
"x" << YAML::Value << msg.x;
115 out << YAML::Key <<
"y" << YAML::Value << msg.y;
116 out << YAML::Key <<
"theta" << YAML::Value << msg.theta;
125 (stdr_msgs::KinematicMsg msg,std::string file_name);
129 YAML::Emitter& operator << <stdr_msgs::KinematicMsg>
130 (YAML::Emitter&
out,
const stdr_msgs::KinematicMsg& msg)
132 out << YAML::BeginMap;
133 out << YAML::Key <<
"kinematic";
135 out << YAML::BeginMap;
136 out << YAML::Key <<
"kinematic_specifications";
138 out << YAML::BeginMap;
139 out << YAML::Key <<
"kinematic_type" << YAML::Value << msg.type;
140 out << YAML::Key <<
"kinematic_parameters";
142 out << YAML::BeginMap;
143 out << YAML::Key <<
"a_ux_ux" << YAML::Value << msg.a_ux_ux;
144 out << YAML::Key <<
"a_ux_uy" << YAML::Value << msg.a_ux_uy;
145 out << YAML::Key <<
"a_ux_w" << YAML::Value << msg.a_ux_w;
146 out << YAML::Key <<
"a_uy_ux" << YAML::Value << msg.a_uy_ux;
147 out << YAML::Key <<
"a_uy_uy" << YAML::Value << msg.a_uy_uy;
148 out << YAML::Key <<
"a_uy_w" << YAML::Value << msg.a_uy_w;
149 out << YAML::Key <<
"a_w_ux" << YAML::Value << msg.a_w_ux;
150 out << YAML::Key <<
"a_w_uy" << YAML::Value << msg.a_w_uy;
151 out << YAML::Key <<
"a_w_w" << YAML::Value << msg.a_w_w;
152 out << YAML::Key <<
"a_g_ux" << YAML::Value << msg.a_g_ux;
153 out << YAML::Key <<
"a_g_uy" << YAML::Value << msg.a_g_uy;
154 out << YAML::Key <<
"a_g_w" << YAML::Value << msg.a_g_w;
166 (stdr_msgs::LaserSensorMsg msg,std::string file_name);
170 YAML::Emitter& operator << <stdr_msgs::LaserSensorMsg>
171 (YAML::Emitter&
out,
const stdr_msgs::LaserSensorMsg& msg)
173 out << YAML::BeginMap;
174 out << YAML::Key <<
"laser";
176 out << YAML::BeginMap;
177 out << YAML::Key <<
"laser_specifications";
179 out << YAML::BeginMap;
180 out << YAML::Key <<
"max_angle" << YAML::Value << msg.maxAngle;
181 out << YAML::Key <<
"min_angle" << YAML::Value << msg.minAngle;
182 out << YAML::Key <<
"max_range" << YAML::Value << msg.maxRange;
183 out << YAML::Key <<
"min_range" << YAML::Value << msg.minRange;
184 out << YAML::Key <<
"num_rays" << YAML::Value << msg.numRays;
185 out << YAML::Key <<
"frequency" << YAML::Value << msg.frequency;
186 out << YAML::Key <<
"frame_id" << YAML::Value << msg.frame_id;
187 out << YAML::Key <<
"pose";
189 out << YAML::BeginMap;
190 out << YAML::Key <<
"x" << YAML::Value << msg.pose.x;
191 out << YAML::Key <<
"y" << YAML::Value << msg.pose.y;
192 out << YAML::Key <<
"theta" << YAML::Value << msg.pose.theta;
194 out << YAML::Key <<
"noise";
196 out << YAML::BeginMap;
197 out << YAML::Key <<
"noise_specifications";
199 out << YAML::BeginMap;
200 out << YAML::Key <<
"noise_mean";
201 out << YAML::Value << msg.noise.noiseMean;
202 out << YAML::Key <<
"noise_std";
203 out << YAML::Value << msg.noise.noiseStd;
215 (stdr_msgs::SonarSensorMsg msg,std::string file_name);
219 YAML::Emitter& operator << <stdr_msgs::SonarSensorMsg>
220 (YAML::Emitter&
out,
const stdr_msgs::SonarSensorMsg& msg)
222 out << YAML::BeginMap;
223 out << YAML::Key <<
"sonar";
225 out << YAML::BeginMap;
226 out << YAML::Key <<
"sonar_specifications";
228 out << YAML::BeginMap;
229 out << YAML::Key <<
"cone_angle" << YAML::Value << msg.coneAngle;
230 out << YAML::Key <<
"max_range" << YAML::Value << msg.maxRange;
231 out << YAML::Key <<
"min_range" << YAML::Value << msg.minRange;
232 out << YAML::Key <<
"frequency" << YAML::Value << msg.frequency;
233 out << YAML::Key <<
"frame_id" << YAML::Value << msg.frame_id;
234 out << YAML::Key <<
"pose";
236 out << YAML::BeginMap;
237 out << YAML::Key <<
"x" << YAML::Value << msg.pose.x;
238 out << YAML::Key <<
"y" << YAML::Value << msg.pose.y;
239 out << YAML::Key <<
"theta" << YAML::Value << msg.pose.theta;
241 out << YAML::Key <<
"noise";
243 out << YAML::BeginMap;
244 out << YAML::Key <<
"noise_specifications";
246 out << YAML::BeginMap;
247 out << YAML::Key <<
"noise_mean";
248 out << YAML::Value << msg.noise.noiseMean;
249 out << YAML::Key <<
"noise_std";
250 out << YAML::Value << msg.noise.noiseStd;
261 (stdr_msgs::RfidSensorMsg msg,std::string file_name);
265 YAML::Emitter& operator << <stdr_msgs::RfidSensorMsg>
266 (YAML::Emitter&
out,
const stdr_msgs::RfidSensorMsg& msg)
268 out << YAML::BeginMap;
269 out << YAML::Key <<
"rfid_reader";
271 out << YAML::BeginMap;
272 out << YAML::Key <<
"rfid_reader_specifications";
274 out << YAML::BeginMap;
275 out << YAML::Key <<
"angle_span" << YAML::Value << msg.angleSpan;
276 out << YAML::Key <<
"max_range" << YAML::Value << msg.maxRange;
277 out << YAML::Key <<
"signal_cutoff" << YAML::Value << msg.signalCutoff;
278 out << YAML::Key <<
"frequency" << YAML::Value << msg.frequency;
279 out << YAML::Key <<
"frame_id" << YAML::Value << msg.frame_id;
280 out << YAML::Key <<
"pose";
282 out << YAML::BeginMap;
283 out << YAML::Key <<
"x" << YAML::Value << msg.pose.x;
284 out << YAML::Key <<
"y" << YAML::Value << msg.pose.y;
285 out << YAML::Key <<
"theta" << YAML::Value << msg.pose.theta;
295 (stdr_msgs::CO2SensorMsg msg,std::string file_name);
299 YAML::Emitter& operator << <stdr_msgs::CO2SensorMsg>
300 (YAML::Emitter&
out,
const stdr_msgs::CO2SensorMsg& msg)
302 out << YAML::BeginMap;
303 out << YAML::Key <<
"co2_sensor";
305 out << YAML::BeginMap;
306 out << YAML::Key <<
"co2_sensor_specifications";
308 out << YAML::BeginMap;
310 out << YAML::Key <<
"max_range" << YAML::Value << msg.maxRange;
312 out << YAML::Key <<
"frequency" << YAML::Value << msg.frequency;
313 out << YAML::Key <<
"frame_id" << YAML::Value << msg.frame_id;
314 out << YAML::Key <<
"pose";
316 out << YAML::BeginMap;
317 out << YAML::Key <<
"x" << YAML::Value << msg.pose.x;
318 out << YAML::Key <<
"y" << YAML::Value << msg.pose.y;
319 out << YAML::Key <<
"theta" << YAML::Value << msg.pose.theta;
329 (stdr_msgs::ThermalSensorMsg msg,std::string file_name);
333 YAML::Emitter& operator << <stdr_msgs::ThermalSensorMsg>
334 (YAML::Emitter&
out,
const stdr_msgs::ThermalSensorMsg& msg)
336 out << YAML::BeginMap;
337 out << YAML::Key <<
"thermal_sensor";
339 out << YAML::BeginMap;
340 out << YAML::Key <<
"thermal_sensor_specifications";
342 out << YAML::BeginMap;
343 out << YAML::Key <<
"angle_span" << YAML::Value << msg.angleSpan;
344 out << YAML::Key <<
"max_range" << YAML::Value << msg.maxRange;
345 out << YAML::Key <<
"frequency" << YAML::Value << msg.frequency;
346 out << YAML::Key <<
"frame_id" << YAML::Value << msg.frame_id;
347 out << YAML::Key <<
"pose";
349 out << YAML::BeginMap;
350 out << YAML::Key <<
"x" << YAML::Value << msg.pose.x;
351 out << YAML::Key <<
"y" << YAML::Value << msg.pose.y;
352 out << YAML::Key <<
"theta" << YAML::Value << msg.pose.theta;
362 (stdr_msgs::SoundSensorMsg msg,std::string file_name);
366 YAML::Emitter& operator << <stdr_msgs::SoundSensorMsg>
367 (YAML::Emitter&
out,
const stdr_msgs::SoundSensorMsg& msg)
369 out << YAML::BeginMap;
370 out << YAML::Key <<
"sound_sensor";
372 out << YAML::BeginMap;
373 out << YAML::Key <<
"sound_sensor_specifications";
375 out << YAML::BeginMap;
376 out << YAML::Key <<
"angle_span" << YAML::Value << msg.angleSpan;
377 out << YAML::Key <<
"max_range" << YAML::Value << msg.maxRange;
378 out << YAML::Key <<
"frequency" << YAML::Value << msg.frequency;
379 out << YAML::Key <<
"frame_id" << YAML::Value << msg.frame_id;
380 out << YAML::Key <<
"pose";
382 out << YAML::BeginMap;
383 out << YAML::Key <<
"x" << YAML::Value << msg.pose.x;
384 out << YAML::Key <<
"y" << YAML::Value << msg.pose.y;
385 out << YAML::Key <<
"theta" << YAML::Value << msg.pose.theta;
396 (stdr_msgs::RobotMsg msg,std::string file_name);
400 YAML::Emitter& operator << <stdr_msgs::RobotMsg>
401 (YAML::Emitter&
out,
const stdr_msgs::RobotMsg& msg)
403 out << YAML::BeginMap;
404 out << YAML::Key <<
"robot";
406 out << YAML::BeginMap;
407 out << YAML::Key <<
"robot_specifications";
409 out << YAML::BeginSeq;
410 out << msg.footprint;
411 out << YAML::BeginMap;
412 out << YAML::Key <<
"initial_pose";
414 out << YAML::BeginMap;
415 out << YAML::Key <<
"x" << YAML::Value << msg.initialPose.x;
416 out << YAML::Key <<
"y" << YAML::Value << msg.initialPose.y;
417 out << YAML::Key <<
"theta" << YAML::Value << msg.initialPose.theta;
420 for(
unsigned int i = 0 ; i < msg.laserSensors.size() ; i++)
422 out << msg.laserSensors[i];
424 for(
unsigned int i = 0 ; i < msg.sonarSensors.size() ; i++)
426 out << msg.sonarSensors[i];
428 for(
unsigned int i = 0 ; i < msg.rfidSensors.size() ; i++)
430 out << msg.rfidSensors[i];
432 for(
unsigned int i = 0 ; i < msg.co2Sensors.size() ; i++)
434 out << msg.co2Sensors[i];
436 for(
unsigned int i = 0 ; i < msg.thermalSensors.size() ; i++)
438 out << msg.thermalSensors[i];
440 for(
unsigned int i = 0 ; i < msg.soundSensors.size() ; i++)
442 out << msg.soundSensors[i];
446 out << msg.kinematicModel;
467 std::ofstream sensorYamlFile;
468 sensorYamlFile.open(file_name.c_str());
469 sensorYamlFile << out.c_str();
470 sensorYamlFile.close();
static void messageToFile(T msg, std::string file_name)
Creates an yaml file from a message - template member function.
The main namespace for STDR GUI XML parser.
YamlFileWriter(void)
Default constructor.