arenaelement.cpp
Go to the documentation of this file.
00001 #include "arena.h"
00002 #include "arenaelement.h"
00003 #include "arenaelementtype.h"
00004 #include "xmlloadingexception.h"
00005 
00006 #include <QList>
00007 #include <QDebug>
00008 
00009 #include <math.h>
00010 
00011 ArenaElement::ArenaElement(const ArenaElementType * const type, int id)
00012     : m_type(type)
00013     , m_rotation(0)
00014     , m_instanceId(id)
00015     , m_itemMountPoint(-1)
00016 {
00017 }
00018 
00019 bool ArenaElement::isWall() const
00020 {
00021     return m_type->type() == ArenaElementType::WallType;
00022 }
00023 
00024 bool ArenaElement::isFloor() const
00025 {
00026     return m_type->type() == ArenaElementType::FloorType;
00027 }
00028 
00029 bool ArenaElement::isMountableItem() const
00030 {
00031     return m_type->type() == ArenaElementType::MountableItemType;
00032 }
00033 
00034 bool ArenaElement::isItem() const
00035 {
00036     return m_type->type() == ArenaElementType::ItemType;
00037 }
00038 
00039 void ArenaElement::setPos(QPoint pos)
00040 {
00041     // Do not ignore if pos is same as before! We always have to emit posChanged().
00042     //if (pos == m_pos)
00043     //    return;
00044     m_pos = pos;
00045     emit posChanged(this, pos);
00046     emit modified(this);
00047 }
00048 
00049 void ArenaElement::setItemOffset(const QPointF &offset)
00050 {
00051     m_itemOffset = offset;
00052     emit posChanged(this, m_pos);
00053 }
00054 
00055 void ArenaElement::setRotation(int rotation)
00056 {
00057     // Do not ignore if rotation is same as before! We always have to emit rotationChanged().
00058     //if (rotation == m_rotation)
00059     //    return;
00060     // normalize also negative number (-2 % 3 == -2 but should be 1)
00061     // (-2 % 3 + 3) % 3 == (-2 + 3) % 3 == 1
00062     int normalizedRotation = (rotation % 360 + 360) % 360;
00063     m_rotation = normalizedRotation;
00064     emit rotationChanged(this, normalizedRotation);
00065     emit modified(this);
00066 }
00067 
00068 void ArenaElement::setItemMountPoint(int mountPoint)
00069 {
00070     m_itemMountPoint = mountPoint;
00071     emit modified(this);
00072 }
00073 
00074 void ArenaElement::load(const QDomElement& node) throw (XmlLoadingException)
00075 {
00076     if (!node.hasAttribute("x"))
00077         throw XmlLoadingException("An arena element is missing an x coordinate.", node);
00078     if (!node.hasAttribute("y"))
00079         throw XmlLoadingException("An arena element is missing a y coordinate.", node);
00080 
00081     QPoint pos;
00082     pos.setX(node.attribute("x").toInt());
00083     pos.setY(node.attribute("y").toInt());
00084     setPos(pos);
00085 
00086     if (node.hasAttribute("offset-x") && node.hasAttribute("offset-y"))
00087     {
00088         QString offsetX = node.attribute("offset-x");
00089         QString offsetY = node.attribute("offset-y");
00090 
00091         QPointF offset = QPointF(offsetX.toFloat(), offsetY.toFloat());
00092         setItemOffset(offset);
00093     }
00094 
00095     if (node.hasAttribute("rotation"))
00096     {
00097         QString rotation = node.attribute("rotation");
00098         setRotation(rotation.toInt());
00099     }
00100 
00101     if (node.hasAttribute("mount-point"))
00102     {
00103         QString mountPointIndex = node.attribute("mount-point");
00104         setItemMountPoint(mountPointIndex.toInt());
00105     }
00106 }
00107 
00108 void ArenaElement::save(QXmlStreamWriter& writer)
00109 {
00110     writer.writeStartElement("element");
00111     writer.writeAttribute("type", m_type->name());
00112     writer.writeAttribute("rotation", QString::number(rotation()));
00113     writer.writeAttribute("x", QString::number(m_pos.x()));
00114     writer.writeAttribute("y", QString::number(m_pos.y()));
00115 
00116     if (!m_itemOffset.isNull())
00117     {
00118         writer.writeAttribute("offset-x", QString::number(m_itemOffset.x()));
00119         writer.writeAttribute("offset-y", QString::number(m_itemOffset.y()));
00120     }
00121     if (m_itemMountPoint != -1)
00122         writer.writeAttribute("mount-point", QString::number(m_itemMountPoint));
00123     writer.writeEndElement();
00124 }
00125 
00126 void ArenaElement::saveWorld(QXmlStreamWriter& writer)
00127 {
00128     writer.writeStartElement("model:physical");
00129     QString name;
00130     QTextStream(&name) << m_type->name() << "_" << m_instanceId;
00131     writer.writeAttribute("name", name);
00132 
00133     ArenaElement *context = m_arena->contextElement(this);
00134     QList<ItemMountPoint> contextItemMountPoints;
00135     if (context)
00136         contextItemMountPoints = context->type()->itemMountPoints();
00137 
00138     // Use "corrected" mount point index
00139     int itemMountPoint = m_itemMountPoint;
00140     if (!isMountableItem() || !context)
00141     {
00142         itemMountPoint = -1;
00143     }
00144     else
00145     {
00146         // When there are possible mount points, use the first if none is assigned
00147         itemMountPoint = qMax(itemMountPoint, 0);
00148         // But make sure the index doesn't exceed the last mount point
00149         if (contextItemMountPoints.count() <= itemMountPoint)
00150             itemMountPoint = -1;
00151     }
00152 
00153     qreal offsetX = 0;
00154     qreal offsetZ = 0;
00155     if (itemMountPoint >= 0)
00156     {
00157         Q_ASSERT(context);
00158         Q_ASSERT(contextItemMountPoints.count() > itemMountPoint);
00159         ItemMountPoint mountPoint = contextItemMountPoints[itemMountPoint];
00160         offsetX = mountPoint.second.x() - 0.6;
00161         offsetZ = mountPoint.second.y();
00162     }
00163 
00164     QTransform trans;
00165     trans.rotate(-m_rotation);
00166     QPointF mappedOffset = trans.map(QPointF(offsetX, 0));
00167     mappedOffset += m_itemOffset * 1.2;
00168 
00169     QString xyz;
00170     // One unit is 1.2 meters, gazebo is set up to think in meters
00171     QTextStream(&xyz) << m_pos.x() * 1.2 + mappedOffset.x() << " "
00172                       << m_pos.y() * 1.2 + mappedOffset.y() << " " << offsetZ;
00173     writer.writeTextElement("xyz", xyz);
00174     QString rpy;
00175     QTextStream(&rpy) << "0 0 " << -m_rotation;
00176     writer.writeTextElement("rpy", rpy);
00177     writer.writeTextElement("static", "true");
00178 
00179     writer.writeStartElement("body:trimesh");
00180     writer.writeAttribute("name", name + "_body");
00181 
00182     writer.writeStartElement("geom:trimesh");
00183     writer.writeAttribute("name", name + "_geom");
00184 
00185     writer.writeTextElement("mesh", m_type->mesh());
00186     writer.writeTextElement("scale", "1.0 1.0 1.0");
00188     writer.writeTextElement("genTexCoord", "true");
00189 
00190     writer.writeStartElement("visual");
00191     writer.writeTextElement("scale", "1.0 1.0 1.0");
00192     writer.writeTextElement("rpy", "0 0 0");
00193     writer.writeTextElement("mesh", m_type->mesh());
00194     //writer.writeTextElement("material", "Gazebo/WoodPallet");
00195     // visual
00196     writer.writeEndElement();
00197 
00198     // geom:trimesh
00199     writer.writeEndElement();
00200 
00201     // body:trimesh
00202     writer.writeEndElement();
00203 
00204     // model:physical
00205     writer.writeEndElement();
00206 }
00207 
00208 void ArenaElement::saveWorldSdf(QXmlStreamWriter& writer)
00209 {
00210     writer.writeStartElement("model");
00211     QString name;
00212     QTextStream(&name) << m_type->name() << "_" << m_instanceId;
00213     writer.writeAttribute("name", name);
00214     writer.writeTextElement("static", "true");
00215 
00216     ArenaElement *context = m_arena->contextElement(this);
00217     QList<ItemMountPoint> contextItemMountPoints;
00218     if (context)
00219         contextItemMountPoints = context->type()->itemMountPoints();
00220 
00221     // Use "corrected" mount point index
00222     int itemMountPoint = m_itemMountPoint;
00223     if (!isMountableItem() || !context)
00224     {
00225         itemMountPoint = -1;
00226     }
00227     else
00228     {
00229         // When there are possible mount points, use the first if none is assigned
00230         itemMountPoint = qMax(itemMountPoint, 0);
00231         // But make sure the index doesn't exceed the last mount point
00232         if (contextItemMountPoints.count() <= itemMountPoint)
00233             itemMountPoint = -1;
00234     }
00235 
00236     qreal offsetX = 0;
00237     qreal offsetZ = 0;
00238     if (itemMountPoint >= 0)
00239     {
00240         Q_ASSERT(context);
00241         Q_ASSERT(contextItemMountPoints.count() > itemMountPoint);
00242         ItemMountPoint mountPoint = contextItemMountPoints[itemMountPoint];
00243         offsetX = mountPoint.second.x() - 0.6;
00244         offsetZ = mountPoint.second.y();
00245     }
00246 
00247     QTransform trans;
00248     trans.rotate(-m_rotation);
00249     QPointF mappedOffset = trans.map(QPointF(offsetX, 0));
00250     mappedOffset += m_itemOffset * 1.2;
00251 
00252     QString xyz;
00253     // One unit is 1.2 meters, gazebo is set up to think in meters
00254     QTextStream(&xyz) << m_pos.x() * 1.2 + mappedOffset.x() << " "
00255                       << m_pos.y() * 1.2 + mappedOffset.y() << " " << offsetZ;
00256 
00257     QString rpy;
00258     QTextStream(&rpy) << "0 0 " << -(m_rotation/180.0)*M_PI;
00259     //writer.writeTextElement("rpy", rpy);
00260 
00261     writer.writeStartElement("link");
00262     writer.writeAttribute("name",name+"_link");
00263 
00264     writer.writeTextElement("pose", xyz + " " + rpy);
00265 
00266     writer.writeStartElement("collision");
00267     writer.writeAttribute("name",name+"_collision");
00268 
00269     writer.writeStartElement("geometry");
00270 
00271     writer.writeStartElement("mesh");
00272     writer.writeTextElement("uri", "file://" + m_type->mesh());
00273     writer.writeTextElement("scale", "1 1 1");
00274     writer.writeEndElement(); // mesh
00275 
00276     writer.writeEndElement(); // geometry
00277 
00278     writer.writeEndElement(); // collision
00279 
00280     writer.writeStartElement("visual");
00281     writer.writeAttribute("name",name+"_visual");
00282     writer.writeTextElement("cast_shadows","false");
00283 
00284     writer.writeStartElement("geometry");
00285 
00286     writer.writeStartElement("mesh");
00287     writer.writeTextElement("uri", "file://" + m_type->mesh());
00288     writer.writeTextElement("scale", "1 1 1");
00289     writer.writeEndElement();
00290 
00291     writer.writeEndElement(); // geometry
00292 
00293     writer.writeEndElement(); // visual
00294 
00295     writer.writeEndElement(); // link
00296 
00297     writer.writeEndElement(); // model
00298 
00299 }
00300 
00301 
00302 
00303 
00304 
00305 
00306 
00307 
00308 


hector_nist_arena_designer
Author(s): Stefan Kohlbrecher , Johannes Simon
autogenerated on Wed Oct 4 2017 03:29:29