Menu
▾
▴
[simspark-cvs] simspark/spark/plugin/rosimporter rosimporter.h,1.2,1.3 rosimporter.cpp,1.2,1.3
|
From: Markus R. <rol...@us...> - 2006-02-19 10:44:33
|
Update of /cvsroot/simspark/simspark/spark/plugin/rosimporter In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv23688 Modified Files: rosimporter.h rosimporter.cpp Log Message: - create a capped cylinder as a supplement for the yet unsupported cylinder - added support for the hinge joint Index: rosimporter.cpp =================================================================== RCS file: /cvsroot/simspark/simspark/spark/plugin/rosimporter/rosimporter.cpp,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** rosimporter.cpp 18 Feb 2006 19:54:57 -0000 1.2 --- rosimporter.cpp 19 Feb 2006 10:44:28 -0000 1.3 *************** *** 31,34 **** --- 31,35 ---- #include <oxygen/physicsserver/contactjointhandler.h> #include <oxygen/physicsserver/body.h> + #include <oxygen/physicsserver/hingejoint.h> #include <kerosin/openglserver/glbase.h> #include <kerosin/materialserver/materialsolid.h> *************** *** 46,50 **** using namespace std; ! #define S_FROMSTRING "<from string>"; RosImporter::RosImporter() : SceneImporter() --- 47,54 ---- using namespace std; ! static const string S_FROMSTRING("<from string>"); ! static const string S_BODY("body_"); ! static const string S_GEOM("geometry_"); ! static const string S_VISUAL("visual_"); RosImporter::RosImporter() : SceneImporter() *************** *** 79,83 **** bool RosImporter::ImportScene(const std::string& fileName, ! shared_ptr<BaseNode> root, shared_ptr<ParameterList> parameter) { --- 83,87 ---- bool RosImporter::ImportScene(const std::string& fileName, ! shared_ptr<BaseNode> parent, shared_ptr<ParameterList> parameter) { *************** *** 99,115 **** buffer[file->Size()] = 0; ! return ParseScene(buffer.get(), file->Size(), root, parameter); } bool RosImporter::ParseScene(const std::string& scene, ! shared_ptr<BaseNode> root, shared_ptr<ParameterList> parameter) { mFileName = S_FROMSTRING; ! return ParseScene(scene.c_str(),scene.size(),root,parameter); } bool RosImporter::ParseScene(const char* scene, int size, ! shared_ptr<BaseNode> root, shared_ptr<zeitgeist::ParameterList> parameter) { --- 103,119 ---- buffer[file->Size()] = 0; ! return ParseScene(buffer.get(), file->Size(), parent, parameter); } bool RosImporter::ParseScene(const std::string& scene, ! shared_ptr<BaseNode> parent, shared_ptr<ParameterList> parameter) { mFileName = S_FROMSTRING; ! return ParseScene(scene.c_str(),scene.size(),parent,parameter); } bool RosImporter::ParseScene(const char* scene, int size, ! shared_ptr<BaseNode> parent, shared_ptr<zeitgeist::ParameterList> parameter) { *************** *** 167,171 **** case RosElements::RE_SCENE: ! ok = ReadScene(root, element); } --- 171,175 ---- case RosElements::RE_SCENE: ! ok = ReadScene(parent, element); } *************** *** 341,347 **** } ! bool RosImporter::ReadScene(shared_ptr<BaseNode> root, TiXmlElement* element) { ! if (root.get() == 0) { return false; --- 345,351 ---- } ! bool RosImporter::ReadScene(shared_ptr<BaseNode> parent, TiXmlElement* element) { ! if (parent.get() == 0) { return false; *************** *** 360,374 **** // SimulationParameters stepLength="" standardLength="" applyDynamicsForceFactor="" ! TiXmlElement* elements = GetFirstChild(element, RosElements::RE_ELEMENTS); ! if (elements == 0) { ! // empty scene? ! return true; } ! return ReadElements(root, elements, NC_ELEMENTS); } ! bool RosImporter::ReadElements(shared_ptr<BaseNode> root, TiXmlElement* element, ENodeContext context) { GetLog()->Debug() << "(RosImporter) reading elements node\n"; --- 364,389 ---- // SimulationParameters stepLength="" standardLength="" applyDynamicsForceFactor="" ! return ReadChildElements(parent, element, NC_ELEMENTS); ! } ! ! bool RosImporter::ReadChildElements(shared_ptr<BaseNode> parent, TiXmlElement* element, ENodeContext context) ! { ! for ( ! TiXmlNode* node = GetFirstChild(element, RosElements::RE_ELEMENTS); ! node != 0; ! node = element->IterateChildren(node) ! ) { ! TiXmlElement* childElement = static_cast<TiXmlElement*>(node); ! if (! ReadElements(parent, childElement, context)) ! { ! return false; ! } } ! return true; } ! bool RosImporter::ReadElements(shared_ptr<BaseNode> parent, TiXmlElement* element, ENodeContext context) { GetLog()->Debug() << "(RosImporter) reading elements node\n"; *************** *** 397,422 **** case RosElements::RE_ELEMENTS: ! ok = ReadElements(root,element,context); break; case RosElements::RE_MOVABLE: ! ok = ReadMovable(root,element,context); break; case RosElements::RE_BOX: ! ok = ReadBox(root,element,context); break; case RosElements::RE_SPHERE: ! ok = ReadSphere(root,element,context); break; case RosElements::RE_CYLINDER: ! ok = ReadCylinder(root,element,context); break; case RosElements::RE_CAPPEDCYLINDER: ! ok = ReadCappedCylinder(root,element,context); break; } --- 412,441 ---- case RosElements::RE_ELEMENTS: ! ok = ReadElements(parent,element,context); break; case RosElements::RE_MOVABLE: ! ok = ReadMovable(parent,element,context); break; case RosElements::RE_BOX: ! ok = ReadBox(parent,element,context); break; case RosElements::RE_SPHERE: ! ok = ReadSphere(parent,element,context); break; case RosElements::RE_CYLINDER: ! ok = ReadCylinder(parent,element,context); break; case RosElements::RE_CAPPEDCYLINDER: ! ok = ReadCappedCylinder(parent,element,context); break; + + case RosElements::RE_HINGE: + ok = ReadHinge(parent,element,context); + break; } *************** *** 430,434 **** } ! bool RosImporter::ReadMovable(shared_ptr<BaseNode> root, TiXmlElement* element, ENodeContext context) { GetLog()->Debug() << "(RosImporter) reading moveable node\n"; --- 449,453 ---- } ! bool RosImporter::ReadMovable(shared_ptr<BaseNode> parent, TiXmlElement* element, ENodeContext context) { GetLog()->Debug() << "(RosImporter) reading moveable node\n"; *************** *** 459,463 **** case RosElements::RE_ELEMENTS: ! ok = ReadElements(root,element,context); break; } --- 478,482 ---- case RosElements::RE_ELEMENTS: ! ok = ReadElements(parent,element,context); break; } *************** *** 495,499 **** } ! shared_ptr<Transform> RosImporter::CreateTransform(shared_ptr<BaseNode> root, const Trans& trans) { shared_ptr<Transform> transform = shared_dynamic_cast<Transform> --- 514,518 ---- } ! shared_ptr<Transform> RosImporter::CreateTransform(shared_ptr<BaseNode> parent, const Trans& trans) { shared_ptr<Transform> transform = shared_dynamic_cast<Transform> *************** *** 503,507 **** transform->SetLocalPos(trans.translate); ! root->AddChildReference(transform); return transform; } --- 522,526 ---- transform->SetLocalPos(trans.translate); ! parent->AddChildReference(transform); return transform; } *************** *** 610,640 **** } ! bool RosImporter::ReadCylinder(shared_ptr<BaseNode> root, TiXmlElement* element, ENodeContext context) { ! string name; ! double radius; ! double height; ! Trans trans; ! Appearance appear; ! Physical physical; ! ! if ( ! (! ReadAttribute(element, RA_NAME, name)) || ! (! ReadAttribute(element, RA_RADIUS, radius)) || ! (! ReadAttribute(element, RA_HEIGHT, height)) || ! (! ReadTrans(element, trans)) || ! (! ReadAppearance(element, appear)) || ! (! ReadPhysical(element, physical, context)) ! ) ! { ! return false; ! } ! ! //! todo: Cylinder primitive not yet supported ! GetLog()->Error() << "(RosImporter) Cylinder primitive not supported\n"; ! return true; } ! bool RosImporter::ReadCappedCylinder(shared_ptr<BaseNode> root, TiXmlElement* element, ENodeContext context) { string name; --- 629,639 ---- } ! bool RosImporter::ReadCylinder(shared_ptr<BaseNode> parent, TiXmlElement* element, ENodeContext context) { ! GetLog()->Debug() << "(RosImporter) cylinder geom unsupported yet. Created a capped cylinder geom\n"; ! return ReadCappedCylinder(parent, element, context); } ! bool RosImporter::ReadCappedCylinder(shared_ptr<BaseNode> parent, TiXmlElement* element, ENodeContext context) { string name; *************** *** 658,662 **** // transform ! shared_ptr<Transform> transform = CreateTransform(root, trans); transform->SetName(name); --- 657,661 ---- // transform ! shared_ptr<Transform> transform = CreateTransform(parent, trans); transform->SetName(name); *************** *** 664,670 **** shared_ptr<CCylinder> ccylinder = shared_dynamic_cast<CCylinder> (GetCore()->New("/kerosin/CCylinder")); - transform->AddChildReference(ccylinder); ccylinder->SetParams(radius, height); ccylinder->SetMaterial(appear.ref); --- 663,669 ---- 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); *************** *** 683,687 **** transform->AddChildReference(body); ! body->SetCylinderTotal(physical.mass, radius, height); //! TODO: set mass center break; --- 682,687 ---- transform->AddChildReference(body); ! body->SetName(S_BODY+name); ! body->SetCappedCylinderTotal(physical.mass, radius, height); //! TODO: set mass center break; *************** *** 692,695 **** --- 692,696 ---- shared_ptr<CCylinderCollider> collider = shared_dynamic_cast<CCylinderCollider> (GetCore()->New("/oxygen/CCylinderCollider")); + collider->SetName(S_GEOM+name); transform->AddChildReference(collider); *************** *** 702,709 **** GetLog()->Debug() << "(RosImporter) created capped cylinder " << name << "\n"; ! return true; } ! bool RosImporter::ReadSphere(shared_ptr<BaseNode> root, TiXmlElement* element, ENodeContext context) { string name; --- 703,710 ---- GetLog()->Debug() << "(RosImporter) created capped cylinder " << name << "\n"; ! return ReadChildElements(transform, element, context); } ! bool RosImporter::ReadSphere(shared_ptr<BaseNode> parent, TiXmlElement* element, ENodeContext context) { string name; *************** *** 725,729 **** // transform ! shared_ptr<Transform> transform = CreateTransform(root, trans); transform->SetName(name); --- 726,730 ---- // transform ! shared_ptr<Transform> transform = CreateTransform(parent, trans); transform->SetName(name); *************** *** 734,737 **** --- 735,739 ---- transform->AddChildReference(sphere); + sphere->SetName(S_VISUAL+name); sphere->SetRadius(radius); sphere->SetMaterial(appear.ref); *************** *** 750,753 **** --- 752,756 ---- transform->AddChildReference(body); + body->SetName(S_BODY+name); body->SetSphereTotal(physical.mass, radius); //! TODO: set mass center *************** *** 768,776 **** GetLog()->Debug() << "(RosImporter) created sphere " << name << "\n"; ! ! return true; } ! bool RosImporter::ReadBox(shared_ptr<BaseNode> root, TiXmlElement* element, ENodeContext context) { string name; --- 771,778 ---- GetLog()->Debug() << "(RosImporter) created sphere " << name << "\n"; ! return ReadChildElements(transform, element, context); } ! bool RosImporter::ReadBox(shared_ptr<BaseNode> parent, TiXmlElement* element, ENodeContext context) { string name; *************** *** 796,800 **** // transform ! shared_ptr<Transform> transform = CreateTransform(root, trans); transform->SetName(name); --- 798,802 ---- // transform ! shared_ptr<Transform> transform = CreateTransform(parent, trans); transform->SetName(name); *************** *** 806,809 **** --- 808,812 ---- Vector3f boxDim = Vector3f(length, width, height); + box->SetName(S_VISUAL+name); box->SetExtents(boxDim); box->SetMaterial(appear.ref); *************** *** 822,825 **** --- 825,829 ---- transform->AddChildReference(body); + body->SetName(S_BODY+name); body->SetBoxTotal(physical.mass, boxDim); //! TODO: set mass center *************** *** 833,836 **** --- 837,841 ---- transform->AddChildReference(collider); + collider->SetName(S_GEOM+name); collider->SetBoxLengths(boxDim); *************** *** 840,844 **** GetLog()->Debug() << "(RosImporter) created box " << name << "\n"; ! return true; } --- 845,950 ---- GetLog()->Debug() << "(RosImporter) created box " << name << "\n"; + return ReadChildElements(transform, element, context); + } ! 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, salt::Vector3f& axis) ! { ! TiXmlElement* axisElem = GetFirstChild(element,RosElements::RE_AXIS); ! if (axisElem == 0) ! { ! GetLog()->Error() << "(RosImporter) ERROR: missing axis in " ! << GetXMLPath(element) << "\n"; ! return false; ! } ! ! return ReadVector(axisElem, axis); ! } ! ! shared_ptr<Body> RosImporter::GetJointParentBody(shared_ptr<BaseNode> parent) ! { ! return parent->FindChildSupportingClass<Body>(); ! } ! ! shared_ptr<Body> RosImporter::GetJointChildBody(shared_ptr<BaseNode> parent) ! { ! // find the body created most recently ! Leaf::TLeafList children; ! parent->ListChildrenSupportingClass<Transform>(children,false); ! ! for ( ! Leaf::TLeafList::reverse_iterator iter = children.rbegin(); ! iter != children.rend(); ! ++iter ! ) ! { ! shared_ptr<Body> body = ! (*iter)->FindChildSupportingClass<Body>(false); ! ! if (body.get() != 0) ! { ! return body; ! } ! } ! ! return shared_ptr<Body>(); ! } ! ! bool RosImporter::ReadHinge(shared_ptr<BaseNode> parent, TiXmlElement* element, ENodeContext context) ! { ! string name; ! Vector3f anchor; ! Vector3f axis; ! ! if ( ! (! ReadAttribute(element, RA_NAME, name)) || ! (! ReadAnchorPoint(element, anchor)) || ! (! ReadAxis(element, axis)) ! ) ! { ! return false; ! } ! ! shared_ptr<Body> body1 = GetJointParentBody(parent); ! ! if (! ReadChildElements(parent, element, context)) ! { ! return false; ! } ! ! shared_ptr<Body> body2 = GetJointChildBody(parent); ! ! if ( ! (body1.get() == 0) && ! (body2.get() == 0) ! ) ! { ! GetLog()->Error() << "(RosImporter::ReadHinge) found no bodies to attach hinge to in " ! << GetXMLPath(element) << "\n"; ! return false; ! } ! ! shared_ptr<HingeJoint> hinge = shared_dynamic_cast<HingeJoint> ! (GetCore()->New("/oxygen/HingeJoint")); ! ! parent->AddChildReference(hinge); ! ! hinge->SetName(name); ! hinge->Attach(body1, body2); ! hinge->SetAnchor(anchor); ! hinge->SetAxis(axis); ! ! GetLog()->Debug() << "(RosImporter) created hinge joint " << name << "\n"; } Index: rosimporter.h =================================================================== RCS file: /cvsroot/simspark/simspark/spark/plugin/rosimporter/rosimporter.h,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** rosimporter.h 18 Feb 2006 19:54:57 -0000 1.2 --- rosimporter.h 19 Feb 2006 10:44:28 -0000 1.3 *************** *** 23,26 **** --- 23,27 ---- #define ROSIMPORTER_H + #include <list> #include <oxygen/sceneserver/sceneimporter.h> #include <tinyxml/tinyxml.h> *************** *** 32,35 **** --- 33,37 ---- class Transform; class ContactJointHandler; + class Body; } *************** *** 44,56 **** enum ENodeContext { ! /** the node is read within an <Elements> tag, where no physical bodies are created */ NC_ELEMENTS, ! /** the node is read within an <Movable> tag, where ! physical bodies are created */ ! NC_MOVABLE }; --- 46,58 ---- 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, }; *************** *** 81,85 **** public: Physical() ! : mass(1.0), massCenter(0.0, 0.0, 0.0) {} }; --- 83,87 ---- public: Physical() ! : mass(0.0), massCenter(0.0, 0.0, 0.0) {} }; *************** *** 89,102 **** virtual bool ImportScene(const std::string& fileName, ! boost::shared_ptr<oxygen::BaseNode> root, boost::shared_ptr<zeitgeist::ParameterList> parameter); virtual bool ParseScene(const std::string& scene, ! boost::shared_ptr<oxygen::BaseNode> root, boost::shared_ptr<zeitgeist::ParameterList> parameter); protected: virtual bool ParseScene(const char* scene, int size, ! boost::shared_ptr<oxygen::BaseNode> root, boost::shared_ptr<zeitgeist::ParameterList> parameter); --- 91,104 ---- virtual bool ImportScene(const std::string& fileName, ! boost::shared_ptr<oxygen::BaseNode> parent, boost::shared_ptr<zeitgeist::ParameterList> parameter); 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); *************** *** 109,113 **** boost::shared_ptr<oxygen::Transform> CreateTransform ! (boost::shared_ptr<oxygen::BaseNode> root, const Trans& trans); boost::shared_ptr<oxygen::ContactJointHandler> --- 111,115 ---- boost::shared_ptr<oxygen::Transform> CreateTransform ! (boost::shared_ptr<oxygen::BaseNode> parent, const Trans& trans); boost::shared_ptr<oxygen::ContactJointHandler> *************** *** 122,128 **** bool ReadAppearenceDef(TiXmlElement* element); ! bool ReadScene(boost::shared_ptr<oxygen::BaseNode> root, TiXmlElement* element); ! bool ReadElements(boost::shared_ptr<oxygen::BaseNode> root, TiXmlElement* element, ENodeContext context); ! bool ReadMovable(boost::shared_ptr<oxygen::BaseNode> root, TiXmlElement* element, ENodeContext context); bool ReadTrans(TiXmlElement* element, Trans& trans); --- 124,133 ---- bool ReadAppearenceDef(TiXmlElement* element); ! bool ReadScene(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element); ! ! 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 ReadMovable(boost::shared_ptr<oxygen::BaseNode> parent, TiXmlElement* element, ENodeContext context); bool ReadTrans(TiXmlElement* element, Trans& trans); *************** *** 133,140 **** bool ReadPhysical(TiXmlElement* element, Physical& physical, ENodeContext context); ! bool ReadBox(boost::shared_ptr<oxygen::BaseNode> root, TiXmlElement* element, ENodeContext context); ! bool ReadSphere(boost::shared_ptr<oxygen::BaseNode> root, TiXmlElement* element, ENodeContext context); ! bool ReadCylinder(boost::shared_ptr<oxygen::BaseNode> root, TiXmlElement* element, ENodeContext context); ! bool ReadCappedCylinder(boost::shared_ptr<oxygen::BaseNode> root, TiXmlElement* element, ENodeContext context); protected: --- 138,154 ---- 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, 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); protected: |
