Menu
▾
▴
[simspark-cvs] simspark/spark/plugin/rosimporter roselements.cpp, 1.5, 1.5.8.1 roselements.h, 1.5, 1.5.8.1 rosimporter.cpp, 1.10, 1.10.4.1 rosimporter.h, 1.8, 1.8.8.1
|
From: Markus R. <rol...@us...> - 2007-05-20 16:32:15
|
Update of /cvsroot/simspark/simspark/spark/plugin/rosimporter In directory sc8-pr-cvs8.sourceforge.net:/tmp/cvs-serv18827/plugin/rosimporter Modified Files: Tag: ROSIMPORTER_XLAB roselements.cpp roselements.h rosimporter.cpp rosimporter.h Log Message: Index: roselements.h =================================================================== RCS file: /cvsroot/simspark/simspark/spark/plugin/rosimporter/roselements.h,v retrieving revision 1.5 retrieving revision 1.5.8.1 diff -C2 -d -r1.5 -r1.5.8.1 *** roselements.h 23 Feb 2006 13:40:23 -0000 1.5 --- roselements.h 20 May 2007 16:31:53 -0000 1.5.8.1 *************** *** 108,113 **** --- 108,116 ---- RE_HINGE, + RE_UNIVERSAL, RE_ANCHORPOINT, RE_AXIS, + RE_AXIS1, + RE_AXIS2, RE_GLOBALPHYSICALPARAMETERS, Index: roselements.cpp =================================================================== RCS file: /cvsroot/simspark/simspark/spark/plugin/rosimporter/roselements.cpp,v retrieving revision 1.5 retrieving revision 1.5.8.1 diff -C2 -d -r1.5 -r1.5.8.1 *** roselements.cpp 23 Feb 2006 13:40:23 -0000 1.5 --- roselements.cpp 20 May 2007 16:31:53 -0000 1.5.8.1 *************** *** 80,85 **** --- 80,88 ---- ROS_DEFINE_ELEMENT(RE_HINGE,"Hinge"); + ROS_DEFINE_ELEMENT(RE_UNIVERSAL,"UniversalJoint"); ROS_DEFINE_ELEMENT(RE_ANCHORPOINT,"AnchorPoint"); ROS_DEFINE_ELEMENT(RE_AXIS,"Axis"); + ROS_DEFINE_ELEMENT(RE_AXIS1,"Axis1"); + ROS_DEFINE_ELEMENT(RE_AXIS2,"Axis2"); ROS_DEFINE_ELEMENT(RE_GLOBALPHYSICALPARAMETERS,"GlobalPhysicalParameters"); Index: rosimporter.cpp =================================================================== RCS file: /cvsroot/simspark/simspark/spark/plugin/rosimporter/rosimporter.cpp,v retrieving revision 1.10 retrieving revision 1.10.4.1 diff -C2 -d -r1.10 -r1.10.4.1 *** rosimporter.cpp 15 Mar 2007 07:26:28 -0000 1.10 --- rosimporter.cpp 20 May 2007 16:31:53 -0000 1.10.4.1 *************** *** 26,29 **** --- 26,30 ---- #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> *************** *** 32,35 **** --- 33,37 ---- #include <oxygen/physicsserver/body.h> #include <oxygen/physicsserver/hingejoint.h> + #include <oxygen/physicsserver/universaljoint.h> #include <oxygen/geometryserver/geometryserver.h> #include <oxygen/geometryserver/trimesh.h> *************** *** 53,56 **** --- 55,59 ---- 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_"); *************** *** 58,61 **** --- 61,66 ---- static const string S_UNNAMED("<unnamed>"); + #pragma warning(disable: 4244) + // ------------------- *************** *** 171,175 **** { mFileName = S_FROMSTRING; ! return ParseScene(scene.c_str(),scene.size(),parent,parameter); } --- 176,180 ---- { mFileName = S_FROMSTRING; ! return ParseScene(scene.c_str(),static_cast<int>(scene.size()),parent,parameter); } *************** *** 543,550 **** --- 548,558 ---- { default: + #if 0 // treat unknown tags like a <element> tag GetLog()->Error() << "(RosImporter::ReadElements) ERROR: skipping unknown element " << GetXMLPath(element) << "\n"; ok = ReadElements(parent, element, context); + #endif + ok = true; break; *************** *** 577,583 **** break; ! case RosElements::RE_HINGE: ! ok = ReadHinge(parent,element,context); ! break; case RosElements::RE_USE: --- 585,595 ---- break; ! case RosElements::RE_UNIVERSAL: ! ok = ReadUniversal(parent,element,context); ! break; ! ! case RosElements::RE_HINGE: ! ok = ReadHinge(parent,element,context); ! break; case RosElements::RE_USE: *************** *** 648,655 **** bool RosImporter::ReadTrans(TiXmlElement* element, Trans& trans) { TiXmlElement* transElem = GetFirstChild(element,RosElements::RE_TRANSLATION); if (transElem != 0) { ! if (! ReadVector(transElem, trans.translate)) { return false; --- 660,669 ---- bool RosImporter::ReadTrans(TiXmlElement* element, Trans& trans) { + trans.matrix.Identity(); + TiXmlElement* transElem = GetFirstChild(element,RosElements::RE_TRANSLATION); if (transElem != 0) { ! if (! ReadVector(transElem, trans.matrix.Pos())) { return false; *************** *** 660,667 **** if (rotElem != 0) { ! if (! ReadVector(rotElem, trans.rotate)) { return false; } } --- 674,686 ---- 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])); } *************** *** 671,676 **** void RosImporter::ApplyTransform(shared_ptr<Transform> transform, const Trans& trans) { ! transform->SetLocalRotationDeg(trans.rotate); ! transform->SetLocalPos(trans.translate); } --- 690,694 ---- void RosImporter::ApplyTransform(shared_ptr<Transform> transform, const Trans& trans) { ! transform->SetLocalTransform(trans.matrix); } *************** *** 686,691 **** --- 704,723 ---- } + 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() { + #if 0 shared_ptr<ContactJointHandler> handler = shared_dynamic_cast<ContactJointHandler> (GetCore()->New("/oxygen/ContactJointHandler")); *************** *** 697,700 **** --- 729,734 ---- return handler; + #endif + return shared_ptr<ContactJointHandler>(); } *************** *** 720,723 **** --- 754,759 ---- ReadAttribute(physElem, RA_ERP, mGlobalERP, true); ReadAttribute(physElem, RA_CFM, mGlobalCFM, true); + + return true; } *************** *** 998,1004 **** } ! bool RosImporter::ReadAxis(TiXmlElement* element, salt::Vector3f& axis) { ! TiXmlElement* axisElem = GetFirstChild(element,RosElements::RE_AXIS); if (axisElem == 0) { --- 1034,1040 ---- } ! bool RosImporter::ReadAxis(TiXmlElement* element, RosElements::ERosElement type, salt::Vector3f& axis) { ! TiXmlElement* axisElem = GetFirstChild(element,type); if (axisElem == 0) { *************** *** 1046,1050 **** { shared_ptr<Body> body = ! (*iter)->FindChildSupportingClass<Body>(false); if (body.get() != 0) --- 1082,1086 ---- { shared_ptr<Body> body = ! (*iter)->FindChildSupportingClass<Body>(true); if (body.get() != 0) *************** *** 1059,1072 **** } ! bool RosImporter::ReadHinge(shared_ptr<BaseNode> parent, TiXmlElement* element, ENodeContext context) { string name; Vector3f anchor; ! Vector3f axis; if ( (! ReadAttribute(element, RA_NAME, name, true)) || (! ReadAnchorPoint(element, anchor)) || ! (! ReadAxis(element, axis)) ) { --- 1095,1110 ---- } ! bool RosImporter::ReadUniversal(shared_ptr<BaseNode> parent, TiXmlElement* element, ENodeContext context) { string name; Vector3f anchor; ! Vector3f axis1; ! Vector3f axis2; if ( (! ReadAttribute(element, RA_NAME, name, true)) || (! ReadAnchorPoint(element, anchor)) || ! (! ReadAxis(element, RosElements::RE_AXIS1, axis1)) || ! (! ReadAxis(element, RosElements::RE_AXIS2, axis2)) ) { *************** *** 1076,1087 **** shared_ptr<Body> body1 = GetJointParentBody(parent); ! //! Todo: figure out if all objects below a joint node are ! //implicitly movable, i.e. are created with a Body node ! if (! ReadChildElements(parent, element, NC_MOVABLE)) { return false; } ! shared_ptr<Body> body2 = GetJointChildBody(parent); if ( --- 1114,1128 ---- shared_ptr<Body> body1 = GetJointParentBody(parent); ! shared_ptr<UniversalJoint> universal = shared_dynamic_cast<UniversalJoint> ! (GetCore()->New("/oxygen/UniversalJoint")); ! ! parent->AddChildReference(universal); ! ! if (! ReadChildElements(universal, element, NC_MOVABLE)) { return false; } ! shared_ptr<Body> body2 = GetJointChildBody(universal); if ( *************** *** 1090,1098 **** ) { ! GetLog()->Error() << "(RosImporter::ReadHinge) found no bodies to attach hinge to in " << GetXMLPath(element) << " named " << name << "\n"; return false; } shared_ptr<HingeJoint> hinge = shared_dynamic_cast<HingeJoint> (GetCore()->New("/oxygen/HingeJoint")); --- 1131,1165 ---- ) { ! GetLog()->Error() << "(RosImporter::ReadUniversal) found no bodies to attach hinge to in " << GetXMLPath(element) << " named " << name << "\n"; return false; } + universal->SetName(name); + universal->Attach(body1, body2); + universal->SetAnchor(anchor); + universal->SetAxis1(axis1); + universal->SetAxis2(axis2); + + return true; + } + + bool RosImporter::ReadHinge(shared_ptr<BaseNode> parent, TiXmlElement* element, ENodeContext context) + { + string name; + Vector3f anchor; + Vector3f axis; + + if ( + (! ReadAttribute(element, RA_NAME, name, true)) || + (! ReadAnchorPoint(element, anchor)) || + (! ReadAxis(element, RosElements::RE_AXIS, axis)) + ) + { + return false; + } + + shared_ptr<Body> body1 = GetJointParentBody(parent); + shared_ptr<HingeJoint> hinge = shared_dynamic_cast<HingeJoint> (GetCore()->New("/oxygen/HingeJoint")); *************** *** 1100,1103 **** --- 1167,1188 ---- parent->AddChildReference(hinge); + if (! ReadChildElements(hinge, element, NC_MOVABLE)) + { + return false; + } + + shared_ptr<Body> body2 = GetJointChildBody(hinge); + + 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); hinge->Attach(body1, body2); *************** *** 1106,1109 **** --- 1191,1195 ---- GetLog()->Debug() << "(RosImporter) created hinge joint " << name << "\n"; + return true; } *************** *** 1120,1123 **** --- 1206,1210 ---- GetLog()->Debug() << "(RosImporter) defined macro " << name << "\n"; + return true; } *************** *** 1266,1269 **** --- 1353,1357 ---- if (! ReadPhysicalRep(transform,element,context)) { + return false; } *************** *** 1417,1420 **** --- 1505,1509 ---- geom.vertices.push_back(ref); + break; } *************** *** 1422,1425 **** --- 1511,1515 ---- } + // std::reverse(geom.vertices.begin(), geom.vertices.end()); return true; } *************** *** 1472,1481 **** bool RosImporter::ReadPhysicalRep(shared_ptr<Transform> transform, TiXmlElement* element, ENodeContext context) { TiXmlElement* physicalRep = GetFirstChild(element, RosElements::RE_PHYSICALREPRESENTATION); if (physicalRep == 0) { - string name = S_UNNAMED; - ReadAttribute(element, RA_NAME, name, true); - GetLog()->Error() << "(RosImporter) ERROR: missing physical representation in " << GetXMLPath(element) << " name " << name << " \n"; --- 1562,1571 ---- bool RosImporter::ReadPhysicalRep(shared_ptr<Transform> transform, TiXmlElement* element, ENodeContext context) { + string name = S_UNNAMED; + ReadAttribute(element, RA_NAME, name, true); + TiXmlElement* physicalRep = GetFirstChild(element, RosElements::RE_PHYSICALREPRESENTATION); if (physicalRep == 0) { GetLog()->Error() << "(RosImporter) ERROR: missing physical representation in " << GetXMLPath(element) << " name " << name << " \n"; *************** *** 1484,1492 **** } ! // we currently read only the first physical representation of the ! // complex shape and use its translation to modify the parent ! // transform under which the static mesh is registered. TODO: ! // figure out how to align the graphical representation with the ! // physical representation if more than one geom is present for ( --- 1574,1581 ---- } ! shared_ptr<Body> body = shared_dynamic_cast<Body> ! (GetCore()->New("/oxygen/Body")); ! transform->AddChildReference(body); ! body->SetName(name); for ( *************** *** 1512,1538 **** case RosElements::RE_SIMPLEBOX: ! return ReadSimpleBox(transform, element, context); case RosElements::RE_SIMPLESPHERE: ! return ReadSimpleSphere(transform, element, context); case RosElements::RE_SIMPLECYLINDER: //! simulate cylinder with a capped cylinder case RosElements::RE_SIMPLECAPPEDCYLINDER: ! return ReadSimpleCappedCylinder(transform, element, context); ! ! ! // RE_SIMPLECYLINDER, ! // RE_SIMPLECAPPEDCYLINDER, ! } } ! GetLog()->Debug() << "(RosImporter) read physical representation\n"; return true; } ! bool RosImporter::ReadSimpleBox(shared_ptr<Transform> transform, TiXmlElement* element, ENodeContext context) { string name; --- 1601,1641 ---- case RosElements::RE_SIMPLEBOX: ! if (! ReadSimpleBox(transform, body, element, context)) ! { ! return false; ! } ! break; case RosElements::RE_SIMPLESPHERE: ! if (! ReadSimpleSphere(transform, body, element, context)) ! { ! return false; ! } ! break; case RosElements::RE_SIMPLECYLINDER: //! simulate cylinder with a capped cylinder case RosElements::RE_SIMPLECAPPEDCYLINDER: ! if (! ReadSimpleCappedCylinder(transform, body, element, context)) ! { ! return false; ! } ! break; } } ! // adjust for the mass center ! Leaf::TLeafList children; ! transform->ListChildrenSupportingClass<TransformCollider>(children,false); ! ! Vector3f offset(body->GetMassCenter()); ! transform->SetLocalPos(transform->GetLocalPos() + offset); + GetLog()->Debug() << "(RosImporter) read physical representation\n"; return true; } ! bool RosImporter::ReadSimpleBox(shared_ptr<Transform> transform, shared_ptr<Body> body, ! TiXmlElement* element, ENodeContext context) { string name; *************** *** 1559,1579 **** Vector3f boxDim = Vector3f(length, width, height); ! if (HasBody(physical, context)) { ! shared_ptr<Body> body = shared_dynamic_cast<Body> ! (GetCore()->New("/oxygen/Body")); ! ! transform->AddChildReference(body); ! ! body->SetName(S_BODY+name); ! body->SetBoxTotal(physical.mass, boxDim); ! //! TODO: set mass center } // geometry shared_ptr<BoxCollider> collider = shared_dynamic_cast<BoxCollider> (GetCore()->New("/oxygen/BoxCollider")); ! transform->AddChildReference(collider); collider->SetName(S_GEOM+name); collider->SetBoxLengths(boxDim); --- 1662,1682 ---- Vector3f boxDim = Vector3f(length, width, height); ! if ( ! (HasBody(physical, context)) && ! (body.get() != 0) ! ) { ! body->AddBoxTotal(physical.mass, boxDim, trans.matrix); } // geometry + shared_ptr<TransformCollider> transCollider + = CreateTransformCollider(transform,trans); + transCollider->SetName(S_GEOMTRANS+name); + shared_ptr<BoxCollider> collider = shared_dynamic_cast<BoxCollider> (GetCore()->New("/oxygen/BoxCollider")); ! transCollider->AddChildReference(collider); collider->SetName(S_GEOM+name); collider->SetBoxLengths(boxDim); *************** *** 1587,1591 **** } ! bool RosImporter::ReadSimpleSphere(shared_ptr<Transform> transform, TiXmlElement* element, ENodeContext context) { string name; --- 1690,1695 ---- } ! bool RosImporter::ReadSimpleSphere(shared_ptr<Transform> transform, shared_ptr<Body> body, ! TiXmlElement* element, ENodeContext context) { string name; *************** *** 1607,1627 **** ApplyTransform(transform, trans); ! if (HasBody(physical, context)) { ! shared_ptr<Body> body = shared_dynamic_cast<Body> ! (GetCore()->New("/oxygen/Body")); ! ! transform->AddChildReference(body); ! ! body->SetName(S_BODY+name); ! body->SetSphereTotal(physical.mass, radius); ! //! TODO: set mass center } // geometry shared_ptr<SphereCollider> collider = shared_dynamic_cast<SphereCollider> (GetCore()->New("/oxygen/SphereCollider")); ! transform->AddChildReference(collider); collider->SetRadius(radius); --- 1711,1731 ---- ApplyTransform(transform, trans); ! if ( ! (HasBody(physical, context)) && ! (body.get() != 0) ! ) { ! body->AddSphereTotal(physical.mass, radius, trans.matrix); } // geometry + shared_ptr<TransformCollider> transCollider + = CreateTransformCollider(transform,trans); + transCollider->SetName(S_GEOMTRANS+name); + shared_ptr<SphereCollider> collider = shared_dynamic_cast<SphereCollider> (GetCore()->New("/oxygen/SphereCollider")); ! transCollider->AddChildReference(collider); collider->SetRadius(radius); *************** *** 1634,1638 **** } ! bool RosImporter::ReadSimpleCappedCylinder(shared_ptr<Transform> transform, TiXmlElement* element, ENodeContext context) { string name; --- 1738,1743 ---- } ! bool RosImporter::ReadSimpleCappedCylinder(shared_ptr<Transform> transform, boost::shared_ptr<Body> body, ! TiXmlElement* element, ENodeContext context) { string name; *************** *** 1656,1677 **** ApplyTransform(transform, trans); ! if (HasBody(physical, context)) { ! shared_ptr<Body> body = shared_dynamic_cast<Body> ! (GetCore()->New("/oxygen/Body")); ! ! transform->AddChildReference(body); ! ! body->SetName(S_BODY+name); ! body->SetCappedCylinderTotal(physical.mass, radius, height); ! //! TODO: set mass center } // geometry shared_ptr<CCylinderCollider> collider = shared_dynamic_cast<CCylinderCollider> (GetCore()->New("/oxygen/CCylinderCollider")); - collider->SetName(S_GEOM+name); ! transform->AddChildReference(collider); collider->SetParams(radius, height); --- 1761,1782 ---- ApplyTransform(transform, trans); ! if ( ! (HasBody(physical, context)) && ! (body.get() != 0) ! ) { ! body->AddCappedCylinderTotal(physical.mass, radius, height, trans.matrix); } // geometry + shared_ptr<TransformCollider> transCollider + = CreateTransformCollider(transform,trans); + transCollider->SetName(S_GEOMTRANS+name); + shared_ptr<CCylinderCollider> collider = shared_dynamic_cast<CCylinderCollider> (GetCore()->New("/oxygen/CCylinderCollider")); ! transCollider->AddChildReference(collider); ! collider->SetName(S_GEOM+name); collider->SetParams(radius, height); Index: rosimporter.h =================================================================== RCS file: /cvsroot/simspark/simspark/spark/plugin/rosimporter/rosimporter.h,v retrieving revision 1.8 retrieving revision 1.8.8.1 diff -C2 -d -r1.8 -r1.8.8.1 *** rosimporter.h 23 Feb 2006 13:40:23 -0000 1.8 --- rosimporter.h 20 May 2007 16:31:53 -0000 1.8.8.1 *************** *** 25,28 **** --- 25,29 ---- #include <map> #include <boost/shared_array.hpp> + #include <salt/matrix.h> #include <oxygen/sceneserver/sceneimporter.h> #include <oxygen/geometryserver/trimesh.h> *************** *** 34,37 **** --- 35,39 ---- class BaseNode; class Transform; + class TransformCollider; class ContactJointHandler; class Body; *************** *** 63,73 **** { public: ! salt::Vector3f translate; ! salt::Vector3f rotate; public: Trans() ! : translate(0.0, 0.0, 0.0), rotate (0.0, 0.0, 0.0) {} ! }; --- 65,75 ---- { public: ! salt::Matrix matrix; public: Trans() ! { ! matrix.Identity(); ! } }; *************** *** 195,198 **** --- 197,203 ---- (boost::shared_ptr<oxygen::BaseNode> parent, const Trans& trans); + boost::shared_ptr<oxygen::TransformCollider> CreateTransformCollider + (boost::shared_ptr<oxygen::BaseNode> parent, const Trans& trans); + boost::shared_ptr<oxygen::ContactJointHandler> RosImporter::CreateContactJointHandler(); *************** *** 230,234 **** 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); --- 235,239 ---- 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); *************** *** 236,239 **** --- 241,245 ---- 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); *************** *** 246,252 **** bool ReadPhysicalRep(boost::shared_ptr<oxygen::Transform> transform, TiXmlElement* element, ENodeContext context); ! bool ReadSimpleBox(boost::shared_ptr<oxygen::Transform> transform, TiXmlElement* element, ENodeContext context); ! bool ReadSimpleSphere(boost::shared_ptr<oxygen::Transform> transform, TiXmlElement* element, ENodeContext context); ! bool ReadSimpleCappedCylinder(boost::shared_ptr<oxygen::Transform> transform, TiXmlElement* element, ENodeContext context); protected: --- 252,261 ---- 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: |
