Revision: 7397
http://playerstage.svn.sourceforge.net/playerstage/?rev=7397&view=rev
Author: natepak
Date: 2009-03-09 07:13:58 +0000 (Mon, 09 Mar 2009)
Log Message:
-----------
Updates to the semaphore locking
Modified Paths:
--------------
code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc
code/branches/federation/gazebo/libgazebo/SimIface.cc
code/branches/federation/gazebo/libgazebo/gazebo.h
code/branches/federation/gazebo/server/Simulator.cc
code/branches/federation/gazebo/server/Simulator.hh
code/branches/federation/gazebo/server/World.cc
code/branches/federation/gazebo/server/World.hh
code/branches/federation/gazebo/server/main.cc
code/branches/federation/gazebo/worlds/pioneer2dx.world
Modified:
code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc
===================================================================
--- code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc
2009-03-09 05:40:51 UTC (rev 7396)
+++ code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc
2009-03-09 07:13:58 UTC (rev 7397)
@@ -1,8 +1,17 @@
#include <string.h>
#include <iostream>
#include <math.h>
+#include <boost/bind.hpp>
#include <gazebo/gazebo.h>
+int gotCallback = 0;
+
+void Callback()
+{
+ printf("Sim iface callback\n");
+ gotCallback = 1;
+}
+
int main()
{
gazebo::Client *client = new gazebo::Client();
@@ -54,10 +63,18 @@
usleep(90000000);
}*/
- simIface->Go(10e6);
+ simIface->Go(2*10e4, &Callback);
+ while (gotCallback == 0)
+ usleep(10000);
- usleep(100000000);
+ printf("Here\n");
+ gotCallback = 0;
+ simIface->Go(10e4, &Callback);
+
+ while (gotCallback == 0)
+ usleep(1000);
+
// Example of resetting the simulator
// simIface->Reset();
//usleep(1000000);
Modified: code/branches/federation/gazebo/libgazebo/SimIface.cc
===================================================================
--- code/branches/federation/gazebo/libgazebo/SimIface.cc 2009-03-09
05:40:51 UTC (rev 7396)
+++ code/branches/federation/gazebo/libgazebo/SimIface.cc 2009-03-09
07:13:58 UTC (rev 7397)
@@ -1,34 +1,99 @@
-#include <boost/thread/thread.hpp>
-#include <boost/signal.hpp>
-#include <boost/bind.hpp>
#include <string.h>
#include "gazebo.h"
using namespace gazebo;
-void SimulationIface::BlockThread()
+////////////////////////////////////////////////////////////////////////////////
+/// Create an interface
+SimulationIface::SimulationIface()
+ : Iface("simulation",sizeof(SimulationIface)+sizeof(SimulationData))
+
{
- usleep(this->blockTimeUs);
- //signal();
+ this->goAckThread = NULL;
+ this->goAckCond = NULL;
+ this->mutex = NULL;
}
+
////////////////////////////////////////////////////////////////////////////////
-/// Tell gazebo to execute for a specified amount of time
-void SimulationIface::Go(unsigned int us)
+/// Destroy an interface
+SimulationIface::~SimulationIface()
{
- this->Lock(1);
- SimulationRequestData *request =
&(this->data->requests[this->data->requestCount++]);
- request->type = SimulationRequestData::GO;
- request->runTime = us;
- this->Unlock();
+ if (this->mutex)
+ delete this->mutex;
+ this->mutex = NULL;
- this->blockTimeUs = us;
+ if (this->goAckThread)
+ delete this->goAckThread;
+ this->goAckThread = NULL;
- boost::thread *thread = new boost::thread(
- boost::bind(&SimulationIface::BlockThread, this));
+ // Deleting this condition causes and error...no time to debug this.
+ /*if (this->goAckCond)
+ {
+ printf("Delete the condition\n");
+ delete this->goAckCond;
+ printf("Done deleting the condition\n");
+ }*/
+ this->goAckCond = NULL;
+
+ this->data = NULL;
}
+
////////////////////////////////////////////////////////////////////////////////
+/// Create a simulation interface
+void SimulationIface::Create(Server *server, std::string id)
+{
+ Iface::Create(server,id);
+ this->data = (SimulationData*)((char*)this->mMap+sizeof(SimulationIface));
+ this->goAckThread = NULL;
+ this->mutex = NULL;
+ this->goAckCond = NULL;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Open a simulation interface
+void SimulationIface::Open(Client *client, std::string id)
+{
+ Iface::Open(client,id);
+ this->data = (SimulationData*)((char*)this->mMap+sizeof(SimulationIface));
+
+ // Create the thread which waits "blockTimeUs" microseconds and
+ // then signals the goAckSignal
+ if (this->goAckThread == NULL)
+ {
+ this->mutex = new boost::mutex;
+ this->goAckCond = new boost::condition;
+ this->goAckThread = new boost::thread(
+ boost::bind(&SimulationIface::BlockThread, this));
+ usleep(100);
+ }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void SimulationIface::BlockThread()
+{
+
+ while (true)
+ {
+ boost::mutex::scoped_lock lock(*(this->mutex));
+ printf("Waiting on a condition\n");
+ // Wait on a condition that signals when the thread should run
+ this->goAckCond->wait(lock);
+ printf("Blocking on gazebo, which is running for %d microseconds\n",
this->blockTimeUs);
+ printf("Thread is waiting at semaphore\n");
+ // Wait for Gazebo to send a Post
+// this->data->sem.post();
+ this->data->sem.wait();
+ printf("Thread is sending a signal to the callback\n");
+ // Signal the callback function
+ this->goAckSignal();
+ }
+
+ printf("Done with the thread\n");
+}
+
+////////////////////////////////////////////////////////////////////////////////
/// Pause the simulation
void SimulationIface::Pause()
{
Modified: code/branches/federation/gazebo/libgazebo/gazebo.h
===================================================================
--- code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-09 05:40:51 UTC
(rev 7396)
+++ code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-09 07:13:58 UTC
(rev 7397)
@@ -32,6 +32,12 @@
#include <sys/types.h>
#include <stdlib.h>
#include <stdint.h>
+#include <boost/signal.hpp>
+#include <boost/interprocess/sync/interprocess_semaphore.hpp>
+#include <boost/thread/thread.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/condition.hpp>
+#include <boost/bind.hpp>
#include "IfaceFactory.hh"
@@ -406,7 +412,7 @@
public: Vec3 modelAngularVel;
public: Vec3 modelLinearAccel;
public: Vec3 modelAngularAccel;
- public: double runTime;
+ public: unsigned int runTime;
};
/// \brief Simulation interface data
@@ -434,39 +440,55 @@
public: SimulationRequestData responses[GAZEBO_SIMULATION_MAX_REQUESTS];
public: unsigned int responseCount;
+ public: boost::interprocess::interprocess_semaphore sem;
};
/// \brief Common simulation interface
class SimulationIface : public Iface
{
/// \brief Create an interface
- public: SimulationIface():
Iface("simulation",sizeof(SimulationIface)+sizeof(SimulationData)) {}
+ public: SimulationIface();
/// \brief Destroy an interface
- public: virtual ~SimulationIface() {this->data = NULL;}
+ public: virtual ~SimulationIface();
/// \brief Create a simulation interface
/// \brief server Pointer to the server
/// \brief id String id
- public: virtual void Create(Server *server, std::string id)
- {
- Iface::Create(server,id);
- this->data =
(SimulationData*)((char*)this->mMap+sizeof(SimulationIface));
- }
+ public: virtual void Create(Server *server, std::string id);
/// \brief Open a simulation interface
/// \param client Pointer to the client
/// \param id String name of the client
- public: virtual void Open(Client *client, std::string id)
- {
- Iface::Open(client,id);
- this->data =
(SimulationData*)((char*)this->mMap+sizeof(SimulationIface));
- }
+ public: virtual void Open(Client *client, std::string id);
/// \brief Tell gazebo to execute for a specified amount of time
/// \param ms Number of milliseconds to run
- public: void Go(unsigned int ms);
+ public: template<typename T>
+ void Go(unsigned int us,T subscriber)
+ {
+ // Send the go command to Gazebo
+ this->Lock(1);
+ SimulationRequestData *request =
&(this->data->requests[this->data->requestCount++]);
+ request->type = SimulationRequestData::GO;
+ request->runTime = us;
+ this->Unlock();
+ // Set the time the thread should block for
+ this->blockTimeUs = us;
+
+ // Connect the callback. This is signaled when the thread
+ // (below) finishes waiting
+ this->goAckSignal.connect( subscriber );
+
+ /*printf("Waiting on the semaphore\n");
+ this->data->sem.wait();
+ printf("Done Waiting on the semaphore\n");
+ */
+
+ this->goAckCond->notify_one();
+ }
+
/// \brief Pause the simulation
public: void Pause();
@@ -495,7 +517,12 @@
private: void BlockThread();
private: unsigned int blockTimeUs;
+ private: boost::signal<void (void)> goAckSignal;
+ private: boost::thread *goAckThread;
+ private: boost::condition *goAckCond;
+ private: boost::mutex *mutex;
+
/// Pointer to the simulation data
public: SimulationData *data;
};
Modified: code/branches/federation/gazebo/server/Simulator.cc
===================================================================
--- code/branches/federation/gazebo/server/Simulator.cc 2009-03-09 05:40:51 UTC
(rev 7396)
+++ code/branches/federation/gazebo/server/Simulator.cc 2009-03-09 07:13:58 UTC
(rev 7397)
@@ -279,8 +279,10 @@
/// Main simulation loop, when this loop ends the simulation finish
void Simulator::MainLoop()
{
- double step = World::Instance()->GetPhysicsEngine()->GetStepTime();
- double physicsUpdateRate =
World::Instance()->GetPhysicsEngine()->GetUpdateRate();
+ World *world = World::Instance();
+
+ 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;
@@ -291,50 +293,33 @@
this->prevPhysicsTime = this->GetRealTime();
this->prevRenderTime = this->GetRealTime();
- this->runLength = 0.1;
- //this->runLength = 1.0;
-
- this->lastUpdateTime = this->GetRealTime();
-
while (!this->userQuit)
{
currTime = this->GetRealTime();
- if (currTime - this->lastUpdateTime > this->runLength)
- {
- World::Instance()->ProcessMessages();
-
- // Update the gui
- if (this->gui)
- {
- this->gui->Update();
- }
-
- continue;
- }
-
if (physicsUpdateRate == 0 ||
currTime - this->prevPhysicsTime >= physicsUpdatePeriod)
{
// Update the physics engine
- if (!this->GetUserPause() ||
+ /*if (!this->GetUserPause() && !this->IsPaused() ||
(this->GetUserPause() && this->GetUserStepInc()))
+ */
+ if (!this->IsPaused())
{
this->simTime += step;
this->iterations++;
- this->pause=false;
this->SetUserStepInc(!this->GetUserStepInc());
}
else
{
this->pauseTime += step;
- this->pause=true;
+ // this->pause=true;
}
this->prevPhysicsTime = this->GetRealTime();
- World::Instance()->Update();
+ world->Update();
}
// Update the rendering
@@ -351,6 +336,8 @@
this->gui->Update();
}
+ world->ProcessMessages();
+
elapsedTime = (this->GetRealTime() - currTime);
// Wait if we're going too fast
@@ -391,6 +378,12 @@
return this->pause;
}
+////////////////////////////////////////////////////////////////////////////////
+/// Set whether the simulation is paused
+void Simulator::SetPaused(bool p)
+{
+ this->pause = p;
+}
////////////////////////////////////////////////////////////////////////////////
/// Get the number of iterations of this simulation session
@@ -555,9 +548,3 @@
return model;
}
-
-void Simulator::Go()
-{
- this->lastUpdateTime = this->GetRealTime();
-}
-
Modified: code/branches/federation/gazebo/server/Simulator.hh
===================================================================
--- code/branches/federation/gazebo/server/Simulator.hh 2009-03-09 05:40:51 UTC
(rev 7396)
+++ code/branches/federation/gazebo/server/Simulator.hh 2009-03-09 07:13:58 UTC
(rev 7397)
@@ -90,6 +90,9 @@
/// \brief Returns the state of the simulation true if paused
public: bool IsPaused() const;
+ /// \brief Set whether the simulation is paused
+ public: void SetPaused(bool p);
+
/// \brief Get the number of iterations
public: unsigned long GetIterations() const;
/*
@@ -159,8 +162,6 @@
/// \brief Get the model that currently selected
public: Model *GetSelectedModel() const;
- public: void Go();
-
///pointer to the XML Data
private: XMLConfig *xmlFile;
@@ -221,10 +222,6 @@
/// The entity currently selected by the user
private: Entity *selectedEntity;
- /// Length of time to run before receiving a "go" command
- private: double runLength;
- private: double lastUpdateTime;
-
//Singleton implementation
private: friend class DestroyerT<Simulator>;
private: friend class SingletonT<Simulator>;
Modified: code/branches/federation/gazebo/server/World.cc
===================================================================
--- code/branches/federation/gazebo/server/World.cc 2009-03-09 05:40:51 UTC
(rev 7396)
+++ code/branches/federation/gazebo/server/World.cc 2009-03-09 07:13:58 UTC
(rev 7397)
@@ -172,6 +172,8 @@
{
std::vector< Model* >::iterator miter;
+ this->simPauseTime = 0;
+
// Init all models
for (miter=this->models.begin(); miter!=this->models.end(); miter++)
{
@@ -199,6 +201,28 @@
std::vector< Model* >::iterator miter;
std::vector< Model* >::iterator miter2;
+ if (this->simPauseTime > 0)
+ {
+ if (Simulator::Instance()->GetSimTime() >= this->simPauseTime)
+ {
+ printf("SimTime[%f] PauseTime[%f]\n",
+Simulator::Instance()->GetSimTime(), this->simPauseTime);
+ //this->simIface->Lock(1);
+ printf("Sem Post\n");
+ this->simIface->data->sem.post();
+ printf("Sem Post done\n");
+ //this->simIface->Unlock();
+
+ this->simPauseTime = 0;
+
+ Simulator::Instance()->SetPaused(true);
+ }
+ else
+ {
+ Simulator::Instance()->SetPaused(false);
+ }
+ }
+
// Update all the models
for (miter=this->models.begin(); miter!=this->models.end(); miter++)
{
@@ -617,8 +641,6 @@
// Set the model's linear and angular acceleration
model->SetLinearAccel(linearAccel);
model->SetAngularAccel(angularAccel);
-
- Simulator::Instance()->Go();
}
else
{
@@ -684,7 +706,13 @@
case SimulationRequestData::GO:
{
- printf("World got a go\n");
+
+ printf("World got a go for %d microseconds\n", req->runTime );
+
+ this->simPauseTime = Simulator::Instance()->GetSimTime()
+ + req->runTime * 10e-6;
+
+ Simulator::Instance()->SetPaused(false);
break;
}
Modified: code/branches/federation/gazebo/server/World.hh
===================================================================
--- code/branches/federation/gazebo/server/World.hh 2009-03-09 05:40:51 UTC
(rev 7396)
+++ code/branches/federation/gazebo/server/World.hh 2009-03-09 07:13:58 UTC
(rev 7397)
@@ -188,6 +188,10 @@
/// Simulation interface
private: SimulationIface *simIface;
+ /// Length of time to run before receiving a "go" command
+ private: double simPauseTime;
+
+
private: friend class DestroyerT<World>;
private: friend class SingletonT<World>;
};
Modified: code/branches/federation/gazebo/server/main.cc
===================================================================
--- code/branches/federation/gazebo/server/main.cc 2009-03-09 05:40:51 UTC
(rev 7396)
+++ code/branches/federation/gazebo/server/main.cc 2009-03-09 07:13:58 UTC
(rev 7397)
@@ -119,6 +119,7 @@
unsigned int optMsgLevel = 1;
int optTimeControl = 1;
bool optPhysicsEnabled = true;
+bool optPaused = false;
////////////////////////////////////////////////////////////////////////////////
// TODO: Implement these options
@@ -156,13 +157,18 @@
{
FILE *tmpFile;
int ch;
- char *flags = (char*)("l:hd:s:fgxt:nqp");
+ char *flags = (char*)("l:hd:s:fgxt:nqpe");
// Get letter options
while ((ch = getopt(argc, argv, flags)) != -1)
{
switch (ch)
{
+ case 'e':
+ std::cout << "Running in federation mode!\n";
+ optPaused = true;
+ break;
+
case 'd':
// Verbose mode
optMsgLevel = atoi(optarg);
@@ -267,6 +273,7 @@
gazebo::Simulator::Instance()->Load(worldFileName, optServerId);
gazebo::Simulator::Instance()->SetTimeout(optTimeout);
gazebo::Simulator::Instance()->SetPhysicsEnabled(optPhysicsEnabled);
+ gazebo::Simulator::Instance()->SetPaused(optPaused);
}
catch (gazebo::GazeboError e)
{
Modified: code/branches/federation/gazebo/worlds/pioneer2dx.world
===================================================================
--- code/branches/federation/gazebo/worlds/pioneer2dx.world 2009-03-09
05:40:51 UTC (rev 7396)
+++ code/branches/federation/gazebo/worlds/pioneer2dx.world 2009-03-09
07:13:58 UTC (rev 7397)
@@ -68,7 +68,7 @@
<model:physical name="sphere1_model">
<xyz>2.15 -1.68 .3</xyz>
<rpy>0.0 0.0 0.0</rpy>
- <static>true</static>
+ <static>false</static>
<body:sphere name="sphere1_body">
<geom:sphere name="sphere1_geom">
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Open Source Business Conference (OSBC), March 24-25, 2009, San Francisco, CA
-OSBC tackles the biggest issue in open source: Open Sourcing the Enterprise
-Strategies to boost innovation and cut costs with open source participation
-Receive a $600 discount off the registration fee with the source code: SFAD
http://p.sf.net/sfu/XcvMzF8H
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
