📄 rosimporter.cpp
字号:
bool RosImporter::ReadSphere(shared_ptr<BaseNode> parent, TiXmlElement* element){ string name; double radius; Trans trans; Appearance appear; Physical physical; if ( (! ReadAttribute(element, RA_NAME, name, true)) || (! ReadAttribute(element, RA_RADIUS, radius)) || (! 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<Sphere> sphere = shared_dynamic_cast<Sphere> (GetCore()->New("/kerosin/Sphere")); transform->AddChildReference(sphere); sphere->SetName(S_VISUAL+name); sphere->SetRadius(radius); sphere->SetMaterial(appear.ref); shared_ptr<Body> body = GetContextBody(transform); if (body.get() != 0) { body->SetName(S_BODY+name); body->SetSphereTotal(physical.mass, radius); GetContext().AddMass(physical.mass, Trans()); } // geometry shared_ptr<SphereCollider> collider = shared_dynamic_cast<SphereCollider> (GetCore()->New("/oxygen/SphereCollider")); transform->AddChildReference(collider); collider->SetRadius(radius); if (physical.canCollide) { // collision handler shared_ptr<ContactJointHandler> handler = CreateContactJointHandler(); collider->AddChildReference(handler); } GetLog()->Debug() << "(RosImporter) created sphere " << name << "\n"; return ReadChildElements(transform, element);}bool RosImporter::ReadBox(shared_ptr<BaseNode> parent, TiXmlElement* element){ string name; double length; double width; double height; Trans trans; Appearance appear; Physical physical; if ( (! ReadAttribute(element, RA_NAME, name, true)) || (! ReadAttribute(element, RA_LENGTH, length)) || (! ReadAttribute(element, RA_WIDTH, width)) || (! 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<Box> box = shared_dynamic_cast<Box> (GetCore()->New("/kerosin/Box")); transform->AddChildReference(box); Vector3f boxDim = Vector3f(length, width, height); box->SetName(S_VISUAL+name); box->SetExtents(boxDim); box->SetMaterial(appear.ref); shared_ptr<Body> body = GetContextBody(transform); if (body.get() != 0) { body->SetName(S_BODY+name); body->SetBoxTotal(physical.mass, boxDim); GetContext().AddMass(physical.mass, Trans()); } if (physical.canCollide) { // geometry shared_ptr<BoxCollider> collider = shared_dynamic_cast<BoxCollider> (GetCore()->New("/oxygen/BoxCollider")); transform->AddChildReference(collider); collider->SetName(S_GEOM+name); collider->SetBoxLengths(boxDim); // collision handler shared_ptr<ContactJointHandler> handler = CreateContactJointHandler(); collider->AddChildReference(handler); } GetLog()->Debug() << "(RosImporter) created box " << name << "\n"; return ReadChildElements(transform, element);}bool RosImporter::ReadAnchorPoint(TiXmlElement* element, Vector3f& anchor){ TiXmlElement* anchorElem = GetFirstChild(element,RosElements::RE_ANCHORPOINT); if (anchorElem == 0) { GetLog()->Error() << "(RosImporter) ERROR: missing anchorpoint in " << GetXMLPath(element) << "\n"; return false; } return ReadVector(anchorElem, anchor);}bool RosImporter::ReadAxis(TiXmlElement* element, RosElements::ERosElement type, JointAxis& axis){ TiXmlElement* axisElem = GetFirstChild(element,type); if (axisElem == 0) { GetLog()->Error() << "(RosImporter) ERROR: missing axis in " << GetXMLPath(element) << "\n"; return false; } if (! ReadVector(axisElem, axis.dir)) { return false; } TiXmlElement* deflectElem = GetFirstChild(axisElem, RosElements::RE_DEFLECTION); if (deflectElem != 0) { double loStop; double hiStop; if ( (! GetXMLAttribute(deflectElem, RA_MIN, loStop)) || (! GetXMLAttribute(deflectElem, RA_MAX, hiStop)) ) { GetLog()->Error() << "(RosImporter) ERROR: invalid axis deflection in " << GetXMLPath(element) << "\n"; return false; } axis.setDeflection = true; axis.loStop = gDegToRad(loStop); axis.hiStop = gDegToRad(hiStop); } return true;}shared_ptr<Body> RosImporter::GetJointParentBody(){ if (mStack.size() < 2) { GetLog()->Debug() << "RosImporter::GetJointParentBody cannot get joint parent body with stack size of " << mStack.size() << "\n"; return shared_ptr<Body>(); } TRosStack::reverse_iterator iter = mStack.rbegin(); ++iter; for (; iter != mStack.rend(); ++iter ) { RosContext& context = (*iter); shared_ptr<Body> body = context.body; if (body.get() != 0) { GetLog()->Debug() << "RosImporter::GetJointParentBody found " << body->GetFullPath() << "\n"; return body; } } GetLog()->Debug() << "RosImporter::GetJointParentBody not found\n"; return shared_ptr<Body>();}shared_ptr<Body> RosImporter::GetJointChildBody(shared_ptr<BaseNode> parent){ if (parent.get() == 0) { return shared_ptr<Body>(); } for (Leaf::TLeafList::iterator iter = parent->begin();iter != parent->end();++iter) { shared_ptr<Body> body = shared_dynamic_cast<Body>(*iter); if (body.get() == 0) { continue; } GetLog()->Debug() << "RosImporter::GetJointParentBody found " << body->GetFullPath() << "\n"; return body; } for (Leaf::TLeafList::iterator iter = parent->begin();iter != parent->end();++iter) { shared_ptr<BaseNode> node = shared_dynamic_cast<BaseNode>(*iter); if (node.get() == 0) { continue; } shared_ptr<Body> body = GetJointChildBody(node); if (body.get() != 0) { return body; } } return shared_ptr<Body>();}bool RosImporter::ReadUniversal(shared_ptr<BaseNode> parent, TiXmlElement* element){ RosContextScope scope(this); GetContext().movable = true; RosJointScope jointScope(this); string name; Vector3f anchor; JointAxis axis1; JointAxis axis2; if ( (! ReadAttribute(element, RA_NAME, name, true)) || (! ReadAnchorPoint(element, anchor)) || (! ReadAxis(element, RosElements::RE_AXIS1, axis1)) || (! ReadAxis(element, RosElements::RE_AXIS2, axis2)) ) { return false; } // transform Trans anchorTrans; anchorTrans.matrix.Translate(anchor); shared_ptr<Transform> contextTransform = GetContextTransform(parent, anchorTrans); // joint shared_ptr<UniversalJoint> universal = shared_dynamic_cast<UniversalJoint> (GetCore()->New("/oxygen/UniversalJoint")); contextTransform->AddChildReference(universal); if (! ReadChildElements(universal, element)) { return false; } shared_ptr<Body> body1 = GetJointParentBody(); shared_ptr<Body> body2 = GetJointContext().body; if ( (body1.get() == 0) || (body2.get() == 0) ) { GetLog()->Error() << "(RosImporter::ReadUniversal) found no bodies to attach hinge to in " << GetXMLPath(element) << " named " << name << "\n"; return false; } universal->SetName(name); Attach(universal, body1, body2, axis1, axis2); return true;}bool RosImporter::ReadHinge(shared_ptr<BaseNode> parent, TiXmlElement* element){ RosContextScope scope(this); GetContext().movable = true; RosJointScope jointScope(this); string name; Vector3f anchor; JointAxis axis; if ( (! ReadAttribute(element, RA_NAME, name, true)) || (! ReadAnchorPoint(element, anchor)) || (! ReadAxis(element, RosElements::RE_AXIS, axis)) ) { return false; } // transform Trans anchorTrans; anchorTrans.matrix.Translate(anchor); shared_ptr<Transform> contextTransform = GetContextTransform(parent, anchorTrans); // joint shared_ptr<HingeJoint> hinge = shared_dynamic_cast<HingeJoint> (GetCore()->New("/oxygen/HingeJoint")); contextTransform->AddChildReference(hinge); if (! ReadChildElements(hinge, element)) { return false; } shared_ptr<Body> body1 = GetJointParentBody(); shared_ptr<Body> body2 = GetJointContext().body; if ( (body1.get() == 0) || (body2.get() == 0) ) { GetLog()->Error() << "(RosImporter::ReadHinge) found no bodies to attach hinge to in " << GetXMLPath(element) << " named " << name << "\n"; return false; } hinge->SetName(name); Attach(hinge, body1, body2, axis); GetLog()->Debug() << "(RosImporter) created hinge joint " << name << "\n"; return true;}bool RosImporter::ReadSlider(shared_ptr<BaseNode> parent, TiXmlElement* element){ RosContextScope scope(this); GetContext().movable = true; RosJointScope jointScope(this); string name; JointAxis axis; if ( (! ReadAttribute(element, RA_NAME, name, true)) || (! ReadAxis(element, RosElements::RE_AXIS, axis)) ) { return false; } // joint shared_ptr<SliderJoint> slider = shared_dynamic_cast<SliderJoint> (GetCore()->New("/oxygen/SliderJoint")); parent->AddChildReference(slider); if (! ReadChildElements(slider, element)) { return false; } shared_ptr<Body> body1 = GetJointParentBody(); shared_ptr<Body> body2 = GetJointContext().body; if ( (body1.get() == 0) && (body2.get() == 0) ) { GetLog()->Error() << "(RosImporter::ReadHinge) found no bodies to attach hinge to in " << GetXMLPath(element) << " named " << name << "\n"; return false; } slider->SetName(name); Attach(slider, body1, body2, axis); GetLog()->Debug() << "(RosImporter) created hinge joint " << name << "\n"; return true;}bool RosImporter::ReadMacro(shared_ptr<BaseNode> parent, TiXmlElement* element){ string name; if (! ReadAttribute(element, RA_NAME, name)) { return false; } shared_ptr<TiXmlElement> macro(new TiXmlElement(*element)); mMacroMap[name] = macro; GetLog()->Debug() << "(RosImporter) defined macro " << name << "\n"; return true;}bool RosImporter::ReadUse(shared_ptr<BaseNode> parent, TiXmlElement* element){ string name; string instance; Trans trans; if ( (! ReadAttribute(element, RA_MACRONAME, name)) || (! ReadAttribute(element, RA_INSTANCENAME, instance, true)) || (! ReadTrans(element, trans)) ) { return false; } TMacroMap::const_iterator iter = mMacroMap.find(name); if (iter == mMacroMap.end()) { GetLog()->Error() << "(RosImporter) use of undefined macro " << name << " in " << GetXMLPath(element) << "\n"; return false; } // todo: apply Translation/Rotation // todo: <SubstituteSurfaces> // todo: <SubstituteAllSurfaces> if (instance.empty()) { instance = name; } GetLog()->Debug() << "(RosImporter) START instancing macro " << name << " as instance " << instance << "\n"; shared_ptr<TiXmlElement> tree = (*iter).second; bool ok = ReadElements(parent, tree.get()); GetLog()->Debug() << "(RosImporter) END instancing macro " << name << "\n"; return ok;}bool RosImporter::ReadVertexList(TiXmlElement* element){ string listName; if (! ReadAttribute(element, RA_NAME, listName)) { return false; } mVertexListMap[listName] = TVertexList(); // work with a reference in to the map to avoid an expensive copy TVertexList& vertices = mVertexListMap[listName]; for ( TiXmlNode* node = GetFirstChild(element, RosElements::RE_VERTEX); node != 0; node = element->IterateChildren(node) ) { if (IgnoreNode(node)) { continue; } TiXmlElement* element = static_cast<TiXmlElement*>(node); RosElements::ERosElement type = GetType(element); switch (type) { default: GetLog()->Error() << "(RosImporter::ReadVertices) ERROR: skipping unknown element " << GetXMLPath(element) << "\n"; break; case RosElements::RE_VERTEX: { TVertex vertex; string name; if ( (! ReadAttribute(element, RA_NAME, name)) || (! ReadVector(element, vertex.vec)) ) { return false; } vertices.AddVertex(name, vertex); break; } } } GetLog()->Debug() << "(RosImporter) read vertex list " << listName << "\n"; return true;}bool RosImporter::ReadComplexShape(shared_ptr<BaseNode> parent, TiXmlElement* element){ shared_ptr<GeometryServer> geometryServer = shared_dynamic_cast<GeometryServer> (GetCore()->Get("/sys/server/geometry")); if (geometryServer.get() == 0) { GetLog()->Error() << "(RosImporter) ERROR: failed to lookup GeometryServer node\n"; return false; } string name; Trans trans; Appearance appear; Physical physical;⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -