hector_nist_arena_designer: hector_nist_arena_designer: arena.cpp Source 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 
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(); //geometry
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(); //ode
00379 
00380     writer.writeEndElement(); //friction
00381 
00382     writer.writeStartElement("bounce");
00383     writer.writeAttribute("restitution_coefficient","0");
00384     writer.writeAttribute("threshold","1000000.0");
00385     writer.writeEndElement(); //bounce
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(); //ode
00397 
00398     writer.writeEndElement(); //contact
00399 
00400     writer.writeEndElement(); //surface
00401     writer.writeEndElement(); //collision
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(); //geometry
00415 
00416     writer.writeStartElement("material");
00417     writer.writeAttribute("script","Gazebo/Grey");
00418     writer.writeEndElement();
00419 
00420 
00421 
00422 
00423     //writer.writeEndElement();
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 }