arena.cpp
Go to the documentation of this file.
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     // Only if loading was successful we will add these elements
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     // Finish loading
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     // Insert line-breaks for readability
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     // Insert line-breaks for readability
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     // see http://opende.sourceforge.net/wiki/index.php/Manual_%28Concepts%29#Constraint_Force_Mixing_.28CFM.29
00135     // TODO: verify
00136     writer.writeTextElement("cfm", "10e-10");
00137     // see http://opende.sourceforge.net/wiki/index.php/Manual_%28Concepts%29#Joint_error_and_the_error_reduction_parameter_.28ERP.29
00138     // TODO: verify
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     // physics:ode
00146     writer.writeEndElement();
00147 
00148     writer.writeStartElement("rendering:gui");
00149     // The old gazebo (< 1.0.0) only supports FLTK
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     // camera
00165     writer.writeEndElement();
00166 
00167     // row
00168     writer.writeEndElement();
00169 
00170     // frames
00171     writer.writeEndElement();
00172 
00173     // rendering:gui
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     // sky
00181     writer.writeEndElement();
00182     writer.writeTextElement("grid", "false");
00183     // rendering:ogre
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     writer.writeStartElement("model:physical");
00203     writer.writeAttribute("name", "plane_model");
00204     writer.writeTextElement("xyz", "0 0 0");
00205     writer.writeTextElement("rpy", "0 0 0");
00206     writer.writeTextElement("static", "true");
00207     writer.writeStartElement("body:plane");
00208     writer.writeAttribute("name", "plane_body");
00209     writer.writeStartElement("geom:plane");
00210     writer.writeAttribute("name", "plane_geom");
00211     writer.writeTextElement("normal", "0 0 1");
00212     writer.writeTextElement("size", "120 120");
00213     writer.writeTextElement("segments", "10 10");
00214     writer.writeTextElement("uvTile", "100 100");
00215     writer.writeTextElement("material", "Gazebo/GrayGrid");
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     // geom:plane
00248     writer.writeEndElement();
00249     // body:plane
00250     writer.writeEndElement();
00251     // model:physical
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     // Insert line-breaks for readability
00270     writer.setAutoFormatting(true);
00271     writer.setAutoFormattingIndent(2);
00272 
00273     writer.writeStartDocument();
00274 
00275     writer.writeStartElement("sdf");
00276     writer.writeAttribute("version", "1.4");
00277 
00278     writer.writeStartElement("world");
00279     writer.writeAttribute("name", "default");
00280 
00281     writer.writeStartElement("scene");
00282     writer.writeTextElement("ambient", "0.5 0.5 0.5 1");
00283     writer.writeTextElement("background","0.5 0.5 0.5 1");
00284     writer.writeTextElement("shadows", "false");
00285     writer.writeEndElement();
00286 
00287     writer.writeStartElement("physics");
00288     writer.writeAttribute("type", "ode");
00289 
00290     writer.writeTextElement("gravity", "0 0 -9.81");
00291 
00292     writer.writeStartElement("ode");
00293 
00294     writer.writeStartElement("solver");
00295     writer.writeTextElement("type","quick");
00296     writer.writeTextElement("iters","10");
00297     writer.writeTextElement("sor","1.3");
00298     writer.writeEndElement();
00299 
00300     writer.writeStartElement("constraints");
00301     writer.writeTextElement("cfm","0.0");
00302     writer.writeTextElement("erp","0.2");
00303     writer.writeTextElement("contact_max_correcting_vel","100.0");
00304     writer.writeTextElement("contact_surface_layer","0.001");
00305     writer.writeEndElement();
00306 
00307     writer.writeEndElement(); // ode
00308     writer.writeTextElement("real_time_update_rate", "1000");
00309     writer.writeTextElement("max_step_size", "0.001");
00310 
00311     writer.writeEndElement(); // physics
00312 
00313     writer.writeStartElement("light");
00314     writer.writeAttribute("type","directional");
00315     writer.writeAttribute("name","directional_light_1");
00316     writer.writeTextElement("cast_shadows","false");
00317 
00318     writer.writeTextElement("pose","0 0 30 0.1 0.1 0");
00319     writer.writeTextElement("diffuse", "1.0 1.0 1.0 1");
00320     writer.writeTextElement("specular", ".1 .1 .1 1");
00321 
00322     writer.writeStartElement("attenuation");
00323     writer.writeTextElement("range", "300");
00324     writer.writeEndElement();
00325 
00326     writer.writeTextElement("direction", "0.1 0.1 -1");
00327     writer.writeEndElement(); // light
00328 
00329     writer.writeStartElement("model");
00330     writer.writeAttribute("name", "plane_model");
00331     writer.writeTextElement("static", "true");
00332 
00333     writer.writeStartElement("link");
00334     writer.writeAttribute("name","plane_model_link");
00335 
00336     writer.writeStartElement("collision");
00337     writer.writeAttribute("name","plane_model_collision");
00338 
00339     writer.writeStartElement("geometry");
00340 
00341     writer.writeStartElement("plane");
00342     writer.writeTextElement("normal", "0 0 1");
00343     writer.writeEndElement();
00344 
00345     writer.writeEndElement(); //geometry
00346 
00347     writer.writeStartElement("surface");
00348 
00349     writer.writeStartElement("friction");
00350 
00351     writer.writeStartElement("ode");
00352     writer.writeTextElement("mu","10.0");
00353     writer.writeTextElement("mu2","10.0");
00354     writer.writeTextElement("fdir1","0 0 0");
00355     writer.writeTextElement("slip1","0");
00356     writer.writeTextElement("slip2","0");
00357     writer.writeEndElement(); //ode
00358 
00359     writer.writeEndElement(); //friction
00360 
00361     writer.writeStartElement("bounce");
00362     writer.writeTextElement("restitution_coefficient","0");
00363     writer.writeTextElement("threshold","1000000.0");
00364     writer.writeEndElement(); //bounce
00365 
00366     writer.writeStartElement("contact");
00367 
00368     writer.writeStartElement("ode");
00369     writer.writeTextElement("soft_cfm","0");
00370     writer.writeTextElement("soft_erp","0.2");
00371     writer.writeTextElement("kp","1e10");
00372     writer.writeTextElement("kd","1");
00373     writer.writeTextElement("max_vel","100.0");
00374     writer.writeTextElement("min_depth","0.0001");
00375     writer.writeEndElement(); //ode
00376 
00377     writer.writeEndElement(); //contact
00378 
00379     writer.writeEndElement(); //surface
00380     writer.writeEndElement(); //collision
00381 
00382 
00383     writer.writeStartElement("visual");
00384     writer.writeAttribute("name","plane_model_visual");
00385     writer.writeTextElement("cast_shadows","false");
00386 
00387     writer.writeStartElement("geometry");
00388 
00389     bool use_textured_ground_plane = true;
00390     
00391     if (!use_textured_ground_plane){
00392       writer.writeStartElement("plane");
00393       writer.writeTextElement("normal","0 0 1");
00394       writer.writeEndElement();
00395     }else{
00396       writer.writeStartElement("mesh");
00397       writer.writeTextElement("uri","file://ground_plane.dae");
00398       writer.writeTextElement("scale","1 1 1");
00399       writer.writeEndElement();
00400     }
00401     
00402     writer.writeEndElement(); //geometry
00403 
00404     writer.writeStartElement("material");
00405     writer.writeTextElement("script","Gazebo/Grey");
00406     writer.writeEndElement();
00407 
00408 
00409 
00410 
00411     //writer.writeEndElement();
00412 
00413     writer.writeEndElement(); //visual
00414 
00415     writer.writeEndElement(); //link
00416 
00417     writer.writeEndElement(); //model
00418 
00419 
00420 
00421 
00422 
00423 
00424 
00425 
00426 
00427 
00428     foreach (ArenaElement *element, m_elements)
00429         element->saveWorldSdf(writer);
00430 
00431 
00432     writer.writeEndElement();
00433 
00434     writer.writeEndDocument();
00435 
00436     out.close();
00437 }
00438 
00439 void Arena::slotModified()
00440 {
00441     emit modified();
00442 }
00443 
00444 void Arena::addElement(ArenaElement *element)
00445 {
00446     Q_ASSERT(!m_elements.contains(element));
00447     m_elements.append(element);
00448 
00449     connect(element, SIGNAL(posChanged(ArenaElement*,QPoint)),
00450             this, SLOT(slotModified()));
00451     connect(element, SIGNAL(rotationChanged(ArenaElement*,int)),
00452             this, SLOT(slotModified()));
00453     connect(element, SIGNAL(modified(ArenaElement*)),
00454             this, SIGNAL(modified()));
00455 
00456     element->setArena(this);
00457 
00458     emit elementAdded(element);
00459 }
00460 
00461 void Arena::removeElement(ArenaElement *element)
00462 {
00463     Q_ASSERT(m_elements.contains(element));
00464     m_elements.removeAll(element);
00465 
00466     emit elementRemoved(element);
00467 }
00468 
00469 void Arena::clear()
00470 {
00471     foreach (ArenaElement *element, m_elements)
00472     {
00473         m_elements.removeAll(element);
00474         emit elementRemoved(element);
00475     }
00476     m_elements.clear();
00477 
00478     emit modified();
00479 }
00480 
00481 ArenaElement* Arena::addElement(const QString &elementType)
00482 {
00483     ArenaElement *element = m_typeRegistry->instantiateElement(elementType);
00484     if (element)
00485         addElement(element);
00486     return element;
00487 }
00488 
00489 QList<ArenaElement*> Arena::elementsAt(const QPoint &pos) const
00490 {
00491     QList<ArenaElement*> res;
00492     foreach (ArenaElement* e, m_elements)
00493         if (e->pos() == pos)
00494             res.push_back(e);
00495     return res;
00496 }
00497 
00498 ArenaElement* Arena::contextElement(ArenaElement *element) const
00499 {
00500     QList<ArenaElement*> elementsAtPos = elementsAt(element->pos());
00501     foreach (ArenaElement *e, elementsAtPos)
00502         if (e != element &&
00503             e->type()->type() == ArenaElementType::WallType &&
00504             e->rotation() == element->rotation())
00505             return e;
00506     return 0;
00507 }


hector_nist_arena_designer
Author(s): Stefan Kohlbrecher , Johannes Simon
autogenerated on Wed May 15 2019 05:00:31