Revision: 8474
http://playerstage.svn.sourceforge.net/playerstage/?rev=8474&view=rev
Author: natepak
Date: 2009-12-18 17:26:23 +0000 (Fri, 18 Dec 2009)
Log Message:
-----------
Improved speed.
Modified Paths:
--------------
code/gazebo/trunk/server/Model.cc
code/gazebo/trunk/server/Model.hh
code/gazebo/trunk/server/Simulator.cc
code/gazebo/trunk/server/World.cc
code/gazebo/trunk/server/controllers/Controller.cc
code/gazebo/trunk/server/gui/Sidebar.cc
code/gazebo/trunk/server/gui/Sidebar.hh
code/gazebo/trunk/server/physics/Body.cc
code/gazebo/trunk/server/physics/Geom.cc
code/gazebo/trunk/server/physics/Geom.hh
code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
code/gazebo/trunk/server/sensors/CMakeLists.txt
code/gazebo/trunk/server/sensors/Sensor.cc
code/gazebo/trunk/server/sensors/Sensor.hh
code/gazebo/trunk/server/sensors/SensorFactory.cc
code/gazebo/trunk/server/sensors/contact/ContactSensor.cc
code/gazebo/trunk/server/sensors/ray/RaySensor.cc
Added Paths:
-----------
code/gazebo/trunk/server/sensors/SensorManager.cc
code/gazebo/trunk/server/sensors/SensorManager.hh
Modified: code/gazebo/trunk/server/Model.cc
===================================================================
--- code/gazebo/trunk/server/Model.cc 2009-12-18 00:04:10 UTC (rev 8473)
+++ code/gazebo/trunk/server/Model.cc 2009-12-18 17:26:23 UTC (rev 8474)
@@ -395,7 +395,7 @@
if (this->controllers.size() == 0 && this->IsStatic())
return;
- DiagnosticTimer timer("Model[" + this->GetName() + "] Update ");
+ //DiagnosticTimer timer("Model[" + this->GetName() + "] Update ");
#ifdef USE_THREADPOOL
World::Instance()->GetPhysicsEngine()->InitForThread();
@@ -407,8 +407,10 @@
Pose3d bodyPose, newPose, oldPose;
+ //this->updateSignal();
+
{
- DiagnosticTimer timer("Model[" + this->GetName() + "] Bodies Update ");
+ //DiagnosticTimer timer("Model[" + this->GetName() + "] Bodies Update ");
for (bodyIter=this->bodies.begin();
bodyIter!=this->bodies.end(); bodyIter++)
@@ -426,8 +428,7 @@
}
{
- DiagnosticTimer timer("Model[" + this->GetName() + "] Controllers Update
");
-
+ //DiagnosticTimer timer("Model[" + this->GetName() + "] Controllers Update
");
for (contIter=this->controllers.begin();
contIter!=this->controllers.end(); contIter++)
{
@@ -443,12 +444,15 @@
}
+ if (World::Instance()->GetShowJoints())
{
- DiagnosticTimer timer("Model[" + this->GetName() + "] Joints Update ");
- for (jointIter = this->joints.begin(); jointIter != this->joints.end();
jointIter++)
+ //DiagnosticTimer timer("Model[" + this->GetName() + "] Joints Update ");
+ for (jointIter = this->joints.begin();
+ jointIter != this->joints.end(); jointIter++)
{
#ifdef USE_THREADPOOL
-
World::Instance()->threadPool->schedule(boost::bind(&Joint::Update,*jointIter));
+ World::Instance()->threadPool->schedule(
+ boost::bind(&Joint::Update,*jointIter));
#else
(*jointIter)->Update();
#endif
@@ -483,7 +487,7 @@
}*/
{
- DiagnosticTimer timer("Model[" + this->GetName() + "] Children Update ");
+ //DiagnosticTimer timer("Model[" + this->GetName() + "] Children Update ");
this->UpdateChild();
}
}
@@ -580,51 +584,6 @@
}
////////////////////////////////////////////////////////////////////////////////
-// Set the current pose
-/*void Model::SetPose(const Pose3d &setPose)
-{
- std::cout << "Set model[" << this->GetName() << "] Pose to["
<<setPose<<"]\n";
- Body *body;
- std::map<std::string, Body* >::iterator iter;
-
- Pose3d newPose, origPose;
-
- origPose = this->pose;
- this->pose = setPose;
-
- for (iter=this->bodies.begin(); iter!=this->bodies.end(); iter++)
- {
- if (!iter->second)
- continue;
-
- body = iter->second;
-
- // Compute the pose relative to the model
- newPose = body->GetPose() - origPose;
-
- // Compute the new pose
- newPose += this->pose;
-
- body->SetPose(newPose);
- }
-
- // Update the child models as well
- std::vector<Entity*>::iterator citer;
- for (citer = this->children.begin(); citer != this->children.end(); citer++)
- {
- Model *childModel = dynamic_cast<Model*>(*citer);
- if (childModel)
- {
- newPose = childModel->GetPose() - origPose;
- newPose += this->pose;
-
- childModel->SetPose(newPose);
- }
- }
-}*/
-
-
-////////////////////////////////////////////////////////////////////////////////
/// Set the linear velocity of the model
void Model::SetLinearVel( const Vector3 &vel )
{
Modified: code/gazebo/trunk/server/Model.hh
===================================================================
--- code/gazebo/trunk/server/Model.hh 2009-12-18 00:04:10 UTC (rev 8473)
+++ code/gazebo/trunk/server/Model.hh 2009-12-18 17:26:23 UTC (rev 8474)
@@ -29,6 +29,7 @@
//#include <python2.4/Python.h>
#include <boost/any.hpp>
+#include <boost/signal.hpp>
#include <map>
#include <string>
#include <vector>
@@ -203,6 +204,13 @@
/// \brief Get the list of interfaces e.g
"pioneer2dx_model1::laser::laser_iface0->laser"
public: void GetModelInterfaceNames(std::vector<std::string>& list) const;
+ /// \brief Connect a boost::slot the the model's update signal
+ public: template<typename T>
+ void ConnectUpdateSignal( T subscriber )
+ {
+ updateSignal.connect(subscriber);
+ }
+
/// \brief Load a body helper function
/// \param node XML Configuration node
private: void LoadBody(XMLConfigNode *node);
@@ -263,10 +271,13 @@
private: GraphicsIfaceHandler *graphicsHandler;
+ private: boost::signal<void ()> updateSignal;
+
/* private: PyObject *pName;
private: PyObject *pModule;
private: PyObject *pFuncUpdate;
*/
+
};
/// \}
}
Modified: code/gazebo/trunk/server/Simulator.cc
===================================================================
--- code/gazebo/trunk/server/Simulator.cc 2009-12-18 00:04:10 UTC (rev
8473)
+++ code/gazebo/trunk/server/Simulator.cc 2009-12-18 17:26:23 UTC (rev
8474)
@@ -333,7 +333,7 @@
{
this->state = RUN;
- DiagnosticTimer timer("--------------------------- START
Simulator::MainLoop() --------------------------");
+ //DiagnosticTimer timer("--------------------------- START
Simulator::MainLoop() --------------------------");
Time currTime = 0;
Time lastTime = 0;
struct timespec timeSpec;
@@ -630,7 +630,7 @@
while (!this->userQuit)
{
- DiagnosticTimer timer("PhysicsLoop Timer ");
+ //DiagnosticTimer timer("PhysicsLoop Timer ");
currTime = this->GetRealTime();
@@ -687,7 +687,7 @@
nanosleep(&req, &rem);
{
- DiagnosticTimer timer("PhysicsLoop UpdateSimIfaces ");
+ //DiagnosticTimer timer("PhysicsLoop UpdateSimIfaces ");
// Process all incoming messages from simiface
world->UpdateSimulationIface();
Modified: code/gazebo/trunk/server/World.cc
===================================================================
--- code/gazebo/trunk/server/World.cc 2009-12-18 00:04:10 UTC (rev 8473)
+++ code/gazebo/trunk/server/World.cc 2009-12-18 17:26:23 UTC (rev 8474)
@@ -39,6 +39,7 @@
#include "PhysicsFactory.hh"
#include "XMLConfig.hh"
#include "Model.hh"
+#include "SensorManager.hh"
#include "Simulator.hh"
#include "gazebo.h"
#include "World.hh"
@@ -325,7 +326,8 @@
{
- DiagnosticTimer timer("World::Update Models");
+ //DiagnosticTimer timer("World::Update Models");
+
// Update all the models
std::vector< Model* >::iterator miter;
for (miter=this->models.begin(); miter!=this->models.end(); miter++)
@@ -336,17 +338,21 @@
(*miter)->Update();
#endif
}
- }
#ifdef USE_THREADPOOL
this->threadPool->wait();
#endif
+ }
+
+ /// Update all the sensors
+ SensorManager::Instance()->Update();
+
if (!Simulator::Instance()->IsPaused() &&
Simulator::Instance()->GetPhysicsEnabled())
{
{
- DiagnosticTimer timer("World::Update Physics");
+ //DiagnosticTimer timer("World::Update Physics");
this->physicsEngine->UpdatePhysics();
}
Modified: code/gazebo/trunk/server/controllers/Controller.cc
===================================================================
--- code/gazebo/trunk/server/controllers/Controller.cc 2009-12-18 00:04:10 UTC
(rev 8473)
+++ code/gazebo/trunk/server/controllers/Controller.cc 2009-12-18 17:26:23 UTC
(rev 8474)
@@ -195,26 +195,19 @@
{
if (this->IsConnected() || **this->alwaysOnP)
{
- DiagnosticTimer timer("Controller[" + this->GetName() +"] Update Timer");
+ //DiagnosticTimer timer("Controller[" + this->GetName() +"] Update Timer");
+
// round time difference to this->physicsEngine->GetStepTime()
Time physics_dt = World::Instance()->GetPhysicsEngine()->GetStepTime();
- // if (this->GetName() == std::string("p3d_base_controller"))
- // std::cout << " sim update: " << this->GetName()
- // << " , " << Simulator::Instance()->GetSimTime() - lastUpdate
- // << " , " << lastUpdate
- // << " , " << updatePeriod
- // << " i1 " <<
round((Simulator::Instance()->GetSimTime()-lastUpdate)/physics_dt)
- // << " i2 " << round(updatePeriod/physics_dt)
- // << std::endl;
- timer.Start();
+ //timer.Start();
Time simTime = Simulator::Instance()->GetSimTime();
if ((simTime-lastUpdate-updatePeriod)/physics_dt >= 0)
{
this->UpdateChild();
lastUpdate = Simulator::Instance()->GetSimTime();
- timer.Report("Update() dt");
+ //timer.Report("Update() dt");
}
}
}
Modified: code/gazebo/trunk/server/gui/Sidebar.cc
===================================================================
--- code/gazebo/trunk/server/gui/Sidebar.cc 2009-12-18 00:04:10 UTC (rev
8473)
+++ code/gazebo/trunk/server/gui/Sidebar.cc 2009-12-18 17:26:23 UTC (rev
8474)
@@ -33,6 +33,7 @@
#include <FL/Fl_Value_Slider.H>
#include <boost/lexical_cast.hpp>
+#include <boost/bind.hpp>
#include "Gui.hh"
#include "World.hh"
@@ -107,6 +108,10 @@
this->resizable( this->entityBrowser );
this->resizable( this->paramBrowser );
+
+
+ World::Instance()->ConnectAddEntitySignal(
+ boost::bind(&Sidebar::AddEntityToBrowser, this, _1) );
}
////////////////////////////////////////////////////////////////////////////////
@@ -143,11 +148,11 @@
std::map<std::string, Body*>::const_iterator iter;
std::map<std::string, Geom*>::const_iterator giter;
- for (unsigned int i=0; i < model->GetJointCount(); i++)
+ /*for (unsigned int i=0; i < model->GetJointCount(); i++)
{
Joint *joint = model->GetJoint(i);
this->jointChoice->add(joint->GetName().c_str(),0,0);
- }
+ }*/
for (iter = bodies->begin(); iter != bodies->end(); iter++)
{
@@ -390,12 +395,17 @@
std::vector<Model*> models = World::Instance()->GetModels();
for (iter = models.begin(); iter != models.end(); iter++)
- {
- this->entityBrowser->add( (*iter)->GetName().c_str() );
- }
+ this->AddEntityToBrowser((*iter));
}
////////////////////////////////////////////////////////////////////////////////
+// Add an entity to the browser
+void Sidebar::AddEntityToBrowser(Entity *entity)
+{
+ this->entityBrowser->add( entity->GetName().c_str() );
+}
+
+////////////////////////////////////////////////////////////////////////////////
/// Joint choice callback
void Sidebar::JointChoiceCB( Fl_Widget *w, void *data )
{
Modified: code/gazebo/trunk/server/gui/Sidebar.hh
===================================================================
--- code/gazebo/trunk/server/gui/Sidebar.hh 2009-12-18 00:04:10 UTC (rev
8473)
+++ code/gazebo/trunk/server/gui/Sidebar.hh 2009-12-18 17:26:23 UTC (rev
8474)
@@ -53,6 +53,9 @@
/// \brief Update the toolbar data
public: void Update();
+ /// \brief Add an entity to the browser
+ public: void AddEntityToBrowser(Entity *model);
+
/// \brief Callback for the parameter browser
public: static void ParamBrowserCB( Fl_Widget * w, void *data);
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2009-12-18 00:04:10 UTC (rev
8473)
+++ code/gazebo/trunk/server/physics/Body.cc 2009-12-18 17:26:23 UTC (rev
8474)
@@ -27,6 +27,7 @@
#include <sstream>
#include <float.h>
+#include "SensorManager.hh"
#include "XMLConfig.hh"
#include "Model.hh"
#include "GazeboMessage.hh"
@@ -107,12 +108,7 @@
this->geoms.clear();
for (siter = this->sensors.begin(); siter != this->sensors.end(); siter++)
- {
- if (*siter)
- delete (*siter);
- (*siter) = NULL;
- }
- this->sensors.clear();
+ SensorManager::Instance()->RemoveSensor(*siter);
if (this->cgVisual)
delete this->cgVisual;
@@ -206,6 +202,8 @@
}
World::Instance()->RegisterBody(this);
+
+ //this->GetModel()->ConnectUpdateSignal( boost::bind(&Body::Update, this) );
}
////////////////////////////////////////////////////////////////////////////////
@@ -245,11 +243,8 @@
void Body::Fini()
{
std::vector< Sensor* >::iterator siter;
-
for (siter = this->sensors.begin(); siter != this->sensors.end(); siter++)
- {
(*siter)->Fini();
- }
}
////////////////////////////////////////////////////////////////////////////////
@@ -336,11 +331,6 @@
if (this->geoms.size()==0 || **this->turnGravityOffP)
this->SetGravityMode(false);
- std::vector< Sensor* >::iterator siter;
-
- for (siter = this->sensors.begin(); siter != this->sensors.end(); siter++)
- (*siter)->Init();
-
// global-inertial damping is implemented in ode svn trunk
if(this->GetId() && this->dampingFactorP->GetValue() > 0)
{
@@ -348,6 +338,10 @@
this->SetAngularDamping(**this->dampingFactorP);
}
+ std::vector< Sensor* >::iterator siter;
+ for (siter = this->sensors.begin(); siter != this->sensors.end(); siter++)
+ (*siter)->Init();
+
this->linearAccel.Set(0,0,0);
this->angularAccel.Set(0,0,0);
@@ -393,14 +387,12 @@
// Update the body
void Body::Update()
{
- DiagnosticTimer timer("Body[" + this->GetName() +"] Update");
+ //DiagnosticTimer timer("Body[" + this->GetName() +"] Update");
#ifdef USE_THREADPOOL
// If calling Body::Update in threadpool
World::Instance()->GetPhysicsEngine()->InitForThread();
#endif
-
- std::vector< Sensor* >::iterator sensorIter;
std::map< std::string, Geom* >::iterator geomIter;
Vector3 vel;
Vector3 avel;
@@ -412,7 +404,7 @@
this->SetTorque(this->angularAccel);
{
- DiagnosticTimer timer("Body[" + this->GetName() +"] Update Geoms");
+ //DiagnosticTimer timer("Body[" + this->GetName() +"] Update Geoms");
for (geomIter=this->geoms.begin();
geomIter!=this->geoms.end(); geomIter++)
@@ -424,21 +416,6 @@
#endif
}
}
-
- {
- DiagnosticTimer timer("Body[" + this->GetName() +"] Update Sensors");
-
- for (sensorIter=this->sensors.begin();
- sensorIter!=this->sensors.end(); sensorIter++)
- {
-#ifdef USE_THREADPOOL
- World::Instance()->threadpool->schedule(boost::bind(&Sensor::Update,
(sensorIter)));
-#else
- (*sensorIter)->Update();
-#endif
- }
- }
-
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/trunk/server/physics/Geom.cc
===================================================================
--- code/gazebo/trunk/server/physics/Geom.cc 2009-12-18 00:04:10 UTC (rev
8473)
+++ code/gazebo/trunk/server/physics/Geom.cc 2009-12-18 17:26:23 UTC (rev
8474)
@@ -61,6 +61,8 @@
this->shape = NULL;
+ this->contactsEnabled = false;
+
Param::Begin(&this->parameters);
this->massP = new ParamT<double>("mass",0.001,0);
this->massP->Callback( &Geom::SetMass, this);
@@ -421,9 +423,26 @@
}
////////////////////////////////////////////////////////////////////////////////
+// Turn contact recording on or off
+void Geom::SetContactsEnabled(bool enable)
+{
+ this->contactsEnabled = enable;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Return true of contact recording is on
+bool Geom::GetContactsEnabled() const
+{
+ return this->contactsEnabled;
+}
+
+////////////////////////////////////////////////////////////////////////////////
/// Add an occurance of a contact to this geom
void Geom::AddContact(const Contact &contact)
{
+ if (!this->contactsEnabled)
+ return;
+
if (this->GetType() == Shape::RAY || this->GetType() == Shape::PLANE)
return;
Modified: code/gazebo/trunk/server/physics/Geom.hh
===================================================================
--- code/gazebo/trunk/server/physics/Geom.hh 2009-12-18 00:04:10 UTC (rev
8473)
+++ code/gazebo/trunk/server/physics/Geom.hh 2009-12-18 17:26:23 UTC (rev
8474)
@@ -147,6 +147,12 @@
/// \brief Get the attached shape
public: Shape *GetShape() const;
+ /// \brief Turn contact recording on or off
+ public: void SetContactsEnabled(bool enable);
+
+ /// \brief Return true of contact recording is on
+ public: bool GetContactsEnabled() const;
+
/// \brief Add an occurance of a contact to this geom
public: void AddContact(const Contact &contact);
@@ -206,6 +212,8 @@
protected: Shape *shape;
+ private: bool contactsEnabled;
+
public: boost::signal< void (const Contact &)> contactSignal;
};
Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2009-12-18 00:04:10 UTC
(rev 8473)
+++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2009-12-18 17:26:23 UTC
(rev 8474)
@@ -515,25 +515,31 @@
self->contactGroup, &contact);
// Store the contact info
- (*self->contactFeedbackIter).contact.depths.push_back(
- contact.geom.depth);
- (*self->contactFeedbackIter).contact.positions.push_back(
- Vector3(contact.geom.pos[0], contact.geom.pos[1],
- contact.geom.pos[2]) );
- (*self->contactFeedbackIter).contact.normals.push_back(
- Vector3(contact.geom.normal[0], contact.geom.normal[1],
- contact.geom.normal[2]) );
- (*self->contactFeedbackIter).contact.time =
- Simulator::Instance()->GetSimTime();
- dJointSetFeedback(c, &((*self->contactFeedbackIter).feedbacks[i]));
+ if (geom1->GetContactsEnabled() ||
+ geom2->GetContactsEnabled())
+ {
+ (*self->contactFeedbackIter).contact.depths.push_back(
+ contact.geom.depth);
+ (*self->contactFeedbackIter).contact.positions.push_back(
+ Vector3(contact.geom.pos[0], contact.geom.pos[1],
+ contact.geom.pos[2]) );
+ (*self->contactFeedbackIter).contact.normals.push_back(
+ Vector3(contact.geom.normal[0], contact.geom.normal[1],
+ contact.geom.normal[2]) );
+ (*self->contactFeedbackIter).contact.time =
+ Simulator::Instance()->GetSimTime();
+ dJointSetFeedback(c, &((*self->contactFeedbackIter).feedbacks[i]));
+ }
dJointAttach (c,b1,b2);
}
- self->contactFeedbackIter++;
- if (self->contactFeedbackIter == self->contactFeedbacks.end())
- self->contactFeedbacks.resize( self->contactFeedbacks.size() + 100);
-
+ if (geom1->GetContactsEnabled() || geom2->GetContactsEnabled())
+ {
+ self->contactFeedbackIter++;
+ if (self->contactFeedbackIter == self->contactFeedbacks.end())
+ self->contactFeedbacks.resize( self->contactFeedbacks.size() + 100);
+ }
}
}
}
Modified: code/gazebo/trunk/server/sensors/CMakeLists.txt
===================================================================
--- code/gazebo/trunk/server/sensors/CMakeLists.txt 2009-12-18 00:04:10 UTC
(rev 8473)
+++ code/gazebo/trunk/server/sensors/CMakeLists.txt 2009-12-18 17:26:23 UTC
(rev 8474)
@@ -8,10 +8,12 @@
SET (sources Sensor.cc
SensorFactory.cc
+ SensorManager.cc
)
SET (headers Sensor.hh
SensorFactory.hh
+ SensorManager.hh
)
#ADD_LIBRARY(gazebo_sensors STATIC ${gazebosensor_sources} ${sources})
Modified: code/gazebo/trunk/server/sensors/Sensor.cc
===================================================================
--- code/gazebo/trunk/server/sensors/Sensor.cc 2009-12-18 00:04:10 UTC (rev
8473)
+++ code/gazebo/trunk/server/sensors/Sensor.cc 2009-12-18 17:26:23 UTC (rev
8474)
@@ -49,6 +49,9 @@
this->controller = NULL;
this->active = true;
+ this->world = World::Instance();
+ this->simulator = Simulator::Instance();
+
Param::Begin(&this->parameters);
this->updateRateP = new ParamT<double>("updateRate", 0, 0);
Param::End();
@@ -119,20 +122,19 @@
/// Update the sensor
void Sensor::Update()
{
- DiagnosticTimer timer("Sensor[" + this->GetName() + "] Update");
+ //DiagnosticTimer timer("Sensor[" + this->GetName() + "] Update");
- Time physics_dt = World::Instance()->GetPhysicsEngine()->GetStepTime();
- if
(((Simulator::Instance()->GetSimTime()-this->lastUpdate-this->updatePeriod)/physics_dt)
>= 0)
+ Time physics_dt = this->world->GetPhysicsEngine()->GetStepTime();
+
+ if (((this->simulator->GetSimTime() - this->lastUpdate -
this->updatePeriod)/physics_dt) >= 0)
{
this->UpdateChild();
- this->lastUpdate = Simulator::Instance()->GetSimTime();
+ this->lastUpdate = this->simulator->GetSimTime();
}
// update any controllers that are children of sensors, e.g. ros_bumper
if (this->controller)
- {
this->controller->Update();
- }
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/trunk/server/sensors/Sensor.hh
===================================================================
--- code/gazebo/trunk/server/sensors/Sensor.hh 2009-12-18 00:04:10 UTC (rev
8473)
+++ code/gazebo/trunk/server/sensors/Sensor.hh 2009-12-18 17:26:23 UTC (rev
8474)
@@ -35,6 +35,8 @@
{
class XMLConfigNode;
class Body;
+ class World;
+ class Simulator;
class Controller;
/// \addtogroup gazebo_sensor
@@ -100,6 +102,8 @@
/// The body this sensor is attached to
protected: Body *body;
+ protected: World *world;
+ protected: Simulator *simulator;
/// \brief Pointer to the controller of the sensor
protected: Controller *controller;
Modified: code/gazebo/trunk/server/sensors/SensorFactory.cc
===================================================================
--- code/gazebo/trunk/server/sensors/SensorFactory.cc 2009-12-18 00:04:10 UTC
(rev 8473)
+++ code/gazebo/trunk/server/sensors/SensorFactory.cc 2009-12-18 17:26:23 UTC
(rev 8474)
@@ -26,6 +26,7 @@
*/
#include "Sensor.hh"
+#include "SensorManager.hh"
#include "Body.hh"
#include "SensorFactory.hh"
@@ -48,7 +49,9 @@
{
if (sensors[classname])
{
- return (sensors[classname]) (body);
+ Sensor *sensor = (sensors[classname]) (body);
+ SensorManager::Instance()->AddSensor(sensor);
+ return sensor;
}
return NULL;
Added: code/gazebo/trunk/server/sensors/SensorManager.cc
===================================================================
--- code/gazebo/trunk/server/sensors/SensorManager.cc
(rev 0)
+++ code/gazebo/trunk/server/sensors/SensorManager.cc 2009-12-18 17:26:23 UTC
(rev 8474)
@@ -0,0 +1,99 @@
+/*
+ * 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: Class to manager all sensors
+ * Author: Nate Koenig
+ * Date: 18 Dec 2009
+ * SVN info: $Id$
+ */
+
+#include "Sensor.hh"
+#include "SensorManager.hh"
+
+using namespace gazebo;
+
+////////////////////////////////////////////////////////////////////////////////
+/// Constructor
+SensorManager::SensorManager()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Destructor
+SensorManager::~SensorManager()
+{
+ this->sensors.erase(this->sensors.begin(), this->sensors.end());
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Update all the sensors
+void SensorManager::Update()
+{
+ std::list<Sensor*>::iterator iter;
+ for (iter = this->sensors.begin(); iter != this->sensors.end(); iter++)
+ (*iter)->Update();
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Init all the sensors
+void SensorManager::Init()
+{
+ std::list<Sensor*>::iterator iter;
+ for (iter = this->sensors.begin(); iter != this->sensors.end(); iter++)
+ (*iter)->Init();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Finalize all the sensors
+void SensorManager::Fini()
+{
+ std::list<Sensor*>::iterator iter;
+ for (iter = this->sensors.begin(); iter != this->sensors.end(); iter++)
+ (*iter)->Fini();
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+/// Add a sensor
+void SensorManager::AddSensor(Sensor *sensor)
+{
+ if (!sensor)
+ return;
+
+ this->sensors.push_back(sensor);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Remove a sensor
+void SensorManager::RemoveSensor(Sensor *sensor)
+{
+ if (!sensor)
+ return;
+
+ std::list<Sensor*>::iterator iter;
+ for (iter = this->sensors.begin(); iter != this->sensors.end(); iter++)
+ if (*iter == sensor)
+ break;
+
+ if (iter != this->sensors.end())
+ this->sensors.erase(iter);
+}
Added: code/gazebo/trunk/server/sensors/SensorManager.hh
===================================================================
--- code/gazebo/trunk/server/sensors/SensorManager.hh
(rev 0)
+++ code/gazebo/trunk/server/sensors/SensorManager.hh 2009-12-18 17:26:23 UTC
(rev 8474)
@@ -0,0 +1,70 @@
+/*
+ * 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: Class to manager all sensors
+ * Author: Nate Koenig
+ * Date: 18 Dec 2009
+ * SVN info: $Id$
+ */
+
+#ifndef SENSORMANAGER_HH
+#define SENSORMANAGER_HH
+
+#include <list>
+
+#include "SingletonT.hh"
+
+namespace gazebo
+{
+ class Sensor;
+
+ /// \brief Class to manage and update all sensors
+ class SensorManager : public SingletonT<SensorManager>
+ {
+ /// \brief Constructor
+ public: SensorManager();
+
+ /// \brief Destructor
+ public: virtual ~SensorManager();
+
+ /// \brief Update all the sensors
+ public: void Update();
+
+ /// \brief Init all the sensors
+ public: void Init();
+
+ /// \brief Finalize all the sensors
+ public: void Fini();
+
+ /// \brief Add a sensor
+ public: void AddSensor(Sensor *sensor);
+
+ /// \brief Remove a sensor
+ public: void RemoveSensor(Sensor *sensor);
+
+ private: std::list<Sensor *> sensors;
+
+ private: friend class DestroyerT<SensorManager>;
+ private: friend class SingletonT<SensorManager>;
+ };
+}
+
+#endif
Modified: code/gazebo/trunk/server/sensors/contact/ContactSensor.cc
===================================================================
--- code/gazebo/trunk/server/sensors/contact/ContactSensor.cc 2009-12-18
00:04:10 UTC (rev 8473)
+++ code/gazebo/trunk/server/sensors/contact/ContactSensor.cc 2009-12-18
17:26:23 UTC (rev 8474)
@@ -99,6 +99,7 @@
{
// Get the geom from the body
Geom *geom = this->body->GetGeom( **(*iter) );
+ geom->SetContactsEnabled(true);
this->geoms.push_back(geom);
}
}
Modified: code/gazebo/trunk/server/sensors/ray/RaySensor.cc
===================================================================
--- code/gazebo/trunk/server/sensors/ray/RaySensor.cc 2009-12-18 00:04:10 UTC
(rev 8473)
+++ code/gazebo/trunk/server/sensors/ray/RaySensor.cc 2009-12-18 17:26:23 UTC
(rev 8474)
@@ -196,6 +196,6 @@
// Update the sensor information
void RaySensor::UpdateChild()
{
- //if (this->active)
+ if (this->active)
this->laserShape->Update();
}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
This SF.Net email is sponsored by the Verizon Developer Community
Take advantage of Verizon's best-in-class app development support
A streamlined, 14 day to market process makes app distribution fast and easy
Join now and get one step closer to millions of Verizon customers
http://p.sf.net/sfu/verizon-dev2dev
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
