Revision: 7466
http://playerstage.svn.sourceforge.net/playerstage/?rev=7466&view=rev
Author: natepak
Date: 2009-03-12 21:52:35 +0000 (Thu, 12 Mar 2009)
Log Message:
-----------
Added more thread safety
Modified Paths:
--------------
code/branches/federation/gazebo/server/Model.cc
code/branches/federation/gazebo/server/Simulator.cc
code/branches/federation/gazebo/server/Simulator.hh
code/branches/federation/gazebo/server/physics/Body.cc
code/branches/federation/gazebo/server/physics/Body.hh
code/branches/federation/gazebo/server/physics/Geom.cc
code/branches/federation/gazebo/server/physics/Geom.hh
code/branches/federation/gazebo/server/rendering/OgreCreator.cc
code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc
code/branches/federation/gazebo/server/sensors/ray/RaySensor.hh
Modified: code/branches/federation/gazebo/server/Model.cc
===================================================================
--- code/branches/federation/gazebo/server/Model.cc 2009-03-12 18:50:11 UTC
(rev 7465)
+++ code/branches/federation/gazebo/server/Model.cc 2009-03-12 21:52:35 UTC
(rev 7466)
@@ -404,12 +404,6 @@
this->rpyP->SetValue(this->pose.rot);
}
- /*if (this->parent)
- this->visualNode->SetPose(this->GetPose() - this->parent->GetPose());
- else
- this->visualNode->SetPose(this->GetPose());
- */
-
if (this->graphicsHandler)
this->graphicsHandler->Update();
return this->UpdateChild();
Modified: code/branches/federation/gazebo/server/Simulator.cc
===================================================================
--- code/branches/federation/gazebo/server/Simulator.cc 2009-03-12 18:50:11 UTC
(rev 7465)
+++ code/branches/federation/gazebo/server/Simulator.cc 2009-03-12 21:52:35 UTC
(rev 7466)
@@ -28,6 +28,7 @@
#include <fstream>
#include <sys/time.h>
#include <boost/bind.hpp>
+#include <boost/thread/recursive_mutex.hpp>
#include "Body.hh"
#include "Geom.hh"
@@ -75,6 +76,8 @@
timeout(-1),
selectedEntity(NULL)
{
+
+ this->mutex = new boost::recursive_mutex();
}
////////////////////////////////////////////////////////////////////////////////
@@ -82,6 +85,9 @@
Simulator::~Simulator()
{
this->Close();
+
+ delete this->mutex;
+ this->mutex = NULL;
}
////////////////////////////////////////////////////////////////////////////////
@@ -533,7 +539,11 @@
this->prevPhysicsTime = this->GetRealTime();
- world->Update();
+
+ {
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
+ world->Update();
+ }
}
// Process all incoming messages from simiface
@@ -547,4 +557,11 @@
}
}
+////////////////////////////////////////////////////////////////////////////////
+/// Get the simulator mutex
+boost::recursive_mutex *Simulator::GetMutex()
+{
+ return this->mutex;
+}
+
Modified: code/branches/federation/gazebo/server/Simulator.hh
===================================================================
--- code/branches/federation/gazebo/server/Simulator.hh 2009-03-12 18:50:11 UTC
(rev 7465)
+++ code/branches/federation/gazebo/server/Simulator.hh 2009-03-12 21:52:35 UTC
(rev 7466)
@@ -31,6 +31,11 @@
#include "SingletonT.hh"
+namespace boost
+{
+ class recursive_mutex;
+}
+
namespace gazebo
{
/// \addtogroup gazebo_server
@@ -163,6 +168,9 @@
/// \brief Get the model that currently selected
public: Model *GetSelectedModel() const;
+ /// \brief Get the simulator mutex
+ public:boost::recursive_mutex *GetMutex();
+
/// \brief Function to run gui. Used by guiThread
private: void PhysicsLoop();
@@ -229,6 +237,8 @@
/// Thread in which to run the gui
private: boost::thread *physicsThread;
+ private: boost::recursive_mutex *mutex;
+
//Singleton implementation
private: friend class DestroyerT<Simulator>;
private: friend class SingletonT<Simulator>;
Modified: code/branches/federation/gazebo/server/physics/Body.cc
===================================================================
--- code/branches/federation/gazebo/server/physics/Body.cc 2009-03-12
18:50:11 UTC (rev 7465)
+++ code/branches/federation/gazebo/server/physics/Body.cc 2009-03-12
21:52:35 UTC (rev 7466)
@@ -25,6 +25,7 @@
*/
#include <sstream>
+#include <boost/thread/recursive_mutex.hpp>
#include "Model.hh"
#include "GazeboMessage.hh"
@@ -53,6 +54,8 @@
: Entity(parent)
{
+ this->mutex = new boost::recursive_mutex();
+
if ( !this->IsStatic() )
{
this->bodyId = dBodyCreate(worldId);
@@ -101,6 +104,8 @@
delete this->rpyP;
delete this->dampingFactorP;
+ delete this->mutex;
+ this->mutex = NULL;
}
////////////////////////////////////////////////////////////////////////////////
@@ -271,8 +276,6 @@
this->UpdatePose();
-// this->visualNode->SetPose(this->GetPose() - this->parent->GetPose());
-
for (geomIter=this->geoms.begin();
geomIter!=this->geoms.end(); geomIter++)
{
@@ -325,6 +328,7 @@
// Set the pose of the body
void Body::SetPose(const Pose3d &_pose)
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
if (this->IsStatic())
{
@@ -361,6 +365,8 @@
// Return the pose of the body
Pose3d Body::GetPose() const
{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
+
/*if (this->IsStatic())
return this->staticPose;
else
@@ -372,6 +378,7 @@
// 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/branches/federation/gazebo/server/physics/Body.hh
===================================================================
--- code/branches/federation/gazebo/server/physics/Body.hh 2009-03-12
18:50:11 UTC (rev 7465)
+++ code/branches/federation/gazebo/server/physics/Body.hh 2009-03-12
21:52:35 UTC (rev 7466)
@@ -36,6 +36,10 @@
#include "Pose3d.hh"
#include "Param.hh"
+namespace boost
+{
+ class recursive_mutex;
+}
namespace gazebo
{
@@ -207,6 +211,8 @@
private: ParamT<Quatern> *rpyP;
private: ParamT<double> *dampingFactorP;
+
+ private: boost::recursive_mutex *mutex;
};
/// \}
Modified: code/branches/federation/gazebo/server/physics/Geom.cc
===================================================================
--- code/branches/federation/gazebo/server/physics/Geom.cc 2009-03-12
18:50:11 UTC (rev 7465)
+++ code/branches/federation/gazebo/server/physics/Geom.cc 2009-03-12
21:52:35 UTC (rev 7466)
@@ -25,6 +25,7 @@
*/
#include <sstream>
+#include <boost/thread/recursive_mutex.hpp>
#include "OgreVisual.hh"
#include "OgreCreator.hh"
@@ -44,6 +45,8 @@
Geom::Geom( Body *body)
: Entity(body)
{
+ this->mutex = new boost::recursive_mutex();
+
this->typeName = "unknown";
this->body = body;
@@ -90,6 +93,9 @@
if (this->transId)
dGeomDestroy(this->transId);
+ delete this->mutex;
+ this->mutex = NULL;
+
/*for (iter = this->visuals.begin(); iter != this->visuals.end(); iter++)
{
GZ_DELETE (*iter)
@@ -269,7 +275,6 @@
// Update
void Geom::Update()
{
- //this->visualNode->SetPose(this->GetPose());
this->UpdateChild();
}
@@ -308,6 +313,8 @@
// 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)
{
Pose3d localPose;
@@ -339,6 +346,8 @@
{
Pose3d pose;
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
+
if (this->placeable && this->geomId)
{
const dReal *p;
@@ -371,6 +380,7 @@
void Geom::SetPosition(const Vector3 &pos)
{
Pose3d pose;
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
pose = this->GetPose();
pose.pos = pos;
@@ -382,6 +392,7 @@
void Geom::SetRotation(const Quatern &rot)
{
Pose3d pose;
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
pose = this->GetPose();
pose.rot = rot;
Modified: code/branches/federation/gazebo/server/physics/Geom.hh
===================================================================
--- code/branches/federation/gazebo/server/physics/Geom.hh 2009-03-12
18:50:11 UTC (rev 7465)
+++ code/branches/federation/gazebo/server/physics/Geom.hh 2009-03-12
21:52:35 UTC (rev 7466)
@@ -34,6 +34,11 @@
#include "Pose3d.hh"
#include "Vector3.hh"
+namespace boost
+{
+ class recursive_mutex;
+}
+
namespace gazebo
{
@@ -206,6 +211,7 @@
private: std::string typeName;
+ private: boost::recursive_mutex *mutex;
};
/// \}
Modified: code/branches/federation/gazebo/server/rendering/OgreCreator.cc
===================================================================
--- code/branches/federation/gazebo/server/rendering/OgreCreator.cc
2009-03-12 18:50:11 UTC (rev 7465)
+++ code/branches/federation/gazebo/server/rendering/OgreCreator.cc
2009-03-12 21:52:35 UTC (rev 7466)
@@ -32,6 +32,7 @@
#include <FL/Fl.H>
#include <FL/x.H>
+#include "Simulator.hh"
#include "Geom.hh"
#include "Global.hh"
#include "Entity.hh"
@@ -689,20 +690,28 @@
(*iter)->Update();
}
- // Update the visuals
- for (viter = this->visuals.begin(); viter != this->visuals.end(); viter++)
{
- vis = viter->second;
- owner = vis->GetOwner();
- if (!owner)
- continue;
- Geom *geom = dynamic_cast<Geom*>(owner);
+ boost::recursive_mutex::scoped_lock
lock(*Simulator::Instance()->GetMutex());
+ // Update the visuals
+ for (viter = this->visuals.begin(); viter != this->visuals.end(); viter++)
+ {
+ vis = viter->second;
+ owner = vis->GetOwner();
+ if (!owner)
+ continue;
- if (geom || !owner->GetParent())
- vis->SetPose(owner->GetPose());
- else
- vis->SetPose(owner->GetPose() - owner->GetParent()->GetPose());
+ Geom *geom = dynamic_cast<Geom*>(owner);
+
+ if (owner->GetName() == "box1_geom")
+ std::cout << "Pose[" << owner->GetPose() << "]\n";
+
+ if (geom || !owner->GetParent())
+ vis->SetPose(owner->GetPose());
+ else
+ vis->SetPose(owner->GetPose() - owner->GetParent()->GetPose());
+
+ }
}
}
Modified: code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc
===================================================================
--- code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc
2009-03-12 18:50:11 UTC (rev 7465)
+++ code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc
2009-03-12 21:52:35 UTC (rev 7466)
@@ -61,6 +61,10 @@
this->rayFan = OgreCreator::Instance()->CreateDynamicLine(
OgreDynamicRenderable::OT_TRIANGLE_FAN);
+ this->rayFanOutline = OgreCreator::Instance()->CreateDynamicLine(
+ OgreDynamicRenderable::OT_LINE_STRIP);
+
+
Param::Begin(&this->parameters);
this->rayCountP = new ParamT<int>("rayCount",0,1);
this->rangeCountP = new ParamT<int>("rangeCount",0,1);
@@ -149,6 +153,7 @@
this->prevPose = bodyPose;
this->rayFan->AddPoint(Vector3(0,0,0));
+ this->rayFanOutline->AddPoint(Vector3(0,0,0));
// Create and array of ray geoms
for (int i = 0; i < this->rayCountP->GetValue(); i++)
@@ -163,6 +168,7 @@
end = (axis * this->maxRangeP->GetValue()) + this->originP->GetValue();
this->rayFan->AddPoint(end);
+ this->rayFanOutline->AddPoint(end);
ray = new RayGeom(this->body, displayRaysP->GetValue());
ray->SetPoints(start, end);
@@ -172,7 +178,11 @@
this->rayFan->AddPoint(Vector3(0,0,0));
this->rayFan->setMaterial("Gazebo/BlueLaser");
+ this->rayFanOutline->AddPoint(Vector3(0,0,0));
+ this->rayFanOutline->setMaterial("Gazebo/BlueEmissive");
+
this->visualNode->AttachObject(this->rayFan);
+ this->visualNode->AttachObject(this->rayFanOutline);
}
//////////////////////////////////////////////////////////////////////////////
@@ -325,6 +335,7 @@
(*iter)->GetRelativePoints(a,b);
this->rayFan->SetPoint(i,b);
+ this->rayFanOutline->SetPoint(i,b);
}
}
}
Modified: code/branches/federation/gazebo/server/sensors/ray/RaySensor.hh
===================================================================
--- code/branches/federation/gazebo/server/sensors/ray/RaySensor.hh
2009-03-12 18:50:11 UTC (rev 7465)
+++ code/branches/federation/gazebo/server/sensors/ray/RaySensor.hh
2009-03-12 21:52:35 UTC (rev 7466)
@@ -145,6 +145,7 @@
private: ParamT<bool> *displayRaysP;
private: OgreDynamicLines *rayFan;
+ private: OgreDynamicLines *rayFanOutline;
};
/// \}
/// \}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Apps built with the Adobe(R) Flex(R) framework and Flex Builder(TM) are
powering Web 2.0 with engaging, cross-platform capabilities. Quickly and
easily build your RIAs with Flex Builder, the Eclipse(TM)based development
software that enables intelligent coding and step-through debugging.
Download the free 60 day trial. http://p.sf.net/sfu/www-adobe-com
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
