00001 #include "arena.h"
00002
00003 #include "arenaelement.h"
00004 #include "arenaelementtype.h"
00005 #include "arenaelementtyperegistry.h"
00006 #include "xmlloadingexception.h"
00007
00008 #include <QFile>
00009 #include <QDomDocument>
00010 #include <QDebug>
00011 #include <QList>
00012 #include <QDomNodeList>
00013 #include <QMessageBox>
00014
00015 Arena::Arena(ArenaElementTypeRegistry* typeRegistry)
00016 : m_typeRegistry(typeRegistry)
00017 {
00018 }
00019
00020 Arena::~Arena()
00021 {
00022 }
00023
00024 void Arena::load(const QString& filename)
00025 {
00026 QFile in(filename);
00027 in.open(QFile::ReadOnly);
00028
00029 QDomDocument doc;
00030 doc.setContent(&in);
00031
00032 in.close();
00033
00034
00035 QList<ArenaElement*> loadedElements;
00036 bool loadingSuccessful = true;
00037
00038 try
00039 {
00040 QDomNodeList nodes = doc.elementsByTagName("element");
00041 for(int i = 0; i < nodes.count(); i++)
00042 {
00043 QDomElement node = nodes.at(i).toElement();
00044 QString type = node.attribute("type");
00045 if (type.isEmpty())
00046 {
00047 qDebug() << "[Rescue Arena Designer] Error: Empty element type"
00048 << "specified in" << filename << ". Ignoring.";
00049 continue;
00050 }
00051
00052 if (!m_typeRegistry->hasElement(type))
00053 {
00054 qDebug() << "[Rescue Arena Designer] Error: Unknown element type"
00055 << type << "in" << filename << ". Ignoring.";
00056 continue;
00057 }
00058 ArenaElement *element = m_typeRegistry->instantiateElement(type);
00059 Q_ASSERT(element);
00060 loadedElements.append(element);
00061
00062 element->load(node);
00063 }
00064 }
00065 catch (XmlLoadingException e)
00066 {
00067 loadingSuccessful = false;
00068 QString msg;
00069 QTextStream(&msg) << "There's an error in the arena XML file in line "
00070 << e.lineNumber() << ", column " << e.columnNumber()
00071 << ":\n" << e.msg();
00072 QMessageBox msgBox(QMessageBox::Critical,
00073 QString("Error while loading arena XML file"),
00074 msg);
00075 msgBox.exec();
00076 }
00077
00078
00079 if (loadingSuccessful)
00080 {
00081 foreach (ArenaElement *element, loadedElements)
00082 {
00083 addElement(element);
00084 }
00085 }
00086 }
00087
00088 void Arena::save(const QString& filename)
00089 {
00090 QFile out(filename);
00091 out.open(QFile::WriteOnly);
00092
00093 if (out.error() != QFile::NoError)
00094 qDebug() << "[Hector Arena Designer] Error saving arena file:" << out.errorString();
00095
00096 QXmlStreamWriter writer(&out);
00097
00098 writer.setAutoFormatting(true);
00099
00100 writer.writeStartDocument();
00101 writer.writeStartElement("elements");
00102
00103 foreach (ArenaElement *element, m_elements)
00104 element->save(writer);
00105
00106 writer.writeEndElement();
00107 writer.writeEndDocument();
00108
00109 out.close();
00110 }
00111
00112 void Arena::saveWorld(const QString& filename)
00113 {
00114 QFile out(filename);
00115 out.open(QFile::WriteOnly);
00116
00117 QXmlStreamWriter writer(&out);
00118
00119 writer.setAutoFormatting(true);
00120
00121 writer.writeStartDocument();
00122
00123 writer.writeStartElement("gazebo:world");
00124 writer.writeAttribute("xmlns:gazebo", "http://playerstage.sourceforge.net/gazebo/xmlschema/#gz");
00125 writer.writeAttribute("xmlns:model", "http://playerstage.sourceforge.net/gazebo/xmlschema/#model");
00126 writer.writeAttribute("xmlns:body", "http://playerstage.sourceforge.net/gazebo/xmlschema/#body");
00127 writer.writeAttribute("xmlns:geom", "http://playerstage.sourceforge.net/gazebo/xmlschema/#geom");
00128 writer.writeAttribute("xmlns:physics", "http://playerstage.sourceforge.net/gazebo/xmlschema/#physics");
00129 writer.writeAttribute("xmlns:rendering", "http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering");
00130
00131 writer.writeStartElement("physics:ode");
00132 writer.writeTextElement("stepTime", "0.001");
00133 writer.writeTextElement("gravity", "0 0 -9.81");
00134
00135
00136 writer.writeTextElement("cfm", "10e-10");
00137
00138
00139 writer.writeTextElement("erp", "0.2");
00140 writer.writeTextElement("stepType", "quick");
00141 writer.writeTextElement("stepIters", "10");
00142 writer.writeTextElement("contactMaxCorrectingVel", "100.0");
00143 writer.writeTextElement("contactSurfaceLayer", "0.001");
00144 writer.writeTextElement("updateRate", "-0.1");
00145
00146 writer.writeEndElement();
00147
00148 writer.writeStartElement("rendering:gui");
00149
00150 writer.writeTextElement("type", "flkt");
00151 writer.writeTextElement("size", "800 600");
00152 writer.writeTextElement("pos", "0 0");
00153
00154 writer.writeStartElement("frames");
00155
00156 writer.writeStartElement("row");
00157 writer.writeAttribute("height", "100%");
00158 writer.writeStartElement("camera");
00159 writer.writeAttribute("width", "100%");
00160
00161 writer.writeTextElement("xyz", "0 0 3");
00162 writer.writeTextElement("rpy", "0 90 90");
00163
00164
00165 writer.writeEndElement();
00166
00167
00168 writer.writeEndElement();
00169
00170
00171 writer.writeEndElement();
00172
00173
00174 writer.writeEndElement();
00175
00176 writer.writeStartElement("rendering:ogre");
00177 writer.writeTextElement("ambient", "0.2 0.2 0.2 1.0");
00178 writer.writeStartElement("sky");
00179 writer.writeTextElement("material", "Gazebo/CloudySky");
00180
00181 writer.writeEndElement();
00182 writer.writeTextElement("grid", "false");
00183
00184 writer.writeEndElement();
00185
00186 writer.writeStartElement("model:renderable");
00187 writer.writeAttribute("name", "point_white");
00188 writer.writeTextElement("xyz", "0 0 20");
00189 writer.writeTextElement("rpy", "0 50 50");
00190 writer.writeTextElement("enableGravity", "false");
00191 writer.writeStartElement("light");
00192 writer.writeTextElement("type", "directional");
00193 writer.writeTextElement("diffuseColor", "0.7 0.7 0.7");
00194 writer.writeTextElement("specularColor", "0.1 0.1 0.1");
00195 writer.writeTextElement("attenuation", "0.2 0.1 0");
00196 writer.writeTextElement("range", "100.0");
00197
00198 writer.writeEndElement();
00199 writer.writeEndElement();
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218 writer.writeStartElement("model:physical");
00219 writer.writeAttribute("name", "gplane");
00220
00221 writer.writeTextElement("xyz", "0 0 0");
00222 writer.writeTextElement("rpy", "0 0 0");
00223 writer.writeTextElement("static", "true");
00224
00225 writer.writeStartElement("body:plane");
00226 writer.writeAttribute("name", "plane");
00227
00228 writer.writeStartElement("geom:plane");
00229 writer.writeAttribute("name", "plane");
00230
00231 writer.writeTextElement("laserRetro", "2000.0");
00232 writer.writeTextElement("mu1", "50.0");
00233 writer.writeTextElement("mu2", "50.0");
00234
00235 writer.writeTextElement("kp", "1000000000.0");
00236 writer.writeTextElement("kd", "1.0");
00237
00238 writer.writeTextElement("normal", "0 0 1");
00239 writer.writeTextElement("size", "120 120");
00240
00241 writer.writeTextElement("segments", "10 10");
00242 writer.writeTextElement("uvTile", "100 100");
00243
00244 writer.writeTextElement("material", "Gazebo/GrayGrid");
00245
00246
00247
00248 writer.writeEndElement();
00249
00250 writer.writeEndElement();
00251
00252 writer.writeEndElement();
00253
00254 foreach (ArenaElement *element, m_elements)
00255 element->saveWorld(writer);
00256
00257 writer.writeEndElement();
00258 writer.writeEndDocument();
00259
00260 out.close();
00261 }
00262
00263 void Arena::saveWorldSdf(const QString& filename)
00264 {
00265 QFile out(filename);
00266 out.open(QFile::WriteOnly);
00267
00268 QXmlStreamWriter writer(&out);
00269
00270 writer.setAutoFormatting(true);
00271
00272 writer.writeStartDocument();
00273
00274 writer.writeStartElement("gazebo");
00275 writer.writeAttribute("version", "1.0");
00276
00277 writer.writeStartElement("world");
00278 writer.writeAttribute("name", "default");
00279
00280 writer.writeStartElement("scene");
00281 writer.writeStartElement("ambient");
00282 writer.writeAttribute("rgba","0.5 0.5 0.5 1");
00283 writer.writeEndElement();
00284
00285 writer.writeStartElement("background");
00286 writer.writeAttribute("rgba","0.5 0.5 0.5 1");
00287 writer.writeEndElement();
00288
00289 writer.writeStartElement("shadows");
00290 writer.writeAttribute("enabled","false");
00291 writer.writeEndElement();
00292
00293 writer.writeEndElement();
00294
00295 writer.writeStartElement("physics");
00296 writer.writeAttribute("type","ode");
00297
00298 writer.writeStartElement("gravity");
00299 writer.writeAttribute("xyz","0 0 -9.81");
00300 writer.writeEndElement();
00301
00302 writer.writeStartElement("ode");
00303
00304 writer.writeStartElement("solver");
00305 writer.writeAttribute("type","quick");
00306 writer.writeAttribute("dt","0.001");
00307 writer.writeAttribute("iters","10");
00308 writer.writeAttribute("sor","1.3");
00309 writer.writeEndElement();
00310
00311 writer.writeStartElement("constraints");
00312 writer.writeAttribute("cfm","0.0");
00313 writer.writeAttribute("erp","0.2");
00314 writer.writeAttribute("contact_max_correcting_vel","10");
00315 writer.writeAttribute("contact_surface_layer","0.001");
00316 writer.writeEndElement();
00317
00318 writer.writeEndElement();
00319
00320 writer.writeEndElement();
00321
00322 writer.writeStartElement("light");
00323 writer.writeAttribute("type","directional");
00324 writer.writeAttribute("name","directional_light_1");
00325 writer.writeAttribute("cast_shadows","false");
00326
00327 writer.writeStartElement("origin");
00328 writer.writeAttribute("pose","0 0 30 0.1 0.1 0");
00329 writer.writeEndElement();
00330
00331 writer.writeStartElement("diffuse");
00332 writer.writeAttribute("rgba",".5 .5 .5 1");
00333 writer.writeEndElement();
00334
00335 writer.writeStartElement("specular");
00336 writer.writeAttribute("rgba",".1 .1 .1 1");
00337 writer.writeEndElement();
00338
00339 writer.writeStartElement("attenuation");
00340 writer.writeAttribute("range","300");
00341 writer.writeEndElement();
00342
00343 writer.writeStartElement("direction");
00344 writer.writeAttribute("xyz","0.1 0.1 -1");
00345 writer.writeEndElement();
00346
00347 writer.writeEndElement();
00348
00349 writer.writeStartElement("model");
00350
00351 writer.writeAttribute("name", "plane_model");
00352 writer.writeAttribute("static", "true");
00353
00354 writer.writeStartElement("link");
00355 writer.writeAttribute("name","plane_model_link");
00356
00357 writer.writeStartElement("collision");
00358 writer.writeAttribute("name","plane_model_collision");
00359
00360 writer.writeStartElement("geometry");
00361
00362 writer.writeStartElement("plane");
00363 writer.writeAttribute("normal","0 0 1");
00364 writer.writeEndElement();
00365
00366 writer.writeEndElement();
00367
00368 writer.writeStartElement("surface");
00369
00370 writer.writeStartElement("friction");
00371
00372 writer.writeStartElement("ode");
00373 writer.writeAttribute("mu","10.0");
00374 writer.writeAttribute("mu2","10.0");
00375 writer.writeAttribute("fdir1","0 0 0");
00376 writer.writeAttribute("slip1","0");
00377 writer.writeAttribute("slip2","0");
00378 writer.writeEndElement();
00379
00380 writer.writeEndElement();
00381
00382 writer.writeStartElement("bounce");
00383 writer.writeAttribute("restitution_coefficient","0");
00384 writer.writeAttribute("threshold","1000000.0");
00385 writer.writeEndElement();
00386
00387 writer.writeStartElement("contact");
00388
00389 writer.writeStartElement("ode");
00390 writer.writeAttribute("soft_cfm","0");
00391 writer.writeAttribute("soft_erp","0.2");
00392 writer.writeAttribute("kp","1e10");
00393 writer.writeAttribute("kd","1");
00394 writer.writeAttribute("max_vel","100.0");
00395 writer.writeAttribute("min_depth","0.0001");
00396 writer.writeEndElement();
00397
00398 writer.writeEndElement();
00399
00400 writer.writeEndElement();
00401 writer.writeEndElement();
00402
00403
00404 writer.writeStartElement("visual");
00405 writer.writeAttribute("name","plane_model_visual");
00406 writer.writeAttribute("cast_shadows","false");
00407
00408 writer.writeStartElement("geometry");
00409
00410 writer.writeStartElement("plane");
00411 writer.writeAttribute("normal","0 0 1");
00412 writer.writeEndElement();
00413
00414 writer.writeEndElement();
00415
00416 writer.writeStartElement("material");
00417 writer.writeAttribute("script","Gazebo/Grey");
00418 writer.writeEndElement();
00419
00420
00421
00422
00423
00424
00425 writer.writeEndElement();
00426
00427 writer.writeEndElement();
00428
00429 writer.writeEndElement();
00430
00431
00432
00433
00434
00435
00436
00437
00438
00439
00440 foreach (ArenaElement *element, m_elements)
00441 element->saveWorldSdf(writer);
00442
00443
00444 writer.writeEndElement();
00445
00446 writer.writeEndDocument();
00447
00448 out.close();
00449 }
00450
00451 void Arena::slotModified()
00452 {
00453 emit modified();
00454 }
00455
00456 void Arena::addElement(ArenaElement *element)
00457 {
00458 Q_ASSERT(!m_elements.contains(element));
00459 m_elements.append(element);
00460
00461 connect(element, SIGNAL(posChanged(ArenaElement*,QPoint)),
00462 this, SLOT(slotModified()));
00463 connect(element, SIGNAL(rotationChanged(ArenaElement*,int)),
00464 this, SLOT(slotModified()));
00465 connect(element, SIGNAL(modified(ArenaElement*)),
00466 this, SIGNAL(modified()));
00467
00468 element->setArena(this);
00469
00470 emit elementAdded(element);
00471 }
00472
00473 void Arena::removeElement(ArenaElement *element)
00474 {
00475 Q_ASSERT(m_elements.contains(element));
00476 m_elements.removeAll(element);
00477
00478 emit elementRemoved(element);
00479 }
00480
00481 void Arena::clear()
00482 {
00483 foreach (ArenaElement *element, m_elements)
00484 {
00485 m_elements.removeAll(element);
00486 emit elementRemoved(element);
00487 }
00488 m_elements.clear();
00489
00490 emit modified();
00491 }
00492
00493 ArenaElement* Arena::addElement(const QString &elementType)
00494 {
00495 ArenaElement *element = m_typeRegistry->instantiateElement(elementType);
00496 if (element)
00497 addElement(element);
00498 return element;
00499 }
00500
00501 QList<ArenaElement*> Arena::elementsAt(const QPoint &pos) const
00502 {
00503 QList<ArenaElement*> res;
00504 foreach (ArenaElement* e, m_elements)
00505 if (e->pos() == pos)
00506 res.push_back(e);
00507 return res;
00508 }
00509
00510 ArenaElement* Arena::contextElement(ArenaElement *element) const
00511 {
00512 QList<ArenaElement*> elementsAtPos = elementsAt(element->pos());
00513 foreach (ArenaElement *e, elementsAtPos)
00514 if (e != element &&
00515 e->type()->type() == ArenaElementType::WallType &&
00516 e->rotation() == element->rotation())
00517 return e;
00518 return 0;
00519 }