rosimporter.cpp

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

    rgba.r() = (static_cast<float>(r) / 255.0);
    rgba.g() = (static_cast<float>(g) / 255.0);
    rgba.b() = (static_cast<float>(b) / 255.0);

    double a;
    rgba.a() = GetXMLAttribute(element, RA_A, a) ? a : 1.0;

    return true;
}

bool RosImporter::ReadVector(TiXmlElement* element, Vector3f& vec, bool succeedIfMissing)
{
    if (
        (! GetXMLAttribute(element, RA_X, vec[0])) ||
        (! GetXMLAttribute(element, RA_Y, vec[1])) ||
        (! GetXMLAttribute(element, RA_Z, vec[2]))
        )
        {
            if (! succeedIfMissing)
                {
                    string name = S_UNNAMED;
                    ReadAttribute(element, RA_NAME, name, true);

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

    return true;
}

bool RosImporter::ReadAppearenceDef(TiXmlElement* element)
{
    shared_ptr<MaterialServer> materialServer = shared_dynamic_cast<MaterialServer>
        (GetCore()->Get("/sys/server/material"));

    if (materialServer.get() == 0)
        {
            GetLog()->Error() << "(RosImporter) ERROR: failed to lookup MaterialServer node\n";
            return false;
        }

    string name;
    if (! ReadAttribute(element, RA_NAME, name))
        {
            return false;
        }

    RGBA rgba;

    TiXmlElement* colorElem = GetFirstChild(element, RosElements::RE_COLOR);
    if (colorElem == 0)
        {
            // default to a white material
            GetLog()->Debug() << "(RosImporter) missing color attribute in AppearanceDefinition\n";
            rgba = RGBA(1.0,1.0,1.0,1.0);
        }


    if (! ReadRGBA(colorElem, rgba))
        {
            return false;
        }

    shared_ptr<MaterialSolid> material = shared_dynamic_cast<MaterialSolid>
        (GetCore()->New("/kerosin/MaterialSolid"));

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

    material->SetName(name);
    material->SetDiffuse(rgba);

    materialServer->RegisterMaterial(material);
    GetLog()->Debug() << "(RosImporter) defined SolidMaterial " << name << "\n";

    return true;
}

bool RosImporter::ReadAmbientLight(TiXmlElement* element)
{
    RGBA rgba;
    TiXmlElement* colorElem = GetFirstChild(element, RosElements::RE_AMBIENTLIGHTCOLOR);
    if (
        (colorElem == 0) ||
        (! ReadRGBA(colorElem, rgba))
        )
        {
            return false;
        }

    shared_ptr<RenderServer> renderServer = shared_dynamic_cast<RenderServer>
        (GetCore()->Get("/sys/server/render"));

    if (renderServer.get() == 0)
        {
            GetLog()->Error() << "(RosImporter) ERROR: failed to lookup RenderServer node\n";
            return false;
        }

    renderServer->SetAmbientColor(rgba);
    return true;
}

bool RosImporter::ReadScene(shared_ptr<BaseNode> parent, TiXmlElement* element)
{
    RosContextScope scope(this);
    if (parent.get() == 0)
        {
            return false;
        }

    GetLog()->Debug() << "(RosImporter) reading scene node\n";

    ReadDefaultAppearance(element);
    ReadGlobalPhsyParams(element);
    ReadAmbientLight(element);

    // TODO:
    // Background surface="..."
    // GlobalPhysicalParameters gravity="" erp="" cfm="" defaultRollingFrictionCoefficient=""
    // SimulationParameters stepLength="" standardLength="" applyDynamicsForceFactor=""

    return ReadChildElements(parent, element);
}

bool RosImporter::ReadChildElements(shared_ptr<BaseNode> parent, TiXmlElement* element)
{
    for (
         TiXmlNode* node = GetFirstChild(element, RosElements::RE_ELEMENTS);
         node != 0;
         node = element->IterateChildren(node)
         )
        {
            if (IgnoreNode(node))
                {
                    continue;
                }

            TiXmlElement* childElement = static_cast<TiXmlElement*>(node);
            if (! ReadElements(parent, childElement))
                {
                    return false;
                }
        }

    return true;
}

bool RosImporter::ReadCompound(shared_ptr<BaseNode> parent, TiXmlElement* element)
{
    string name;
    Trans trans;

    if (
        (! ReadAttribute(element, RA_NAME, name, true)) ||
        (! ReadTrans(element, trans))
        )
        {
            return false;
        }

    // transform
    shared_ptr<Transform> contextTransform = GetContextTransform(parent,trans);
    contextTransform->SetName(name);

    GetLog()->Debug() << "(RosImporter) read compound node " << name << "\n";
    return ReadChildElements(contextTransform, element);
}

bool RosImporter::ReadElements(shared_ptr<BaseNode> parent, TiXmlElement* element)
{
    GetLog()->Debug() << "(RosImporter) reading elements node\n";

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

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

            RosElements::ERosElement type = GetType(element);
            switch (type)
                {
                default:
                    // treat unknown tags like a <element> tag
                    GetLog()->Error() << "(RosImporter::ReadElements) ERROR: skipping unknown element '"
                                      << GetXMLValue(element) << "' "
                                      << GetXMLPath(element) << "\n";
                    ok = ReadElements(parent, element);
                    break;

                case RosElements::RE_ELEMENTS:
                    ok = ReadElements(parent,element);
                    break;

                case RosElements::RE_COMPOUND:
                    ok = ReadCompound(parent,element);
                    break;

                case RosElements::RE_MOVABLE:
                    ok = ReadMovable(parent,element);
                    break;

                case RosElements::RE_BOX:
                    ok = ReadBox(parent,element);
                    break;

                case RosElements::RE_SPHERE:
                    ok = ReadSphere(parent,element);
                    break;

                case RosElements::RE_CYLINDER:
                    ok = ReadCylinder(parent,element);
                    break;

                case RosElements::RE_CAPPEDCYLINDER:
                    ok = ReadCappedCylinder(parent,element);
                    break;

                case RosElements::RE_UNIVERSAL:
                    ok = ReadUniversal(parent,element);
                    break;

                case RosElements::RE_HINGE:
                    ok = ReadHinge(parent,element);
                    break;

                case RosElements::RE_SLIDER:
                    ok = ReadSlider(parent, element);
                    break;

                case RosElements::RE_USE:
                    ok = ReadUse(parent,element);
                    break;

                case RosElements::RE_COMPLEXSHAPE:
                    ok = ReadComplexShape(parent,element);
                    break;
                }

            if (! ok)
                {
                    return false;
                }
        }

    return true;
}

bool RosImporter::ReadMovable(shared_ptr<BaseNode> parent, TiXmlElement* element)
{
    RosContextScope scope(this);
    GetContext().movable = true;

    GetLog()->Debug() << "(RosImporter) reading moveable node\n";

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

            TiXmlElement* element = static_cast<TiXmlElement*>(node);
            if (element == 0)
                {
                    continue;
                }

            bool ok = true;

            RosElements::ERosElement type = GetType(element);
            switch (type)
                {
                default:
                    // treat unknown tags like a <element> tag
                    ok = ReadElements(parent,element);
                    break;

                case RosElements::RE_ELEMENTS:
                    ok = ReadElements(parent,element);
                    break;
                }

            if (! ok)
                {
                    return false;
                }
        }

    return true;
}

bool RosImporter::ReadTrans(TiXmlElement* element, Trans& trans)
{
    trans.matrix.Identity();

    TiXmlElement* transElem = GetFirstChild(element,RosElements::RE_TRANSLATION);
    if (transElem != 0)
        {
            Vector3f vec;
            if (! ReadVector(transElem, vec))
                {
                    return false;
                }

            trans.matrix.Translate(vec);
        }

    TiXmlElement* rotElem = GetFirstChild(element,RosElements::RE_ROTATION);
    if (rotElem != 0)
        {
            Vector3f rot;
            if (! ReadVector(rotElem, rot))
                {
                    return false;
                }

            trans.matrix.RotateX(gDegToRad(rot[0]));
            trans.matrix.RotateY(gDegToRad(rot[1]));
            trans.matrix.RotateZ(gDegToRad(rot[2]));
        }

    return true;
}

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

    parent->AddChildReference(transCollider);

    transCollider->SetRotation(trans.matrix);
    transCollider->SetPosition(trans.matrix.Pos());

    return transCollider;
}

shared_ptr<ContactJointHandler> RosImporter::CreateContactJointHandler()
{
    shared_ptr<ContactJointHandler> handler = shared_dynamic_cast<ContactJointHandler>
        (GetCore()->New("/oxygen/ContactJointHandler"));

    handler->SetContactSoftERPMode(true);
    handler->SetContactSoftERP(mGlobalERP);
    handler->SetContactSoftCFMMode(true);
    handler->SetContactSoftCFM(mGlobalCFM);

    return handler;
}


bool RosImporter::ReadGlobalPhsyParams(TiXmlElement* element)
{
    // prsetset defaults
    mGlobalERP = 0.2;
    mGlobalCFM = 0.0001;

    /** TODO: figure out how RoSim interprets the gravity value (a
        single value is given, not a vector)
    */
    double gravity = 1.0;

    TiXmlElement* physElem = GetFirstChild(element, RosElements::RE_GLOBALPHYSICALPARAMETERS);
    if (physElem == 0)
        {
            return true;
        }

    ReadAttribute(physElem, RA_GRAVITY, gravity, true);
    ReadAttribute(physElem, RA_ERP, mGlobalERP, true);
    ReadAttribute(physElem, RA_CFM, mGlobalCFM, true);

    return true;
}

bool RosImporter::ReadDefaultAppearance(TiXmlElement* element)
{
    TiXmlElement* appearElem = GetFirstChild(element,RosElements::RE_DEFAULTAPPEARANCE);
    if (appearElem != 0)
        {
            return ReadAttribute(appearElem, RA_REF, mDefaultAppearance.ref);
        }

    // fall back to predefined MaterialServer default material
    mDefaultAppearance.ref = "default";
    return true;
}

bool RosImporter::ReadAppearance(TiXmlElement* element, Appearance& appear)
{
    TiXmlElement* appearElem = GetFirstChild(element,RosElements::RE_APPEARANCE);
    if (appearElem != 0)
        {
            return ReadAttribute(appearElem, RA_REF, appear.ref);
        }

    appear = mDefaultAppearance;
    return true;
}

bool RosImporter::ReadPhysical(TiXmlElement* element, Physical& physical)
{
    physical.valid = false;

    TiXmlElement* physElem = GetFirstChild(element,RosElements::RE_PHYSICALATTRIBUTES);
    if (physElem == 0)
        {
            return true;
        }

    // check for mass
    TiXmlElement* massElem = GetFirstChild(physElem, RosElements::RE_MASS);
    if (
        (massElem != 0) &&
        (! ReadAttribute(massElem, RA_VALUE, physical.mass))
        )
        {
            return false;
        }

    // a mass value is enough for a valid physical body description
    physical.valid = true;

    // check if a collider is present
    int canCollide = 0;
    if (GetXMLAttribute(element, RA_CANCOLLIDE, canCollide))
        {
            physical.canCollide = (canCollide == 1);
        }

    // check for mass center
    TiXmlElement* centerElem = GetFirstChild(physElem,RosElements::RE_CENTEROFMASS);
    if (
        (centerElem != 0) &&
        (! ReadVector(centerElem, physical.massCenter))
        )
        {
            return false;
        }

    // TODO: read friction def

    return true;
}

bool RosImporter::ReadCylinder(shared_ptr<BaseNode> parent, TiXmlElement* element)
{
    GetLog()->Debug() << "(RosImporter) cylinder geom unsupported yet. Created a capped cylinder geom\n";
    return ReadCappedCylinder(parent, element);
}

bool RosImporter::ReadCappedCylinder(shared_ptr<BaseNode> parent, TiXmlElement* element)
{
    string name;
    double radius;
    double height;
    Trans trans;
    Appearance appear;
    Physical physical;

    if (
        (! ReadAttribute(element, RA_NAME, name, true)) ||
        (! ReadAttribute(element, RA_RADIUS, radius)) ||
        (! ReadAttribute(element, RA_HEIGHT, height)) ||
        (! ReadTrans(element, trans)) ||
        (! ReadAppearance(element, appear)) ||
        (! ReadPhysical(element, physical))
        )
        {
            return false;
        }

    // transform
    shared_ptr<Transform> contextTransform = GetContextTransform(parent, Trans());
    shared_ptr<Transform> transform = CreateTransform(contextTransform, trans);
    transform->SetName(name);

    // visual
    shared_ptr<CCylinder> ccylinder = shared_dynamic_cast<CCylinder>
        (GetCore()->New("/kerosin/CCylinder"));
    transform->AddChildReference(ccylinder);

    ccylinder->SetName(S_VISUAL+name);
    ccylinder->SetParams(radius, height);
    ccylinder->SetMaterial(appear.ref);

    // physical
    shared_ptr<Body> body = GetContextBody(transform);
    if (body.get() != 0)
        {
            body->SetName(S_BODY+name);
            body->SetCappedCylinderTotal(physical.mass, radius, height);
            GetContext().AddMass(physical.mass, Trans());
        }

    if (physical.canCollide)
        {
            // geometry
            shared_ptr<CCylinderCollider> collider = shared_dynamic_cast<CCylinderCollider>
                (GetCore()->New("/oxygen/CCylinderCollider"));

            transform->AddChildReference(collider);
            collider->SetName(S_GEOM+name);
            collider->SetParams(radius, height);

            // collision handler
            shared_ptr<ContactJointHandler> handler = CreateContactJointHandler();
            collider->AddChildReference(handler);
        }

    GetLog()->Debug() << "(RosImporter) created capped cylinder " << name << "\n";
    return ReadChildElements(transform, element);
}