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
