Revision: 7403
http://playerstage.svn.sourceforge.net/playerstage/?rev=7403&view=rev
Author: natepak
Date: 2009-03-09 08:36:48 +0000 (Mon, 09 Mar 2009)
Log Message:
-----------
Time sync
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/World.cc
Modified:
code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc
===================================================================
--- code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc
2009-03-09 08:10:57 UTC (rev 7402)
+++ code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc
2009-03-09 08:36:48 UTC (rev 7403)
@@ -12,6 +12,7 @@
gotCallback = 1;
}
+
int main()
{
gazebo::Client *client = new gazebo::Client();
@@ -63,15 +64,26 @@
usleep(90000000);
}*/
- simIface->Go(2*10e4, &Callback);
+ usleep(1000000);
+ printf("GO!\n");
+ // First GO
+ simIface->Go(5*10e4, &Callback);
+
+ // Wait until the callback is called
while (gotCallback == 0)
usleep(10000);
- printf("Here\n");
gotCallback = 0;
- simIface->Go(10e4, &Callback);
+ // Pause just for easy of visualization
+ usleep(4000000);
+
+ printf("GO!\n");
+ // Second GO
+ simIface->Go(5*10e4, &Callback);
+
+ // Wait until the callback is called
while (gotCallback == 0)
usleep(1000);
Modified: code/branches/federation/gazebo/libgazebo/SimIface.cc
===================================================================
--- code/branches/federation/gazebo/libgazebo/SimIface.cc 2009-03-09
08:10:57 UTC (rev 7402)
+++ code/branches/federation/gazebo/libgazebo/SimIface.cc 2009-03-09
08:36:48 UTC (rev 7403)
@@ -1,4 +1,12 @@
#include <string.h>
+
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <sys/ipc.h>
+#include <sys/sem.h>
+#include <signal.h>
+
+
#include "gazebo.h"
using namespace gazebo;
@@ -10,8 +18,8 @@
{
this->goAckThread = NULL;
- // this->goAckCond = NULL;
- //this->mutex = NULL;
+ this->goAckCond = NULL;
+ this->mutex = NULL;
}
@@ -19,14 +27,13 @@
/// Destroy an interface
SimulationIface::~SimulationIface()
{
- /*if (this->mutex)
+ if (this->mutex)
delete this->mutex;
this->mutex = NULL;
if (this->goAckThread)
delete this->goAckThread;
this->goAckThread = NULL;
- */
// Deleting this condition causes and error...no time to debug this.
/*if (this->goAckCond)
@@ -35,7 +42,7 @@
delete this->goAckCond;
printf("Done deleting the condition\n");
}*/
- //this->goAckCond = NULL;
+ this->goAckCond = NULL;
this->data = NULL;
}
@@ -45,11 +52,31 @@
/// Create a simulation interface
void SimulationIface::Create(Server *server, std::string id)
{
+ union semun
+ {
+ int val; /* Value for SETVAL */
+ struct semid_ds *buf; /* Buffer for IPC_STAT, IPC_SET */
+ unsigned short *array; /* Array for GETALL, SETALL */
+ struct seminfo *__buf;
+ } arg;
+
Iface::Create(server,id);
this->data = (SimulationData*)((char*)this->mMap+sizeof(SimulationIface));
- //this->goAckThread = NULL;
- //this->mutex = NULL;
- //this->goAckCond = NULL;
+
+ // Bad...get a more unique sem key
+ this->data->semKey = GZ_SEM_KEY - 10;
+
+ // Create a single semaphore
+ this->data->semId = semget(this->data->semKey,1, IPC_CREAT | S_IRWXU);
+
+ if (this->data->semId < 0)
+ printf("Error createing semaphore\n");
+
+ arg.val = 0;
+
+ // Set the semaphore value
+ if (semctl(this->data->semId, 0, SETVAL, arg) < 0)
+ printf("Semctl failed\n");
}
////////////////////////////////////////////////////////////////////////////////
@@ -63,8 +90,8 @@
// then signals the goAckSignal
if (this->goAckThread == NULL)
{
- //this->mutex = new boost::mutex;
- //this->goAckCond = new boost::condition;
+ this->mutex = new boost::mutex;
+ this->goAckCond = new boost::condition;
this->goAckThread = new boost::thread(
boost::bind(&SimulationIface::BlockThread, this));
usleep(100);
@@ -77,16 +104,14 @@
while (true)
{
- boost::mutex::scoped_lock lock(this->mutex);
- printf("Waiting on a condition\n");
+ boost::mutex::scoped_lock lock(*this->mutex);
+
// 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");
+ this->goAckCond->wait(lock);
+
// 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");
+ this->GoAckWait();
+
// Signal the callback function
this->goAckSignal();
}
@@ -203,3 +228,27 @@
this->Unlock();
}
+////////////////////////////////////////////////////////////////////////////////
+// Wait for a post on the go ack semaphore
+void SimulationIface::GoAckWait()
+{
+ struct sembuf semoperation;
+
+ semoperation.sem_num = 0;
+ semoperation.sem_op = -1;
+ semoperation.sem_flg = 0;
+
+ semop(this->data->semId, &semoperation, 1);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Post the go ack semaphore
+void SimulationIface::GoAckPost()
+{
+ struct sembuf semoperation;
+ semoperation.sem_num = 0;
+ semoperation.sem_op = 1;
+ semoperation.sem_flg = 0;
+
+ semop(this->data->semId, &semoperation, 1);
+}
Modified: code/branches/federation/gazebo/libgazebo/gazebo.h
===================================================================
--- code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-09 08:10:57 UTC
(rev 7402)
+++ code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-09 08:36:48 UTC
(rev 7403)
@@ -33,7 +33,6 @@
#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>
@@ -440,7 +439,9 @@
public: SimulationRequestData responses[GAZEBO_SIMULATION_MAX_REQUESTS];
public: unsigned int responseCount;
-// public: boost::interprocess::interprocess_semaphore sem;
+ public: int semId;
+ public: int semKey;
+
};
/// \brief Common simulation interface
@@ -475,22 +476,20 @@
this->Unlock();
{
- boost::mutex::scoped_lock lock(this->mutex);
+ boost::mutex::scoped_lock lock(*this->mutex);
// Set the time the thread should block for
this->blockTimeUs = us;
+ if (this->currentConnection.connected())
+ this->currentConnection.disconnect();
+
// Connect the callback. This is signaled when the thread
// (below) finishes waiting
- this->goAckSignal.connect( subscriber );
+ this->currentConnection = 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();
+ this->goAckCond->notify_one();
}
/// \brief Pause the simulation
@@ -519,12 +518,16 @@
const Vec3 &linearVel, const Vec3 &angularVel,
const Vec3 &linearAccel, const Vec3 &angularAccel );
+ public: void GoAckWait();
+ public: void GoAckPost();
+
private: void BlockThread();
private: unsigned int blockTimeUs;
private: boost::signal<void (void)> goAckSignal;
+ private: boost::signals::connection currentConnection;
private: boost::thread *goAckThread;
- private: boost::condition goAckCond;
- private: boost::mutex mutex;
+ private: boost::condition *goAckCond;
+ private: boost::mutex *mutex;
/// Pointer to the simulation data
Modified: code/branches/federation/gazebo/server/World.cc
===================================================================
--- code/branches/federation/gazebo/server/World.cc 2009-03-09 08:10:57 UTC
(rev 7402)
+++ code/branches/federation/gazebo/server/World.cc 2009-03-09 08:36:48 UTC
(rev 7403)
@@ -205,14 +205,11 @@
{
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();
+ //printf("SimTime[%f] PauseTime[%f]\n",
Simulator::Instance()->GetSimTime(), this->simPauseTime);
+ // Tell the simiface that it's okay to trigger the go ack
+ this->simIface->GoAckPost();
+
this->simPauseTime = 0;
Simulator::Instance()->SetPaused(true);
@@ -707,8 +704,6 @@
case SimulationRequestData::GO:
{
- printf("World got a go for %d microseconds\n", req->runTime );
-
this->simPauseTime = Simulator::Instance()->GetSimTime()
+ req->runTime * 10e-6;
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
