|
From: Markus R. <rol...@us...> - 2007-06-13 13:24:32
|
Update of /cvsroot/simspark/simspark/spark/plugin/rosimporter In directory sc8-pr-cvs8.sourceforge.net:/tmp/cvs-serv26292 Modified Files: Tag: ROSIMPORTER_XLAB rosimporter.cpp rosimporter.h Log Message: - roughly working version of RosImporter plugin Index: rosimporter.cpp =================================================================== RCS file: /cvsroot/simspark/simspark/spark/plugin/rosimporter/rosimporter.cpp,v retrieving revision 1.10.4.1 retrieving revision 1.10.4.2 diff -C2 -d -r1.10.4.1 -r1.10.4.2 *** rosimporter.cpp 20 May 2007 16:31:53 -0000 1.10.4.1 --- rosimporter.cpp 13 Jun 2007 13:24:29 -0000 1.10.4.2 *************** *** 33,36 **** --- 33,37 ---- #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> *************** *** 55,59 **** static const string S_FROMSTRING("<from string>"); static const string S_BODY("body_"); [...1960 lines suppressed...] + if (ja.axis2.setDeflection) + { + universal->SetParameter(dParamLoStop2, ja.axis2.loStop); + universal->SetParameter(dParamHiStop2, ja.axis2.hiStop); + universal->SetParameter(dParamLoStop2, ja.axis2.loStop); + } + + return; + } + + shared_ptr<SliderJoint> slider = shared_dynamic_cast<SliderJoint>(ja.joint); + if (slider.get() != 0) + { + // slider axis is set via parent transform node + // slider->SetAxis(ja.axis1); + return; + } + + assert(false); + } Index: rosimporter.h =================================================================== RCS file: /cvsroot/simspark/simspark/spark/plugin/rosimporter/rosimporter.h,v retrieving revision 1.8.8.1 retrieving revision 1.8.8.2 diff -C2 -d -r1.8.8.1 -r1.8.8.2 *** rosimporter.h 20 May 2007 16:31:53 -0000 1.8.8.1 --- rosimporter.h 13 Jun 2007 13:24:29 -0000 1.8.8.2 *************** *** 39,42 **** --- 39,43 ---- class Body; class Transform; + class Joint; } *************** *** 49,65 **** { public: - enum ENodeContext - { - /** the node is read within a <Elements> tag, where no - physical bodies are created - */ - NC_ELEMENTS, - - /** the node is read within a <Movable> tag or a joint - node, where physical bodies are created - */ - NC_MOVABLE, - }; - struct Trans { --- 50,53 ---- *************** *** 74,77 **** --- 62,154 ---- }; + /** JointAxis describes a single joint axis along with associated + joint parameters + */ + struct JointAxis + { + public: + salt::Vector3f dir; + + //! true, iff loStop and hiStop angle are valid + bool setDeflection; + + //! low stop angle (rad) + double loStop; + + //! high stop angle (rad) + double hiStop; + + public: + JointAxis() + : dir(0.0,0.0,0.0), setDeflection(false), + loStop(0.0), hiStop(0.0) + { + } + }; + + /** JointAttach describes how a joint connects two bodies along + with the associated axis parameters + */ + struct JointAttach + { + public: + boost::shared_ptr<oxygen::Joint> joint; + boost::shared_ptr<oxygen::Body> body1; + boost::shared_ptr<oxygen::Body> body2; + JointAxis axis1; + JointAxis axis2; + + public: + JointAttach() + { + } + }; + + /** RosContext refers to the current Transform parent node that is + used to construct associated visual, geometric and physical + nodes. It further describes mass and mass center of composite + bodies. + */ + struct RosContext + { + public: + boost::shared_ptr<oxygen::Transform> transform; + boost::shared_ptr<oxygen::Body> body; + bool adjustedPos; + salt::Vector3f massCenter; + double totalMass; + bool movable; + + public: + RosContext() + : adjustedPos(false), massCenter(0.0,0.0,0.0), + totalMass(0.0), movable(false) + { + } + + /** moves the Transform parent node in order to put the Body + into the mass center of a composite body + */ + void AdjustPos(); + + /** accumulates mass and adjusts the mass center */ + void AddMass(double mass, const Trans& trans); + }; + + /** declare a stack of RosContext nodes */ + typedef std::vector<RosContext > TRosStack; + + /** RosJointContext holds the child Body node that is connected to + the Body refered to in the current RosContext + */ + struct RosJointContext + { + boost::shared_ptr<oxygen::Body> body; + }; + + /** declare a stack of RosContext nodes */ + typedef std::vector<RosJointContext> TRosJointStack; + + /** Appearance holds a material definition */ struct Appearance { *************** *** 80,83 **** --- 157,163 ---- }; + /** Physical holds physical properties of the current + RosContext. + */ struct Physical { *************** *** 85,97 **** bool valid; double mass; salt::Vector3f massCenter; public: Physical() ! : valid(false), mass(0.0), massCenter(0.0, 0.0, 0.0) {} }; /** define a registry of macros; a macro is a XML subtree with the ! <Macro> node a the root element */ typedef std::map<std::string, boost::shared_ptr<TiXmlElement> > TMacroMap; --- 165,179 ---- bool valid; double mass; + bool canCollide; salt::Vector3f massCenter; public: Physical() ! : valid(false), mass(0.0), ! canCollide(true), massCenter(0.0, 0.0, 0.0) {} }; /** define a registry of macros; a macro is a XML subtree with the ! <Macro> node as the root element */ typedef std::map<std::string, boost::shared_ptr<TiXmlElement> > TMacroMap; *************** *** 103,109 **** salt::Vector3f vec; ! //! the index into an associated flat vertex array as used in ! //the TriMesh class int idx; public: TVertex() : idx(-1) {} --- 185,193 ---- salt::Vector3f vec; ! /** the index into an associated flat vertex array as used in ! the TriMesh class ! */ int idx; + public: TVertex() : idx(-1) {} *************** *** 152,155 **** --- 236,240 ---- }; + /** define a complex geom and it's associated vertices */ struct ComplexGeom { *************** *** 174,184 **** virtual bool ParseScene(const std::string& scene, ! boost::shared_ptr<oxygen::BaseNode> parent, ! boost::shared_ptr<zeitgeist::ParameterList> parameter); protected: virtual bool ParseScene(const char* scene, int size, ! boost::shared_ptr<oxygen::BaseNode> parent, ! boost::shared_ptr<zeitgeist::ParameterList> parameter); --- 259,280 ---- virtual bool ParseScene(const std::string& scene, ! boost::shared_ptr<oxygen::BaseNode> parent, ! boost::shared_ptr<zeitgeist::ParameterList> parameter); protected: + void PushContext(); + void PopContext(); + void PushJointContext(); + void PopJointContext(); + RosContext& GetContext(); + RosJointContext& RosImporter::GetJointContext(); + boost::shared_ptr<oxygen::Transform> GetContextTransform(boost::shared_ptr<oxygen::BaseNode> parent, const Trans& trans); + boost::shared_ptr<oxygen::Transform> CreateTransform(boost::shared_ptr<oxygen::BaseNode> parent, const Trans& trans); + boost::shared_ptr<oxygen::Body> GetContextBody(boost::shared_ptr<oxygen::BaseNode> parent); + void SetJointBody(boost::shared_ptr<oxygen::Body> body); + virtual bool ParseScene(const char* scene, int size, ! boost::shared_ptr<oxygen::BaseNode> parent, ! boost::shared_ptr<zeitgeist::ParameterList> parameter); *************** *** 187,191 **** bool IgnoreNode(const TiXmlNode* node) const; - bool HasBody(const Physical& physical, ENodeContext context); RosElements::ERosElement GetType(const TiXmlElement* element) const; --- 283,286 ---- *************** *** 194,200 **** void ApplyTransform(boost::shared_ptr<oxygen::Transform> transform, const Trans& trans); - boost::shared_ptr<oxygen::Transform> CreateTransform - (boost::shared_ptr<oxygen::BaseNode> parent, const Trans& trans); - boost::shared_ptr<oxygen::TransformCollider> CreateTransformCollider (boost::shared_ptr<oxygen::BaseNode> parent, const Trans& trans); --- 289,292 ---- *************** *** 214,224 **** bool ReadMacro(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element); ! bool ReadUse(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element, ENodeContext context); ! bool ReadChildElements(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element, ENodeContext context); ! bool ReadElements(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element, ENodeContext context); ! bool ReadCompound(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element, ENodeContext context); ! bool ReadMovable(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element, ENodeContext context); bool ReadTrans(TiXmlElement* element, Trans& trans); --- 306,316 ---- bool ReadMacro(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element); ! bool ReadUse(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element); ! bool ReadChildElements(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element); ! bool ReadElements(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element); ! bool ReadCompound(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element); ! bool ReadMovable(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element); bool ReadTrans(TiXmlElement* element, Trans& trans); *************** *** 227,248 **** bool ReadAppearance(TiXmlElement* element, Appearance& appear); ! bool ReadPhysical(TiXmlElement* element, Physical& physical, ENodeContext context); ! bool ReadBox(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element, ENodeContext context); ! bool ReadSphere(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element, ENodeContext context); ! bool ReadCylinder(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element, ENodeContext context); ! bool ReadCappedCylinder(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element, ENodeContext context); bool ReadAnchorPoint(TiXmlElement* element, salt::Vector3f& anchor); ! bool ReadAxis(TiXmlElement* element, RosElements::ERosElement type, salt::Vector3f& axis); ! boost::shared_ptr<oxygen::Body> RosImporter::GetJointParentBody(boost::shared_ptr<oxygen::BaseNode> parent); boost::shared_ptr<oxygen::Body> RosImporter::GetJointChildBody(boost::shared_ptr<oxygen::BaseNode> parent); ! bool ReadHinge(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element, ENodeContext context); ! bool ReadUniversal(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element, ENodeContext context); bool ReadVertexList(TiXmlElement* element); ! bool ReadComplexShape(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element, ENodeContext context); bool ReadGraphicalRep(TiXmlElement* element, boost::shared_ptr<oxygen::TriMesh> mesh, const Appearance& appear); bool ReadComplexElements(TiXmlElement* element, TComplexGeomList& geomList); --- 319,341 ---- bool ReadAppearance(TiXmlElement* element, Appearance& appear); ! bool ReadPhysical(TiXmlElement* element, Physical& physical); ! bool ReadBox(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element); ! bool ReadSphere(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element); ! bool ReadCylinder(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element); ! bool ReadCappedCylinder(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element); bool ReadAnchorPoint(TiXmlElement* element, salt::Vector3f& anchor); ! bool ReadAxis(TiXmlElement* element, RosElements::ERosElement type, JointAxis& axis); ! boost::shared_ptr<oxygen::Body> RosImporter::GetJointParentBody(); boost::shared_ptr<oxygen::Body> RosImporter::GetJointChildBody(boost::shared_ptr<oxygen::BaseNode> parent); ! bool ReadHinge(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element); ! bool ReadSlider(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element); ! bool ReadUniversal(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element); bool ReadVertexList(TiXmlElement* element); ! bool ReadComplexShape(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element); bool ReadGraphicalRep(TiXmlElement* element, boost::shared_ptr<oxygen::TriMesh> mesh, const Appearance& appear); bool ReadComplexElements(TiXmlElement* element, TComplexGeomList& geomList); *************** *** 251,263 **** void BuildPolygon(oxygen::IndexBuffer& ibuffer, TVertexList& vertexList, const ComplexGeom& geom); ! bool ReadPhysicalRep(boost::shared_ptr<oxygen::Transform> transform, TiXmlElement* element, ENodeContext context); ! bool ReadSimpleBox(boost::shared_ptr<oxygen::Transform> transform, boost::shared_ptr<oxygen::Body> body, ! TiXmlElement* element, ENodeContext context); ! bool ReadSimpleSphere(boost::shared_ptr<oxygen::Transform> transform, boost::shared_ptr<oxygen::Body> body, ! TiXmlElement* element, ENodeContext context); ! bool ReadSimpleCappedCylinder(boost::shared_ptr<oxygen::Transform> transform, boost::shared_ptr<oxygen::Body> body, ! TiXmlElement* element, ENodeContext context); protected: /** the last supplied fileName */ std::string mFileName; --- 344,404 ---- void BuildPolygon(oxygen::IndexBuffer& ibuffer, TVertexList& vertexList, const ComplexGeom& geom); ! bool ReadPhysicalRep(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element); ! bool ReadSimpleBox(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element); ! bool ReadSimpleSphere(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element); ! bool ReadSimpleCappedCylinder(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element); ! ! void Attach(boost::shared_ptr<oxygen::Joint> joint, boost::shared_ptr<oxygen::Body> body1, boost::shared_ptr<oxygen::Body> body2, ! const JointAxis& axis1, const JointAxis& axis2 = JointAxis()); ! void AttachJoint(const JointAttach& ja); ! ! private: ! /** RosContextScope is a helper class that creates a new ! RosContext and destroys it when it goes ot ouf scope ! */ ! struct RosContextScope ! { ! public: ! RosContextScope(RosImporter* i) ! : importer(i) ! { ! importer->PushContext(); ! } ! ! ~RosContextScope() ! { ! importer->PopContext(); ! } ! ! protected: ! RosImporter* importer; ! }; ! ! /** RosJointScope is a helper class that creates a new joint scope ! and destroys it when it goes out of scope ! */ ! struct RosJointScope ! { ! RosJointScope(RosImporter* i) ! : importer(i) ! { ! importer->PushJointContext(); ! } ! ! ~RosJointScope() ! { ! importer->PopJointContext(); ! } ! ! protected: ! RosImporter* importer; ! }; protected: + /** reference to the parent node under wich the imported scene is + constructed + */ + boost::shared_ptr<oxygen::BaseNode> mSceneParent; + /** the last supplied fileName */ std::string mFileName; *************** *** 277,282 **** /** the static macro registry is shared across RosImporter instances ! */ static TMacroMap mMacroMap; }; --- 418,429 ---- /** the static macro registry is shared across RosImporter instances ! */ static TMacroMap mMacroMap; + + /** the stack of RosContext instances */ + TRosStack mStack; + + /* the stack of JointContext instances */ + TRosJointStack mJointStack; }; |