Revision: 7723
http://playerstage.svn.sourceforge.net/playerstage/?rev=7723&view=rev
Author: hsujohnhsu
Date: 2009-05-26 18:13:32 +0000 (Tue, 26 May 2009)
Log Message:
-----------
more threads.
1. collision
2. models update
3. physics step
1 and 2 runs in parallel
3 runs after 1 and 2 are done.
Modified Paths:
--------------
code/gazebo/branches/threads/server/Simulator.cc
code/gazebo/branches/threads/server/Simulator.hh
code/gazebo/branches/threads/server/World.cc
code/gazebo/branches/threads/server/World.hh
code/gazebo/branches/threads/server/physics/ode/ODEPhysics.cc
Modified: code/gazebo/branches/threads/server/Simulator.cc
===================================================================
--- code/gazebo/branches/threads/server/Simulator.cc 2009-05-26 17:23:05 UTC
(rev 7722)
+++ code/gazebo/branches/threads/server/Simulator.cc 2009-05-26 18:13:32 UTC
(rev 7723)
@@ -47,7 +47,7 @@
#include "Simulator.hh"
-#define MAX_FRAME_RATE 60
+#define MAX_FRAME_RATE 60.0
using namespace gazebo;
@@ -78,6 +78,13 @@
selectedBody(NULL)
{
this->mutex = new boost::recursive_mutex();
+ this->p_mutex = new boost::mutex();
+ this->c_mutex = new boost::mutex();
+ this->m_mutex = new boost::mutex();
+
+ this->p_cond = new boost::condition_variable();
+ this->c_cond = new boost::condition_variable();
+ this->m_cond = new boost::condition_variable();
}
////////////////////////////////////////////////////////////////////////////////
@@ -115,11 +122,42 @@
this->physicsThread = NULL;
}
- if (this->mutex)
+ if (this->p_mutex)
{
- delete this->mutex;
- this->mutex = NULL;
+ delete this->p_mutex;
+ this->p_mutex = NULL;
}
+
+ if (this->c_mutex)
+ {
+ delete this->c_mutex;
+ this->c_mutex = NULL;
+ }
+
+ if (this->m_mutex)
+ {
+ delete this->m_mutex;
+ this->m_mutex = NULL;
+ }
+
+ if (this->p_cond)
+ {
+ delete this->p_cond;
+ this->p_cond = NULL;
+ }
+
+ if (this->c_cond)
+ {
+ delete this->c_cond;
+ this->c_cond = NULL;
+ }
+
+ if (this->m_cond)
+ {
+ delete this->m_cond;
+ this->m_cond = NULL;
+ }
+
}
////////////////////////////////////////////////////////////////////////////////
@@ -330,8 +368,8 @@
void Simulator::MainLoop()
{
double currTime = 0;
- double lastTime = 0;
- double freq = 30.0;
+ double renderUpdateRate =
std::max(MAX_FRAME_RATE,OgreAdaptor::Instance()->GetUpdateRate());
+ double renderUpdatePeriod = 1.0 / renderUpdateRate;
#ifdef TIMING
double tmpT1 = this->GetWallTime();
@@ -342,13 +380,15 @@
this->physicsThread = new boost::thread(
boost::bind(&Simulator::PhysicsLoop, this));
+ this->prevRenderTime = this->GetRealTime();
+
// Update the gui
while (!this->userQuit)
{
- currTime = this->GetWallTime();
- if ( currTime - lastTime > 1/freq)
+ currTime = this->GetRealTime();
+ if ( currTime - this->prevRenderTime > renderUpdatePeriod)
{
- lastTime = this->GetWallTime();
+ this->prevRenderTime = this->GetRealTime();
OgreAdaptor::Instance()->UpdateCameras();
@@ -359,16 +399,14 @@
World::Instance()->ProcessEntitiesToLoad();
- currTime = this->GetWallTime();
-
- if (currTime - lastTime < 1/freq)
+ if (this->GetRealTime() - this->prevRenderTime < renderUpdatePeriod)
{
- usleep((1/freq - (currTime - lastTime)) * 1e6);
+ usleep((int)((renderUpdatePeriod - (currTime - this->prevRenderTime))
* 1e6));
}
}
else
{
- usleep((1/freq - currTime - lastTime) * 1e6);
+ usleep((int)((renderUpdatePeriod - (currTime - this->prevRenderTime)) *
1e6));
}
}
@@ -467,6 +505,12 @@
}
////////////////////////////////////////////////////////////////////////////////
+bool Simulator::GetUserQuit() const
+{
+ return this->userQuit;
+}
+
+////////////////////////////////////////////////////////////////////////////////
void Simulator::SetUserPause(bool pause)
{
this->userPause = pause;
@@ -617,26 +661,59 @@
double step = world->GetPhysicsEngine()->GetStepTime();
double physicsUpdateRate = world->GetPhysicsEngine()->GetUpdateRate();
- //double renderUpdateRate = OgreAdaptor::Instance()->GetUpdateRate();
double physicsUpdatePeriod = 1.0 / physicsUpdateRate;
- //double renderUpdatePeriod = 1.0 / renderUpdateRate;
double currTime;
this->prevPhysicsTime = this->GetRealTime();
- this->prevRenderTime = this->GetRealTime();
+ /*************************************************/
+ /* */
+ /* thread control */
+ /* */
+ /*************************************************/
+ // spawning thread for collision checking
+ this->collisionsThread = new
boost::thread(boost::bind(&World::CollisionsLoop, world));
+
+ // spawning thread for models update
+ this->modelsThread = new boost::thread(boost::bind(&World::ModelsLoop,
world));
+
+ this->run_collision = true;
+ this->run_models = true;
+
+ /*********************************************/
+ /* loop for physics / constraints update */
+ /*********************************************/
while (!this->userQuit)
{
+ // thread control
+ {
+ boost::lock_guard<boost::mutex> c_lock(*this->c_mutex);
+ this->run_collision=true;
+ boost::lock_guard<boost::mutex> m_lock(*this->m_mutex);
+ this->run_models=true;
+ }
+ this->c_cond->notify_all();
+ this->m_cond->notify_all();
+
+
+
+ // wait for c and m threads to finish
+ boost::unique_lock<boost::mutex> p_lock(*this->p_mutex);
+ while (this->run_collision||this->run_models)
+ {
+ this->p_cond->wait(p_lock);
+ }
+
+
currTime = this->GetRealTime();
if (physicsUpdateRate == 0 ||
currTime - this->prevPhysicsTime >= physicsUpdatePeriod)
{
+ this->prevPhysicsTime = this->GetRealTime();
// Update the physics engine
- //if (!this->GetUserPause() && !this->IsPaused() ||
- // (this->GetUserPause() && this->GetUserStepInc()))
if (!this->IsPaused())
{
this->simTime += step;
@@ -646,15 +723,21 @@
else
{
this->pauseTime += step;
- // this->pause=true;
}
- this->prevPhysicsTime = this->GetRealTime();
-
+ // lock mutex and update physics
{
boost::recursive_mutex::scoped_lock lock(*this->mutex);
- world->Update();
+ //world->Update();
+ world->UpdatePhysics();
}
+
+ // if we are throttling physics update rate
+ // if (physicsUpdateRate != 0 &&
+ // this->GetRealTime() - this->prevPhysicsTime < physicsUpdatePeriod)
+ // {
+ // usleep((physicsUpdatePeriod - (currTime - this->prevRenderTime)) *
1e6);
+ // }
usleep(1);
}
@@ -666,7 +749,16 @@
this->userQuit = true;
break;
}
+
+ // // thread control
+
}
+
+ // // get ready to quit
+ // this->cp_mutex->unlock();
+ // this->mp_mutex->unlock();
+
+
}
////////////////////////////////////////////////////////////////////////////////
@@ -676,4 +768,74 @@
return this->mutex;
}
+////////////////////////////////////////////////////////////////////////////////
+/// Mutex
+boost::mutex* Simulator::GetC()
+{
+ return this->c_mutex;
+}
+////////////////////////////////////////////////////////////////////////////////
+/// Mutex
+boost::mutex* Simulator::GetP()
+{
+ return this->p_mutex;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Mutex
+boost::mutex* Simulator::GetM()
+{
+ return this->m_mutex;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Mutex
+boost::condition_variable* Simulator::GetPCond()
+{
+ return this->p_cond;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Mutex
+boost::condition_variable* Simulator::GetCCond()
+{
+ return this->c_cond;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Mutex
+boost::condition_variable* Simulator::GetMCond()
+{
+ return this->m_cond;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Mutex
+bool Simulator::RunCollision()
+{
+ return this->run_collision;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Mutex
+bool Simulator::RunModels()
+{
+ return this->run_models;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Mutex
+void Simulator::CollisionDone()
+{
+ this->run_collision = false;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Mutex
+void Simulator::ModelsDone()
+{
+ this->run_models = false;
+}
+
+
Modified: code/gazebo/branches/threads/server/Simulator.hh
===================================================================
--- code/gazebo/branches/threads/server/Simulator.hh 2009-05-26 17:23:05 UTC
(rev 7722)
+++ code/gazebo/branches/threads/server/Simulator.hh 2009-05-26 18:13:32 UTC
(rev 7723)
@@ -34,6 +34,7 @@
namespace boost
{
class recursive_mutex;
+ class mutex;
}
namespace gazebo
@@ -133,6 +134,9 @@
/// \brief Return true if the user has pased
public: bool GetUserPause() const;
+ /// \brief Return true if the user has decided to quit simulation
+ public: bool GetUserQuit() const;
+
/// \brief Set whether the user has paused
public: void SetUserPause(bool pause);
@@ -249,9 +253,36 @@
/// Thread in which to run the gui
private: boost::thread *physicsThread;
+ private: boost::thread *collisionsThread;
+ private: boost::thread *modelsThread;
+ // mutex between rendering and (physics,collision,models update)
private: boost::recursive_mutex *mutex;
+ // mutex for physics, collisions, models update sequence control
+ private: boost::mutex *c_mutex;
+ private: boost::mutex *m_mutex;
+ private: boost::mutex *p_mutex;
+
+ private: boost::condition_variable *p_cond;
+ private: boost::condition_variable *c_cond;
+ private: boost::condition_variable *m_cond;
+
+ public: boost::mutex* GetC();
+ public: boost::mutex* GetP();
+ public: boost::mutex* GetM();
+ public: boost::condition_variable* GetPCond();
+ public: boost::condition_variable* GetCCond();
+ public: boost::condition_variable* GetMCond();
+
+ private: bool run_collision;
+ private: bool run_models;
+
+ public: bool RunCollision();
+ public: bool RunModels();
+ public: void CollisionDone();
+ public: void ModelsDone();
+
//Singleton implementation
private: friend class DestroyerT<Simulator>;
private: friend class SingletonT<Simulator>;
Modified: code/gazebo/branches/threads/server/World.cc
===================================================================
--- code/gazebo/branches/threads/server/World.cc 2009-05-26 17:23:05 UTC
(rev 7722)
+++ code/gazebo/branches/threads/server/World.cc 2009-05-26 18:13:32 UTC
(rev 7723)
@@ -273,6 +273,159 @@
}
////////////////////////////////////////////////////////////////////////////////
+// Update physics / constraints
+void World::UpdatePhysics()
+{
+
+ if (this->simPauseTime > 0)
+ {
+ if (Simulator::Instance()->GetSimTime() >= this->simPauseTime)
+ {
+ //printf("SimTime[%f] PauseTime[%f]\n",
Simulator::Instance()->GetSimTime(), this->simPauseTime);
+
+ this->simPauseTime = 0;
+ Simulator::Instance()->SetPaused(true);
+
+ // Tell the simiface that it's okay to trigger the go ack
+ this->simIface->GoAckPost();
+ }
+ else
+ {
+ Simulator::Instance()->SetPaused(false);
+ }
+ }
+
+#ifdef TIMING
+ double tmpT1 = Simulator::Instance()->GetWallTime();
+#endif
+
+ if (!Simulator::Instance()->IsPaused() &&
+ Simulator::Instance()->GetPhysicsEnabled())
+ {
+ this->physicsEngine->UpdatePhysics();
+ }
+
+#ifdef TIMING
+ double tmpT2 = Simulator::Instance()->GetWallTime();
+ std::cout << " World::Update() Physics engine DT(" << tmpT2-tmpT1 << ")" <<
std::endl;
+#endif
+
+ this->graphics->Update();
+
+#ifdef TIMING
+ double tmpT3 = Simulator::Instance()->GetWallTime();
+ std::cout << " World::Update() graphics DT(" << tmpT3-tmpT2 << ")" <<
std::endl;
+#endif
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Update collision detection
+void World::CollisionsLoop()
+{
+
+ Simulator* simulator = Simulator::Instance();
+
+ // initialize
+ //std::cout << "C: locking pc" << std::endl;
+ //simulator->GetPC()->lock(); // make sure pc is locked before releasing
c_init
+ //std::cout << "C: unlocking c_init" << std::endl;
+ //simulator->GetCInit()->unlock(); // unlock c_init so main thread can begin
+
+ while (!simulator->GetUserQuit())
+ {
+
+ boost::unique_lock<boost::mutex> lock(*simulator->GetC());
+ while (!simulator->RunCollision())
+ {
+ simulator->GetCCond()->wait(lock);
+ }
+
+#ifdef TIMING
+ double tmpT1 = Simulator::Instance()->GetWallTime();
+#endif
+
+ if (!Simulator::Instance()->IsPaused() &&
+ Simulator::Instance()->GetPhysicsEnabled())
+ {
+ boost::recursive_mutex::scoped_lock
lock(*Simulator::Instance()->GetMutex());
+ this->physicsEngine->UpdateCollision();
+ }
+
+#ifdef TIMING
+ double tmpT2 = Simulator::Instance()->GetWallTime();
+ std::cout << " World::Update() Collision DT(" << tmpT2-tmpT1 << ")" <<
std::endl;
+#endif
+
+ // thread control
+ // std::cout << "C: unlocking pc" << std::endl;
+ // simulator->GetPC()->unlock();
+ // std::cout << "C: locking cp" << std::endl;
+ // simulator->GetCP()->lock();
+ // std::cout << "C: locking pc" << std::endl;
+ // simulator->GetPC()->lock();
+ // std::cout << "C: unlocking cp" << std::endl;
+ // simulator->GetCP()->unlock();
+ {
+ boost::lock_guard<boost::mutex> p_lock(*simulator->GetP());
+ simulator->CollisionDone();
+ }
+ simulator->GetPCond()->notify_all();
+ }
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Update models and their interfaces
+void World::ModelsLoop()
+{
+
+ Simulator* simulator = Simulator::Instance();
+
+ // initialize
+
+ while (!simulator->GetUserQuit())
+ {
+
+ boost::unique_lock<boost::mutex> lock(*simulator->GetM());
+ while (!simulator->RunModels())
+ {
+ simulator->GetMCond()->wait(lock);
+ }
+
+#ifdef TIMING
+ double tmpT1 = Simulator::Instance()->GetWallTime();
+#endif
+
+ {
+ boost::recursive_mutex::scoped_lock
lock(*Simulator::Instance()->GetMutex());
+ // Update all the models
+ std::vector< Model* >::iterator miter;
+ for (miter=this->models.begin(); miter!=this->models.end(); miter++)
+ {
+ if (*miter)
+ {
+ (*miter)->Update();
+ }
+ }
+ }
+
+#ifdef TIMING
+ double tmpT2 = Simulator::Instance()->GetWallTime();
+ std::cout << " World::Update() ALL Models update DT(" << tmpT2-tmpT1 <<
")" << std::endl;
+#endif
+
+ // thread control
+ {
+ boost::lock_guard<boost::mutex> p_lock(*simulator->GetP());
+ simulator->ModelsDone();
+ }
+ simulator->GetPCond()->notify_all();
+ }
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
// Process messages
void World::ProcessMessages()
{
Modified: code/gazebo/branches/threads/server/World.hh
===================================================================
--- code/gazebo/branches/threads/server/World.hh 2009-05-26 17:23:05 UTC
(rev 7722)
+++ code/gazebo/branches/threads/server/World.hh 2009-05-26 18:13:32 UTC
(rev 7723)
@@ -82,6 +82,13 @@
/// Update the world
public: void Update();
+ /// Update physics / constraints
+ public: void UpdatePhysics();
+ /// Update collision detection
+ public: void CollisionsLoop();
+ /// Update models and their interfaces
+ public: void ModelsLoop();
+
/// Finilize the world
public: void Fini();
@@ -109,10 +116,6 @@
/// \return The real time
public: double GetRealTime() const;
- /// \brief Get the wall clock time
- /// \return The wall clock time
- public: double GetWallTime() const;
-
/// \brief Load all entities
/// \param node XMLConfg node pointer
/// \param parent Parent of the model to load
Modified: code/gazebo/branches/threads/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/gazebo/branches/threads/server/physics/ode/ODEPhysics.cc
2009-05-26 17:23:05 UTC (rev 7722)
+++ code/gazebo/branches/threads/server/physics/ode/ODEPhysics.cc
2009-05-26 18:13:32 UTC (rev 7723)
@@ -164,47 +164,13 @@
////////////////////////////////////////////////////////////////////////////////
// Update the ODE engine
-// void ODEPhysics::UpdatePhysics()
-// {
-// #ifdef TIMING
-// double tmpT1 = Simulator::Instance()->GetWallTime();
-// #endif
-//
-// // Update the dynamical model
-// if (this->quickStepP->GetValue())
-// dWorldQuickStep(this->worldId, this->stepTimeP->GetValue() );
-// else
-// dWorldStep( this->worldId, this->stepTimeP->GetValue() );
-//
-// #ifdef TIMING
-// double tmpT3 = Simulator::Instance()->GetWallTime();
-// std::cout << " ODE step DT (" << tmpT3-tmpT1 << ")" << std::endl;
-// //std::cout << " Physics Total DT (" << tmpT3-tmpT1 << ")" << std::endl;
-// #endif
-//
-// // Very important to clear out the contact group
-// dJointGroupEmpty( this->contactGroup );
-//
-// }
-
-////////////////////////////////////////////////////////////////////////////////
-// Update the ODE engine
void ODEPhysics::UpdatePhysics()
{
#ifdef TIMING
double tmpT1 = Simulator::Instance()->GetWallTime();
#endif
-
+
this->LockMutex();
- // Do collision detection; this will add contacts to the contact group
- dSpaceCollide( this->spaceId, this, CollisionCallback );
-
- //usleep(1000000);
-#ifdef TIMING
- double tmpT2 = Simulator::Instance()->GetWallTime();
- std::cout << " Collision DT (" << tmpT2-tmpT1 << ")" << std::endl;
-#endif
-
// Update the dynamical model
if (this->quickStepP->GetValue())
dWorldQuickStep(this->worldId, this->stepTimeP->GetValue() );
@@ -212,18 +178,57 @@
dWorldStep( this->worldId, this->stepTimeP->GetValue() );
#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;
+ double tmpT2 = Simulator::Instance()->GetWallTime();
+ std::cout << " ODE step DT (" << tmpT2-tmpT1 << ")" << std::endl;
#endif
// Very important to clear out the contact group
dJointGroupEmpty( this->contactGroup );
+ this->UnlockMutex();
- this->UnlockMutex();
+#ifdef TIMING
+ double tmpT3 = Simulator::Instance()->GetWallTime();
+ std::cout << " Empty Joint Group DT (" << tmpT3-tmpT2 << ")" <<
std::endl;
+#endif
}
+////////////////////////////////////////////////////////////////////////////////
+// Update the ODE engine
+// void ODEPhysics::UpdatePhysics()
+// {
+// #ifdef TIMING
+// double tmpT1 = Simulator::Instance()->GetWallTime();
+// #endif
+//
+// this->LockMutex();
+// // Do collision detection; this will add contacts to the contact group
+// dSpaceCollide( this->spaceId, this, CollisionCallback );
+//
+// //usleep(1000000);
+// #ifdef TIMING
+// double tmpT2 = Simulator::Instance()->GetWallTime();
+// std::cout << " Collision DT (" << tmpT2-tmpT1 << ")" << std::endl;
+// #endif
+//
+// // Update the dynamical model
+// if (this->quickStepP->GetValue())
+// dWorldQuickStep(this->worldId, this->stepTimeP->GetValue() );
+// else
+// dWorldStep( this->worldId, this->stepTimeP->GetValue() );
+//
+// #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 );
+//
+// this->UnlockMutex();
+// }
+
////////////////////////////////////////////////////////////////////////////////
// Finilize the ODE engine
void ODEPhysics::Fini()
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 as they present alongside digital heavyweights like Barbarian
Group, R/GA, & Big Spaceship. http://p.sf.net/sfu/creativitycat-com
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
