Revision: 8443
http://playerstage.svn.sourceforge.net/playerstage/?rev=8443&view=rev
Author: natepak
Date: 2009-11-24 19:39:13 +0000 (Tue, 24 Nov 2009)
Log Message:
-----------
Added a diagnostic timer
Modified Paths:
--------------
code/gazebo/trunk/server/Entity.cc
code/gazebo/trunk/server/Global.hh
code/gazebo/trunk/server/Model.cc
code/gazebo/trunk/server/Simulator.cc
code/gazebo/trunk/server/Timer.hh
code/gazebo/trunk/server/World.cc
code/gazebo/trunk/server/controllers/Controller.cc
code/gazebo/trunk/server/physics/Body.cc
code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
code/gazebo/trunk/server/sensors/Sensor.cc
Modified: code/gazebo/trunk/server/Entity.cc
===================================================================
--- code/gazebo/trunk/server/Entity.cc 2009-11-24 01:53:58 UTC (rev 8442)
+++ code/gazebo/trunk/server/Entity.cc 2009-11-24 19:39:13 UTC (rev 8443)
@@ -324,12 +324,13 @@
// Handle a change of pose
void Entity::PoseChange(bool notify)
{
-/* if (Simulator::Instance()->GetState() == Simulator::RUN)
- this->visualNode->SetDirty(true, this->relativePose);
- else
- */
if (Simulator::Instance()->GetRenderEngineEnabled())
- this->visualNode->SetPose(this->relativePose);
+ {
+ if (Simulator::Instance()->GetState() == Simulator::RUN)
+ this->visualNode->SetDirty(true, this->relativePose);
+ else
+ this->visualNode->SetPose(this->relativePose);
+ }
if (notify)
{
Modified: code/gazebo/trunk/server/Global.hh
===================================================================
--- code/gazebo/trunk/server/Global.hh 2009-11-24 01:53:58 UTC (rev 8442)
+++ code/gazebo/trunk/server/Global.hh 2009-11-24 19:39:13 UTC (rev 8443)
@@ -80,7 +80,6 @@
#define ROUND(x) ( (int)( floor((x)+0.5) ) )
-#undef TIMING
#undef USE_THREADPOOL
#endif
Modified: code/gazebo/trunk/server/Model.cc
===================================================================
--- code/gazebo/trunk/server/Model.cc 2009-11-24 01:53:58 UTC (rev 8442)
+++ code/gazebo/trunk/server/Model.cc 2009-11-24 19:39:13 UTC (rev 8443)
@@ -48,11 +48,6 @@
#include "IfaceFactory.hh"
#include "Model.hh"
-
-#ifdef TIMING
-#include "Simulator.hh"
-#endif
-
using namespace gazebo;
uint Model::lightNumber = 0;
@@ -393,6 +388,11 @@
// Update the model
void Model::Update()
{
+ if (this->controllers.size() == 0 && this->IsStatic())
+ return;
+
+ DiagnosticTimer timer("Model[" + this->GetName() + "] Update ");
+
#ifdef USE_THREADPOOL
World::Instance()->GetPhysicsEngine()->InitForThread();
#endif
@@ -403,59 +403,54 @@
Pose3d bodyPose, newPose, oldPose;
-#ifdef TIMING
- double tmpT1 = Simulator::Instance()->GetWallTime();
-#endif
-
- for (bodyIter=this->bodies.begin(); bodyIter!=this->bodies.end(); bodyIter++)
{
- if (bodyIter->second)
+ DiagnosticTimer timer("Model[" + this->GetName() + "] Bodies Update ");
+
+ for (bodyIter=this->bodies.begin();
+ bodyIter!=this->bodies.end(); bodyIter++)
{
+ if (bodyIter->second)
+ {
#ifdef USE_THREADPOOL
-
World::Instance()->threadPool->schedule(boost::bind(&Body::Update,(bodyIter->second)));
+ World::Instance()->threadPool->schedule(
+ boost::bind(&Body::Update,(bodyIter->second)));
#else
- bodyIter->second->Update();
+ bodyIter->second->Update();
#endif
+ }
}
}
-#ifdef TIMING
- double tmpT2 = Simulator::Instance()->GetWallTime();
- std::cout << " ALL Bodies DT (" << this->GetName() << ") (" <<
tmpT2-tmpT1 << ")" << std::endl;
-#endif
-
- for (contIter=this->controllers.begin();
- contIter!=this->controllers.end(); contIter++)
{
- if (contIter->second)
+ DiagnosticTimer timer("Model[" + this->GetName() + "] Controllers Update
");
+
+ for (contIter=this->controllers.begin();
+ contIter!=this->controllers.end(); contIter++)
{
+ if (contIter->second)
+ {
#ifdef USE_THREADPOOL
-
World::Instance()->threadPool->schedule(boost::bind(&Controller::Update,(contIter->second)));
+
World::Instance()->threadPool->schedule(boost::bind(&Controller::Update,(contIter->second)));
#else
- contIter->second->Update();
+ contIter->second->Update();
#endif
+ }
}
}
-#ifdef TIMING
- double tmpT3 = Simulator::Instance()->GetWallTime();
- std::cout << " ALL Controllers DT (" << this->GetName() << ") (" <<
tmpT3-tmpT2 << ")" << std::endl;
-#endif
- 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();
+ (*jointIter)->Update();
#endif
+ }
}
-#ifdef TIMING
- double tmpT4 = Simulator::Instance()->GetWallTime();
- std::cout << " ALL Joints/canonical body (" << this->GetName() << ") DT
(" << tmpT4-tmpT3 << ")" << std::endl;
-#endif
-
// Call the model's python update function, if one exists
/*if (this->pFuncUpdate)
{
@@ -483,16 +478,10 @@
this->rpyP->SetValue(this->pose.rot);
}*/
- this->UpdateChild();
-
-#ifdef TIMING
- double tmpT5 = Simulator::Instance()->GetWallTime();
- std::cout << " ALL child (" << this->GetName() << ") update DT ("
- << tmpT5-tmpT4 << ")" << std::endl;
- std::cout << " Models::Update() (" << this->GetName()
- << ") Total DT (" << tmpT5-tmpT1 << ")" << std::endl;
-#endif
-
+ {
+ DiagnosticTimer timer("Model[" + this->GetName() + "] Children Update ");
+ this->UpdateChild();
+ }
}
Modified: code/gazebo/trunk/server/Simulator.cc
===================================================================
--- code/gazebo/trunk/server/Simulator.cc 2009-11-24 01:53:58 UTC (rev
8442)
+++ code/gazebo/trunk/server/Simulator.cc 2009-11-24 19:39:13 UTC (rev
8443)
@@ -333,22 +333,15 @@
{
this->state = RUN;
+ DiagnosticTimer timer("--------------------------- START
Simulator::MainLoop() --------------------------");
Time currTime = 0;
Time lastTime = 0;
+ struct timespec timeSpec;
double freq = 80.0;
-
-#ifdef TIMING
- double tmpT1 = this->GetWallTime();
- std::cout << "--------------------------- START Simulator::MainLoop()
--------------------------" << std::endl;
- std::cout << "Simulator::MainLoop() simTime(" << this->simTime << ") world
time (" << tmpT1 << ")" << std::endl;
-#endif
-
this->physicsThread = new boost::thread(
boost::bind(&Simulator::PhysicsLoop, this));
- struct timespec timeSpec;
-
// Update the gui
while (!this->userQuit)
{
@@ -389,11 +382,6 @@
}
this->physicsThread->join();
-
-#ifdef TIMING
- std::cout << "--------------------------- END Simulator::MainLoop()
--------------------------" << std::endl;
-#endif
-
}
////////////////////////////////////////////////////////////////////////////////
@@ -639,15 +627,11 @@
Time lastTime = this->GetRealTime();
struct timespec req, rem;
- Timer timer(Timer::REAL_TIMER);
while (!this->userQuit)
{
+ DiagnosticTimer timer("PhysicsLoop Timer ");
-#ifdef TIMING
- timer.Start();
-#endif
-
currTime = this->GetRealTime();
userStepped = false;
@@ -673,19 +657,9 @@
{
boost::recursive_mutex::scoped_lock lock(*this->mutex);
-
-#ifdef TIMING
- timer.Report("Lock DT");
- //double tmpT2 = Simulator::Instance()->GetWallTime();
- //std::cout << " LOCK DT(" << tmpT2-tmpT1 << ")" << std::endl;
-#endif
world->Update();
}
-#ifdef TIMING
- std::cout << " World::Update() DT(" << timer << ")" << std::endl;
-#endif
-
currTime = this->GetRealTime();
// Set a default sleep time
@@ -716,12 +690,12 @@
nanosleep(&req, &rem);
- // Process all incoming messages from simiface
- world->UpdateSimulationIface();
+ {
+ DiagnosticTimer timer("PhysicsLoop UpdateSimIfaces ");
-#ifdef TIMING
- std::cout << " World::UpdatSimulationIface() DT(" << timer << ")" <<
std::endl;
-#endif
+ // Process all incoming messages from simiface
+ world->UpdateSimulationIface();
+ }
if (this->timeout > 0 && this->GetRealTime() > this->timeout)
{
@@ -734,10 +708,6 @@
this->SetStepInc(false);
this->SetPaused(true);
}
-
-#ifdef TIMING
- std::cout << " Simulator::PhysicsLoop() DT(" << timer << ")" << std::endl;
-#endif
}
}
Modified: code/gazebo/trunk/server/Timer.hh
===================================================================
--- code/gazebo/trunk/server/Timer.hh 2009-11-24 01:53:58 UTC (rev 8442)
+++ code/gazebo/trunk/server/Timer.hh 2009-11-24 19:39:13 UTC (rev 8443)
@@ -26,6 +26,7 @@
#ifndef TIMER_HH
#define TIMER_HH
+#include "GazeboMessage.hh"
#include "Time.hh"
namespace gazebo
@@ -62,6 +63,36 @@
private: Time start;
private: Type type;
};
+
+ /// \brief A timer designed for diagnostics
+ class DiagnosticTimer : public Timer
+ {
+ /// \brief Constructor
+ public: DiagnosticTimer(const std::string &name, int level = 5,
+ Type t=Timer::REAL_TIMER) : Timer(t)
+ {
+ this->name = name;
+ this->msgLevel = level;
+ this->Report("Start @ ");
+ }
+
+ /// \brief Destructor
+ public: virtual ~DiagnosticTimer()
+ {
+ this->Report("Complete @ ");
+ }
+
+ /// \brief Report a time
+ public: void Report(const std::string msg)
+ {
+ gzmsg(this->msgLevel) << this->name << "["
+ << msg << *this << "]\n";
+ }
+
+ private: int msgLevel;
+ private: std::string name;
+
+ };
}
#endif
Modified: code/gazebo/trunk/server/World.cc
===================================================================
--- code/gazebo/trunk/server/World.cc 2009-11-24 01:53:58 UTC (rev 8442)
+++ code/gazebo/trunk/server/World.cc 2009-11-24 19:39:13 UTC (rev 8443)
@@ -298,7 +298,6 @@
// Update the world
void World::Update()
{
-
if (this->simPauseTime > 0)
{
if (Simulator::Instance()->GetSimTime() >= this->simPauseTime)
@@ -315,15 +314,12 @@
}
}
-#ifdef TIMING
- double tmpT1 = Simulator::Instance()->GetWallTime();
-#endif
- // Update all the models
- std::vector< Model* >::iterator miter;
- for (miter=this->models.begin(); miter!=this->models.end(); miter++)
{
- if (!(*miter)->IsStatic())
+ DiagnosticTimer timer("World::Update Models");
+ // Update all the models
+ std::vector< Model* >::iterator miter;
+ for (miter=this->models.begin(); miter!=this->models.end(); miter++)
{
#ifdef USE_THREADPOOL
this->threadPool->schedule(boost::bind(&Model::Update,(*miter)));
@@ -337,31 +333,22 @@
this->threadPool->wait();
#endif
-
-#ifdef TIMING
- double tmpT2 = Simulator::Instance()->GetWallTime();
- std::cout << " World::Update() ALL Models update DT(" << tmpT2-tmpT1 << ")"
<< std::endl;
-#endif
-
if (!Simulator::Instance()->IsPaused() &&
Simulator::Instance()->GetPhysicsEnabled())
{
- this->physicsEngine->UpdatePhysics();
+ {
+ DiagnosticTimer timer("World::Update Physics");
+ this->physicsEngine->UpdatePhysics();
+ }
- if (this->saveStateTimer.GetElapsed() > **this->saveStateTimeoutP)
+ /*if (this->saveStateTimer.GetElapsed() > **this->saveStateTimeoutP)
{
this->SaveState();
this->saveStateTimer.Start();
- }
+ }*/
}
this->factory->Update();
-
-#ifdef TIMING
- double tmpT4 = Simulator::Instance()->GetWallTime();
- std::cout << " World::Update() Physics engine DT(" << tmpT4-tmpT2 << ")" <<
std::endl;
-#endif
-
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/trunk/server/controllers/Controller.cc
===================================================================
--- code/gazebo/trunk/server/controllers/Controller.cc 2009-11-24 01:53:58 UTC
(rev 8442)
+++ code/gazebo/trunk/server/controllers/Controller.cc 2009-11-24 19:39:13 UTC
(rev 8443)
@@ -25,6 +25,7 @@
* SVN info: $Id$
*/
+#include "Timer.hh"
#include "Model.hh"
#include "Sensor.hh"
#include "gazebo.h"
@@ -192,8 +193,9 @@
/// Update the controller. Called every cycle.
void Controller::Update()
{
- if (this->IsConnected() || this->alwaysOnP->GetValue())
+ if (this->IsConnected() || **this->alwaysOnP)
{
+ DiagnosticTimer timer("Controller[" + this->GetName() +"] Update Timer");
// round time difference to this->physicsEngine->GetStepTime()
Time physics_dt = World::Instance()->GetPhysicsEngine()->GetStepTime();
@@ -205,18 +207,14 @@
// << " i1 " <<
round((Simulator::Instance()->GetSimTime()-lastUpdate)/physics_dt)
// << " i2 " << round(updatePeriod/physics_dt)
// << std::endl;
-#ifdef TIMING
- double tmpT1 = Simulator::Instance()->GetWallTime();
-#endif
+ timer.Start();
+
Time simTime = Simulator::Instance()->GetSimTime();
if ((simTime-lastUpdate-updatePeriod)/physics_dt >= 0)
{
this->UpdateChild();
lastUpdate = Simulator::Instance()->GetSimTime();
-#ifdef TIMING
- double tmpT2 = Simulator::Instance()->GetWallTime();
- std::cout << " Controller::Update() Name (" << this->GetName()
<< ") dt (" << tmpT2-tmpT1 << ")" << std::endl;
-#endif
+ timer.Report("Update() dt");
}
}
}
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2009-11-24 01:53:58 UTC (rev
8442)
+++ code/gazebo/trunk/server/physics/Body.cc 2009-11-24 19:39:13 UTC (rev
8443)
@@ -31,7 +31,7 @@
#include "Model.hh"
#include "GazeboMessage.hh"
#include "Geom.hh"
-
+#include "Timer.hh"
#include "OgreVisual.hh"
#include "OgreCreator.hh"
#include "OgreDynamicLines.hh"
@@ -47,10 +47,6 @@
#include "Body.hh"
-#ifdef TIMING
-#include "Simulator.hh"// for timing
-#endif
-
using namespace gazebo;
////////////////////////////////////////////////////////////////////////////////
@@ -395,6 +391,8 @@
// Update the body
void Body::Update()
{
+ DiagnosticTimer timer("Body[" + this->GetName() +"] Update");
+
#ifdef USE_THREADPOOL
// If calling Body::Update in threadpool
World::Instance()->GetPhysicsEngine()->InitForThread();
@@ -405,58 +403,40 @@
Vector3 vel;
Vector3 avel;
-#ifdef TIMING
- double tmpT1 = Simulator::Instance()->GetWallTime();
-#endif
-
-#ifdef TIMING
- double tmpT2 = Simulator::Instance()->GetWallTime();
- std::cout << " Body Name (" << this->nameP->GetValue() << ")" <<
std::endl;
- std::cout << " UpdatePose dt (" << tmpT2-tmpT1 << ")" <<
std::endl;
-#endif
-
// Apply our linear accel
this->SetForce(this->linearAccel);
// Apply our angular accel
this->SetTorque(this->angularAccel);
-#ifdef TIMING
- double tmpT3 = Simulator::Instance()->GetWallTime();
- std::cout << " Static SetPose dt (" << tmpT3-tmpT2 << ")" <<
std::endl;
-#endif
-
- for (geomIter=this->geoms.begin();
- geomIter!=this->geoms.end(); geomIter++)
{
+ DiagnosticTimer timer("Body[" + this->GetName() +"] Update Geoms");
+
+ for (geomIter=this->geoms.begin();
+ geomIter!=this->geoms.end(); geomIter++)
+ {
#ifdef USE_THREADPOOL
- World::Instance()->threadpool->schedule(boost::bind(&Geom::Update,
(geomIter->second)));
+ World::Instance()->threadpool->schedule(boost::bind(&Geom::Update,
(geomIter->second)));
#else
- geomIter->second->Update();
+ geomIter->second->Update();
#endif
+ }
}
-#ifdef TIMING
- double tmpT4 = Simulator::Instance()->GetWallTime();
- std::cout << " Geom Update DT (" << tmpT4-tmpT3 << ")" <<
std::endl;
-#endif
-
- for (sensorIter=this->sensors.begin();
- sensorIter!=this->sensors.end(); sensorIter++)
{
+ 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)));
+ World::Instance()->threadpool->schedule(boost::bind(&Sensor::Update,
(sensorIter)));
#else
- (*sensorIter)->Update();
+ (*sensorIter)->Update();
#endif
+ }
}
-#ifdef TIMING
- double tmpT5 = Simulator::Instance()->GetWallTime();
- std::cout << " ALL Sensors Update DT (" << tmpT5-tmpT4 << ")"
<< std::endl;
- std::cout << " Body::Update Total DT (" << tmpT5-tmpT1 << ")"
<< std::endl;
-#endif
-
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2009-11-24 01:53:58 UTC
(rev 8442)
+++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2009-11-24 19:39:13 UTC
(rev 8443)
@@ -26,6 +26,7 @@
#include <assert.h>
+#include "Timer.hh"
#include "PhysicsFactory.hh"
#include "Global.hh"
#include "GazeboMessage.hh"
@@ -55,10 +56,6 @@
#include "ODEPhysics.hh"
-#ifdef TIMING
-#include "Simulator.hh"// for timing
-#endif
-
using namespace gazebo;
GZ_REGISTER_PHYSICS_ENGINE("ode", ODEPhysics);
@@ -83,8 +80,8 @@
// If auto-disable is active, then user interaction with the joints
// doesn't behave properly
- dWorldSetAutoDisableFlag(this->worldId, 1);
- dWorldSetAutoDisableTime(this->worldId, 2.0);
+ //dWorldSetAutoDisableFlag(this->worldId, 1);
+ //dWorldSetAutoDisableTime(this->worldId, 2.0);
Param::Begin(&this->parameters);
this->globalCFMP = new ParamT<double>("cfm", 10e-5, 0);
@@ -195,15 +192,14 @@
// Update the ODE collisions, create joints
void ODEPhysics::UpdateCollision()
{
+ DiagnosticTimer timer("ODEPhysics Collision Update");
std::vector<ContactFeedback>::iterator iter;
std::vector<dJointFeedback>::iterator jiter;
+
+ timer.Start();
-#ifdef TIMING
- double tmpT1 = Simulator::Instance()->GetWallTime();
-#endif
-
- this->LockMutex();
// Do collision detection; this will add contacts to the contact group
+ this->LockMutex();
dSpaceCollide( this->spaceId, this, CollisionCallback );
this->UnlockMutex();
@@ -241,42 +237,24 @@
// Reset the contact pointer
this->contactFeedbackIter = this->contactFeedbacks.begin();
-
-#ifdef TIMING
- double tmpT2 = Simulator::Instance()->GetWallTime();
- std::cout << " Collision DT (" << tmpT2-tmpT1 << ")" << std::endl;
-#endif
}
////////////////////////////////////////////////////////////////////////////////
// Update the ODE engine
void ODEPhysics::UpdatePhysics()
{
-#ifdef TIMING
- double tmpT1 = Simulator::Instance()->GetWallTime();
-#endif
-
this->UpdateCollision();
-#ifdef TIMING
- double tmpT2 = Simulator::Instance()->GetWallTime();
- std::cout << " Collision DT (" << tmpT2-tmpT1 << ")" << std::endl;
-#endif
-
this->LockMutex();
+ DiagnosticTimer timer("ODEPhysics Step Update");
+
// Update the dynamical model
if (this->quickStepP->GetValue())
dWorldQuickStep(this->worldId, (**this->stepTimeP).Double());
else
dWorldStep( this->worldId, (**this->stepTimeP).Double() );
-#ifdef TIMING
- double tmpT3 = Simulator::Instance()->GetWallTime();
- std::cout << " ODE step DT (" << tmpT3-tmpT2 << ")" << std::endl;
- //std::cout << " Physics Total DT (" << tmpT3-tmpT1 << ")" << std::endl;
-#endif
-
// Very important to clear out the contact group
dJointGroupEmpty( this->contactGroup );
Modified: code/gazebo/trunk/server/sensors/Sensor.cc
===================================================================
--- code/gazebo/trunk/server/sensors/Sensor.cc 2009-11-24 01:53:58 UTC (rev
8442)
+++ code/gazebo/trunk/server/sensors/Sensor.cc 2009-11-24 19:39:13 UTC (rev
8443)
@@ -23,6 +23,7 @@
* SVN: $Id$
*/
+#include "Timer.hh"
#include "Controller.hh"
#include "gazebo.h"
#include "GazeboMessage.hh"
@@ -118,11 +119,8 @@
/// Update the sensor
void Sensor::Update()
{
+ DiagnosticTimer timer("Sensor[" + this->GetName() + "] Update");
-#ifdef TIMING
- double tmpT4 = Simulator::Instance()->GetWallTime();
-#endif
-
Time physics_dt = World::Instance()->GetPhysicsEngine()->GetStepTime();
if
(((Simulator::Instance()->GetSimTime()-this->lastUpdate-this->updatePeriod)/physics_dt)
>= 0)
{
@@ -135,12 +133,6 @@
{
this->controller->Update();
}
-
-#ifdef TIMING
- double tmpT5 = Simulator::Instance()->GetWallTime();
- std::cout << " Sensor::Update (" << this->GetName() << ") dt
(" << tmpT5-tmpT4 << ")" << std::endl;
-#endif
-
}
////////////////////////////////////////////////////////////////////////////////
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Let Crystal Reports handle the reporting - Free Crystal Reports 2008 30-Day
trial. Simplify your report design, integration and deployment - and focus on
what you do best, core application coding. Discover what's new with
Crystal Reports now. http://p.sf.net/sfu/bobj-july
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
