arena.cpp
Go to the documentation of this file.
1 #include "arena.h"
2 
3 #include "arenaelement.h"
4 #include "arenaelementtype.h"
5 #include "arenaelementtyperegistry.h"
6 #include "xmlloadingexception.h"
7 
8 #include <QFile>
9 #include <QDomDocument>
10 #include <QDebug>
11 #include <QList>
12 #include <QDomNodeList>
13 #include <QMessageBox>
14 
15 Arena::Arena(ArenaElementTypeRegistry* typeRegistry)
16  : m_typeRegistry(typeRegistry)
17 {
18 }
19 
20 Arena::~Arena()
21 {
22 }
23 
24 void Arena::load(const QString& filename)
25 {
26  QFile in(filename);
27  in.open(QFile::ReadOnly);
28 
29  QDomDocument doc;
30  doc.setContent(&in);
31 
32  in.close();
33 
34  // Only if loading was successful we will add these elements
35  QList<ArenaElement*> loadedElements;
36  bool loadingSuccessful = true;
37 
38  try
39  {
40  QDomNodeList nodes = doc.elementsByTagName("element");
41  for(int i = 0; i < nodes.count(); i++)
42  {
43  QDomElement node = nodes.at(i).toElement();
44  QString type = node.attribute("type");
45  if (type.isEmpty())
46  {
47  qDebug() << "[Rescue Arena Designer] Error: Empty element type"
48  << "specified in" << filename << ". Ignoring.";
49  continue;
50  }
51 
52  if (!m_typeRegistry->hasElement(type))
53  {
54  qDebug() << "[Rescue Arena Designer] Error: Unknown element type"
55  << type << "in" << filename << ". Ignoring.";
56  continue;
57  }
58  ArenaElement *element = m_typeRegistry->instantiateElement(type);
59  Q_ASSERT(element);
60  loadedElements.append(element);
61 
62  element->load(node);
63  }
64  }
65  catch (XmlLoadingException e)
66  {
67  loadingSuccessful = false;
68  QString msg;
69  QTextStream(&msg) << "There's an error in the arena XML file in line "
70  << e.lineNumber() << ", column " << e.columnNumber()
71  << ":\n" << e.msg();
72  QMessageBox msgBox(QMessageBox::Critical,
73  QString("Error while loading arena XML file"),
74  msg);
75  msgBox.exec();
76  }
77 
78  // Finish loading
79  if (loadingSuccessful)
80  {
81  foreach (ArenaElement *element, loadedElements)
82  {
83  addElement(element);
84  }
85  }
86 }
87 
88 void Arena::save(const QString& filename)
89 {
90  QFile out(filename);
91  out.open(QFile::WriteOnly);
92 
93  if (out.error() != QFile::NoError)
94  qDebug() << "[Hector Arena Designer] Error saving arena file:" << out.errorString();
95 
96  QXmlStreamWriter writer(&out);
97  // Insert line-breaks for readability
98  writer.setAutoFormatting(true);
99 
100  writer.writeStartDocument();
101  writer.writeStartElement("elements");
102 
103  foreach (ArenaElement *element, m_elements)
104  element->save(writer);
105 
106  writer.writeEndElement();
107  writer.writeEndDocument();
108 
109  out.close();
110 }
111 
112 void Arena::saveWorld(const QString& filename)
113 {
114  QFile out(filename);
115  out.open(QFile::WriteOnly);
116 
117  QXmlStreamWriter writer(&out);
118  // Insert line-breaks for readability
119  writer.setAutoFormatting(true);
120 
121  writer.writeStartDocument();
122 
123  writer.writeStartElement("gazebo:world");
124  writer.writeAttribute("xmlns:gazebo", "http://playerstage.sourceforge.net/gazebo/xmlschema/#gz");
125  writer.writeAttribute("xmlns:model", "http://playerstage.sourceforge.net/gazebo/xmlschema/#model");
126  writer.writeAttribute("xmlns:body", "http://playerstage.sourceforge.net/gazebo/xmlschema/#body");
127  writer.writeAttribute("xmlns:geom", "http://playerstage.sourceforge.net/gazebo/xmlschema/#geom");
128  writer.writeAttribute("xmlns:physics", "http://playerstage.sourceforge.net/gazebo/xmlschema/#physics");
129  writer.writeAttribute("xmlns:rendering", "http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering");
130 
131  writer.writeStartElement("physics:ode");
132  writer.writeTextElement("stepTime", "0.001");
133  writer.writeTextElement("gravity", "0 0 -9.81");
134  // see http://opende.sourceforge.net/wiki/index.php/Manual_%28Concepts%29#Constraint_Force_Mixing_.28CFM.29
135  // TODO: verify
136  writer.writeTextElement("cfm", "10e-10");
137  // see http://opende.sourceforge.net/wiki/index.php/Manual_%28Concepts%29#Joint_error_and_the_error_reduction_parameter_.28ERP.29
138  // TODO: verify
139  writer.writeTextElement("erp", "0.2");
140  writer.writeTextElement("stepType", "quick");
141  writer.writeTextElement("stepIters", "10");
142  writer.writeTextElement("contactMaxCorrectingVel", "100.0");
143  writer.writeTextElement("contactSurfaceLayer", "0.001");
144  writer.writeTextElement("updateRate", "-0.1");
145  // physics:ode
146  writer.writeEndElement();
147 
148  writer.writeStartElement("rendering:gui");
149  // The old gazebo (< 1.0.0) only supports FLTK
150  writer.writeTextElement("type", "flkt");
151  writer.writeTextElement("size", "800 600");
152  writer.writeTextElement("pos", "0 0");
153 
154  writer.writeStartElement("frames");
155 
156  writer.writeStartElement("row");
157  writer.writeAttribute("height", "100%");
158  writer.writeStartElement("camera");
159  writer.writeAttribute("width", "100%");
160 
161  writer.writeTextElement("xyz", "0 0 3");
162  writer.writeTextElement("rpy", "0 90 90");
163 
164  // camera
165  writer.writeEndElement();
166 
167  // row
168  writer.writeEndElement();
169 
170  // frames
171  writer.writeEndElement();
172 
173  // rendering:gui
174  writer.writeEndElement();
175 
176  writer.writeStartElement("rendering:ogre");
177  writer.writeTextElement("ambient", "0.2 0.2 0.2 1.0");
178  writer.writeStartElement("sky");
179  writer.writeTextElement("material", "Gazebo/CloudySky");
180  // sky
181  writer.writeEndElement();
182  writer.writeTextElement("grid", "false");
183  // rendering:ogre
184  writer.writeEndElement();
185 
186  writer.writeStartElement("model:renderable");
187  writer.writeAttribute("name", "point_white");
188  writer.writeTextElement("xyz", "0 0 20");
189  writer.writeTextElement("rpy", "0 50 50");
190  writer.writeTextElement("enableGravity", "false");
191  writer.writeStartElement("light");
192  writer.writeTextElement("type", "directional");
193  writer.writeTextElement("diffuseColor", "0.7 0.7 0.7");
194  writer.writeTextElement("specularColor", "0.1 0.1 0.1");
195  writer.writeTextElement("attenuation", "0.2 0.1 0");
196  writer.writeTextElement("range", "100.0");
197 
198  writer.writeEndElement();
199  writer.writeEndElement();
200 
201  /*
202  writer.writeStartElement("model:physical");
203  writer.writeAttribute("name", "plane_model");
204  writer.writeTextElement("xyz", "0 0 0");
205  writer.writeTextElement("rpy", "0 0 0");
206  writer.writeTextElement("static", "true");
207  writer.writeStartElement("body:plane");
208  writer.writeAttribute("name", "plane_body");
209  writer.writeStartElement("geom:plane");
210  writer.writeAttribute("name", "plane_geom");
211  writer.writeTextElement("normal", "0 0 1");
212  writer.writeTextElement("size", "120 120");
213  writer.writeTextElement("segments", "10 10");
214  writer.writeTextElement("uvTile", "100 100");
215  writer.writeTextElement("material", "Gazebo/GrayGrid");
216  */
217 
218  writer.writeStartElement("model:physical");
219  writer.writeAttribute("name", "gplane");
220 
221  writer.writeTextElement("xyz", "0 0 0");
222  writer.writeTextElement("rpy", "0 0 0");
223  writer.writeTextElement("static", "true");
224 
225  writer.writeStartElement("body:plane");
226  writer.writeAttribute("name", "plane");
227 
228  writer.writeStartElement("geom:plane");
229  writer.writeAttribute("name", "plane");
230 
231  writer.writeTextElement("laserRetro", "2000.0");
232  writer.writeTextElement("mu1", "50.0");
233  writer.writeTextElement("mu2", "50.0");
234 
235  writer.writeTextElement("kp", "1000000000.0");
236  writer.writeTextElement("kd", "1.0");
237 
238  writer.writeTextElement("normal", "0 0 1");
239  writer.writeTextElement("size", "120 120");
240 
241  writer.writeTextElement("segments", "10 10");
242  writer.writeTextElement("uvTile", "100 100");
243 
244  writer.writeTextElement("material", "Gazebo/GrayGrid");
245 
246 
247  // geom:plane
248  writer.writeEndElement();
249  // body:plane
250  writer.writeEndElement();
251  // model:physical
252  writer.writeEndElement();
253 
254  foreach (ArenaElement *element, m_elements)
255  element->saveWorld(writer);
256 
257  writer.writeEndElement();
258  writer.writeEndDocument();
259 
260  out.close();
261 }
262 
263 void Arena::saveWorldSdf(const QString& filename)
264 {
265  QFile out(filename);
266  out.open(QFile::WriteOnly);
267 
268  QXmlStreamWriter writer(&out);
269  // Insert line-breaks for readability
270  writer.setAutoFormatting(true);
271  writer.setAutoFormattingIndent(2);
272 
273  writer.writeStartDocument();
274 
275  writer.writeStartElement("sdf");
276  writer.writeAttribute("version", "1.4");
277 
278  writer.writeStartElement("world");
279  writer.writeAttribute("name", "default");
280 
281  writer.writeStartElement("scene");
282  writer.writeTextElement("ambient", "0.5 0.5 0.5 1");
283  writer.writeTextElement("background","0.5 0.5 0.5 1");
284  writer.writeTextElement("shadows", "false");
285  writer.writeEndElement();
286 
287  writer.writeStartElement("physics");
288  writer.writeAttribute("type", "ode");
289 
290  writer.writeTextElement("gravity", "0 0 -9.81");
291 
292  writer.writeStartElement("ode");
293 
294  writer.writeStartElement("solver");
295  writer.writeTextElement("type","quick");
296  writer.writeTextElement("iters","10");
297  writer.writeTextElement("sor","1.3");
298  writer.writeEndElement();
299 
300  writer.writeStartElement("constraints");
301  writer.writeTextElement("cfm","0.0");
302  writer.writeTextElement("erp","0.2");
303  writer.writeTextElement("contact_max_correcting_vel","100.0");
304  writer.writeTextElement("contact_surface_layer","0.001");
305  writer.writeEndElement();
306 
307  writer.writeEndElement(); // ode
308  writer.writeTextElement("real_time_update_rate", "1000");
309  writer.writeTextElement("max_step_size", "0.001");
310 
311  writer.writeEndElement(); // physics
312 
313  writer.writeStartElement("light");
314  writer.writeAttribute("type","directional");
315  writer.writeAttribute("name","directional_light_1");
316  writer.writeTextElement("cast_shadows","false");
317 
318  writer.writeTextElement("pose","0 0 30 0.1 0.1 0");
319  writer.writeTextElement("diffuse", "1.0 1.0 1.0 1");
320  writer.writeTextElement("specular", ".1 .1 .1 1");
321 
322  writer.writeStartElement("attenuation");
323  writer.writeTextElement("range", "300");
324  writer.writeEndElement();
325 
326  writer.writeTextElement("direction", "0.1 0.1 -1");
327  writer.writeEndElement(); // light
328 
329  writer.writeStartElement("model");
330  writer.writeAttribute("name", "plane_model");
331  writer.writeTextElement("static", "true");
332 
333  writer.writeStartElement("link");
334  writer.writeAttribute("name","plane_model_link");
335 
336  writer.writeStartElement("collision");
337  writer.writeAttribute("name","plane_model_collision");
338 
339  writer.writeStartElement("geometry");
340 
341  writer.writeStartElement("plane");
342  writer.writeTextElement("normal", "0 0 1");
343  writer.writeEndElement();
344 
345  writer.writeEndElement(); //geometry
346 
347  writer.writeStartElement("surface");
348 
349  writer.writeStartElement("friction");
350 
351  writer.writeStartElement("ode");
352  writer.writeTextElement("mu","10.0");
353  writer.writeTextElement("mu2","10.0");
354  writer.writeTextElement("fdir1","0 0 0");
355  writer.writeTextElement("slip1","0");
356  writer.writeTextElement("slip2","0");
357  writer.writeEndElement(); //ode
358 
359  writer.writeEndElement(); //friction
360 
361  writer.writeStartElement("bounce");
362  writer.writeTextElement("restitution_coefficient","0");
363  writer.writeTextElement("threshold","1000000.0");
364  writer.writeEndElement(); //bounce
365 
366  writer.writeStartElement("contact");
367 
368  writer.writeStartElement("ode");
369  writer.writeTextElement("soft_cfm","0");
370  writer.writeTextElement("soft_erp","0.2");
371  writer.writeTextElement("kp","1e10");
372  writer.writeTextElement("kd","1");
373  writer.writeTextElement("max_vel","100.0");
374  writer.writeTextElement("min_depth","0.0001");
375  writer.writeEndElement(); //ode
376 
377  writer.writeEndElement(); //contact
378 
379  writer.writeEndElement(); //surface
380  writer.writeEndElement(); //collision
381 
382 
383  writer.writeStartElement("visual");
384  writer.writeAttribute("name","plane_model_visual");
385  writer.writeTextElement("cast_shadows","false");
386 
387  writer.writeStartElement("geometry");
388 
389  bool use_textured_ground_plane = true;
390 
391  if (!use_textured_ground_plane){
392  writer.writeStartElement("plane");
393  writer.writeTextElement("normal","0 0 1");
394  writer.writeEndElement();
395  }else{
396  writer.writeStartElement("mesh");
397  writer.writeTextElement("uri","file://ground_plane.dae");
398  writer.writeTextElement("scale","1 1 1");
399  writer.writeEndElement();
400  }
401 
402  writer.writeEndElement(); //geometry
403 
404  writer.writeStartElement("material");
405  writer.writeTextElement("script","Gazebo/Grey");
406  writer.writeEndElement();
407 
408 
409 
410 
411  //writer.writeEndElement();
412 
413  writer.writeEndElement(); //visual
414 
415  writer.writeEndElement(); //link
416 
417  writer.writeEndElement(); //model
418 
419 
420 
421 
422 
423 
424 
425 
426 
427 
428  foreach (ArenaElement *element, m_elements)
429  element->saveWorldSdf(writer);
430 
431 
432  writer.writeEndElement();
433 
434  writer.writeEndDocument();
435 
436  out.close();
437 }
438 
439 void Arena::slotModified()
440 {
441  emit modified();
442 }
443 
444 void Arena::addElement(ArenaElement *element)
445 {
446  Q_ASSERT(!m_elements.contains(element));
447  m_elements.append(element);
448 
449  connect(element, SIGNAL(posChanged(ArenaElement*,QPoint)),
450  this, SLOT(slotModified()));
451  connect(element, SIGNAL(rotationChanged(ArenaElement*,int)),
452  this, SLOT(slotModified()));
453  connect(element, SIGNAL(modified(ArenaElement*)),
454  this, SIGNAL(modified()));
455 
456  element->setArena(this);
457 
458  emit elementAdded(element);
459 }
460 
461 void Arena::removeElement(ArenaElement *element)
462 {
463  Q_ASSERT(m_elements.contains(element));
464  m_elements.removeAll(element);
465 
466  emit elementRemoved(element);
467 }
468 
469 void Arena::clear()
470 {
471  foreach (ArenaElement *element, m_elements)
472  {
473  m_elements.removeAll(element);
474  emit elementRemoved(element);
475  }
476  m_elements.clear();
477 
478  emit modified();
479 }
480 
481 ArenaElement* Arena::addElement(const QString &elementType)
482 {
483  ArenaElement *element = m_typeRegistry->instantiateElement(elementType);
484  if (element)
485  addElement(element);
486  return element;
487 }
488 
489 QList<ArenaElement*> Arena::elementsAt(const QPoint &pos) const
490 {
491  QList<ArenaElement*> res;
492  foreach (ArenaElement* e, m_elements)
493  if (e->pos() == pos)
494  res.push_back(e);
495  return res;
496 }
497 
498 ArenaElement* Arena::contextElement(ArenaElement *element) const
499 {
500  QList<ArenaElement*> elementsAtPos = elementsAt(element->pos());
501  foreach (ArenaElement *e, elementsAtPos)
502  if (e != element &&
503  e->type()->type() == ArenaElementType::WallType &&
504  e->rotation() == element->rotation())
505  return e;
506  return 0;
507 }
ArenaElement::saveWorld
void saveWorld(QXmlStreamWriter &writer)
Writes .world format-compliant XML.
Definition: arenaelement.cpp:126
ArenaElement::saveWorldSdf
void saveWorldSdf(QXmlStreamWriter &writer)
Definition: arenaelement.cpp:208
Arena::modified
void modified()
ArenaElementType::type
Type type() const
Definition: arenaelementtype.h:47
ArenaElement::type
ArenaElementType const * type() const
Definition: arenaelement.h:43
Arena::removeElement
void removeElement(ArenaElement *element)
Definition: arena.cpp:461
XmlLoadingException::msg
QString msg() const
Definition: xmlloadingexception.h:18
Arena::clear
void clear()
Removes all elements from this arena.
Definition: arena.cpp:469
Arena::slotModified
void slotModified()
Definition: arena.cpp:439
ArenaElement::setArena
void setArena(Arena *arena)
Called from Arena::addElement()
Definition: arenaelement.h:39
Arena::saveWorld
void saveWorld(const QString &filename)
Writes .world format-compliant XML.
Definition: arena.cpp:112
Arena::load
void load(const QString &filename)
Deserializes this arena from XML.
Definition: arena.cpp:24
Arena::elementsAt
QList< ArenaElement * > elementsAt(const QPoint &pos) const
Definition: arena.cpp:489
Arena::~Arena
~Arena()
Definition: arena.cpp:20
Arena::m_typeRegistry
ArenaElementTypeRegistry *const m_typeRegistry
Definition: arena.h:54
Arena::contextElement
ArenaElement * contextElement(ArenaElement *element) const
Definition: arena.cpp:498
ArenaElement::rotation
int rotation() const
Returns the rotation in degrees.
Definition: arenaelement.h:26
ArenaElement::save
void save(QXmlStreamWriter &writer)
Serializes this element in XML.
Definition: arenaelement.cpp:108
ArenaElement::load
void load(const QDomElement &node)
Deserializes this element from XML.
Definition: arenaelement.cpp:74
ArenaElementTypeRegistry::instantiateElement
ArenaElement * instantiateElement(const QString &typeName)
Definition: arenaelementtyperegistry.cpp:32
ArenaElement::pos
QPoint pos() const
Returns the position in arena coordinates.
Definition: arenaelement.h:22
Arena::addElement
ArenaElement * addElement(const QString &elementType)
Creates and adds an element of type elementType.
Definition: arena.cpp:481
Arena::Arena
Arena(ArenaElementTypeRegistry *typeRegistry)
Definition: arena.cpp:15
Arena::elementRemoved
void elementRemoved(ArenaElement *element)
Arena::saveWorldSdf
void saveWorldSdf(const QString &filename)
Definition: arena.cpp:263
ArenaElementTypeRegistry::hasElement
bool hasElement(const QString &typeName)
Definition: arenaelementtyperegistry.cpp:27
Arena::elementAdded
void elementAdded(ArenaElement *element)
Arena::m_elements
QList< ArenaElement * > m_elements
Definition: arena.h:55
Arena::save
void save(const QString &filename)
Serializes this arena in XML.
Definition: arena.cpp:88

hector_nist_arena_designer
Author(s): Stefan Kohlbrecher , Johannes Simon
autogenerated on Fri Aug 21 2020 10:45:27