Revision: 7706 http://playerstage.svn.sourceforge.net/playerstage/?rev=7706&view=rev Author: natepak Date: 2009-05-21 16:46:22 +0000 (Thu, 21 May 2009)
Log Message: ----------- Moved some ODE stuff from the sensors into the ode physics library Modified Paths: -------------- code/gazebo/branches/bullet/server/World.cc code/gazebo/branches/bullet/server/physics/CMakeLists.txt code/gazebo/branches/bullet/server/physics/PhysicsEngine.hh code/gazebo/branches/bullet/server/physics/ode/CMakeLists.txt code/gazebo/branches/bullet/server/physics/ode/ODEPhysics.cc code/gazebo/branches/bullet/server/physics/ode/ODEPhysics.hh code/gazebo/branches/bullet/server/physics/ode/ODERayGeom.cc code/gazebo/branches/bullet/server/physics/ode/ODERayGeom.hh code/gazebo/branches/bullet/server/sensors/contact/ContactSensor.hh code/gazebo/branches/bullet/server/sensors/ir/IRSensor.cc code/gazebo/branches/bullet/server/sensors/ir/IRSensor.hh code/gazebo/branches/bullet/server/sensors/ray/RaySensor.cc code/gazebo/branches/bullet/server/sensors/ray/RaySensor.hh code/gazebo/branches/bullet/worlds/models/sicklms200.model code/gazebo/branches/bullet/worlds/pioneer2dx.world Added Paths: ----------- code/gazebo/branches/bullet/server/physics/PhysicsFactory.cc code/gazebo/branches/bullet/server/physics/PhysicsFactory.hh code/gazebo/branches/bullet/server/physics/PhysicsRaySensor.hh code/gazebo/branches/bullet/server/physics/ode/ODERaySensor.cc code/gazebo/branches/bullet/server/physics/ode/ODERaySensor.hh Modified: code/gazebo/branches/bullet/server/World.cc =================================================================== --- code/gazebo/branches/bullet/server/World.cc 2009-05-20 23:51:46 UTC (rev 7705) +++ code/gazebo/branches/bullet/server/World.cc 2009-05-21 16:46:22 UTC (rev 7706) @@ -34,7 +34,7 @@ #include "GazeboError.hh" #include "GazeboMessage.hh" #include "PhysicsEngine.hh" -#include "ODEPhysics.hh" +#include "PhysicsFactory.hh" #include "XMLConfig.hh" #include "Model.hh" #include "Simulator.hh" @@ -62,6 +62,8 @@ this->server = NULL; this->graphics = NULL; this->openAL = NULL; + + PhysicsFactory::RegisterAll(); } //////////////////////////////////////////////////////////////////////////////// @@ -147,14 +149,16 @@ // Load OpenAL audio if (rootNode->GetChild("openal","audio")) { - //this->openAL = new OpenALAPI(); - //this->openAL->Load(rootNode->GetChild("openal", "audio")); this->openAL = OpenAL::Instance(); this->openAL->Load(rootNode->GetChild("openal", "audio")); } - this->physicsEngine = new ODEPhysics(); //TODO: use exceptions here + XMLConfigNode *physicsNode = rootNode->GetChildByNSPrefix("physics"); + if (Simulator::Instance()->GetPhysicsEnabled() && physicsNode) + this->physicsEngine = PhysicsFactory::NewPhysicsEngine( + physicsNode->GetName()); + this->LoadEntities(rootNode, NULL, false); /*std::vector<Model*>::iterator miter; @@ -163,8 +167,8 @@ this->SetModelPose(*miter, (*miter)->GetPose() + Global::poseOffset); }*/ - this->physicsEngine->Load(rootNode); - + if (this->physicsEngine) + this->physicsEngine->Load(rootNode); } //////////////////////////////////////////////////////////////////////////////// @@ -201,7 +205,8 @@ } // Initialize the physics engine - this->physicsEngine->Init(); + if (this->physicsEngine) + this->physicsEngine->Init(); // Initialize openal if (this->openAL) @@ -257,8 +262,7 @@ std::cout << " World::Update() ALL Models update DT(" << tmpT2-tmpT1 << ")" << std::endl; #endif - if (!Simulator::Instance()->IsPaused() && - Simulator::Instance()->GetPhysicsEnabled()) + if (!Simulator::Instance()->IsPaused() && this->physicsEngine) { this->physicsEngine->UpdatePhysics(); } @@ -294,7 +298,8 @@ (*miter)->Fini(); } - this->physicsEngine->Fini(); + if (this->physicsEngine) + this->physicsEngine->Fini(); // Done with the external interface try Modified: code/gazebo/branches/bullet/server/physics/CMakeLists.txt =================================================================== --- code/gazebo/branches/bullet/server/physics/CMakeLists.txt 2009-05-20 23:51:46 UTC (rev 7705) +++ code/gazebo/branches/bullet/server/physics/CMakeLists.txt 2009-05-21 16:46:22 UTC (rev 7706) @@ -8,6 +8,7 @@ ContactParams.cc PhysicsEngine.cc Mass.cc + PhysicsFactory.cc ) SET (headers Joint.hh @@ -16,6 +17,8 @@ ContactParams.hh PhysicsEngine.hh Mass.hh + PhysicsFactory.hh + PhysicsRaySensor.hh ) #ADD_LIBRARY(gazebo_physics STATIC ${sources}) Modified: code/gazebo/branches/bullet/server/physics/PhysicsEngine.hh =================================================================== --- code/gazebo/branches/bullet/server/physics/PhysicsEngine.hh 2009-05-20 23:51:46 UTC (rev 7705) +++ code/gazebo/branches/bullet/server/physics/PhysicsEngine.hh 2009-05-21 16:46:22 UTC (rev 7706) @@ -46,6 +46,7 @@ class Body; class XMLConfigNode; class Geom; + class PhysicsRaySensor; /// \addtogroup gazebo_physics_engine /** \{ @@ -121,6 +122,9 @@ /// \brief Create a new joint public: virtual Joint *CreateJoint(Joint::Type type) = 0; + + /// \brief Create a physics based ray sensor + public: virtual PhysicsRaySensor *CreateRaySensor(Body *body) = 0; /// \brief Return the gavity vector /// \return The gavity vector Added: code/gazebo/branches/bullet/server/physics/PhysicsFactory.cc =================================================================== --- code/gazebo/branches/bullet/server/physics/PhysicsFactory.cc (rev 0) +++ code/gazebo/branches/bullet/server/physics/PhysicsFactory.cc 2009-05-21 16:46:22 UTC (rev 7706) @@ -0,0 +1,64 @@ +/* + * Gazebo - Outdoor Multi-Robot Simulator + * Copyright (C) 2003 + * Nate Koenig & Andrew Howard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/* + * Desc: Factory for creating physics engine + * Author: Nate Koenig + * Date: 21 May 2009 + * SVN info: $Id:$ + */ + +#include "ODEPhysics.hh" + +#include "PhysicsEngine.hh" +#include "PhysicsFactory.hh" + +void RegisterODEPhysics(); + +using namespace gazebo; + +std::map<std::string, PhysicsFactoryFn> PhysicsFactory::engines; + +//////////////////////////////////////////////////////////////////////////////// +/// Register everything +void PhysicsFactory::RegisterAll() +{ + RegisterODEPhysics(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Register a physics engine class. +void PhysicsFactory::RegisterPhysicsEngine(std::string classname, + PhysicsFactoryFn factoryfn) +{ + engines[classname] = factoryfn; +} + +//////////////////////////////////////////////////////////////////////////////// +// Create a new instance of a physics engine. +PhysicsEngine *PhysicsFactory::NewPhysicsEngine(const std::string &classname) +{ + if (engines[classname]) + { + return (engines[classname]) (); + } + + return NULL; +} Added: code/gazebo/branches/bullet/server/physics/PhysicsFactory.hh =================================================================== --- code/gazebo/branches/bullet/server/physics/PhysicsFactory.hh (rev 0) +++ code/gazebo/branches/bullet/server/physics/PhysicsFactory.hh 2009-05-21 16:46:22 UTC (rev 7706) @@ -0,0 +1,84 @@ +/* + * Gazebo - Outdoor Multi-Robot Simulator + * Copyright (C) 2003 + * Nate Koenig & Andrew Howard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/* + * Desc: Factory for creating physics engine + * Author: Nate Koenig + * Date: 21 May 2009 + * SVN info:$ + */ + +#ifndef PHYSICSFACTORY_HH +#define PHYSICSFACTORY_HH + +#include <string> +#include <map> + +namespace gazebo +{ + + // Forward declarations + class PhysicsEngine; + + // Prototype for sensor factory functions + typedef PhysicsEngine* (*PhysicsFactoryFn) (); + + /// \addtogroup gazebo_physics + /// \brief The physics factory + /// \{ + + /// \brief The physics factory + class PhysicsFactory + { + /// \brief Register everything + public: static void RegisterAll(); + + /// \brief Register a physics class. + public: static void RegisterPhysicsEngine(std::string classname, + PhysicsFactoryFn factoryfn); + + /// \brief Create a new instance of a physics engine. + public: static PhysicsEngine *NewPhysicsEngine(const std::string &classname); + + /// \brief A list of registered physics classes + private: static std::map<std::string, PhysicsFactoryFn> engines; + }; + + + /// \brief Static sensor registration macro + /// + /// Use this macro to register sensors with the server. + /// @param name Physics type name, as it appears in the world file. + /// @param classname C++ class name for the sensor. +#define GZ_REGISTER_PHYSICS_ENGINE(name, classname) \ +PhysicsEngine *New##classname() \ +{ \ + return new classname(); \ +} \ +void Register##classname() \ +{\ + PhysicsFactory::RegisterPhysicsEngine(name, New##classname);\ +} +//StaticPluginRegister Registered##classname (Register##classname); + + /// \} +} + +#endif Added: code/gazebo/branches/bullet/server/physics/PhysicsRaySensor.hh =================================================================== --- code/gazebo/branches/bullet/server/physics/PhysicsRaySensor.hh (rev 0) +++ code/gazebo/branches/bullet/server/physics/PhysicsRaySensor.hh 2009-05-21 16:46:22 UTC (rev 7706) @@ -0,0 +1,69 @@ +/* + * Gazebo - Outdoor Multi-Robot Simulator + * Copyright (C) 2003 + * Nate Koenig & Andrew Howard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/* Desc: Base class for a physics based ray sensor + * Author: Nate Koenig + * Date: 21 May 2009 + * SVN: $Id:$ + */ + +#ifndef PHYSICSRAYSENSOR_HH +#define PHYSICSRAYSENSOR_HH + +#include "Vector3.hh" +namespace gazebo +{ + class Body; + + /// \brief A Physics Ray sensor + class PhysicsRaySensor + { + /// \brief Constructor + public: PhysicsRaySensor(Body * /*body*/) {}; + + /// \brief Destructor + public: virtual ~PhysicsRaySensor() {}; + + /// \brief Add a ray to the sensor + public: virtual void AddRay(Vector3 start, Vector3 end, + double minRange, double maxRange, + bool display) = 0; + + /// \brief Get the number of rays + public: virtual int GetCount() const = 0; + + /// \brief Get the relative starting and ending points of a ray + public: virtual void GetRelativePoints(int index, Vector3 &a, Vector3 &b) = 0; + + /// \brief Get the range of a ray + public: virtual double GetRange(int index) const = 0; + + /// \brief Get the retro reflectance value of a ray + public: virtual double GetRetro(int index) const = 0; + + /// \brief Get the fiducial value of a ray + public: virtual double GetFiducial(int index) const = 0; + + /// \brief Update the ray sensor + public: virtual void Update() = 0; + }; +} + +#endif Modified: code/gazebo/branches/bullet/server/physics/ode/CMakeLists.txt =================================================================== --- code/gazebo/branches/bullet/server/physics/ode/CMakeLists.txt 2009-05-20 23:51:46 UTC (rev 7705) +++ code/gazebo/branches/bullet/server/physics/ode/CMakeLists.txt 2009-05-21 16:46:22 UTC (rev 7706) @@ -9,6 +9,7 @@ ODEJoint.cc ODESliderJoint.cc ODEUniversalJoint.cc + ODERaySensor.cc ODEBoxGeom.cc ODECylinderGeom.cc Modified: code/gazebo/branches/bullet/server/physics/ode/ODEPhysics.cc =================================================================== --- code/gazebo/branches/bullet/server/physics/ode/ODEPhysics.cc 2009-05-20 23:51:46 UTC (rev 7705) +++ code/gazebo/branches/bullet/server/physics/ode/ODEPhysics.cc 2009-05-21 16:46:22 UTC (rev 7706) @@ -26,6 +26,7 @@ #include <assert.h> +#include "PhysicsFactory.hh" #include "Mass.hh" #include "Global.hh" #include "GazeboMessage.hh" @@ -44,6 +45,7 @@ #include "ODEPlaneGeom.hh" #include "ODESphereGeom.hh" #include "ODETrimeshGeom.hh" +#include "ODERaySensor.hh" #include "ODEBody.hh" #include "ODESliderJoint.hh" @@ -61,6 +63,10 @@ using namespace gazebo; +GZ_REGISTER_PHYSICS_ENGINE("ode", ODEPhysics); + +void CollisionCallback( void *data, dGeomID o1, dGeomID o2); + //////////////////////////////////////////////////////////////////////////////// // Constructor ODEPhysics::ODEPhysics() @@ -406,10 +412,24 @@ odeMass->I[0*4+2], odeMass->I[1*4+2] ); } +//////////////////////////////////////////////////////////////////////////////// +/// Get the contact group +dJointGroupID ODEPhysics::GetContactGroup() +{ + return this->contactGroup; +} //////////////////////////////////////////////////////////////////////////////// +// Create an object to hold a set of ray geoms +PhysicsRaySensor *ODEPhysics::CreateRaySensor(Body *body) +{ + ODERaySensor *sensor = new ODERaySensor(body); +} + + +//////////////////////////////////////////////////////////////////////////////// // Collision Callback -void ODEPhysics::CollisionCallback( void *data, dGeomID o1, dGeomID o2) +void CollisionCallback( void *data, dGeomID o1, dGeomID o2) { ODEPhysics *self; ODEGeom *geom1 = NULL; @@ -476,7 +496,7 @@ // Compute the CFM and ERP by assuming the two bodies form a // spring-damper system. - h = self->stepTimeP->GetValue(); + h = self->GetStepTime(); kp = 1.0 / (1.0 / cp1->kp + 1.0 / cp2->kp); kd = cp1->kd + cp2->kd; contact.surface.soft_erp = h * kp / (h * kp + kd); @@ -489,8 +509,8 @@ contact.surface.slip1 = std::min(cp1->slip1, cp2->slip1); contact.surface.slip2 = std::min(cp1->slip2, cp2->slip2); - dJointID c = dJointCreateContact (self->worldId, - self->contactGroup, &contact); + dJointID c = dJointCreateContact (self->GetWorldId(), + self->GetContactGroup(), &contact); // save "dJointID *c" in Geom, so we can do a // dJointFeedback *jft = dJointGetFeedback( c[i] ) later Modified: code/gazebo/branches/bullet/server/physics/ode/ODEPhysics.hh =================================================================== --- code/gazebo/branches/bullet/server/physics/ode/ODEPhysics.hh 2009-05-20 23:51:46 UTC (rev 7705) +++ code/gazebo/branches/bullet/server/physics/ode/ODEPhysics.hh 2009-05-21 16:46:22 UTC (rev 7706) @@ -126,17 +126,23 @@ /// \brief Create a new joint public: virtual Joint *CreateJoint(Joint::Type type); + /// \brief Create a physics based ray sensor + public: virtual PhysicsRaySensor *CreateRaySensor(Body *body); + /// \brief Return the space id public: dSpaceID GetSpaceId() const; /// \brief Get the world id public: dWorldID GetWorldId(); + /// \brief Get the contact group + public: dJointGroupID GetContactGroup(); + /// \brief Convert an odeMass to Mass public: void ConvertMass(Mass *mass, dMass *odeMass); /// \brief Do collision detection - private: static void CollisionCallback( void *data, dGeomID o1, dGeomID o2); + //private: static void CollisionCallback( void *data, dGeomID o1, dGeomID o2); /// \brief Top-level world for all bodies private: dWorldID worldId; Modified: code/gazebo/branches/bullet/server/physics/ode/ODERayGeom.cc =================================================================== --- code/gazebo/branches/bullet/server/physics/ode/ODERayGeom.cc 2009-05-20 23:51:46 UTC (rev 7705) +++ code/gazebo/branches/bullet/server/physics/ode/ODERayGeom.cc 2009-05-21 16:46:22 UTC (rev 7706) @@ -62,6 +62,9 @@ this->contactLen = DBL_MAX; this->contactRetro = 0.0; this->contactFiducial = -1; + + this->minLength = 0; + this->maxLength = 0; } @@ -214,3 +217,30 @@ } +////////////////////////////////////////////////////////////////////////////// +/// Get the min length +double ODERayGeom::GetMinLength() const +{ + return this->minLength; +} + +////////////////////////////////////////////////////////////////////////////// +/// Get the max length +double ODERayGeom::GetMaxLength() const +{ + return this->maxLength; +} + +////////////////////////////////////////////////////////////////////////////// +/// Set the min length +void ODERayGeom::SetMinLength(double len) +{ + this->minLength = len; +} + +////////////////////////////////////////////////////////////////////////////// +/// Set the max length +void ODERayGeom::SetMaxLength(double len) +{ + this->maxLength = len; +} Modified: code/gazebo/branches/bullet/server/physics/ode/ODERayGeom.hh =================================================================== --- code/gazebo/branches/bullet/server/physics/ode/ODERayGeom.hh 2009-05-20 23:51:46 UTC (rev 7705) +++ code/gazebo/branches/bullet/server/physics/ode/ODERayGeom.hh 2009-05-21 16:46:22 UTC (rev 7706) @@ -96,6 +96,18 @@ /// \brief Get the fiducial id detected by this ray public: int GetFiducial() const; + /// \brief Get the min length + public: double GetMinLength() const; + + /// \brief Get the max length + public: double GetMaxLength() const; + + /// \brief Set the min length + public: void SetMinLength(double len); + + /// \brief Set the max length + public: void SetMaxLength(double len); + /// \brief Load thte ray public: virtual void Load(XMLConfigNode *node); @@ -117,6 +129,8 @@ /// Start and end positions of the ray in global cs private: Vector3 globalStartPos; private: Vector3 globalEndPos; + + private: double minLength, maxLength; }; /// \} Added: code/gazebo/branches/bullet/server/physics/ode/ODERaySensor.cc =================================================================== --- code/gazebo/branches/bullet/server/physics/ode/ODERaySensor.cc (rev 0) +++ code/gazebo/branches/bullet/server/physics/ode/ODERaySensor.cc 2009-05-21 16:46:22 UTC (rev 7706) @@ -0,0 +1,265 @@ +/* + * Gazebo - Outdoor Multi-Robot Simulator + * Copyright (C) 2003 + * Nate Koenig & Andrew Howard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/* Desc: ODE ray sensor + * Author: Nate Koenig + * Date: 21 May 2009 + * SVN: $Id:$ + */ + +#include "Global.hh" +#include "World.hh" + +#include "ODERayGeom.hh" +#include "ODEPhysics.hh" +#include "ODEBody.hh" +#include "ODERaySensor.hh" + +using namespace gazebo; + +//////////////////////////////////////////////////////////////////////////////// +/// Constructor +ODERaySensor::ODERaySensor(Body *_body) + : PhysicsRaySensor(_body) +{ + this->body = dynamic_cast<ODEBody*>(_body); + this->physicsEngine = dynamic_cast<ODEPhysics*>(World::Instance()->GetPhysicsEngine()); + + if (this->body == NULL) + gzthrow("ODERaySensor requires an ODEBody"); + + // Create a space to contain the ray space + this->superSpaceId = dSimpleSpaceCreate( 0 ); + + // Create a space to contain all the rays + this->raySpaceId = dSimpleSpaceCreate( this->superSpaceId ); + + // Set collision bits + dGeomSetCategoryBits((dGeomID) this->raySpaceId, GZ_SENSOR_COLLIDE); + dGeomSetCollideBits((dGeomID) this->raySpaceId, ~GZ_SENSOR_COLLIDE); + + body->SetSpaceId( this->raySpaceId ); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Destructor +ODERaySensor::~ODERaySensor() +{ + std::vector<ODERayGeom*>::iterator iter; + + for (iter = this->rays.begin(); iter != this->rays.end(); iter++) + { + delete (*iter); + } + this->rays.clear(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Add a ray to the sensor +void ODERaySensor::AddRay(Vector3 start, Vector3 end, double minRange, + double maxRange, bool display) +{ + ODERayGeom *rayGeom; + + rayGeom = (ODERayGeom*)this->physicsEngine->CreateGeom("ray", this->body ); + rayGeom->SetDisplayRays(display); + rayGeom->SetMinLength(minRange); + rayGeom->SetMaxLength(maxRange); + + rayGeom->SetPoints(start,end); + this->rays.push_back(rayGeom); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Get the number of rays +int ODERaySensor::GetCount() const +{ + return this->rays.size(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Get the relative starting and ending points of a ray +void ODERaySensor::GetRelativePoints(int index, Vector3 &a, Vector3 &b) +{ + if (index <0 || index >= (int)(this->rays.size())) + { + std::ostringstream stream; + stream << "index[" << index << "] is out of range[0-" + << this->GetCount() << "]"; + gzthrow(stream.str()); + } + + this->rays[index]->GetRelativePoints(a,b); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Get the range of a ray +double ODERaySensor::GetRange(int index) const +{ + if (index <0 || index >= (int)(this->rays.size())) + { + std::ostringstream stream; + stream << "index[" << index << "] is out of range[0-" + << this->GetCount() << "]"; + gzthrow(stream.str()); + } + + return this->rays[index]->GetLength(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Get the retro reflectance value of a ray +double ODERaySensor::GetRetro(int index) const +{ + if (index <0 || index >= (int)(this->rays.size())) + { + std::ostringstream stream; + stream << "index[" << index << "] is out of range[0-" + << this->GetCount() << "]"; + gzthrow(stream.str()); + } + + return this->rays[index]->GetRetro(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Get the fiducial value of a ray +double ODERaySensor::GetFiducial(int index) const +{ + if (index <0 || index >= (int)(this->rays.size())) + { + std::ostringstream stream; + stream << "index[" << index << "] is out of range[0-" + << this->GetCount() << "]"; + gzthrow(stream.str()); + } + + return this->rays[index]->GetFiducial(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Update the ray sensor +void ODERaySensor::Update() +{ + std::vector<ODERayGeom*>::iterator iter; + + for (iter = this->rays.begin(); iter != this->rays.end(); iter++) + { + (*iter)->SetLength( (*iter)->GetMaxLength() ); + (*iter)->SetRetro( 0.0 ); + (*iter)->SetFiducial( -1 ); + + // Get the global points of the line + (*iter)->Update(); + } + + this->physicsEngine->LockMutex(); + dSpaceCollide2( (dGeomID)(this->superSpaceId), + (dGeomID)(this->physicsEngine->GetSpaceId()), + this, &CollisionCallback ); + + this->physicsEngine->UnlockMutex(); +} + +///////////////////////////////////////////////////////////////////////////// +// Callback for ray intersection test +void ODERaySensor::CollisionCallback( void *data, dGeomID o1, dGeomID o2 ) +{ + int n = 0; + dContactGeom contact; + dxGeom *geom1, *geom2 = NULL; + ODERayGeom *rayGeom = NULL; + ODEGeom *hitGeom = NULL; + ODERaySensor *self = NULL; + + self = (ODERaySensor*) data; + + // Check space + if ( dGeomIsSpace( o1 ) || dGeomIsSpace( o2 ) ) + { + if (dGeomGetSpace(o1) == self->superSpaceId || + dGeomGetSpace(o2) == self->superSpaceId) + { + dSpaceCollide2( o1, o2, self, &CollisionCallback ); + } + if (dGeomGetSpace(o1) == self->raySpaceId || + dGeomGetSpace(o2) == self->raySpaceId) + { + dSpaceCollide2( o1, o2, self, &CollisionCallback ); + } + } + else + { + geom1 = NULL; + geom2 = NULL; + + // Get pointers to the underlying geoms + if (dGeomGetClass(o1) == dGeomTransformClass) + geom1 = (dxGeom*) dGeomGetData(dGeomTransformGetGeom(o1)); + else + geom1 = (dxGeom*) dGeomGetData(o1); + + if (dGeomGetClass(o2) == dGeomTransformClass) + geom2 = (dxGeom*) dGeomGetData(dGeomTransformGetGeom(o2)); + else + geom2 = (dxGeom*) dGeomGetData(o2); + + assert(geom1 && geom2); + + rayGeom = NULL; + hitGeom = NULL; + + // Figure out which one is a ray; note that this assumes + // that the ODE dRayClass is used *soley* by the RayGeom. + if (dGeomGetClass(o1) == dRayClass) + { + rayGeom = (ODERayGeom*) geom1; + hitGeom = (ODEGeom*) geom2; + dGeomRaySetParams(o1, 0, 0); + dGeomRaySetClosestHit(o1, 1); + } + + if (dGeomGetClass(o2) == dRayClass) + { + assert(rayGeom == NULL); + rayGeom = (ODERayGeom*) geom2; + hitGeom = (ODEGeom* )geom1; + dGeomRaySetParams(o2, 0, 0); + dGeomRaySetClosestHit(o2, 1); + } + + // Check for ray/geom intersections + if ( rayGeom && hitGeom ) + { + + n = dCollide(o1, o2, 1, &contact, sizeof(contact)); + + if ( n > 0 ) + { + if (contact.depth < rayGeom->GetLength()) + { + rayGeom->SetLength( contact.depth ); + rayGeom->SetRetro( hitGeom->GetLaserRetro() ); + rayGeom->SetFiducial( hitGeom->GetLaserFiducialId() ); + } + } + } + } +} Added: code/gazebo/branches/bullet/server/physics/ode/ODERaySensor.hh =================================================================== --- code/gazebo/branches/bullet/server/physics/ode/ODERaySensor.hh (rev 0) +++ code/gazebo/branches/bullet/server/physics/ode/ODERaySensor.hh 2009-05-21 16:46:22 UTC (rev 7706) @@ -0,0 +1,86 @@ +/* + * Gazebo - Outdoor Multi-Robot Simulator + * Copyright (C) 2003 + * Nate Koenig & Andrew Howard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/* Desc: ODE ray sensor + * Author: Nate Koenig + * Date: 21 May 2009 + * SVN: $Id:$ + */ + +#ifndef ODERAYSENSOR_HH +#define ODERAYSENSOR_HH + +#include <ode/ode.h> +#include <vector> + +#include "PhysicsRaySensor.hh" + +namespace gazebo +{ + class ODEPhysics; + class ODERayGeom; + class ODEBody; + + /// \brief An ODE Ray sensor + class ODERaySensor : public PhysicsRaySensor + { + /// \brief Constructor + public: ODERaySensor(Body *body); + + /// \brief Destructor + public: virtual ~ODERaySensor(); + + /// \brief Add a ray to the sensor + public: void AddRay(Vector3 start, Vector3 end, double minRange, + double maxRange, bool display); + + /// \brief Get the number of rays + public: int GetCount() const; + + /// \brief Get the relative starting and ending points of a ray + public: void GetRelativePoints(int index, Vector3 &a, Vector3 &b); + + /// \brief Get the range of a ray + public: double GetRange(int index) const; + + /// \brief Get the retro reflectance value of a ray + public: double GetRetro(int index) const; + + /// \brief Get the fiducial value of a ray + public: double GetFiducial(int index) const; + + /// \brief Update the ray sensor + public: virtual void Update(); + + /// \brief Callback for ray intersection test + private: static void CollisionCallback( void *data, dGeomID o1, dGeomID o2 ); + + /// All the rays + private: std::vector<ODERayGeom*> rays; + + private: ODEPhysics *physicsEngine; + + private: ODEBody *body; + + private: dSpaceID superSpaceId; + private: dSpaceID raySpaceId; + }; +} +#endif Modified: code/gazebo/branches/bullet/server/sensors/contact/ContactSensor.hh =================================================================== --- code/gazebo/branches/bullet/server/sensors/contact/ContactSensor.hh 2009-05-20 23:51:46 UTC (rev 7705) +++ code/gazebo/branches/bullet/server/sensors/contact/ContactSensor.hh 2009-05-21 16:46:22 UTC (rev 7706) @@ -109,6 +109,7 @@ private: double *contactTimes; private: unsigned int contactCount; private: std::vector<std::string> contactNames; + /// \brief a place for storing joint feedbacks, including contact joints private: dJointFeedback contactFeedbacks[GAZEBO_MAX_CONTACT_FB_DATA]; Modified: code/gazebo/branches/bullet/server/sensors/ir/IRSensor.cc =================================================================== --- code/gazebo/branches/bullet/server/sensors/ir/IRSensor.cc 2009-05-20 23:51:46 UTC (rev 7705) +++ code/gazebo/branches/bullet/server/sensors/ir/IRSensor.cc 2009-05-21 16:46:22 UTC (rev 7706) @@ -33,9 +33,8 @@ #include "Global.hh" #include "World.hh" #include "PhysicsEngine.hh" +#include "PhysicsRaySensor.hh" #include "GazeboError.hh" -#include "ODEPhysics.hh" -#include "ODERayGeom.hh" #include "XMLConfig.hh" #include "IRSensor.hh" @@ -95,18 +94,17 @@ this->maxRange = new double[this->IRCount]; this->origin = new Vector3[this->IRCount]; - //Read configuration from XML file for(i=0, iNode = node->GetChild("ir"); iNode;i++) - { - if(i >= this->IRCount) - { - std::ostringstream stream; - stream<< "please check the irCount is correct!"; - gzthrow(stream.str()); - } - - std::string name = iNode->GetString("name","",1); + { + if(i >= this->IRCount) + { + std::ostringstream stream; + stream<< "please check the irCount is correct!"; + gzthrow(stream.str()); + } + + std::string name = iNode->GetString("name","",1); this->rayCount[i] = iNode->GetInt("rayCount",0,1); this->rangeCount[i] = iNode->GetInt("rangeCount",0,1); this->minAngle[i] = DTOR(iNode->GetDouble("minAngle",-90,1)); @@ -115,24 +113,13 @@ this->maxRange[i] = iNode->GetDouble("maxRange",8,1); this->origin[i] = iNode->GetVector3("origin", Vector3(0,0,0)); iNode = iNode->GetNext("ir"); - } + } - this->displayRays = node->GetBool("displayRays", true); + this->displayRays = node->GetString("displayRays", "off", 0); - // TODO BULLET: Need to fix this - /* - // Create a space to contain the ray space - this->superSpaceId = dSimpleSpaceCreate( 0 ); + this->physicsEngine = World::Instance()->GetPhysicsEngine(); - // Create a space to contain all the rays - this->raySpaceId = dSimpleSpaceCreate( this->superSpaceId ); - - // Set collision bits - dGeomSetCategoryBits((dGeomID) this->raySpaceId, GZ_SENSOR_COLLIDE); - dGeomSetCollideBits((dGeomID) this->raySpaceId, ~GZ_SENSOR_COLLIDE); - - this->body->SetSpaceId( this->raySpaceId ); - */ + this->rays = this->physicsEngine->CreateRaySensor(this->body); } ////////////////////////////////////////////////////////////////////////////// @@ -142,13 +129,10 @@ Pose3d bodyPose; double angle; Vector3 start, end, axis; - ODERayGeom *ray; bodyPose = this->body->GetPose(); this->prevPose = bodyPose; - PhysicsEngine *physicsEngine = World::Instance()->GetPhysicsEngine(); - // Create and array of ray geoms //for (int i = 0; i < this->rayCount; i++) for(int j=0; j<this->IRCount;j++) @@ -166,13 +150,8 @@ start = (axis * this->minRange[j]) + this->origin[j]; end = (axis * this->maxRange[j]) + this->origin[j]; - ray = (ODERayGeom*)(physicsEngine->CreateGeom("ray",this->body)); - ray->SetDisplayRays( displayRays ); - - ray->SetPoints(start, end); - - this->rays.push_back(ray); - + this->rays->AddRay(start, end, minRange[j], maxRange[j], + this->displayRays == "lines" ); } } @@ -182,12 +161,9 @@ // Init the ray void IRSensor::FiniChild() { - std::vector<ODERayGeom*>::iterator iter; - for (iter=this->rays.begin(); iter!=this->rays.end(); iter++) - { - delete *iter; - } - this->rays.clear(); + if (this->rays) + delete this->rays; + if(this->rayCount!=NULL) { delete []this->rayCount; @@ -275,26 +251,25 @@ { std::ostringstream stream; stream << "index[" << index << "] out of range[0-" - << this->IRCount << "]"; + << this->IRCount << "]"; gzthrow(stream.str()); } - + //get the ray index range for this specific IR int start_index=0; int end_index=0; for(int i=0;i<index;i++) { - start_index +=this->rayCount[i]; + start_index +=this->rayCount[i]; } end_index = start_index + this->rayCount[index] - 1; - + double range = this->maxRange[index]; for(int i=start_index;i<=end_index;i++) { - range = std::min(this->rays[i]->GetLength(),range); + range = std::min(this->rays->GetRange(i),range); } return range; - } Pose IRSensor::GetPose(int index) @@ -325,7 +300,6 @@ { // if (this->active) { - std::vector<ODERayGeom*>::iterator iter; Pose3d poseDelta; Vector3 a, b; @@ -333,112 +307,6 @@ poseDelta = this->body->GetPose() - this->prevPose; this->prevPose = this->body->GetPose(); - // Reset the ray lengths and mark the geoms as dirty (so they get - // redrawn) - for (iter = this->rays.begin(); iter != this->rays.end(); iter++) - { - (*iter)->SetLength( this->maxRange[0] ); - (*iter)->SetRetro( 0.0 ); - (*iter)->SetFiducial( -1 ); - - // Get the global points of the line - (*iter)->Update(); - } - - ODEPhysics *ode = dynamic_cast<ODEPhysics*>(World::Instance()->GetPhysicsEngine()); - - if (ode == NULL) - { - gzthrow( "Invalid physics engine. Must use ODE." ); - } - - // Do collision detection - dSpaceCollide2( ( dGeomID ) ( this->superSpaceId ), - ( dGeomID ) ( ode->GetSpaceId() ), - this, &UpdateCallback ); + this->rays->Update( ); } } - - - -///////////////////////////////////////////////////////////////////////////// -// Callback for ray intersection test -void IRSensor::UpdateCallback( void *data, dGeomID o1, dGeomID o2 ) -{ - int n = 0; - dContactGeom contact; - dxGeom *geom1, *geom2 = NULL; - ODERayGeom *rayGeom = NULL; - Geom *hitGeom = NULL; - IRSensor *self = NULL; - - self = (IRSensor*) data; - - // Check space - if ( dGeomIsSpace( o1 ) || dGeomIsSpace( o2 ) ) - { - if (dGeomGetSpace(o1) == self->superSpaceId || dGeomGetSpace(o2) == self->superSpaceId) - { - dSpaceCollide2( o1, o2, self, &UpdateCallback ); - } - if (dGeomGetSpace(o1) == self->raySpaceId || dGeomGetSpace(o2) == self->raySpaceId) - { - dSpaceCollide2( o1, o2, self, &UpdateCallback ); - } - } - else - { - geom1 = NULL; - geom2 = NULL; - - // Get pointers to the underlying geoms - if (dGeomGetClass(o1) == dGeomTransformClass) - geom1 = (dxGeom*) dGeomGetData(dGeomTransformGetGeom(o1)); - else - geom1 = (dxGeom*) dGeomGetData(o1); - - if (dGeomGetClass(o2) == dGeomTransformClass) - geom2 = (dxGeom*) dGeomGetData(dGeomTransformGetGeom(o2)); - else - geom2 = (dxGeom*) dGeomGetData(o2); - - assert(geom1 && geom2); - - rayGeom = NULL; - hitGeom = NULL; - - // Figure out which one is a ray; note that this assumes - // that the ODE dRayClass is used *soley* by the RayGeom. - if (dGeomGetClass(o1) == dRayClass) - { - rayGeom = (ODERayGeom*) geom1; - hitGeom = (Geom*) geom2; - dGeomRaySetParams(o1, 0, 0); - dGeomRaySetClosestHit(o1, 1); - } - - if (dGeomGetClass(o2) == dRayClass) - { - assert(rayGeom == NULL); - rayGeom = (ODERayGeom*) geom2; - hitGeom = (Geom* )geom1; - dGeomRaySetParams(o2, 0, 0); - dGeomRaySetClosestHit(o2, 1); - } - - // Check for ray/geom intersections - if ( rayGeom && hitGeom ) - { - - n = dCollide(o1, o2, 1, &contact, sizeof(contact)); - - if ( n > 0 ) - { - if (contact.depth < rayGeom->GetLength()) - { - rayGeom->SetLength( contact.depth ); - } - } - } - } -} Modified: code/gazebo/branches/bullet/server/sensors/ir/IRSensor.hh =================================================================== --- code/gazebo/branches/bullet/server/sensors/ir/IRSensor.hh 2009-05-20 23:51:46 UTC (rev 7705) +++ code/gazebo/branches/bullet/server/sensors/ir/IRSensor.hh 2009-05-21 16:46:22 UTC (rev 7706) @@ -36,8 +36,10 @@ namespace gazebo { + class PhysicsEngine; + class PhysicsRaySensor; class XMLConfigNode; - class ODERayGeom; + /// \addtogroup gazebo_sensor /// \brief Sensor with one or more rays. /// \{ @@ -113,16 +115,9 @@ public: double GetRange(int index); public: Pose GetPose(int index); - /// \brief Ray-intersection callback - private: static void UpdateCallback( void *data, dGeomID o1, dGeomID o2 ); - - /// Ray space for collision detector - private: dSpaceID superSpaceId; - private: dSpaceID raySpaceId; + private: PhysicsEngine *physicsEngine; - /// Ray data - private: std::vector<ODERayGeom*> rays; - + private: PhysicsRaySensor *rays; private: double *minAngle, *maxAngle; private: double *minRange, *maxRange; @@ -134,7 +129,7 @@ private: int IRCount; /// Display rays when rendering images - private: bool displayRays; + private: std::string displayRays; }; /// \} Modified: code/gazebo/branches/bullet/server/sensors/ray/RaySensor.cc =================================================================== --- code/gazebo/branches/bullet/server/sensors/ray/RaySensor.cc 2009-05-20 23:51:46 UTC (rev 7705) +++ code/gazebo/branches/bullet/server/sensors/ray/RaySensor.cc 2009-05-21 16:46:22 UTC (rev 7706) @@ -28,17 +28,16 @@ #include <float.h> #include <sstream> +#include "PhysicsRaySensor.hh" #include "OgreCreator.hh" #include "OgreVisual.hh" #include "OgreDynamicLines.hh" #include "SensorFactory.hh" #include "XMLConfig.hh" #include "Global.hh" -#include "ODERayGeom.hh" #include "World.hh" #include "PhysicsEngine.hh" #include "GazeboError.hh" -#include "ODEPhysics.hh" #include "XMLConfig.hh" #include "Controller.hh" #include "RaySensor.hh" @@ -54,6 +53,8 @@ RaySensor::RaySensor(Body *body) : Sensor(body) { + + this->physicsEngine = NULL; this->active = false; this->typeName = "ray"; @@ -134,21 +135,9 @@ this->verticalMinAngleP->Load(node); this->verticalMaxAngleP->Load(node); + this->physicsEngine = World::Instance()->GetPhysicsEngine(); - // TODO BULLET: need to fix this - /* - // Create a space to contain the ray space - this->superSpaceId = dSimpleSpaceCreate( 0 ); - - // Create a space to contain all the rays - this->raySpaceId = dSimpleSpaceCreate( this->superSpaceId ); - - // Set collision bits - dGeomSetCategoryBits((dGeomID) this->raySpaceId, GZ_SENSOR_COLLIDE); - dGeomSetCollideBits((dGeomID) this->raySpaceId, ~GZ_SENSOR_COLLIDE); - - this->body->SetSpaceId( this->raySpaceId ); - */ + this->rays = this->physicsEngine->CreateRaySensor(this->body); } ////////////////////////////////////////////////////////////////////////////// @@ -177,10 +166,15 @@ Pose3d bodyPose; double yawAngle, pitchAngle; Vector3 start, end, axis; - ODERayGeom *ray; - ODEPhysics *ode = dynamic_cast<ODEPhysics*>(World::Instance()->GetPhysicsEngine()); bodyPose = this->body->GetPose(); this->prevPose = bodyPose; + + if (this->physicsEngine == NULL) + { + gzerr(2) << "Physics engine is disabled."; + return; + } + double pDiff = (**(this->verticalMaxAngleP) - **(this->verticalMinAngleP)).GetAsRadian(); double yDiff = (**(this->maxAngleP) - **(this->minAngleP)).GetAsRadian(); @@ -206,10 +200,6 @@ start = (axis * this->minRangeP->GetValue()) + this->originP->GetValue(); end = (axis * this->maxRangeP->GetValue()) + this->originP->GetValue(); - - ray = (ODERayGeom*)(ode->CreateGeom("ray", this->body)); - ray->SetDisplayRays((**this->displayRaysP) == "lines" ); - if ((**this->displayRaysP) == "fan") { if (i == 0) @@ -222,9 +212,8 @@ this->rayFanOutline->AddPoint(end); } - ray->SetPoints(start, end); - - this->rays.push_back(ray); + this->rays->AddRay(start, end, (**minRangeP), (**this->maxRangeP), + (**this->displayRaysP) == "lines" ); } } @@ -246,12 +235,7 @@ // Init the ray void RaySensor::FiniChild() { - std::vector<ODERayGeom*>::iterator iter; - for (iter=this->rays.begin(); iter!=this->rays.end(); iter++) - { - delete *iter; - } - this->rays.clear(); + delete this->rays; } ////////////////////////////////////////////////////////////////////////////// @@ -335,56 +319,32 @@ // Get detected range for a ray double RaySensor::GetRange(int index) { - if (index < 0 || index >= (int)this->rays.size()) - { - std::ostringstream stream; - stream << "index[" << index << "] out of range[0-" - << this->rays.size() << "]"; - gzthrow(stream.str()); - } - - return this->rays[index]->GetLength(); + return this->rays->GetRange(index); } - ////////////////////////////////////////////////////////////////////////////// // Get detected retro (intensity) value for a ray. double RaySensor::GetRetro(int index) { - if (index < 0 || index >= (int)this->rays.size()) - { - std::ostringstream stream; - stream << "index[" << index << "] out of range[0-" - << this->rays.size() << "]"; - gzthrow(stream.str()); - } - - return this->rays[index]->GetRetro(); + return this->rays->GetRetro(index); } - ////////////////////////////////////////////////////////////////////////////// // Get detected fiducial value for a ray. int RaySensor::GetFiducial(int index) { - if (index < 0 || index >= (int)this->rays.size()) - { - std::ostringstream stream; - stream << "index[" << index << "] out of range[0-" - << this->rays.size() << "]"; - gzthrow(stream.str()); - } - - return this->rays[index]->GetFiducial(); + return this->rays->GetFiducial(index); } ////////////////////////////////////////////////////////////////////////////// // Update the sensor information void RaySensor::UpdateChild() { + if (this->physicsEngine == NULL) + return; + // if (this->active) { - std::vector<ODERayGeom*>::iterator iter; Pose3d poseDelta; Vector3 a, b; int i = 1; @@ -393,132 +353,18 @@ poseDelta = this->body->GetPose() - this->prevPose; this->prevPose = this->body->GetPose(); - // Reset the ray lengths and mark the geoms as dirty (so they get - // redrawn) - for (iter = this->rays.begin(); iter != this->rays.end(); iter++, i++) - { + this->rays->Update(); - (*iter)->SetLength( this->maxRangeP->GetValue() ); - (*iter)->SetRetro( 0.0 ); - (*iter)->SetFiducial( -1 ); - - // Get the global points of the line - (*iter)->Update(); - - } - - ODEPhysics *ode = dynamic_cast<ODEPhysics*>(World::Instance()->GetPhysicsEngine()); - - if (ode == NULL) - { - gzthrow( "Invalid physics engine. Must use ODE." ); - } - - ode->LockMutex(); - // Do collision detection - dSpaceCollide2( ( dGeomID ) ( this->superSpaceId ), - ( dGeomID ) ( ode->GetSpaceId() ), - this, &UpdateCallback ); - ode->UnlockMutex(); - if ((**this->displayRaysP) == "fan") { - i = 1; - for (iter = this->rays.begin(); iter != this->rays.end(); iter++, i++) + for (i = 0; i < this->rays->GetCount(); i++) { - (*iter)->Update(); - - (*iter)->GetRelativePoints(a,b); - - this->rayFan->SetPoint(i,b); - this->rayFanOutline->SetPoint(i,b); + this->rays->GetRelativePoints(i, a, b); + + this->rayFan->SetPoint(i+1,b); + this->rayFanOutline->SetPoint(i+1,b); + } } } } - - - -///////////////////////////////////////////////////////////////////////////// -// Callback for ray intersection test -void RaySensor::UpdateCallback( void *data, dGeomID o1, dGeomID o2 ) -{ - int n = 0; - dContactGeom contact; - dxGeom *geom1, *geom2 = NULL; - ODERayGeom *rayGeom = NULL; - Geom *hitGeom = NULL; - RaySensor *self = NULL; - - self = (RaySensor*) data; - - // Check space - if ( dGeomIsSpace( o1 ) || dGeomIsSpace( o2 ) ) - { - if (dGeomGetSpace(o1) == self->superSpaceId || dGeomGetSpace(o2) == self->superSpaceId) - { - dSpaceCollide2( o1, o2, self, &UpdateCallback ); - } - if (dGeomGetSpace(o1) == self->raySpaceId || dGeomGetSpace(o2) == self->raySpaceId) - { - dSpaceCollide2( o1, o2, self, &UpdateCallback ); - } - } - else - { - geom1 = NULL; - geom2 = NULL; - - // Get pointers to the underlying geoms - if (dGeomGetClass(o1) == dGeomTransformClass) - geom1 = (dxGeom*) dGeomGetData(dGeomTransformGetGeom(o1)); - else - geom1 = (dxGeom*) dGeomGetData(o1); - - if (dGeomGetClass(o2) == dGeomTransformClass) - geom2 = (dxGeom*) dGeomGetData(dGeomTransformGetGeom(o2)); - else - geom2 = (dxGeom*) dGeomGetData(o2); - - assert(geom1 && geom2); - - rayGeom = NULL; - hitGeom = NULL; - - // Figure out which one is a ray; note that this assumes - // that the ODE dRayClass is used *soley* by the RayGeom. - if (dGeomGetClass(o1) == dRayClass) - { - rayGeom = (ODERayGeom*) geom1; - hitGeom = (Geom*) geom2; - dGeomRaySetParams(o1, 0, 0); - dGeomRaySetClosestHit(o1, 1); - } - - if (dGeomGetClass(o2) == dRayClass) - { - assert(rayGeom == NULL); - rayGeom = (ODERayGeom*) geom2; - hitGeom = (Geom* )geom1; - dGeomRaySetParams(o2, 0, 0); - dGeomRaySetClosestHit(o2, 1); - } - - // Check for ray/geom intersections - if ( rayGeom && hitGeom ) - { - - n = dCollide(o1, o2, 1, &contact, sizeof(contact)); - - if ( n > 0 ) - { - if (contact.depth < rayGeom->GetLength()) - { - rayGeom->SetLength( contact.depth ); - rayGeom->SetRetro( hitGeom->GetLaserRetro() ); - rayGeom->SetFiducial( hitGeom->GetLaserFiducialId() ); - } - } - } - } -} Modified: code/gazebo/branches/bullet/server/sensors/ray/RaySensor.hh =================================================================== --- code/gazebo/branches/bullet/server/sensors/ray/RaySensor.hh 2009-05-20 23:51:46 UTC (rev 7705) +++ code/gazebo/branches/bullet/server/sensors/ray/RaySensor.hh 2009-05-21 16:46:22 UTC (rev 7706) @@ -38,7 +38,8 @@ class OgreDynamicLines; class XMLConfigNode; - class ODERayGeom; + class PhysicsRaySensor; + class PhysicsEngine; /// \addtogroup gazebo_sensor /// \brief Sensor with one or more rays. @@ -55,7 +56,7 @@ class RaySensor: public Sensor { /// \brief Constructor - /// \param body The underlying collision test uses an ODE geom, so + /// \param body The underlying collision test uses a geom, so /// ray sensors must be attached to a body. public: RaySensor(Body *body); @@ -142,15 +143,9 @@ /// \brief Get detected fiducial value for a ray. public: int GetFiducial(int index); - /// \brief Ray-intersection callback - private: static void UpdateCallback( void *data, dGeomID o1, dGeomID o2 ); - - /// Ray space for collision detector - private: dSpaceID superSpaceId; - private: dSpaceID raySpaceId; + private: PhysicsEngine *physicsEngine; - /// Ray data - private: std::vector<ODERayGeom*> rays; + private: PhysicsRaySensor *rays; private: ParamT<Angle> *minAngleP, *maxAngleP; private: ParamT<double> *minRangeP, *maxRangeP, *resRangeP; Modified: code/gazebo/branches/bullet/worlds/models/sicklms200.model =================================================================== --- code/gazebo/branches/bullet/worlds/models/sicklms200.model 2009-05-20 23:51:46 UTC (rev 7705) +++ code/gazebo/branches/bullet/worlds/models/sicklms200.model 2009-05-21 16:46:22 UTC (rev 7706) @@ -53,10 +53,12 @@ <maxRange>8</maxRange> <resRange>.1</resRange> + <!-- <controller:sicklms200_laser name="laser_controller_1"> <interface:laser name="laser_iface_0"/> <interface:fiducial name="fiducial_iface_0"/> </controller:sicklms200_laser> + --> </sensor:ray> </body:box> Modified: code/gazebo/branches/bullet/worlds/pioneer2dx.world =================================================================== --- code/gazebo/branches/bullet/worlds/pioneer2dx.world 2009-05-20 23:51:46 UTC (rev 7705) +++ code/gazebo/branches/bullet/worlds/pioneer2dx.world 2009-05-21 16:46:22 UTC (rev 7706) @@ -19,10 +19,13 @@ <verbosity>5</verbosity> <physics:ode> - <stepTime>0.001</stepTime> + <stepTime>0.002</stepTime> <gravity>0 0 -9.8</gravity> + <quickStep>true</quickStep> + <quickStepIters>10</quickStepIters> + <quickStepW>1.3</quickStepW> <cfm>10e-5</cfm> - <erp>0.1</erp> + <erp>0.3</erp> </physics:ode> <rendering:gui> @@ -108,7 +111,6 @@ </controller:differential_position2d> --> - <!-- <model:physical name="laser"> <xyz>0.15 0 0.18</xyz> @@ -121,7 +123,6 @@ <xi:include href="models/sicklms200.model" /> </include> </model:physical> - --> <include embedded="true"> <xi:include href="models/pioneer2dx.model" /> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. ------------------------------------------------------------------------------ Register Now for Creativity and Technology (CaT), June 3rd, NYC. CaT is a gathering of tech-side developers & brand creativity professionals. Meet the minds behind Google Creative Lab, Visual Complexity, Processing, & iPhoneDevCamp asthey present alongside digital heavyweights like Barbarian Group, R/GA, & Big Spaceship. http://www.creativitycat.com _______________________________________________ Playerstage-commit mailing list Playerstage-commit@lists.sourceforge.net https://lists.sourceforge.net/lists/listinfo/playerstage-commit