rosimporter.cpp

rcssserver3d Robocup 3D比赛官方指定平台

/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
   $Id: rosimporter.cpp,v 1.2 2008/02/27 18:06:53 rollmark Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "rosimporter.h"
#include <tinyxml/xmlfunctions.h>
#include <zeitgeist/logserver/logserver.h>
#include <zeitgeist/fileserver/fileserver.h>
#include <oxygen/sceneserver/basenode.h>
#include <oxygen/sceneserver/transform.h>
#include <oxygen/physicsserver/transformcollider.h>
#include <oxygen/physicsserver/boxcollider.h>
#include <oxygen/physicsserver/spherecollider.h>
#include <oxygen/physicsserver/ccylindercollider.h>
#include <oxygen/physicsserver/contactjointhandler.h>
#include <oxygen/physicsserver/body.h>
#include <oxygen/physicsserver/hingejoint.h>
#include <oxygen/physicsserver/sliderjoint.h>
#include <oxygen/physicsserver/universaljoint.h>
#include <oxygen/geometryserver/geometryserver.h>
#include <oxygen/geometryserver/trimesh.h>
#include <kerosin/openglserver/glbase.h>
#include <kerosin/materialserver/materialserver.h>
#include <kerosin/materialserver/materialsolid.h>
#include <kerosin/sceneserver/box.h>
#include <kerosin/sceneserver/sphere.h>
#include <kerosin/sceneserver/ccylinder.h>
#include <kerosin/sceneserver/staticmesh.h>
#include <kerosin/renderserver/renderserver.h>
#include <boost/scoped_array.hpp>

using namespace salt;
using namespace zeitgeist;
using namespace kerosin;
using namespace oxygen;
using namespace boost;
using namespace std;

static const string S_FROMSTRING("<from string>");
static const string S_BODY("body_");
static const string S_GEOMTRANS("geomtrans_");
static const string S_GEOM("geometry_");
static const string S_VISUAL("visual_");
static const string S_MACRO("macro_");
static const string S_UNNAMED("<unnamed>");

#pragma warning(disable: 4244)

// --- RosImporter::TVertexList

boost::shared_array<float> RosImporter::TVertexList::GetPos()
{
    if (pos.get() != 0)
        {
            return pos;
        }

    pos = shared_array<float> (new float[vertexRef.size() * 3]);

    int i=0;
    for (
         TVertexRef::iterator iter = vertexRef.begin();
         iter != vertexRef.end();
         ++iter
         )
        {
            TVertex& vertex = (*iter).second;
            vertex.idx = i;

            pos[(i*3)+0] = vertex.vec[0];
            pos[(i*3)+1] = vertex.vec[1];
            pos[(i*3)+2] = vertex.vec[2];

            ++i;
        }

    return pos;
}

int RosImporter::TVertexList::GetIndex(const std::string& name)
{
    TVertexRef::const_iterator iter = vertexRef.find(name);
    if (iter == vertexRef.end())
        {
            return -1;
        }

    return (*iter).second.idx;
}


void RosImporter::TVertexList::AddVertex(const std::string& name, const TVertex& vertex)
{
    pos.reset();
    vertexRef[name] = vertex;
}

// --- RosImporter

RosImporter::TMacroMap RosImporter::mMacroMap;


RosImporter::RosImporter() : SceneImporter()
{
    mGlobalERP = 0.2;
    mGlobalCFM = 0.0001;
}

RosImporter::~RosImporter()
{
}

RosElements::ERosElement RosImporter::GetType(const TiXmlElement* element) const
{
    return RosElements::GetInstance().Lookup(GetXMLValue(element));
}

string RosImporter::GetName(RosElements::ERosElement element) const
{
    return RosElements::GetInstance().Lookup(element);
}

TiXmlElement* RosImporter::GetFirstChild(TiXmlNode* node, RosElements::ERosElement type)
{
    return ::GetFirstChild(node, GetName(type));
}

TiXmlElement* RosImporter::IterateChildren(TiXmlNode* node, RosElements::ERosElement type)
{
    return ::IterateChildren(node, GetName(type));
}

bool RosImporter::ImportScene(const string& fileName,
                              shared_ptr<BaseNode> parent,
                              shared_ptr<ParameterList> parameter)
{
    // try to open the file
    shared_ptr<salt::RFile> file = GetFile()->OpenResource(fileName);

    if (file.get() == 0)
        {
            GetLog()->Error() << "(RosImporter) ERROR: cannot open file '"
                              << fileName << "'\n";
            return false;
        }

    mFileName = fileName;
    mSceneParent = parent;

    // read entire file into a temporary buffer
    scoped_array<char> buffer(new char[file->Size() + 1]);
    file->Read(buffer.get(), file->Size());
    buffer[file->Size()] = 0;

    return ParseScene(buffer.get(), file->Size(), parent, parameter);
}

bool RosImporter::ParseScene(const string& scene,
                             shared_ptr<BaseNode> parent,
                             shared_ptr<ParameterList> parameter)
{
    mFileName = S_FROMSTRING;
    return ParseScene(scene.c_str(),static_cast<int>(scene.size()),parent,parameter);
}

bool RosImporter::IgnoreNode(const TiXmlNode* node) const
{
    if (node == 0)
        {
            return true;
        }

    switch(node->Type())
        {
        case TiXmlNode::ELEMENT:
            return false;

        default:
            /** ignore declarations, comments and other
                unknown node types
            */
            return true;
        }
}

void RosImporter::PushContext()
{
    RosContext context;
    context.movable = (mStack.empty()) ?
        false : GetContext().movable;

    mStack.push_back(context);
}

void RosImporter::PopContext()
{
    RosContext& context = mStack.back();
    context.AdjustPos();
    mStack.pop_back();
}

void RosImporter::PushJointContext()
{
    mJointStack.push_back(RosJointContext());
}

void RosImporter::PopJointContext()
{
    mJointStack.pop_back();
}

void RosImporter::RosContext::AdjustPos()
{
    if (adjustedPos)
        {
            return;
        }

    adjustedPos = true;
    if (body.get() == 0)
        {
            return;
        }

    // adjust for the mass center
    shared_ptr<Transform> transform = shared_dynamic_cast<Transform>
        (body->GetParent().lock());

    if (transform.get() == 0)
        {
            assert(false);
            return;
        }

    massCenter /= totalMass;
    transform->SetLocalPos(transform->GetLocalPos() + massCenter);
    body->SetPosition(body->GetPosition() + massCenter);
}

void RosImporter::RosContext::AddMass(double mass, const Trans& trans)
{
    massCenter += trans.matrix.Pos() * mass;
    totalMass += mass;
}

RosImporter::RosContext& RosImporter::GetContext()
{
    return mStack.back();
}

RosImporter::RosJointContext& RosImporter::GetJointContext()
{
    return mJointStack.back();
}

void RosImporter::ApplyTransform(shared_ptr<Transform> transform, const Trans& trans)
{
    transform->SetLocalTransform(trans.matrix);
}

shared_ptr<Transform> RosImporter::CreateTransform(shared_ptr<BaseNode> parent, const Trans& trans)
{
    shared_ptr<Transform> transform = shared_dynamic_cast<Transform>
        (GetCore()->New("/oxygen/Transform"));

    ApplyTransform(transform, trans);
    parent->AddChildReference(transform);

    Vector3f pos = transform->GetWorldTransform().Pos();

    return transform;
}

shared_ptr<Transform> RosImporter::GetContextTransform(shared_ptr<BaseNode> parent, const Trans& trans)
{
    RosContext& context = GetContext();
    if (context.transform.get() != 0)
        {
            return context.transform;
        }

    assert(parent.get() != 0);

    Leaf::TLeafList parentList;
    shared_ptr<Leaf> current(parent);
    while (current.get() != 0)
        {
            if (current.get() == mSceneParent.get())
                {
                    break;
                }
            shared_ptr<Transform> transform = shared_dynamic_cast<Transform>(current);
            if (transform.get() != 0)
                {
                    parentList.push_back(transform);
                }

            current = current->GetParent().lock();
        }

    Trans globalTrans;

    for (
         Leaf::TLeafList::reverse_iterator iter =  parentList.rbegin();
         iter != parentList.rend();
         ++iter
         )
        {
            shared_ptr<Transform> transform
                = shared_static_cast<Transform>(*iter);

            globalTrans.matrix = globalTrans.matrix * transform->GetLocalTransform();
        }

    globalTrans.matrix = globalTrans.matrix * trans.matrix;

    context.transform = CreateTransform(mSceneParent, globalTrans);
    return context.transform;
}

void RosImporter::SetJointBody(shared_ptr<Body> body)
{
    if (mJointStack.empty())
        {
            return;
        }

    RosJointContext& context = GetJointContext();
    if (context.body.get() == 0)
        {
            context.body = body;
        }
}

shared_ptr<Body> RosImporter::GetContextBody(shared_ptr<BaseNode> parent)
{
    RosContext& context = GetContext();
    if (
        (! context.movable) ||
        (parent.get() == 0)
        )
        {
            return shared_ptr<Body>();
        }

    if (context.body.get() != 0)
        {
            return context.body;
        }

    if (context.transform.get() == 0)
        {
            assert(false);
            return shared_ptr<Body>();
        }

    context.body = shared_dynamic_cast<Body>
        (GetCore()->New("/oxygen/Body"));


    SetJointBody(context.body);

    parent->AddChildReference(context.body);

    return context.body;
}

bool RosImporter::ParseScene(const char* scene, int size,
                             shared_ptr<BaseNode> parent,
                             shared_ptr<zeitgeist::ParameterList> parameter)
{
    TiXmlDocument document;
    document.Parse(scene);

    if (document.Error())
        {
            GetLog()->Error() << "(RosImporter) ERROR: xml parsing error: "
                              << document.ErrorDesc() << "\n";
            return false;
        }

    TiXmlElement* xmlRoot = document.RootElement();
    if (xmlRoot == 0)
        {
            GetLog()->Error() << "(RosImporter) ERROR: empty xml document\n";
            return false;
        }

    if (
        (GetType(xmlRoot) != RosElements::RE_SIMULATION) &&
        (GetType(xmlRoot) != RosElements::RE_ROSIINCLUDEFILE)
        )
        {
            GetLog()->Error() << "(RosImporter) ERROR: unknown xml root element type "
                              << GetXMLValue(xmlRoot) << "\n";
            return false;
        }

    for (
         TiXmlNode* node = xmlRoot->FirstChild();
         node != 0;
         node = xmlRoot->IterateChildren(node)
         )
        {
            if (IgnoreNode(node))
                {
                    continue;
                }

            TiXmlElement* element = static_cast<TiXmlElement*>(node);
            bool ok = true;

            RosElements::ERosElement type = GetType(element);
            switch (type)
                {
                default:
                    GetLog()->Error() << "(RosImporter::ParseScene) ERROR: skipping unknown toplevel element "
                                      << GetXMLPath(element) << "\n";
                    break;

                case RosElements::RE_APPEARANCEDEFINITION:
                    ok = ReadAppearenceDef(element);
                    break;

                case RosElements::RE_SCENE:
                    ok = ReadScene(parent, element);
                    break;

                case RosElements::RE_MACRO:
                    ok = ReadMacro(parent, element);
                    break;

                case RosElements::RE_VERTEXLIST:
                    ok = ReadVertexList(element);
                    break;
                }

            if (! ok)
                {
                    break;
                }

        }

    return true;
}

bool RosImporter::ReadAttribute(TiXmlElement* element, const string& attribute,
                                string& value, bool succeedIfMissing)
{
    if (element == 0)
        {
            return false;
        }

    if (! GetXMLAttribute(element, attribute, value))
        {
            if (! succeedIfMissing)
                {
                    string name = S_UNNAMED;
                    ReadAttribute(element, RA_NAME, name, true);

                    GetLog()->Error() << "(RosImporter) ERROR: missing string attribute " << attribute << " in "
                                      << GetXMLPath(element) << " name " << name << " \n";
                    return false;
                }
        }

    return true;
}

bool RosImporter::ReadAttribute(TiXmlElement* element, const string& attribute,
                                double& value, bool succeedIfMissing)
{
    if (element == 0)
        {
            return false;
        }

    if (! GetXMLAttribute(element, attribute, value))
        {
            if (! succeedIfMissing)
                {
                    string name = S_UNNAMED;
                    ReadAttribute(element, RA_NAME, name, true);

                    GetLog()->Error() << "(RosImporter) ERROR: missing float attribute " << attribute << " in "
                                      << GetXMLPath(element) << " name " << name << "\n";
                    return false;
                }
        }

    return true;
}


bool RosImporter::ReadRGBA(TiXmlElement* element, RGBA& rgba)
{
    int r,g,b;
    if (
        (! GetXMLAttribute(element, RA_R, r)) ||
        (! GetXMLAttribute(element, RA_G, g)) ||
        (! GetXMLAttribute(element, RA_B, b))
        )
        {
            string name = S_UNNAMED;
            ReadAttribute(element, RA_NAME, name, true);

            GetLog()->Error() << "(RosImporter) ERROR: missing color attributes in "
                              << GetXMLPath(element) << " name " << name << "\n";
            return false;
        }