[simspark-cvs] simspark/spark/plugin/rosimporter rosimporter.cpp,1.1,1.2 rosimporter.h,1.1,1.2

[Markus R. <rol...@us...>]

Update of /cvsroot/simspark/simspark/spark/plugin/rosimporter
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv16781

Modified Files:
	rosimporter.cpp rosimporter.h 
Log Message:
- initial working version of the rosimporter. All basic primitives (box,
sphere, cappedyclinder) except plane and cylinder are supported, complex
bodies and joints missing. Physical properties and colliders are setup.


Index: rosimporter.cpp
===================================================================
RCS file: /cvsroot/simspark/simspark/spark/plugin/rosimporter/rosimporter.cpp,v
retrieving revision 1.1
retrieving revision 1.2
diff -C2 -d -r1.1 -r1.2
*** rosimporter.cpp	24 Jan 2006 19:16:32 -0000	1.1
--- rosimporter.cpp	18 Feb 2006 19:54:57 -0000	1.2
***************
*** 21,29 ****
--- 21,45 ----
  */
  #include "rosimporter.h"
+ #include <tinyxml/xmlfunctions.h>
  #include <zeitgeist/logserver/logserver.h>
  #include <zeitgeist/fileserver/fileserver.h>
+ #include <oxygen/sceneserver/basenode.h>
+ #include <oxygen/sceneserver/transform.h>
+ #include <oxygen/physicsserver/boxcollider.h>
+ #include <oxygen/physicsserver/spherecollider.h>
+ #include <oxygen/physicsserver/ccylindercollider.h>
+ #include <oxygen/physicsserver/contactjointhandler.h>
+ #include <oxygen/physicsserver/body.h>
+ #include <kerosin/openglserver/glbase.h>
+ #include <kerosin/materialserver/materialsolid.h>
+ #include <kerosin/sceneserver/box.h>
+ #include <kerosin/sceneserver/sphere.h>
+ #include <kerosin/sceneserver/ccylinder.h>
+ #include <kerosin/renderserver/renderserver.h>
  #include <boost/scoped_array.hpp>
  
+ using namespace salt;
  using namespace zeitgeist;
+ using namespace kerosin;
  using namespace oxygen;
  using namespace boost;
***************
*** 34,37 ****
--- 50,55 ----
  RosImporter::RosImporter() : SceneImporter()
  {
+     mGlobalERP = 0.2;
+     mGlobalCFM = 0.0001;
  }
  
***************
*** 40,73 ****
  }
  
! TiXmlElement* RosImporter::GetFirstChild(TiXmlNode* node, const std::string& type)
  {
!     TiXmlNode* childNode
!         = node->FirstChild(type.c_str());
! 
!     if (
!         (childNode == 0) ||
!         (childNode->Type() != TiXmlNode::ELEMENT)
!         )
!         {
!             return 0;
!         }
! 
!     return static_cast<TiXmlElement*>(childNode);
  }
  
! TiXmlElement* RosImporter::IterateChildren(TiXmlNode* node, const std::string& type)
  {
!     TiXmlNode* childNode
!         = node->Parent()->IterateChildren(type.c_str(),node);
  
!     if (
!         (childNode == 0) ||
!         (childNode->Type() != TiXmlNode::ELEMENT)
!         )
!         {
!             return 0;
!         }
  
!     return static_cast<TiXmlElement*>(childNode);
  }
  
--- 58,79 ----
  }
  
! RosElements::ERosElement RosImporter::GetType(const TiXmlElement* element) const
  {
!     return RosElements::GetInstance().Lookup(GetXMLValue(element));
  }
  
! string RosImporter::GetName(RosElements::ERosElement element) const
  {
!     return RosElements::GetInstance().Lookup(element);
! }
  
! TiXmlElement* RosImporter::GetFirstChild(TiXmlNode* node, RosElements::ERosElement type)
! {
!     return ::GetFirstChild(node, GetName(type));
! }
  
! TiXmlElement* RosImporter::IterateChildren(TiXmlNode* node, RosElements::ERosElement type)
! {
!     return ::IterateChildren(node, GetName(type));
  }
  
***************
*** 105,110 ****
  
  bool RosImporter::ParseScene(const char* scene, int size,
!                                    boost::shared_ptr<oxygen::BaseNode> root,
!                                    boost::shared_ptr<zeitgeist::ParameterList> parameter)
  {
      TiXmlDocument document;
--- 111,116 ----
  
  bool RosImporter::ParseScene(const char* scene, int size,
!                                    shared_ptr<BaseNode> root,
!                                    shared_ptr<zeitgeist::ParameterList> parameter)
  {
      TiXmlDocument document;
***************
*** 114,118 ****
          {
              GetLog()->Error() << "(RosImporter) ERROR: xml parsing error: "
!                               << document.ErrorDesc() << std::endl;
              return false;
          }
--- 120,124 ----
          {
              GetLog()->Error() << "(RosImporter) ERROR: xml parsing error: "
!                               << document.ErrorDesc() << "\n";
              return false;
          }
***************
*** 121,140 ****
      if (xmlRoot == 0)
          {
!             // empty doc
!             return true;
          }
  
      for (
           TiXmlNode* node = xmlRoot->FirstChild();
!          xmlRoot != 0;
           node = xmlRoot->IterateChildren(node)
           )
          {
              TiXmlElement* element = static_cast<TiXmlElement*>(node);
  
!             std::string value(node->Value());
          }
  
      return true;
  }
  
--- 127,844 ----
      if (xmlRoot == 0)
          {
!             GetLog()->Error() << "(RosImporter) ERROR: empty xml document\n";
!             return false;
          }
  
+     if (
+         (GetType(xmlRoot) != RosElements::RE_SIMULATION) &&
+         (GetType(xmlRoot) != RosElements::RE_ROSIINCLUDEFILE)
+         )
+         {
+             GetLog()->Error() << "(RosImporter) ERROR: unknown xml root element type "
+                               << GetXMLValue(xmlRoot) << "\n";
+             return false;
+         }
      for (
           TiXmlNode* node = xmlRoot->FirstChild();
!          node != 0;
           node = xmlRoot->IterateChildren(node)
           )
          {
              TiXmlElement* element = static_cast<TiXmlElement*>(node);
+             if (element == 0)
+                 {
+                     continue;
+                 }
  
!             bool ok = true;
! 
!             RosElements::ERosElement type = GetType(element);
!             switch (type)
!                 {
!                 default:
!                     GetLog()->Error() << "(RosImporter::ParseScene) ERROR: skipping unknown toplevel element "
!                                       << GetXMLPath(element) << "\n";
!                     break;
! 
!                 case RosElements::RE_APPEARANCEDEFINITION:
!                     ok = ReadAppearenceDef(element);
!                     break;
! 
!                 case RosElements::RE_SCENE:
!                     ok = ReadScene(root, element);
!                 }
! 
!             if (! ok)
!                 {
!                     break;
!                 }
! 
!         }
! 
!     return true;
! }
! 
! bool RosImporter::ReadAttribute(TiXmlElement* element, const std::string& attribute,
!                                 std::string& value, bool succeedIfMissing)
! {
!     if (element == 0)
!         {
!             return false;
!         }
! 
!     if (! GetXMLAttribute(element, attribute, value))
!         {
!             if (! succeedIfMissing)
!                 {
!                     GetLog()->Error() << "(RosImporter) ERROR: missing string attribute " << attribute << " in "
!                                       << GetXMLPath(element) << "\n";
!                     return false;
!                 }
!         }
! 
!     return true;
! }
! 
! bool RosImporter::ReadAttribute(TiXmlElement* element, const std::string& attribute,
!                                 double& value, bool succeedIfMissing)
! {
!     if (element == 0)
!         {
!             return false;
!         }
! 
!     if (! GetXMLAttribute(element, attribute, value))
!         {
!             if (! succeedIfMissing)
!                 {
!                     GetLog()->Error() << "(RosImporter) ERROR: missing float attribute " << attribute << " in "
!                                       << GetXMLPath(element) << "\n";
!                     return false;
!                 }
!         }
! 
!     return true;
! }
! 
! 
! bool RosImporter::ReadRGBA(TiXmlElement* element, RGBA& rgba)
! {
!     int r,g,b;
!     if (
!         (! GetXMLAttribute(element, RA_R, r)) ||
!         (! GetXMLAttribute(element, RA_G, g)) ||
!         (! GetXMLAttribute(element, RA_B, b))
!         )
!         {
!             GetLog()->Error() << "(RosImporter) ERROR: missing color attributes in "
!                               << GetXMLPath(element) << "\n";
!             return false;
!         }
! 
!     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)
!                 {
!                     GetLog()->Error() << "(RosImporter) ERROR: invalid or missing vector attributes in "
!                                       << GetXMLPath(element) << "\n";
!                     return false;
!                 }
!         }
! 
!     return true;
! }
! 
! bool RosImporter::ReadAppearenceDef(TiXmlElement* element)
! {
!     shared_ptr<Node> materialServer = shared_dynamic_cast<Node>
!         (GetCore()->Get("/sys/server/material"));
! 
!     if (materialServer.get() == 0)
!         {
!             GetLog()->Error() << "(RosImporter) 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->AddChildReference(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) failed to lookup RenderServer node\n";
!             return false;
!         }
! 
!     renderServer->SetAmbientColor(rgba);
!     return true;
! }
! 
! bool RosImporter::ReadScene(shared_ptr<BaseNode> root, TiXmlElement* element)
! {
!     if (root.get() == 0)
!         {
!             return false;
!         }
! 
!     GetLog()->Debug() << "(RosImporter) reading scene node\n";
! 
!     ReadDefaultAppearance(element);
!     ReadGlobalPhsyParams(element);
!     ReadAmbientLight(element);
! 
!     // TODO:
!     // Background surface="..."
!     // AmbientLightColor r="" g="" b=""
!     // GlobalPhysicalParameters gravity="" erp="" cfm="" defaultRollingFrictionCoefficient=""
!     // 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";
! 
!     for (
!          TiXmlNode* node = element->FirstChild();
!          node != 0;
!          node = element->IterateChildren(node)
!          )
!         {
!             TiXmlElement* element = static_cast<TiXmlElement*>(node);
!             if (element == 0)
!                 {
!                     continue;
!                 }
! 
!             bool ok = true;
! 
!             RosElements::ERosElement type = GetType(element);
!             switch (type)
!                 {
!                 default:
!                     GetLog()->Error() << "(RosImporter::ReadElements) ERROR: skipping unknown element "
!                                       << GetXMLPath(element) << "\n";
!                     break;
! 
!                 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;
!                 }
! 
!             if (! ok)
!                 {
!                     return false;
!                 }
!         }
! 
!     return true;
! }
! 
! bool RosImporter::ReadMovable(shared_ptr<BaseNode> root, TiXmlElement* element, ENodeContext context)
! {
!      GetLog()->Debug() << "(RosImporter) reading moveable node\n";
! 
!      context = NC_MOVABLE;
! 
!      for (
!           TiXmlNode* node = element->FirstChild();
!           node != 0;
!           node = element->IterateChildren(node)
!           )
!          {
!              TiXmlElement* element = static_cast<TiXmlElement*>(node);
!              if (element == 0)
!                  {
!                      continue;
!                  }
! 
!              bool ok = true;
! 
!              RosElements::ERosElement type = GetType(element);
!              switch (type)
!                  {
!                  default:
!                      GetLog()->Error() << "(RosImporter::ReadMovable) ERROR: skipping unknown element "
!                                        << GetXMLPath(element) << "\n";
!                      break;
! 
!                  case RosElements::RE_ELEMENTS:
!                      ok = ReadElements(root,element,context);
!                      break;
!                  }
! 
!              if (! ok)
!                  {
!                      return false;
!                  }
!          }
! 
!      return true;
! }
! 
! bool RosImporter::ReadTrans(TiXmlElement* element, Trans& trans)
! {
!     TiXmlElement* transElem = GetFirstChild(element,RosElements::RE_TRANSLATION);
!     if (transElem != 0)
!         {
!             if (! ReadVector(transElem, trans.translate))
!                 {
!                     return false;
!                 }
!         }
! 
!     TiXmlElement* rotElem = GetFirstChild(element,RosElements::RE_ROTATION);
!     if (rotElem != 0)
!         {
!             if (! ReadVector(rotElem, trans.rotate))
!                 {
!                     return false;
!                 }
!         }
! 
!     return true;
! }
! 
! shared_ptr<Transform> RosImporter::CreateTransform(shared_ptr<BaseNode> root, const Trans& trans)
! {
!     shared_ptr<Transform> transform = shared_dynamic_cast<Transform>
!         (GetCore()->New("/oxygen/Transform"));
! 
!     transform->SetLocalRotationDeg(trans.rotate);
!     transform->SetLocalPos(trans.translate);
! 
!     root->AddChildReference(transform);
!     return transform;
! }
! 
! 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, 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);
! }
! 
! 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, ENodeContext context)
! {
!     if (context != NC_MOVABLE)
!         {
!             return true;
!         }
! 
!     TiXmlElement* physElem = GetFirstChild(element,RosElements::RE_PHYSICALATTRIBUTES);
!     if (physElem == 0)
!         {
!             GetLog()->Error() << "(RosImporter) ERROR: missing physical attributes in "
!                               << GetXMLPath(element) << "\n";
!             return false;
!         }
! 
! 
!     // we must have a mass
!     TiXmlElement* massElem = GetFirstChild(physElem, RosElements::RE_MASS);
!     if (massElem == 0)
!         {
!             GetLog()->Error() << "(RosImporter) ERROR: missing mass element in physical attributes in "
!                               << GetXMLPath(element) << "\n";
!             return false;
!         }
! 
!     if (! ReadAttribute(massElem, RA_VALUE, physical.mass))
!         {
!             return false;
!         }
! 
!     // check for mass center
!     TiXmlElement* centerElem = GetFirstChild(physElem,RosElements::RE_CENTEROFMASS);
!     if (centerElem != 0)
!         {
!             ReadVector(centerElem, physical.massCenter);
!         }
! 
!     // TODO: read friction def
! 
!     return true;
! }
! 
! 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;
+     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;
+         }
+ 
+     // transform
+     shared_ptr<Transform> transform = CreateTransform(root, trans);
+     transform->SetName(name);
+ 
+     // visual
+     shared_ptr<CCylinder> ccylinder = shared_dynamic_cast<CCylinder>
+         (GetCore()->New("/kerosin/CCylinder"));
+ 
+     transform->AddChildReference(ccylinder);
+ 
+     ccylinder->SetParams(radius, height);
+     ccylinder->SetMaterial(appear.ref);
+ 
+     // add body?
+     switch (context)
+         {
+         case NC_ELEMENTS:
+             break;
+ 
+         case NC_MOVABLE:
+             {
+                 shared_ptr<Body> body = shared_dynamic_cast<Body>
+                     (GetCore()->New("/oxygen/Body"));
+ 
+                 transform->AddChildReference(body);
+ 
+                 body->SetCylinderTotal(physical.mass, radius, height);
+                 //! TODO: set mass center
+                 break;
+             }
+         }
+ 
+     // geometry
+     shared_ptr<CCylinderCollider> collider = shared_dynamic_cast<CCylinderCollider>
+         (GetCore()->New("/oxygen/CCylinderCollider"));
+ 
+     transform->AddChildReference(collider);
+     collider->SetParams(radius, height);
+ 
+     // collision handler
+     shared_ptr<ContactJointHandler> handler = CreateContactJointHandler();
+     collider->AddChildReference(handler);
+ 
+     GetLog()->Debug() << "(RosImporter) created capped cylinder " << name << "\n";
+ 
+     return true;
+ }
+ 
+ bool RosImporter::ReadSphere(shared_ptr<BaseNode> root, TiXmlElement* element, ENodeContext context)
+ {
+     string name;
+     double radius;
+     Trans trans;
+     Appearance appear;
+     Physical physical;
+ 
+     if (
+         (! ReadAttribute(element, RA_NAME, name)) ||
+         (! ReadAttribute(element, RA_RADIUS, radius)) ||
+         (! ReadTrans(element, trans)) ||
+         (! ReadAppearance(element, appear)) ||
+         (! ReadPhysical(element, physical, context))
+         )
+         {
+             return false;
+         }
+ 
+     // transform
+     shared_ptr<Transform> transform = CreateTransform(root, trans);
+     transform->SetName(name);
+ 
+     // visual
+     shared_ptr<Sphere> sphere = shared_dynamic_cast<Sphere>
+         (GetCore()->New("/kerosin/Sphere"));
+ 
+     transform->AddChildReference(sphere);
+ 
+     sphere->SetRadius(radius);
+     sphere->SetMaterial(appear.ref);
+ 
+     // add body?
+     switch (context)
+         {
+         case NC_ELEMENTS:
+             break;
+ 
+         case NC_MOVABLE:
+             {
+                 shared_ptr<Body> body = shared_dynamic_cast<Body>
+                     (GetCore()->New("/oxygen/Body"));
+ 
+                 transform->AddChildReference(body);
+ 
+                 body->SetSphereTotal(physical.mass, radius);
+                 //! TODO: set mass center
+                 break;
+             }
+         }
+ 
+     // geometry
+     shared_ptr<SphereCollider> collider = shared_dynamic_cast<SphereCollider>
+         (GetCore()->New("/oxygen/SphereCollider"));
+ 
+     transform->AddChildReference(collider);
+     collider->SetRadius(radius);
+ 
+     // collision handler
+     shared_ptr<ContactJointHandler> handler = CreateContactJointHandler();
+     collider->AddChildReference(handler);
+ 
+     GetLog()->Debug() << "(RosImporter) created sphere " << name << "\n";
+ 
+     return true;
+ }
+ 
+ bool RosImporter::ReadBox(shared_ptr<BaseNode> root, TiXmlElement* element, ENodeContext context)
+ {
+     string name;
+     double length;
+     double width;
+     double height;
+     Trans trans;
+     Appearance appear;
+     Physical physical;
+ 
+     if (
+         (! ReadAttribute(element, RA_NAME, name)) ||
+         (! ReadAttribute(element, RA_LENGTH, length)) ||
+         (! ReadAttribute(element, RA_WIDTH, width)) ||
+         (! ReadAttribute(element, RA_HEIGHT, height)) ||
+         (! ReadTrans(element, trans)) ||
+         (! ReadAppearance(element, appear)) ||
+         (! ReadPhysical(element, physical, context))
+         )
+         {
+             return false;
+         }
+ 
+     // transform
+     shared_ptr<Transform> transform = CreateTransform(root, 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->SetExtents(boxDim);
+     box->SetMaterial(appear.ref);
+ 
+     // add body?
+     switch (context)
+         {
+         case NC_ELEMENTS:
+             break;
+ 
+         case NC_MOVABLE:
+             {
+                 shared_ptr<Body> body = shared_dynamic_cast<Body>
+                     (GetCore()->New("/oxygen/Body"));
+ 
+                 transform->AddChildReference(body);
+ 
+                 body->SetBoxTotal(physical.mass, boxDim);
+                 //! TODO: set mass center
+                 break;
+             }
+         }
+ 
+     // geometry
+     shared_ptr<BoxCollider> collider = shared_dynamic_cast<BoxCollider>
+         (GetCore()->New("/oxygen/BoxCollider"));
+ 
+     transform->AddChildReference(collider);
+     collider->SetBoxLengths(boxDim);
+ 
+     // collision handler
+     shared_ptr<ContactJointHandler> handler = CreateContactJointHandler();
+     collider->AddChildReference(handler);
+ 
+     GetLog()->Debug() << "(RosImporter) created box " << name << "\n";
+ 
+     return true;
+ }

Index: rosimporter.h
===================================================================
RCS file: /cvsroot/simspark/simspark/spark/plugin/rosimporter/rosimporter.h,v
retrieving revision 1.1
retrieving revision 1.2
diff -C2 -d -r1.1 -r1.2
*** rosimporter.h	24 Jan 2006 19:16:32 -0000	1.1
--- rosimporter.h	18 Feb 2006 19:54:57 -0000	1.2
***************
*** 24,33 ****
  
  #include <oxygen/sceneserver/sceneimporter.h>
- #include <oxygen/sceneserver/basenode.h>
  #include <tinyxml/tinyxml.h>
  
  class RosImporter : public oxygen::SceneImporter
  {
  public:
      RosImporter();
      virtual ~RosImporter();
--- 24,88 ----
  
  #include <oxygen/sceneserver/sceneimporter.h>
  #include <tinyxml/tinyxml.h>
+ #include "roselements.h"
+ 
+ namespace oxygen
+ {
+     class BaseNode;
+     class Transform;
+     class ContactJointHandler;
+ }
+ 
+ namespace kerosin
+ {
+     class RGBA;
+ }
  
  class RosImporter : public oxygen::SceneImporter
  {
  public:
+     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
+         };
+ 
+     struct Trans
+     {
+     public:
+         salt::Vector3f translate;
+         salt::Vector3f rotate;
+ 
+     public:
+         Trans()
+         : translate(0.0, 0.0, 0.0), rotate (0.0, 0.0, 0.0) {}
+ 
+     };
+ 
+     struct Appearance
+     {
+     public:
+         std::string ref;
+     };
+ 
+     struct Physical
+     {
+     public:
+         double mass;
+         salt::Vector3f massCenter;
+ 
+     public:
+         Physical()
+             : mass(1.0), massCenter(0.0, 0.0, 0.0) {}
+     };
+ 
+ public:
      RosImporter();
      virtual ~RosImporter();
***************
*** 46,55 ****
                               boost::shared_ptr<zeitgeist::ParameterList> parameter);
  
!     TiXmlElement* GetFirstChild(TiXmlNode* node, const std::string& type);
!     TiXmlElement* IterateChildren(TiXmlNode* node, const std::string& type);
  
  protected:
      /** the last supplied fileName */
      std::string mFileName;
  };
  
--- 101,154 ----
                               boost::shared_ptr<zeitgeist::ParameterList> parameter);
  
! 
!     TiXmlElement* GetFirstChild(TiXmlNode* node, RosElements::ERosElement type);
!     TiXmlElement* IterateChildren(TiXmlNode* node, RosElements::ERosElement type);
! 
!     RosElements::ERosElement GetType(const TiXmlElement* element) const;
!     std::string GetName(RosElements::ERosElement element) const;
! 
!     boost::shared_ptr<oxygen::Transform> CreateTransform
!     (boost::shared_ptr<oxygen::BaseNode> root, const Trans& trans);
! 
!     boost::shared_ptr<oxygen::ContactJointHandler>
!     RosImporter::CreateContactJointHandler();
! 
!     bool ReadAmbientLight(TiXmlElement* element);
! 
!     bool ReadAttribute(TiXmlElement* element, const std::string& attribute, double& value, bool succeedIfMissing=false);
!     bool ReadAttribute(TiXmlElement* element, const std::string& attribute, std::string& value, bool succeedIfMissing=false);
!     bool ReadRGBA(TiXmlElement* element, kerosin::RGBA& rgba);
!     bool ReadVector(TiXmlElement* element, salt::Vector3f& vec, bool succeedIfMissing = false);
! 
!     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);
!     bool ReadDefaultAppearance(TiXmlElement* element);
!     bool ReadGlobalPhsyParams(TiXmlElement* element);
! 
!     bool ReadAppearance(TiXmlElement* element, Appearance& appear);
!     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:
      /** the last supplied fileName */
      std::string mFileName;
+ 
+     /** the default appearence definition if omitted in a ROS node */
+     Appearance mDefaultAppearance;
+ 
+     /** default global erp value */
+     double mGlobalERP;
+ 
+     /** default global cfm value */
+     double mGlobalCFM;
+ 
  };