Revision: 7614
http://playerstage.svn.sourceforge.net/playerstage/?rev=7614&view=rev
Author: natepak
Date: 2009-05-03 17:51:49 +0000 (Sun, 03 May 2009)
Log Message:
-----------
Made the player drivers thread safe
Modified Paths:
--------------
code/gazebo/trunk/SConstruct
code/gazebo/trunk/build.py
code/gazebo/trunk/libgazebo/Iface.cc
code/gazebo/trunk/libgazebo/gazebo.h
code/gazebo/trunk/player/ActarrayInterface.cc
code/gazebo/trunk/player/ActarrayInterface.hh
code/gazebo/trunk/player/BumperInterface.cc
code/gazebo/trunk/player/BumperInterface.hh
code/gazebo/trunk/player/CameraInterface.cc
code/gazebo/trunk/player/CameraInterface.hh
code/gazebo/trunk/player/FiducialInterface.cc
code/gazebo/trunk/player/FiducialInterface.hh
code/gazebo/trunk/player/GpsInterface.cc
code/gazebo/trunk/player/GpsInterface.hh
code/gazebo/trunk/player/Graphics3dInterface.cc
code/gazebo/trunk/player/Graphics3dInterface.hh
code/gazebo/trunk/player/GripperInterface.cc
code/gazebo/trunk/player/GripperInterface.hh
code/gazebo/trunk/player/IRInterface.cc
code/gazebo/trunk/player/IRInterface.hh
code/gazebo/trunk/player/LaserInterface.cc
code/gazebo/trunk/player/LaserInterface.hh
code/gazebo/trunk/player/OpaqueInterface.cc
code/gazebo/trunk/player/OpaqueInterface.hh
code/gazebo/trunk/player/PTZInterface.cc
code/gazebo/trunk/player/PTZInterface.hh
code/gazebo/trunk/player/Position2dInterface.cc
code/gazebo/trunk/player/Position2dInterface.hh
code/gazebo/trunk/player/Position3dInterface.cc
code/gazebo/trunk/player/Position3dInterface.hh
code/gazebo/trunk/player/SimulationInterface.cc
code/gazebo/trunk/player/SimulationInterface.hh
code/gazebo/trunk/server/Model.cc
code/gazebo/trunk/server/Simulator.cc
code/gazebo/trunk/server/World.cc
code/gazebo/trunk/server/physics/Body.cc
code/gazebo/trunk/server/physics/Geom.cc
code/gazebo/trunk/server/rendering/OgreCreator.cc
code/gazebo/trunk/server/sensors/Sensor.cc
Modified: code/gazebo/trunk/SConstruct
===================================================================
--- code/gazebo/trunk/SConstruct 2009-05-01 12:22:15 UTC (rev 7613)
+++ code/gazebo/trunk/SConstruct 2009-05-03 17:51:49 UTC (rev 7614)
@@ -21,7 +21,7 @@
#######
# Setup the Options
#######
-opts = Options()
+opts = Variables()
opts.Add('prefix', 'The install path "prefix"', '/usr/local')
opts.Add('destdir', 'The root directory to install into. Useful mainly for
binary package building', '/')
opts.Add('mode','Defines how Gazebo will be built, options available:
optimized, profile, debug','debug')
@@ -55,7 +55,7 @@
'pkgcfg' : 'pkg-config --cflags --libs openal',
'check' : 'openal',
'msg' : 'Warning: OpenAL not found. 3D audio will be disbled',
- 'flags' : ['-DHAVE_OPENAL'],
+ 'flags' : '-DHAVE_OPENAL',
'web' : 'http://www.openal.org',
'required' : False
},
@@ -113,7 +113,7 @@
'pkgcfg' : 'pkg-config --cflags --libs playerc++',
'check' : 'playerc++',
'msg' : 'Warning: Player not found, bindings will not be built.',
- 'flags' : ['-DHAVE_PLAYER'],
+ 'flags' : '-DHAVE_PLAYER',
'web' : 'http://playerstage.sourceforge.net',
'required' : False
},
@@ -121,7 +121,7 @@
'header' : 'event.h',
'lib' : 'event',
'msg' : 'Warning: libevent not found. Webgazebo will no be
built.',
- 'flags' : ['-DHAVE_EVENT'],
+ 'flags' : '-DHAVE_EVENT',
'web' : 'http://www.monkey.org/~provos/libevent/',
'required' : False
},
@@ -129,7 +129,7 @@
'header' : 'yaml.h',
'lib' : 'yaml',
'msg' : 'Warning: yaml not found. Webgazebo will not be built.',
- 'flags' : ['-DHAVE_YAML'],
+ 'flags' : '-DHAVE_YAML',
'web' : 'http://www.yaml.org',
'required' : False
},
@@ -137,7 +137,7 @@
'pkgcfg' : 'pkg-config --cflags --libs websim',
'check' : 'websim',
'msg' : 'Warning: websim not found. Webgazebo will not be
built.',
- 'flags' : ['-DHAVE_WEBSIM'],
+ 'flags' : '-DHAVE_WEBSIM',
'web' : 'http://www.playerstage.sourceforge.net',
'required' : False
},
Modified: code/gazebo/trunk/build.py
===================================================================
--- code/gazebo/trunk/build.py 2009-05-01 12:22:15 UTC (rev 7613)
+++ code/gazebo/trunk/build.py 2009-05-03 17:51:49 UTC (rev 7614)
@@ -177,6 +177,7 @@
try:
if not check:
print "Checking for "+key+"...",
+ pkgcfg = "PKG_CONFIG_PATH="+os.environ["PKG_CONFIG_PATH"]+" "+pkgcfg;
env.ParseConfig(pkgcfg)
if not check:
print 'yes'
@@ -199,8 +200,7 @@
# If valid so far, apply any flags to the environment
if valid:
- for flag in flags:
- env.Append(CCFLAGS = flag)
+ env.Append(CCFLAGS = flags)
# Check for trimesh support in ODE
#if not conf.CheckODELib():
Modified: code/gazebo/trunk/libgazebo/Iface.cc
===================================================================
--- code/gazebo/trunk/libgazebo/Iface.cc 2009-05-01 12:22:15 UTC (rev
7613)
+++ code/gazebo/trunk/libgazebo/Iface.cc 2009-05-03 17:51:49 UTC (rev
7614)
@@ -331,23 +331,28 @@
//////////////////////////////////////////////////////////////////////////////
// Lock the interface.
-int Iface::Lock(int /*blocking*/)
+bool Iface::Lock(int blocking)
{
// Some 2.4 kernels seem to screw up the lock count somehow; keep an eye out
//printf(" lock %p %s\n", this, this->filename);
+ int op = LOCK_EX;
+
+ if (!blocking)
+ op |= LOCK_NB;
+
// Lock the file
- if (flock(this->mmapFd, LOCK_EX) != 0)
+ if (flock(this->mmapFd, op) != 0)
{
std::ostringstream stream;
stream << "flock " << this->filename.c_str() << "error: " <<
strerror(errno);
std::cout << "ERROR[" << stream.str() << "]\n";
- return 0;
+ return false;
//throw(stream.str().c_str());
}
- return 1;
+ return true;
}
@@ -400,9 +405,11 @@
if (this->mMap)
{
- this->Lock(1);
- result = ((GazeboData*)this->mMap)->openCount;
- this->Unlock();
+ if (this->Lock(1))
+ {
+ result = ((GazeboData*)this->mMap)->openCount;
+ this->Unlock();
+ }
}
return result;
Modified: code/gazebo/trunk/libgazebo/gazebo.h
===================================================================
--- code/gazebo/trunk/libgazebo/gazebo.h 2009-05-01 12:22:15 UTC (rev
7613)
+++ code/gazebo/trunk/libgazebo/gazebo.h 2009-05-03 17:51:49 UTC (rev
7614)
@@ -40,6 +40,7 @@
#include "IfaceFactory.hh"
+
namespace gazebo
{
@@ -328,8 +329,8 @@
/// \brief Lock the interface.
/// \param blocking 1=caller should block, 0=no-block
- /// \return 0 if the lock is acquired
- public: int Lock(int blocking);
+ /// \return True if the lock is acquired
+ public: bool Lock(int blocking);
/// \brief Unlock the interface
public: int Unlock();
Modified: code/gazebo/trunk/player/ActarrayInterface.cc
===================================================================
--- code/gazebo/trunk/player/ActarrayInterface.cc 2009-05-01 12:22:15 UTC
(rev 7613)
+++ code/gazebo/trunk/player/ActarrayInterface.cc 2009-05-03 17:51:49 UTC
(rev 7614)
@@ -26,6 +26,7 @@
#include <math.h>
+#include <boost/thread/recursive_mutex.hpp>
#include "GazeboError.hh"
#include "gazebo.h"
@@ -34,6 +35,8 @@
using namespace gazebo;
+boost::recursive_mutex *ActarrayInterface::mutex = NULL;
+
//
// The data for the interface (gz_actarray_data_t in gazebo.h) contains
information about each joint
// plus variables to store the commands. When a new command is sent, it
modifies the cmd_pos, cmd_speed, ...
@@ -63,6 +66,9 @@
memset(&this->actData, 0, sizeof(this->actData));
this->datatime = -1;
+
+ if (this->mutex == NULL)
+ this->mutex = new boost::recursive_mutex();
}
///////////////////////////////////////////////////////////////////////////////
@@ -82,6 +88,8 @@
int ActarrayInterface::ProcessMessage(QueuePointer &respQueue,
player_msghdr_t *hdr, void *data)
{
+
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
PLAYER_ACTARRAY_CMD_POS, this->device_addr))
{
@@ -256,6 +264,7 @@
// called from GazeboDriver::Update
void ActarrayInterface::Update()
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Lock(1);
// Only Update when new data is present
@@ -309,6 +318,7 @@
{
try
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Open(GazeboClient::client, this->gz_id);
}
catch (std::string e)
@@ -324,5 +334,6 @@
// Close a SHM interface. This is called from GazeboDriver::Unsubscribe
void ActarrayInterface::Unsubscribe()
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Close();
}
Modified: code/gazebo/trunk/player/ActarrayInterface.hh
===================================================================
--- code/gazebo/trunk/player/ActarrayInterface.hh 2009-05-01 12:22:15 UTC
(rev 7613)
+++ code/gazebo/trunk/player/ActarrayInterface.hh 2009-05-03 17:51:49 UTC
(rev 7614)
@@ -28,6 +28,11 @@
#include "GazeboInterface.hh"
+namespace boost
+{
+ class recursive_mutex;
+}
+
namespace gazebo
{
/// \addtogroup player_iface
@@ -72,6 +77,8 @@
private: double datatime;
private: player_actarray_data_t actData;
+
+ private: static boost::recursive_mutex *mutex;
};
/// \}
Modified: code/gazebo/trunk/player/BumperInterface.cc
===================================================================
--- code/gazebo/trunk/player/BumperInterface.cc 2009-05-01 12:22:15 UTC (rev
7613)
+++ code/gazebo/trunk/player/BumperInterface.cc 2009-05-03 17:51:49 UTC (rev
7614)
@@ -36,12 +36,14 @@
#include <iostream>
#include <math.h>
+#include <boost/thread/recursive_mutex.hpp>
#include "gazebo.h"
#include "GazeboDriver.hh"
#include "BumperInterface.hh"
using namespace gazebo;
+boost::recursive_mutex *BumperInterface::mutex = NULL;
///////////////////////////////////////////////////////////////////////////////
// Constructor
@@ -62,6 +64,9 @@
this->data.bumpers_count = 1;
this->data.bumpers = new uint8_t[1];
+
+ if (this->mutex == NULL)
+ this->mutex = new boost::recursive_mutex();
}
///////////////////////////////////////////////////////////////////////////////
@@ -87,6 +92,7 @@
// called from GazeboDriver::Update
void BumperInterface::Update()
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Lock(1);
// Only Update when new data is present
@@ -123,6 +129,7 @@
{
try
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Open( GazeboClient::client, this->gz_id);
}
catch (std::string e)
@@ -137,5 +144,6 @@
// Close a SHM interface. This is called from GazeboDriver::Unsubscribe
void BumperInterface::Unsubscribe()
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Close();
}
Modified: code/gazebo/trunk/player/BumperInterface.hh
===================================================================
--- code/gazebo/trunk/player/BumperInterface.hh 2009-05-01 12:22:15 UTC (rev
7613)
+++ code/gazebo/trunk/player/BumperInterface.hh 2009-05-03 17:51:49 UTC (rev
7614)
@@ -29,6 +29,11 @@
#include "GazeboInterface.hh"
+namespace boost
+{
+ class recursive_mutex;
+}
+
namespace gazebo
{
/// \addtogroup player_iface
@@ -73,6 +78,7 @@
private: double datatime;
private: player_bumper_data_t data;
+ private: static boost::recursive_mutex *mutex;
};
/// \}
Modified: code/gazebo/trunk/player/CameraInterface.cc
===================================================================
--- code/gazebo/trunk/player/CameraInterface.cc 2009-05-01 12:22:15 UTC (rev
7613)
+++ code/gazebo/trunk/player/CameraInterface.cc 2009-05-03 17:51:49 UTC (rev
7614)
@@ -25,6 +25,7 @@
*/
#include <math.h>
+#include <boost/thread/recursive_mutex.hpp>
#include "gazebo.h"
#include "GazeboError.hh"
@@ -33,6 +34,8 @@
using namespace gazebo;
+boost::recursive_mutex *CameraInterface::mutex = NULL;
+
///////////////////////////////////////////////////////////////////////////////
// Constructor
CameraInterface::CameraInterface(player_devaddr_t addr,
@@ -52,6 +55,9 @@
this->frameno = 0;
this->datatime = -1;
+
+ if (this->mutex == NULL)
+ this->mutex = new boost::recursive_mutex();
}
///////////////////////////////////////////////////////////////////////////////
@@ -80,10 +86,12 @@
struct timeval ts;
+ // Thread safe locking
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Lock(1);
// Only Update when new data is present
- if (this->iface->data->head.time > this->datatime)
+ if (this->iface->data->head.time > this->datatime )
{
this->datatime = this->iface->data->head.time;
@@ -126,10 +134,11 @@
snprintf(filename, sizeof(filename), "click-%04d.ppm",this->frameno++);
this->SaveFrame(filename);
}
-
}
+ // Done with the interface
this->iface->Unlock();
+
}
@@ -138,9 +147,11 @@
// GazeboDriver::Subscribe
void CameraInterface::Subscribe()
{
+
// Open the interface
try
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Open(GazeboClient::client, this->gz_id);
}
catch (std::string e)
@@ -151,12 +162,15 @@
//gzthrow(stream.str());
exit(0);
}
+
}
///////////////////////////////////////////////////////////////////////////////
// Close a SHM interface. This is called from GazeboDriver::Unsubscribe
void CameraInterface::Unsubscribe()
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
+
this->iface->Close();
}
Modified: code/gazebo/trunk/player/CameraInterface.hh
===================================================================
--- code/gazebo/trunk/player/CameraInterface.hh 2009-05-01 12:22:15 UTC (rev
7613)
+++ code/gazebo/trunk/player/CameraInterface.hh 2009-05-03 17:51:49 UTC (rev
7614)
@@ -29,6 +29,10 @@
#include "GazeboInterface.hh"
+namespace boost
+{
+ class recursive_mutex;
+}
namespace gazebo
{
@@ -83,6 +87,7 @@
private: int save;
private: int frameno;
+ private: static boost::recursive_mutex *mutex;
};
/// \}
Modified: code/gazebo/trunk/player/FiducialInterface.cc
===================================================================
--- code/gazebo/trunk/player/FiducialInterface.cc 2009-05-01 12:22:15 UTC
(rev 7613)
+++ code/gazebo/trunk/player/FiducialInterface.cc 2009-05-03 17:51:49 UTC
(rev 7614)
@@ -33,6 +33,7 @@
#include <math.h>
#include <libplayerxdr/playerxdr.h>
+#include <boost/thread/recursive_mutex.hpp>
#include "GazeboError.hh"
#include "gazebo.h"
@@ -41,6 +42,8 @@
using namespace gazebo;
+boost::recursive_mutex *FiducialInterface::mutex = NULL;
+
///////////////////////////////////////////////////////////////////////////////
// Constructor
FiducialInterface::FiducialInterface(player_devaddr_t addr,
@@ -60,6 +63,8 @@
memset(&this->data, 0, sizeof(this->data));
+ if (this->mutex == NULL)
+ this->mutex = new boost::recursive_mutex();
}
///////////////////////////////////////////////////////////////////////////////
@@ -77,6 +82,7 @@
int FiducialInterface::ProcessMessage(QueuePointer &respQueue,
player_msghdr_t *hdr, void *data)
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
// Request the pose and size of the fiducial device
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_FIDUCIAL_REQ_GET_GEOM, this->device_addr))
@@ -161,6 +167,7 @@
{
struct timeval ts;
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Lock(1);
// Only Update when new data is present
@@ -219,6 +226,7 @@
// Open the interface
try
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Open(GazeboClient::client, this->gz_id);
}
catch (std::string e)
@@ -236,5 +244,6 @@
// Close a SHM interface. This is called from GazeboDriver::Unsubscribe
void FiducialInterface::Unsubscribe()
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Close();
}
Modified: code/gazebo/trunk/player/FiducialInterface.hh
===================================================================
--- code/gazebo/trunk/player/FiducialInterface.hh 2009-05-01 12:22:15 UTC
(rev 7613)
+++ code/gazebo/trunk/player/FiducialInterface.hh 2009-05-03 17:51:49 UTC
(rev 7614)
@@ -29,6 +29,11 @@
#include "GazeboInterface.hh"
+namespace boost
+{
+ class recursive_mutex;
+}
+
namespace gazebo
{
/// \addtogroup player_iface
@@ -82,6 +87,7 @@
private: double datatime;
private: player_fiducial_data_t data;
+ private: static boost::recursive_mutex *mutex;
};
/// \}
/// \}
Modified: code/gazebo/trunk/player/GpsInterface.cc
===================================================================
--- code/gazebo/trunk/player/GpsInterface.cc 2009-05-01 12:22:15 UTC (rev
7613)
+++ code/gazebo/trunk/player/GpsInterface.cc 2009-05-03 17:51:49 UTC (rev
7614)
@@ -30,13 +30,15 @@
- PLAYER_GPS_DATA_STATE
*/
-
#include <math.h>
+#include <boost/thread/recursive_mutex.hpp>
#include "gazebo.h"
#include "GazeboDriver.hh"
#include "GpsInterface.hh"
+boost::recursive_mutex *GpsInterface::mutex = NULL;
+
///////////////////////////////////////////////////////////////////////////////
// Constructor
GpsInterface::GpsInterface(player_devaddr_t addr, GazeboDriver *driver,
@@ -52,6 +54,9 @@
this->iface = gz_gps_alloc();
this->datatime = -1;
+
+ if (this->mutex == NULL)
+ this->mutex = new boost::recursive_mutex();
}
///////////////////////////////////////////////////////////////////////////////
@@ -79,6 +84,7 @@
double e[3];
struct timeval ts;
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
gz_gps_lock(this->iface, 1);
// Only Update when new data is present
@@ -117,6 +123,7 @@
// GazeboDriver::Subscribe
void GpsInterface::Subscribe()
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
// Open the interface
if (gz_gps_open(this->iface, GazeboClient::client, this->gz_id) != 0)
{
@@ -128,5 +135,6 @@
// Close a GPS interface. This is called from GazeboDriver::Unsubscribe
void GpsInterface::Unsubscribe()
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
gz_gps_close(this->iface);
}
Modified: code/gazebo/trunk/player/GpsInterface.hh
===================================================================
--- code/gazebo/trunk/player/GpsInterface.hh 2009-05-01 12:22:15 UTC (rev
7613)
+++ code/gazebo/trunk/player/GpsInterface.hh 2009-05-03 17:51:49 UTC (rev
7614)
@@ -29,6 +29,12 @@
#include "GazeboInterface.hh"
+namespace boost
+{
+ class recursive_mutex;
+}
+
+
// Forward declarations
typedef struct gz_gps gz_gps_t;
@@ -65,6 +71,7 @@
/// @brief Timestamp on last data update
private: double datatime;
+ private: static boost::recursive_mutex *mutex;
};
#endif
Modified: code/gazebo/trunk/player/Graphics3dInterface.cc
===================================================================
--- code/gazebo/trunk/player/Graphics3dInterface.cc 2009-05-01 12:22:15 UTC
(rev 7613)
+++ code/gazebo/trunk/player/Graphics3dInterface.cc 2009-05-03 17:51:49 UTC
(rev 7614)
@@ -31,6 +31,8 @@
- PLAYER_GRAPHICS3D_CMD_DRAW
*/
+#include <boost/thread/recursive_mutex.hpp>
+
#include "gazebo.h"
#include "GazeboError.hh"
#include "GazeboDriver.hh"
@@ -38,6 +40,8 @@
using namespace gazebo;
+boost::recursive_mutex *Graphics3dInterface::mutex = NULL;
+
///////////////////////////////////////////////////////////////////////////////
// Constructor
Graphics3dInterface::Graphics3dInterface(player_devaddr_t addr,
@@ -51,6 +55,10 @@
// Allocate a Graphics3d Interface
this->iface = new Graphics3dIface();
+
+ if (this->mutex == NULL)
+ this->mutex = new boost::recursive_mutex();
+
}
///////////////////////////////////////////////////////////////////////////////
@@ -68,6 +76,7 @@
{
int result = -1;
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Lock(1);
// COMMAND CLEAR:
@@ -129,6 +138,7 @@
// Open the interface
try
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Open(GazeboClient::client, this->gz_id);
}
catch (std::string e)
@@ -145,5 +155,6 @@
// Close a SHM interface. This is called from GazeboDriver::Unsubscribe
void Graphics3dInterface::Unsubscribe()
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Close();
}
Modified: code/gazebo/trunk/player/Graphics3dInterface.hh
===================================================================
--- code/gazebo/trunk/player/Graphics3dInterface.hh 2009-05-01 12:22:15 UTC
(rev 7613)
+++ code/gazebo/trunk/player/Graphics3dInterface.hh 2009-05-03 17:51:49 UTC
(rev 7614)
@@ -29,6 +29,11 @@
#include "GazeboInterface.hh"
+namespace boost
+{
+ class recursive_mutex;
+}
+
namespace gazebo
{
@@ -65,6 +70,7 @@
/// \brief Gazebo id. This needs to match and ID in a Gazebo WorldFile
private: char *gz_id;
+ private: static boost::recursive_mutex *mutex;
};
}
Modified: code/gazebo/trunk/player/GripperInterface.cc
===================================================================
--- code/gazebo/trunk/player/GripperInterface.cc 2009-05-01 12:22:15 UTC
(rev 7613)
+++ code/gazebo/trunk/player/GripperInterface.cc 2009-05-03 17:51:49 UTC
(rev 7614)
@@ -36,6 +36,7 @@
#include <iostream>
#include <math.h>
+#include <boost/thread/recursive_mutex.hpp>
#include "gazebo.h"
#include "GazeboDriver.hh"
@@ -43,6 +44,8 @@
using namespace gazebo;
+boost::recursive_mutex *GripperInterface::mutex = NULL;
+
///////////////////////////////////////////////////////////////////////////////
// Constructor
GripperInterface::GripperInterface(player_devaddr_t addr,
@@ -59,6 +62,9 @@
this->iface = new GripperIface();
this->datatime = -1;
+
+ if (this->mutex == NULL)
+ this->mutex = new boost::recursive_mutex();
}
///////////////////////////////////////////////////////////////////////////////
@@ -75,6 +81,7 @@
player_msghdr_t *hdr, void *data)
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
if (this->iface->Lock(1))
{
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
@@ -157,6 +164,7 @@
player_gripper_data_t data;
struct timeval ts;
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Lock(1);
// Only Update when new data is present
@@ -209,6 +217,7 @@
{
try
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Open( GazeboClient::client, this->gz_id);
}
catch (std::string e)
@@ -223,5 +232,6 @@
// Close a SHM interface. This is called from GazeboDriver::Unsubscribe
void GripperInterface::Unsubscribe()
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Close();
}
Modified: code/gazebo/trunk/player/GripperInterface.hh
===================================================================
--- code/gazebo/trunk/player/GripperInterface.hh 2009-05-01 12:22:15 UTC
(rev 7613)
+++ code/gazebo/trunk/player/GripperInterface.hh 2009-05-03 17:51:49 UTC
(rev 7614)
@@ -29,6 +29,11 @@
#include "GazeboInterface.hh"
+namespace boost
+{
+ class recursive_mutex;
+}
+
namespace gazebo
{
/// \addtogroup player_iface
@@ -72,6 +77,7 @@
/// \brief Timestamp on last data update
private: double datatime;
+ private: static boost::recursive_mutex *mutex;
};
/// \}
Modified: code/gazebo/trunk/player/IRInterface.cc
===================================================================
--- code/gazebo/trunk/player/IRInterface.cc 2009-05-01 12:22:15 UTC (rev
7613)
+++ code/gazebo/trunk/player/IRInterface.cc 2009-05-03 17:51:49 UTC (rev
7614)
@@ -30,6 +30,7 @@
#include <math.h>
#include <libplayerxdr/playerxdr.h>
+#include <boost/thread/recursive_mutex.hpp>
#include "GazeboError.hh"
#include "gazebo.h"
@@ -38,6 +39,8 @@
using namespace gazebo;
+boost::recursive_mutex *IRInterface::mutex = NULL;
+
///////////////////////////////////////////////////////////////////////////////
// Constructor
IRInterface::IRInterface(player_devaddr_t addr,
@@ -54,7 +57,8 @@
this->datatime = -1;
-
+ if (this->mutex == NULL)
+ this->mutex = new boost::recursive_mutex();
}
///////////////////////////////////////////////////////////////////////////////
@@ -73,6 +77,7 @@
player_msghdr_t *hdr, void *data)
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_IR_REQ_POSE, this->device_addr))
{
@@ -110,6 +115,7 @@
{
struct timeval ts;
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Lock(1);
// Only Update when new data is present
@@ -160,6 +166,7 @@
// Open the interface
try
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Open(GazeboClient::client, this->gz_id);
}
catch (std::string e)
@@ -177,5 +184,6 @@
// Close a SHM interface. This is called from GazeboDriver::Unsubscribe
void IRInterface::Unsubscribe()
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Close();
}
Modified: code/gazebo/trunk/player/IRInterface.hh
===================================================================
--- code/gazebo/trunk/player/IRInterface.hh 2009-05-01 12:22:15 UTC (rev
7613)
+++ code/gazebo/trunk/player/IRInterface.hh 2009-05-03 17:51:49 UTC (rev
7614)
@@ -29,6 +29,11 @@
#include "GazeboInterface.hh"
+namespace boost
+{
+ class recursive_mutex;
+}
+
namespace gazebo
{
@@ -84,6 +89,7 @@
private: player_ir_data_t data;
+ private: static boost::recursive_mutex *mutex;
};
/// \}
Modified: code/gazebo/trunk/player/LaserInterface.cc
===================================================================
--- code/gazebo/trunk/player/LaserInterface.cc 2009-05-01 12:22:15 UTC (rev
7613)
+++ code/gazebo/trunk/player/LaserInterface.cc 2009-05-03 17:51:49 UTC (rev
7614)
@@ -29,6 +29,7 @@
#include <math.h>
#include <libplayerxdr/playerxdr.h>
+#include <boost/thread/recursive_mutex.hpp>
#include "GazeboError.hh"
#include "gazebo.h"
@@ -36,6 +37,7 @@
#include "LaserInterface.hh"
using namespace gazebo;
+boost::recursive_mutex *LaserInterface::mutex = NULL;
///////////////////////////////////////////////////////////////////////////////
// Constructor
@@ -56,6 +58,10 @@
this->datatime = -1;
memset(&this->data, 0, sizeof(this->data));
+
+ if (this->mutex == NULL)
+ this->mutex = new boost::recursive_mutex();
+
}
///////////////////////////////////////////////////////////////////////////////
@@ -73,6 +79,7 @@
int LaserInterface::ProcessMessage(QueuePointer &respQueue,
player_msghdr_t *hdr, void *data)
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
// Is it a request to set the laser's config?
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_LASER_REQ_SET_CONFIG,
@@ -167,6 +174,7 @@
{
struct timeval ts;
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
if (this->iface->Lock(1))
{
@@ -233,6 +241,7 @@
// Open the interface
try
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Open(GazeboClient::client, this->gz_id);
}
catch (std::string e)
@@ -249,5 +258,6 @@
// Close a SHM interface. This is called from GazeboDriver::Unsubscribe
void LaserInterface::Unsubscribe()
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Close();
}
Modified: code/gazebo/trunk/player/LaserInterface.hh
===================================================================
--- code/gazebo/trunk/player/LaserInterface.hh 2009-05-01 12:22:15 UTC (rev
7613)
+++ code/gazebo/trunk/player/LaserInterface.hh 2009-05-03 17:51:49 UTC (rev
7614)
@@ -29,6 +29,11 @@
#include "GazeboInterface.hh"
+namespace boost
+{
+ class recursive_mutex;
+}
+
namespace gazebo
{
@@ -85,6 +90,8 @@
private: int scanId;
private: player_laser_data_t data;
+
+ private: static boost::recursive_mutex *mutex;
};
/// \}
Modified: code/gazebo/trunk/player/OpaqueInterface.cc
===================================================================
--- code/gazebo/trunk/player/OpaqueInterface.cc 2009-05-01 12:22:15 UTC (rev
7613)
+++ code/gazebo/trunk/player/OpaqueInterface.cc 2009-05-03 17:51:49 UTC (rev
7614)
@@ -34,6 +34,7 @@
- PLAYER_OPAQUE_REQ_DATA
*/
#include <math.h>
+#include <boost/thread/recursive_mutex.hpp>
#include "GazeboError.hh"
#include "gazebo.h"
@@ -42,6 +43,8 @@
using namespace gazebo;
+boost::recursive_mutex *OpaqueInterface::mutex = NULL;
+
///////////////////////////////////////////////////////////////////////////////
// Constructor
OpaqueInterface::OpaqueInterface(player_devaddr_t addr,
@@ -57,6 +60,9 @@
this->iface = new OpaqueIface();
this->datatime = -1;
+
+ if (this->mutex == NULL)
+ this->mutex = new boost::recursive_mutex();
}
///////////////////////////////////////////////////////////////////////////////
@@ -72,6 +78,8 @@
int OpaqueInterface::ProcessMessage(QueuePointer &respQueue,
player_msghdr_t *hdr, void *data)
{
+
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
if (this->iface->Lock(1))
{
// nothing yet
@@ -88,6 +96,7 @@
// called from GazeboDriver::Update
void OpaqueInterface::Update()
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
player_opaque_data_t data;
struct timeval ts;
@@ -126,6 +135,7 @@
// Open the interface
try
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Open(GazeboClient::client, this->gz_id);
}
catch (std::string e)
@@ -142,5 +152,6 @@
// Close a SHM interface. This is called from GazeboDriver::Unsubscribe
void OpaqueInterface::Unsubscribe()
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Close();
}
Modified: code/gazebo/trunk/player/OpaqueInterface.hh
===================================================================
--- code/gazebo/trunk/player/OpaqueInterface.hh 2009-05-01 12:22:15 UTC (rev
7613)
+++ code/gazebo/trunk/player/OpaqueInterface.hh 2009-05-03 17:51:49 UTC (rev
7614)
@@ -28,6 +28,11 @@
#include "GazeboInterface.hh"
+namespace boost
+{
+ class recursive_mutex;
+}
+
namespace gazebo
{
@@ -72,6 +77,8 @@
/// \brief Timestamp on last data update
private: double datatime;
+
+ private: static boost::recursive_mutex *mutex;
};
/// \}
Modified: code/gazebo/trunk/player/PTZInterface.cc
===================================================================
--- code/gazebo/trunk/player/PTZInterface.cc 2009-05-01 12:22:15 UTC (rev
7613)
+++ code/gazebo/trunk/player/PTZInterface.cc 2009-05-03 17:51:49 UTC (rev
7614)
@@ -35,6 +35,7 @@
*/
#include <math.h>
+#include <boost/thread/recursive_mutex.hpp>
#include "GazeboError.hh"
#include "gazebo.h"
@@ -43,6 +44,8 @@
using namespace gazebo;
+boost::recursive_mutex *PTZInterface::mutex = NULL;
+
///////////////////////////////////////////////////////////////////////////////
// Constructor
PTZInterface::PTZInterface(player_devaddr_t addr,
@@ -58,6 +61,9 @@
this->iface = new PTZIface();
this->datatime = -1;
+
+ if (this->mutex == NULL)
+ this->mutex = new boost::recursive_mutex();
}
///////////////////////////////////////////////////////////////////////////////
@@ -73,6 +79,7 @@
int PTZInterface::ProcessMessage(QueuePointer &respQueue,
player_msghdr_t *hdr, void *data)
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
PLAYER_PTZ_CMD_STATE, this->device_addr))
{
@@ -155,6 +162,7 @@
player_ptz_data_t data;
struct timeval ts;
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Lock(1);
// Only Update when new data is present
@@ -188,6 +196,7 @@
{
try
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Open(GazeboClient::client, this->gz_id);
}
catch (std::string e)
@@ -203,5 +212,6 @@
// Close a SHM interface. This is called from GazeboDriver::Unsubscribe
void PTZInterface::Unsubscribe()
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Close();
}
Modified: code/gazebo/trunk/player/PTZInterface.hh
===================================================================
--- code/gazebo/trunk/player/PTZInterface.hh 2009-05-01 12:22:15 UTC (rev
7613)
+++ code/gazebo/trunk/player/PTZInterface.hh 2009-05-03 17:51:49 UTC (rev
7614)
@@ -29,6 +29,11 @@
#include "GazeboInterface.hh"
+namespace boost
+{
+ class recursive_mutex;
+}
+
namespace gazebo
{
@@ -72,6 +77,8 @@
/// \brief Timestamp on last data update
private: double datatime;
+
+ private: static boost::recursive_mutex *mutex;
};
}
Modified: code/gazebo/trunk/player/Position2dInterface.cc
===================================================================
--- code/gazebo/trunk/player/Position2dInterface.cc 2009-05-01 12:22:15 UTC
(rev 7613)
+++ code/gazebo/trunk/player/Position2dInterface.cc 2009-05-03 17:51:49 UTC
(rev 7614)
@@ -37,6 +37,7 @@
- PLAYER_POSITION2D_REQ_RESET_ODOM
*/
#include <math.h>
+#include <boost/thread/recursive_mutex.hpp>
#include "GazeboError.hh"
#include "gazebo.h"
@@ -45,6 +46,8 @@
using namespace gazebo;
+boost::recursive_mutex *Position2dInterface::mutex = NULL;
+
///////////////////////////////////////////////////////////////////////////////
// Constructor
Position2dInterface::Position2dInterface(player_devaddr_t addr,
@@ -61,6 +64,9 @@
this->datatime = -1;
+ if (this->mutex == NULL)
+ this->mutex = new boost::recursive_mutex();
+
}
///////////////////////////////////////////////////////////////////////////////
@@ -76,6 +82,9 @@
int Position2dInterface::ProcessMessage(QueuePointer &respQueue,
player_msghdr_t *hdr, void *data)
{
+ int result = 0;
+
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
if (this->iface->Lock(1))
{
@@ -91,7 +100,7 @@
this->iface->data->cmdVelocity.pos.y = cmd->vel.py;
this->iface->data->cmdVelocity.yaw = cmd->vel.pa;
- return 0;
+ result = 0;
}
// REQUEST SET ODOMETRY
@@ -101,19 +110,22 @@
if (hdr->size != sizeof(player_position2d_set_odom_req_t))
{
PLAYER_WARN("Arg to odometry set requestes wrong size; ignoring");
- return -1;
+ result = -1;
}
+ else
+ {
+ player_position2d_set_odom_req_t *odom =
+ (player_position2d_set_odom_req_t*)data;
- player_position2d_set_odom_req_t *odom =
(player_position2d_set_odom_req_t*)data;
+ this->iface->data->pose.pos.x = odom->pose.px;
+ this->iface->data->pose.pos.y = odom->pose.py;
+ this->iface->data->pose.yaw = odom->pose.pa;
- this->iface->data->pose.pos.x = odom->pose.px;
- this->iface->data->pose.pos.y = odom->pose.py;
- this->iface->data->pose.yaw = odom->pose.pa;
+ this->driver->Publish(this->device_addr, respQueue,
+ PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_SET_ODOM);
- this->driver->Publish(this->device_addr, respQueue,
- PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_SET_ODOM);
-
- return 0;
+ result = 0;
+ }
}
// CMD Set Motor Power
@@ -121,94 +133,103 @@
PLAYER_POSITION2D_REQ_MOTOR_POWER,
this->device_addr))
{
// TODO
- return 0;
+ result = 0;
}
// REQUEST SET MOTOR POWER
else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
- PLAYER_POSITION2D_REQ_MOTOR_POWER,
this->device_addr))
+ PLAYER_POSITION2D_REQ_MOTOR_POWER, this->device_addr))
{
if (hdr->size != sizeof(player_position2d_power_config_t))
{
PLAYER_WARN("Arg to motor set requestes wrong size; ignoring");
- return -1;
+ result = -1;
}
+ else
+ {
- player_position2d_power_config_t *power;
+ player_position2d_power_config_t *power;
- power = (player_position2d_power_config_t*) data;
+ power = (player_position2d_power_config_t*) data;
- this->iface->data->cmdEnableMotors = power->state;
+ this->iface->data->cmdEnableMotors = power->state;
- this->driver->Publish(this->device_addr, respQueue,
- PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_MOTOR_POWER);
+ this->driver->Publish(this->device_addr, respQueue,
+ PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_MOTOR_POWER);
- return 0;
+ result = 0;
+ }
}
// REQUEST GET GEOMETRY
else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
- PLAYER_POSITION2D_REQ_GET_GEOM,
this->device_addr))
+ PLAYER_POSITION2D_REQ_GET_GEOM, this->device_addr))
{
if (hdr->size != 0)
{
PLAYER_WARN("Arg get robot geom is wrong size; ignoring");
- return -1;
+ result = -1;
}
+ else
+ {
- player_position2d_geom_t geom;
+ player_position2d_geom_t geom;
- // TODO: get correct dimensions; there are for the P2AT
+ // TODO: get correct dimensions; there are for the P2AT
- geom.pose.px = 0;
- geom.pose.py = 0;
- geom.pose.pz = 0;
- geom.pose.pyaw = 0;
- geom.pose.ppitch = 0;
- geom.pose.proll = 0;
- geom.size.sw= 0.53;
- geom.size.sl = 0.38;
- geom.size.sh = 0.31;
+ geom.pose.px = 0;
+ geom.pose.py = 0;
+ geom.pose.pz = 0;
+ geom.pose.pyaw = 0;
+ geom.pose.ppitch = 0;
+ geom.pose.proll = 0;
+ geom.size.sw= 0.53;
+ geom.size.sl = 0.38;
+ geom.size.sh = 0.31;
- this->driver->Publish(this->device_addr, respQueue,
- PLAYER_MSGTYPE_RESP_ACK,
- PLAYER_POSITION2D_REQ_GET_GEOM,
- &geom, sizeof(geom), NULL);
+ this->driver->Publish(this->device_addr, respQueue,
+ PLAYER_MSGTYPE_RESP_ACK,
+ PLAYER_POSITION2D_REQ_GET_GEOM,
+ &geom, sizeof(geom), NULL);
- return 0;
+ result = 0;
+ }
}
// REQUEST RESET ODOMETRY
else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
- PLAYER_POSITION2D_REQ_RESET_ODOM,
this->device_addr))
+ PLAYER_POSITION2D_REQ_RESET_ODOM,
this->device_addr))
{
if (hdr->size != 0)
{
PLAYER_WARN("Arg reset position request is wrong size; ignoring");
- return -1;
+ result = -1;
}
+ else
+ {
+ // TODO: Make this work!!
+ //
+ this->driver->Publish(this->device_addr, respQueue,
+ PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_RESET_ODOM);
- // TODO: Make this work!!
- //
- this->driver->Publish(this->device_addr, respQueue,
- PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_RESET_ODOM);
+ result = 0;
+ }
+ }
- return 0;
- }
else
{
- printf("Unhandled\n");
+ result = -1;
}
this->iface->Unlock();
}
else
{
- printf("unable to get lock\n");
this->Unsubscribe();
+ result = -1;
}
- return -1;
+ return result;
}
@@ -222,6 +243,7 @@
memset(&data, 0, sizeof(data));
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
if (this->iface->Lock(1))
{
// Only Update when new data is present
@@ -264,6 +286,7 @@
// Open the interface
try
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Open(GazeboClient::client, this->gz_id);
}
catch (std::string e)
@@ -280,5 +303,6 @@
// Close a SHM interface. This is called from GazeboDriver::Unsubscribe
void Position2dInterface::Unsubscribe()
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Close();
}
Modified: code/gazebo/trunk/player/Position2dInterface.hh
===================================================================
--- code/gazebo/trunk/player/Position2dInterface.hh 2009-05-01 12:22:15 UTC
(rev 7613)
+++ code/gazebo/trunk/player/Position2dInterface.hh 2009-05-03 17:51:49 UTC
(rev 7614)
@@ -29,6 +29,12 @@
#include "GazeboInterface.hh"
+namespace boost
+{
+ class recursive_mutex;
+}
+
+
namespace gazebo
{
@@ -73,6 +79,8 @@
/// \brief Timestamp on last data update
private: double datatime;
+
+ private: static boost::recursive_mutex *mutex;
};
/// \}
Modified: code/gazebo/trunk/player/Position3dInterface.cc
===================================================================
--- code/gazebo/trunk/player/Position3dInterface.cc 2009-05-01 12:22:15 UTC
(rev 7613)
+++ code/gazebo/trunk/player/Position3dInterface.cc 2009-05-03 17:51:49 UTC
(rev 7614)
@@ -30,6 +30,7 @@
*/
#include <math.h>
+#include <boost/thread/recursive_mutex.hpp>
#include "GazeboError.hh"
#include "gazebo.h"
@@ -38,6 +39,8 @@
using namespace gazebo;
+boost::recursive_mutex *Position3dInterface::mutex = NULL;
+
///////////////////////////////////////////////////////////////////////////////
// Constructor
Position3dInterface::Position3dInterface(player_devaddr_t addr,
@@ -53,6 +56,10 @@
this->iface = new PositionIface();
this->datatime = -1;
+
+ if (this->mutex == NULL)
+ this->mutex = new boost::recursive_mutex();
+
}
///////////////////////////////////////////////////////////////////////////////
@@ -68,6 +75,7 @@
int Position3dInterface::ProcessMessage(QueuePointer &respQueue,
player_msghdr_t *hdr, void *data)
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Lock(1);
// COMMAND VELOCITY:
@@ -198,6 +206,7 @@
player_position3d_data_t data;
struct timeval ts;
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Lock(1);
// Only Update when new data is present
@@ -244,6 +253,7 @@
// Open the interface
try
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Open(GazeboClient::client, this->gz_id);
}
catch (std::string e)
@@ -260,5 +270,6 @@
// Close a SHM interface. This is called from GazeboDriver::Unsubscribe
void Position3dInterface::Unsubscribe()
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Close();
}
Modified: code/gazebo/trunk/player/Position3dInterface.hh
===================================================================
--- code/gazebo/trunk/player/Position3dInterface.hh 2009-05-01 12:22:15 UTC
(rev 7613)
+++ code/gazebo/trunk/player/Position3dInterface.hh 2009-05-03 17:51:49 UTC
(rev 7614)
@@ -29,6 +29,11 @@
#include "GazeboInterface.hh"
+namespace boost
+{
+ class recursive_mutex;
+}
+
// Forward declarations
namespace gazebo
{
@@ -74,6 +79,7 @@
/// \brief Timestamp on last data update
private: double datatime;
+ private: static boost::recursive_mutex *mutex;
};
}
Modified: code/gazebo/trunk/player/SimulationInterface.cc
===================================================================
--- code/gazebo/trunk/player/SimulationInterface.cc 2009-05-01 12:22:15 UTC
(rev 7613)
+++ code/gazebo/trunk/player/SimulationInterface.cc 2009-05-03 17:51:49 UTC
(rev 7614)
@@ -25,6 +25,7 @@
*/
#include <time.h>
+#include <boost/thread/recursive_mutex.hpp>
#include "gazebo.h"
#include "GazeboError.hh"
@@ -33,6 +34,8 @@
using namespace gazebo;
+boost::recursive_mutex *SimulationInterface::mutex = NULL;
+
///////////////////////////////////////////////////////////////////////////////
// Constructor
SimulationInterface::SimulationInterface(player_devaddr_t addr, GazeboDriver
*driver, ConfigFile *cf, int section)
@@ -56,6 +59,9 @@
memset( &this->pose3dReq, 0, sizeof(this->pose3dReq) );
memset( &this->pose2dReq, 0, sizeof(this->pose2dReq) );
+
+ if (this->mutex == NULL)
+ this->mutex = new boost::recursive_mutex();
}
///////////////////////////////////////////////////////////////////////////////
@@ -76,6 +82,7 @@
int SimulationInterface::ProcessMessage(QueuePointer &respQueue,
player_msghdr_t *hdr, void *data)
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
if (this->responseQueue)
delete this->responseQueue;
@@ -236,6 +243,7 @@
// called from GazeboDriver::Update
void SimulationInterface::Update()
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
gazebo::SimulationRequestData *response = NULL;
this->iface->Lock(1);
@@ -322,6 +330,7 @@
// Open the interface
try
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Open(GazeboClient::client, this->gz_id);
}
catch (std::string e)
@@ -338,5 +347,6 @@
// Close a SHM interface. This is called from GazeboDriver::Unsubscribe
void SimulationInterface::Unsubscribe()
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->iface->Close();
}
Modified: code/gazebo/trunk/player/SimulationInterface.hh
===================================================================
--- code/gazebo/trunk/player/SimulationInterface.hh 2009-05-01 12:22:15 UTC
(rev 7613)
+++ code/gazebo/trunk/player/SimulationInterface.hh 2009-05-03 17:51:49 UTC
(rev 7614)
@@ -29,6 +29,11 @@
#include "GazeboInterface.hh"
+namespace boost
+{
+ class recursive_mutex;
+}
+
namespace gazebo
{
/// \addtogroup player_iface
@@ -87,6 +92,7 @@
private: player_simulation_pose3d_req_t pose3dReq;
private: player_simulation_pose2d_req_t pose2dReq;
+ private: static boost::recursive_mutex *mutex;
};
/// \}
Modified: code/gazebo/trunk/server/Model.cc
===================================================================
--- code/gazebo/trunk/server/Model.cc 2009-05-01 12:22:15 UTC (rev 7613)
+++ code/gazebo/trunk/server/Model.cc 2009-05-03 17:51:49 UTC (rev 7614)
@@ -429,7 +429,9 @@
}
if (this->graphicsHandler)
+ {
this->graphicsHandler->Update();
+ }
return this->UpdateChild();
}
Modified: code/gazebo/trunk/server/Simulator.cc
===================================================================
--- code/gazebo/trunk/server/Simulator.cc 2009-05-01 12:22:15 UTC (rev
7613)
+++ code/gazebo/trunk/server/Simulator.cc 2009-05-03 17:51:49 UTC (rev
7614)
@@ -333,7 +333,9 @@
OgreAdaptor::Instance()->UpdateCameras();
if (this->gui)
+ {
this->gui->Update();
+ }
currTime = this->GetWallTime();
Modified: code/gazebo/trunk/server/World.cc
===================================================================
--- code/gazebo/trunk/server/World.cc 2009-05-01 12:22:15 UTC (rev 7613)
+++ code/gazebo/trunk/server/World.cc 2009-05-03 17:51:49 UTC (rev 7614)
@@ -786,7 +786,6 @@
for (miter=this->toDeleteModels.begin();
miter!=this->toDeleteModels.end(); miter++)
{
- std::cout << "Erasing a model[" << (*miter)->GetName() << "]\n";
// (*miter)->Fini();
this->models.erase(
std::remove(this->models.begin(), this->models.end(), *miter) );
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2009-05-01 12:22:15 UTC (rev
7613)
+++ code/gazebo/trunk/server/physics/Body.cc 2009-05-03 17:51:49 UTC (rev
7614)
@@ -297,7 +297,6 @@
avel = this->GetAngularVel();
dBodyAddTorque(this->GetId(), -avel.x * force, -avel.y * force,
-avel.z * force);
}
-
}
////////////////////////////////////////////////////////////////////////////////
@@ -328,7 +327,6 @@
// Set the pose of the body
void Body::SetPose(const Pose3d &_pose)
{
- //boost::recursive_mutex::scoped_lock lock(*this->mutex);
if (this->IsStatic())
{
@@ -365,7 +363,6 @@
// Return the pose of the body
Pose3d Body::GetPose() const
{
- //boost::recursive_mutex::scoped_lock lock(*this->mutex);
/*if (this->IsStatic())
return this->staticPose;
@@ -378,7 +375,6 @@
// Update the pose of the body
void Body::UpdatePose()
{
- //boost::recursive_mutex::scoped_lock lock(*this->mutex);
this->pose.pos = this->GetPosition();
this->pose.rot = this->GetRotation();
Modified: code/gazebo/trunk/server/physics/Geom.cc
===================================================================
--- code/gazebo/trunk/server/physics/Geom.cc 2009-05-01 12:22:15 UTC (rev
7613)
+++ code/gazebo/trunk/server/physics/Geom.cc 2009-05-03 17:51:49 UTC (rev
7614)
@@ -313,7 +313,6 @@
// Set the pose relative to the body
void Geom::SetPose(const Pose3d &newPose, bool updateCoM)
{
- //boost::recursive_mutex::scoped_lock lock(*this->mutex);
if (this->placeable && this->geomId)
{
@@ -346,7 +345,6 @@
{
Pose3d pose;
- //boost::recursive_mutex::scoped_lock lock(*this->mutex);
if (this->placeable && this->geomId)
{
@@ -380,7 +378,6 @@
void Geom::SetPosition(const Vector3 &pos)
{
Pose3d pose;
- //boost::recursive_mutex::scoped_lock lock(*this->mutex);
pose = this->GetPose();
pose.pos = pos;
Modified: code/gazebo/trunk/server/rendering/OgreCreator.cc
===================================================================
--- code/gazebo/trunk/server/rendering/OgreCreator.cc 2009-05-01 12:22:15 UTC
(rev 7613)
+++ code/gazebo/trunk/server/rendering/OgreCreator.cc 2009-05-03 17:51:49 UTC
(rev 7614)
@@ -704,7 +704,10 @@
}
{
+ //printf("OgreCreator scoped lock\n");
boost::recursive_mutex::scoped_lock
lock(*Simulator::Instance()->GetMutex());
+ //printf("OgreCreator got lock\n");
+
// Update the visuals
for (viter = this->visuals.begin(); viter != this->visuals.end(); viter++)
{
Modified: code/gazebo/trunk/server/sensors/Sensor.cc
===================================================================
--- code/gazebo/trunk/server/sensors/Sensor.cc 2009-05-01 12:22:15 UTC (rev
7613)
+++ code/gazebo/trunk/server/sensors/Sensor.cc 2009-05-03 17:51:49 UTC (rev
7614)
@@ -114,7 +114,9 @@
}
if (this->controller)
+ {
this->controller->Update();
+ }
}
////////////////////////////////////////////////////////////////////////////////
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Register Now & Save for Velocity, the Web Performance & Operations
Conference from O'Reilly Media. Velocity features a full day of
expert-led, hands-on workshops and two days of sessions from industry
leaders in dedicated Performance & Operations tracks. Use code vel09scf
and Save an extra 15% before 5/3. http://p.sf.net/sfu/velocityconf
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
