From: <na...@us...> - 2009-03-08 01:56:23
|
Revision: 7377 http://playerstage.svn.sourceforge.net/playerstage/?rev=7377&view=rev Author: natepak Date: 2009-03-08 01:56:07 +0000 (Sun, 08 Mar 2009) Log Message: ----------- Added the stuff from trunk Modified Paths: -------------- code/branches/federation/gazebo/SConstruct code/branches/federation/gazebo/examples/libgazebo/simiface/SConstruct code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc code/branches/federation/gazebo/libgazebo/SConscript code/branches/federation/gazebo/libgazebo/gazebo.h code/branches/federation/gazebo/server/Model.cc code/branches/federation/gazebo/server/Model.hh code/branches/federation/gazebo/server/World.cc code/branches/federation/gazebo/server/physics/Body.cc code/branches/federation/gazebo/server/physics/Body.hh code/branches/federation/gazebo/worlds/pioneer2dx.world Added Paths: ----------- code/branches/federation/gazebo/libgazebo/SimIface.cc Property Changed: ---------------- code/branches/federation/gazebo/ code/branches/federation/gazebo/player_cfgs/epuck.cfg code/branches/federation/gazebo/player_cfgs/epuck_single.cfg Property changes on: code/branches/federation/gazebo ___________________________________________________________________ Added: svn:mergeinfo + /code/gazebo/trunk:7372-7376 Modified: code/branches/federation/gazebo/SConstruct =================================================================== --- code/branches/federation/gazebo/SConstruct 2009-03-08 01:54:38 UTC (rev 7376) +++ code/branches/federation/gazebo/SConstruct 2009-03-08 01:56:07 UTC (rev 7377) @@ -224,11 +224,11 @@ # env.Alias('install', install_prefix) env.Install(install_prefix+'/bin',gazebo) - env.Install(install_prefix+'/share/gazebo','Media') env.Install(install_prefix+'/include/gazebo',headers) #env.Install(install_prefix+'/lib',libgazeboServerStatic ) env.Install(install_prefix+'/lib',libgazeboServerShared ) env.Install(install_prefix+'/share/gazebo','worlds') + env.Install(install_prefix+'/share/gazebo','Media') else: print 'Configure done' Exit() Modified: code/branches/federation/gazebo/examples/libgazebo/simiface/SConstruct =================================================================== --- code/branches/federation/gazebo/examples/libgazebo/simiface/SConstruct 2009-03-08 01:54:38 UTC (rev 7376) +++ code/branches/federation/gazebo/examples/libgazebo/simiface/SConstruct 2009-03-08 01:56:07 UTC (rev 7377) @@ -1,10 +1,11 @@ # 3rd party packages -parseConfigs=['pkg-config --cflags --libs libgazebo', - 'pkg-config --cflags --libs libgazeboServer', - 'pkg-config --cflags --libs playerc++'] +parseConfigs=['pkg-config --cflags --libs libgazebo'] +# 'pkg-config --cflags --libs libgazeboServer', +# 'pkg-config --cflags --libs playerc++'] + env = Environment ( CC = 'g++', Modified: code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc =================================================================== --- code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc 2009-03-08 01:54:38 UTC (rev 7376) +++ code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc 2009-03-08 01:56:07 UTC (rev 7377) @@ -1,5 +1,6 @@ +#include <string.h> +#include <iostream> #include <gazebo/gazebo.h> -#include <gazebo/GazeboError.hh> int main() { @@ -13,7 +14,7 @@ { client->ConnectWait(serverId, GZ_CLIENT_ID_USER_FIRST); } - catch (gazebo::GazeboError e) + catch (std::string e) { std::cout << "Gazebo error: Unable to connect\n" << e << "\n"; return -1; @@ -33,54 +34,30 @@ //simIface->data->reset = 1; // Example of how to move a model (box1_model) - uint8_t name[512] = "pioneer2dx_model1"; + char name[512] = "pioneer2dx_model1"; for (int i=0; i< 10; i++) { - simIface->Lock(1); + + gazebo::Pose pose; + gazebo::Vec3 linearVel(0.1, 0, 0); + gazebo::Vec3 angularVel(0, 0, 0); + gazebo::Vec3 linearAccel(0, 0, 0); + gazebo::Vec3 angularAccel(0, 0, 0); - gazebo::SimulationRequestData *request = &(simIface->data->requests[simIface->data->requestCount++]); - - request->type = gazebo::SimulationRequestData::SET_STATE; - memcpy(request->modelName, name, 512); - - request->modelPose.pos.x = i+1; - request->modelPose.pos.y = 0; - request->modelPose.pos.z = 0; - request->modelPose.roll = 0; - request->modelPose.pitch = 0; - request->modelPose.yaw = 0; - - request->modelLinearVel.x = 0.1; - request->modelLinearVel.y = 0; - request->modelLinearVel.z = 0; - - request->modelAngularVel.x = 0.1; - request->modelAngularVel.y = 0; - request->modelAngularVel.z = 0; - - /*request->type = gazebo::SimulationRequestData::SET_POSE2D; - memcpy(request->modelName, name, 512); - - request->modelPose.pos.x = i+.1; - request->modelPose.pos.y = 0; - */ - - simIface->Unlock(); - - usleep(100000); + simIface->SetState(name, pose, linearVel, angularVel, + linearAccel, angularAccel ); + usleep(9000000); } // Example of resetting the simulator - /*simIface->Lock(1); - simIface->data->reset = 1; - simIface->Unlock(); - */ + // simIface->Reset(); usleep(1000000); simIface->Close(); + delete simIface; return 0; } Modified: code/branches/federation/gazebo/libgazebo/SConscript =================================================================== --- code/branches/federation/gazebo/libgazebo/SConscript 2009-03-08 01:54:38 UTC (rev 7376) +++ code/branches/federation/gazebo/libgazebo/SConscript 2009-03-08 01:56:07 UTC (rev 7377) @@ -22,7 +22,7 @@ # setup a special build environment for libgazebo. Do this so we can control # what libgazebo depends upon # -env = Environment ( +myEnv = Environment ( CC = 'g++', CCFLAGS = Split ('-pthread -pipe -W -Wall -O2'), @@ -41,11 +41,11 @@ pkgconfig = env.PkgConfig(target='libgazebo.pc', source=Value(install_prefix)) env.Install(dir=install_prefix+'/lib/pkgconfig', source=pkgconfig) -sources = Split('Server.cc Client.cc Iface.cc IfaceFactory.cc') +sources = Split('Server.cc Client.cc Iface.cc IfaceFactory.cc SimIface.cc') headers = Split('gazebo.h IfaceFactory.hh') -sharedLib = env.SharedLibrary('gazebo', sources) -staticLib = env.StaticLibrary('gazebo', sources) +sharedLib = myEnv.SharedLibrary('gazebo', sources) +staticLib = myEnv.StaticLibrary('gazebo', sources) env.Install(install_prefix+'/lib', sharedLib) env.Install(install_prefix+'/lib', staticLib) Copied: code/branches/federation/gazebo/libgazebo/SimIface.cc (from rev 7376, code/gazebo/trunk/libgazebo/SimIface.cc) =================================================================== --- code/branches/federation/gazebo/libgazebo/SimIface.cc (rev 0) +++ code/branches/federation/gazebo/libgazebo/SimIface.cc 2009-03-08 01:56:07 UTC (rev 7377) @@ -0,0 +1,114 @@ +#include <string.h> +#include "gazebo.h" + +using namespace gazebo; + +//////////////////////////////////////////////////////////////////////////////// +/// Pause the simulation +void SimulationIface::Pause() +{ + this->Lock(1); + SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); + request->type = SimulationRequestData::PAUSE; + this->Unlock(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Reset the simulation +void SimulationIface::Reset() +{ + this->Lock(1); + SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); + request->type = SimulationRequestData::RESET; + this->Unlock(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Save the simulation +void SimulationIface::Save() +{ + this->Lock(1); + SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); + request->type = SimulationRequestData::SAVE; + this->Unlock(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Get the 3d pose of a model +void SimulationIface::GetPose3d(const char *modelName) +{ + this->Lock(1); + SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); + request->type = SimulationRequestData::GET_POSE3D; + memcpy(request->modelName, modelName, 512); + + this->Unlock(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Get the 2d pose of a model +void SimulationIface::GetPose2d(const char *modelName) +{ + this->Lock(1); + SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); + request->type = SimulationRequestData::GET_POSE2D; + memcpy(request->modelName, modelName, 512); + + this->Unlock(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Set the 3d pose of a model +void SimulationIface::SetPose3d(const char *modelName, const Pose &modelPose) +{ + this->Lock(1); + SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); + + request->type = SimulationRequestData::SET_POSE3D; + request->modelPose = modelPose; + + memcpy(request->modelName, modelName, 512); + + this->Unlock(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Set the 2d pose of a model +void SimulationIface::SetPose2d(const char *modelName, float x, float y, float yaw) +{ + this->Lock(1); + SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); + + request->type = gazebo::SimulationRequestData::SET_POSE2D; + + memcpy(request->modelName, modelName, 512); + + request->modelPose.pos.x = x; + request->modelPose.pos.y = y; + request->modelPose.yaw = yaw; + + this->Unlock(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Set the complete state of a model +void SimulationIface::SetState(const char *modelName, const Pose &modelPose, + const Vec3 &linearVel, const Vec3 &angularVel, const Vec3 &linearAccel, + const Vec3 &angularAccel ) +{ + this->Lock(1); + SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); + + request->type = gazebo::SimulationRequestData::SET_STATE; + memcpy(request->modelName, modelName, 512); + + request->modelPose = modelPose; + request->modelLinearVel = linearVel; + request->modelAngularVel = angularVel; + + request->modelLinearAccel = linearAccel; + request->modelAngularAccel = angularAccel; + + this->Unlock(); +} + Modified: code/branches/federation/gazebo/libgazebo/gazebo.h =================================================================== --- code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-08 01:54:38 UTC (rev 7376) +++ code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-08 01:56:07 UTC (rev 7377) @@ -57,6 +57,15 @@ /// \brief Vector 3 class class Vec3 { + /// \brief Default Constructor + public: Vec3() : x(0), y(0), z(0) {} + + /// \brief Constructor + public: Vec3(float _x, float _y, float _z) : x(_x), y(_y), z(_z) {} + + /// \brief Copy constructor + public: Vec3(const Vec3 &vec) : x(vec.x), y(vec.y), z(vec.z) {} + /// X value public: float x; @@ -71,6 +80,17 @@ /// \brief Pose class class Pose { + /// \brief Default constructor + public: Pose() : pos(0,0,0), roll(0), pitch(0), yaw(0) {} + + /// \brief Constructor + public: Pose(const Vec3 &_pos, float _roll, float _pitch, float _yaw) + : pos(_pos), roll(_roll), pitch(_pitch), yaw(_yaw) {} + + /// \brief Copy constructor + public: Pose(const Pose &pose) + : pos(pose.pos), roll(pose.roll), pitch(pose.pitch), yaw(pose.yaw) {} + /// 3d position public: Vec3 pos; @@ -381,7 +401,6 @@ public: Type type; public: char modelName[512]; public: Pose modelPose; - public: Vec3 modelLinearVel; public: Vec3 modelAngularVel; public: Vec3 modelLinearAccel; @@ -412,6 +431,7 @@ /// Array of request responses from the simulator public: SimulationRequestData responses[GAZEBO_SIMULATION_MAX_REQUESTS]; public: unsigned int responseCount; + }; /// \brief Common simulation interface @@ -441,6 +461,32 @@ this->data = (SimulationData*)((char*)this->mMap+sizeof(SimulationIface)); } + /// \brief Pause the simulation + public: void Pause(); + + /// \brief Reset the simulation + public: void Reset(); + + /// \brief Save the simulation + public: void Save(); + + /// \brief Get the 3d pose of a model + public: void GetPose3d(const char *modelName); + + /// \brief Get the 2d pose of a model + public: void GetPose2d(const char *modelName); + + /// \brief Set the 3d pose of a model + public: void SetPose3d(const char *modelName, const Pose &modelPose); + + /// \brief Set the 2d pose of a model + public: void SetPose2d(const char *modelName, float x, float y, float yaw); + + /// \brief Set the complete state of a model + public: void SetState(const char *modelName, const Pose &modelPose, + const Vec3 &linearVel, const Vec3 &angularVel, + const Vec3 &linearAccel, const Vec3 &angularAccel ); + /// Pointer to the simulation data public: SimulationData *data; }; Property changes on: code/branches/federation/gazebo/player_cfgs/epuck.cfg ___________________________________________________________________ Deleted: svn:mergeinfo - Property changes on: code/branches/federation/gazebo/player_cfgs/epuck_single.cfg ___________________________________________________________________ Deleted: svn:mergeinfo - Modified: code/branches/federation/gazebo/server/Model.cc =================================================================== --- code/branches/federation/gazebo/server/Model.cc 2009-03-08 01:54:38 UTC (rev 7376) +++ code/branches/federation/gazebo/server/Model.cc 2009-03-08 01:56:07 UTC (rev 7377) @@ -518,8 +518,37 @@ body->SetAngularVel( vel ); } } - + //////////////////////////////////////////////////////////////////////////////// +/// Set the linear acceleration of the model +void Model::SetLinearAccel( const Vector3 &accel ) +{ + Body *body; + std::map<std::string, Body* >::iterator iter; + + for (iter=this->bodies.begin(); iter!=this->bodies.end(); iter++) + { + body = iter->second; + + body->SetLinearAccel( accel ); + } +} + +//////////////////////////////////////////////////////////////////////////////// +/// Set the angular acceleration of the model +void Model::SetAngularAccel( const Vector3 &accel ) +{ + Body *body; + std::map<std::string, Body* >::iterator iter; + + for (iter=this->bodies.begin(); iter!=this->bodies.end(); iter++) + { + body = iter->second; + + body->SetAngularAccel( accel ); + } +} +//////////////////////////////////////////////////////////////////////////////// // Get the current pose const Pose3d &Model::GetPose() const { Modified: code/branches/federation/gazebo/server/Model.hh =================================================================== --- code/branches/federation/gazebo/server/Model.hh 2009-03-08 01:54:38 UTC (rev 7376) +++ code/branches/federation/gazebo/server/Model.hh 2009-03-08 01:56:07 UTC (rev 7377) @@ -113,6 +113,12 @@ /// \brief Set the angular velocity of the model public: void SetAngularVel( const Vector3 &vel ); + + /// \brief Set the linear acceleration of the model + public: void SetLinearAccel( const Vector3 &vel ); + + /// \brief Set the angular acceleration of the model + public: void SetAngularAccel( const Vector3 &vel ); /// \brief Get the current pose public: const Pose3d &GetPose() const; Modified: code/branches/federation/gazebo/server/World.cc =================================================================== --- code/branches/federation/gazebo/server/World.cc 2009-03-08 01:54:38 UTC (rev 7376) +++ code/branches/federation/gazebo/server/World.cc 2009-03-08 01:56:07 UTC (rev 7377) @@ -597,19 +597,32 @@ Vector3 angularVel( req->modelAngularVel.x, req->modelAngularVel.y, req->modelAngularVel.z); + Vector3 linearAccel( req->modelLinearAccel.x, + req->modelLinearAccel.y, + req->modelLinearAccel.z); + Vector3 angularAccel( req->modelAngularAccel.x, + req->modelAngularAccel.y, + req->modelAngularAccel.z); + pose.pos.x = req->modelPose.pos.x; pose.pos.y = req->modelPose.pos.y; pose.pos.z = req->modelPose.pos.z; + // The the model's pose pose.rot.SetFromEuler( Vector3(req->modelPose.roll, req->modelPose.pitch, req->modelPose.yaw)); model->SetPose(pose); + // Set the model's linear and angular velocity model->SetLinearVel(linearVel); model->SetAngularVel(angularVel); + + // Set the model's linear and angular acceleration + model->SetLinearAccel(linearAccel); + model->SetAngularAccel(angularAccel); } else { Modified: code/branches/federation/gazebo/server/physics/Body.cc =================================================================== --- code/branches/federation/gazebo/server/physics/Body.cc 2009-03-08 01:54:38 UTC (rev 7376) +++ code/branches/federation/gazebo/server/physics/Body.cc 2009-03-08 01:56:07 UTC (rev 7377) @@ -650,12 +650,43 @@ return vel; } + //////////////////////////////////////////////////////////////////////////////// +/// Set the linear acceleration of the body +void Body::SetLinearAccel(const Vector3 &accel) +{ + this->SetForce( accel * this->GetMass()); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Get the angular acceleration of the body +Vector3 Body::GetLinearAccel() const +{ + return this->GetForce() / this->GetMass(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Set the angular acceleration of the body +void Body::SetAngularAccel(const Vector3 &accel) +{ + this->SetTorque( accel * this->GetMass()); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Get the angular acceleration of the body +Vector3 Body::GetAngularAccel() const +{ + return this->GetTorque() / this->GetMass(); +} + +//////////////////////////////////////////////////////////////////////////////// /// \brief Set the force applied to the body void Body::SetForce(const Vector3 &force) { if (this->bodyId) - dBodySetForce(this->bodyId, force.x, force.y, force.z); + { + dBodyAddForce(this->bodyId, force.x, force.y, force.z); + } } //////////////////////////////////////////////////////////////////////////////// Modified: code/branches/federation/gazebo/server/physics/Body.hh =================================================================== --- code/branches/federation/gazebo/server/physics/Body.hh 2009-03-08 01:54:38 UTC (rev 7376) +++ code/branches/federation/gazebo/server/physics/Body.hh 2009-03-08 01:56:07 UTC (rev 7377) @@ -129,6 +129,18 @@ /// \brief Get the angular velocity of the body public: Vector3 GetAngularVel() const; + /// \brief Set the linear acceleration of the body + public: void SetLinearAccel(const Vector3 &accel); + + /// \brief Get the linear acceleration of the body + public: Vector3 GetLinearAccel() const; + + /// \brief Set the angular acceleration of the body + public: void SetAngularAccel(const Vector3 &accel); + + /// \brief Get the angular acceleration of the body + public: Vector3 GetAngularAccel() const; + /// \brief Set the force applied to the body public: void SetForce(const Vector3 &force); @@ -151,7 +163,7 @@ public: Model *GetModel() const; /// \brief Get the mass of the body - public: float GetMass() { return mass.mass; } + public: float GetMass() const { return mass.mass; } /// Load a new geom helper function /// \param node XMLConfigNode used to load the geom Modified: code/branches/federation/gazebo/worlds/pioneer2dx.world =================================================================== --- code/branches/federation/gazebo/worlds/pioneer2dx.world 2009-03-08 01:54:38 UTC (rev 7376) +++ code/branches/federation/gazebo/worlds/pioneer2dx.world 2009-03-08 01:56:07 UTC (rev 7377) @@ -96,6 +96,7 @@ <model:physical name="pioneer2dx_model1"> <xyz>0 0 0.145</xyz> <rpy>0.0 0.0 0.0</rpy> + <disableGravity/> <controller:differential_position2d name="controller1"> <leftJoint>left_wheel_hinge</leftJoint> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-03-08 02:34:38
|
Revision: 7378 http://playerstage.svn.sourceforge.net/playerstage/?rev=7378&view=rev Author: natepak Date: 2009-03-08 02:34:23 +0000 (Sun, 08 Mar 2009) Log Message: ----------- Added enableGravity Modified Paths: -------------- code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc code/branches/federation/gazebo/server/Model.cc code/branches/federation/gazebo/server/Model.hh code/branches/federation/gazebo/server/physics/Body.cc code/branches/federation/gazebo/worlds/audio.world code/branches/federation/gazebo/worlds/bandit.world code/branches/federation/gazebo/worlds/bumper.world code/branches/federation/gazebo/worlds/factory.world code/branches/federation/gazebo/worlds/laser.world code/branches/federation/gazebo/worlds/lights.world code/branches/federation/gazebo/worlds/map.world code/branches/federation/gazebo/worlds/openal.world code/branches/federation/gazebo/worlds/pioneer2at.world code/branches/federation/gazebo/worlds/pioneer2dx.world code/branches/federation/gazebo/worlds/pioneer2dx_camera.world code/branches/federation/gazebo/worlds/pioneer2dx_gripper.world code/branches/federation/gazebo/worlds/simplecar.world code/branches/federation/gazebo/worlds/simpleshapes.world code/branches/federation/gazebo/worlds/test.world code/branches/federation/gazebo/worlds/trimesh.world code/branches/federation/gazebo/worlds/wizbot.world Modified: code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc =================================================================== --- code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc 2009-03-08 02:34:23 UTC (rev 7378) @@ -40,6 +40,7 @@ { gazebo::Pose pose; + pose.pos.z = 1.0; gazebo::Vec3 linearVel(0.1, 0, 0); gazebo::Vec3 angularVel(0, 0, 0); gazebo::Vec3 linearAccel(0, 0, 0); @@ -53,9 +54,8 @@ // Example of resetting the simulator // simIface->Reset(); + //usleep(1000000); - usleep(1000000); - simIface->Close(); delete simIface; Modified: code/branches/federation/gazebo/server/Model.cc =================================================================== --- code/branches/federation/gazebo/server/Model.cc 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/server/Model.cc 2009-03-08 02:34:23 UTC (rev 7378) @@ -68,6 +68,10 @@ this->rpyP = new ParamT<Quatern>("rpy", Quatern(1,0,0,0), 0); this->rpyP->Callback( &Model::SetRotation, this); + + this->enableGravityP = new ParamT<bool>("enableGravity", true, 0); + this->enableGravityP->Callback( &Model::SetGravityMode, this ); + Param::End(); this->parentBodyNameP = NULL; @@ -118,11 +122,12 @@ this->canonicalBodyNameP->Load(node); this->xyzP->Load(node); this->rpyP->Load(node); + this->enableGravityP->Load(node); this->xmlNode = node; this->type=node->GetName(); - this->SetStatic(this->staticP->GetValue()); + this->SetStatic( **(this->staticP) ); if (this->type == "physical") this->LoadPhysical(node); @@ -172,6 +177,11 @@ // Record the model's initial pose (for reseting) this->SetInitPose(pose); + // This must be placed after creation of the bodies + // Static variable overrides the gravity + if (**this->staticP == false) + this->SetGravityMode( **(this->enableGravityP) ); + return 0; // Get the name of the python module @@ -224,6 +234,7 @@ stream << " name=\"" << this->nameP->GetValue() << "\">\n"; stream << prefix << " " << *(this->xyzP) << "\n"; stream << prefix << " " << *(this->rpyP) << "\n"; + stream << prefix << " " << *(this->enableGravityP) << "\n"; if (this->type == "physical") { @@ -772,7 +783,21 @@ return this->bodies[this->canonicalBodyNameP->GetValue()]; } +//////////////////////////////////////////////////////////////////////////////// +/// Set the gravity mode of the model +void Model::SetGravityMode( const bool &v ) +{ + Body *body; + std::map<std::string, Body* >::iterator iter; + for (iter=this->bodies.begin(); iter!=this->bodies.end(); iter++) + { + body = iter->second; + + body->SetGravityMode( v ); + } +} + //////////////////////////////////////////////////////////////////////////////// // Load a renderable model (like a light source). void Model::LoadRenderable(XMLConfigNode *node) @@ -785,7 +810,7 @@ char lightNumBuf[8]; sprintf(lightNumBuf, "%d", lightNumber++); body->SetName(this->GetName() + "_RenderableBody_" + lightNumBuf); - body->SetGravityMode(false); + //body->SetGravityMode(false); body->SetPose(Pose3d()); this->bodies[body->GetName()] = body; Modified: code/branches/federation/gazebo/server/Model.hh =================================================================== --- code/branches/federation/gazebo/server/Model.hh 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/server/Model.hh 2009-03-08 02:34:23 UTC (rev 7378) @@ -155,6 +155,9 @@ /// \return Pointer to the body public: Body *GetCanonicalBody(); + /// \brief Set the gravity mode of the model + public: void SetGravityMode( const bool &v ); + /// \brief Load a body helper function /// \param node XML Configuration node private: void LoadBody(XMLConfigNode *node); @@ -205,8 +208,8 @@ private: ParamT<Quatern> *rpyP; private: ParamT<std::string> *parentBodyNameP; private: ParamT<std::string> *myBodyNameP; + private: ParamT<bool> *enableGravityP; - // Name of a light (if the model is renderable:light) private: std::string lightName; Modified: code/branches/federation/gazebo/server/physics/Body.cc =================================================================== --- code/branches/federation/gazebo/server/physics/Body.cc 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/server/physics/Body.cc 2009-03-08 02:34:23 UTC (rev 7378) @@ -198,7 +198,7 @@ void Body::SetGravityMode(bool mode) { if (this->bodyId) - dBodySetGravityMode(this->bodyId, mode); + dBodySetGravityMode(this->bodyId, mode ? 1: 0); } //////////////////////////////////////////////////////////////////////////////// Modified: code/branches/federation/gazebo/worlds/audio.world =================================================================== --- code/branches/federation/gazebo/worlds/audio.world 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/worlds/audio.world 2009-03-08 02:34:23 UTC (rev 7378) @@ -18,8 +18,8 @@ <physics:ode> <stepTime>0.03</stepTime> <gravity>0 0 -9.8</gravity> - <cfm>0.008</cfm> - <erp>0.2</erp> + <cfm>10e-5</cfm> + <erp>0.3</erp> </physics:ode> <rendering:gui> @@ -87,9 +87,10 @@ <rpy>0.0 0.0 0.0</rpy> <body:cylinder name="cylinder1_body"> - <sensor:microphone> + <!--<sensor:microphone name="microphone_sensor"> <gain>1.0</gain> </sensor:microphone> + --> <geom:cylinder name="cylinder1_geom"> <size>0.5 1</size> <mass>1.0</mass> @@ -135,4 +136,18 @@ </body:empty> </model:physical> + <!-- White Point light --> + <model:renderable name="point_white"> + <xyz>0 2 2</xyz> + <enableGravity>false</enableGravity> + + <light> + <type>point</type> + <diffuseColor>0.8 0.8 0.8</diffuseColor> + <specularColor>0.1 0.1 0.1</specularColor> + <attenuation>20 0.5 0.1 0</attenuation> + </light> + </model:renderable> + + </gazebo:world> Modified: code/branches/federation/gazebo/worlds/bandit.world =================================================================== --- code/branches/federation/gazebo/worlds/bandit.world 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/worlds/bandit.world 2009-03-08 02:34:23 UTC (rev 7378) @@ -161,6 +161,7 @@ <!-- White Directional light --> <model:renderable name="point_white"> <xyz>0 2 2</xyz> + <enableGravity>false</enableGravity> <light> <type>point</type> <diffuseColor>0.8 0.8 0.8</diffuseColor> Modified: code/branches/federation/gazebo/worlds/bumper.world =================================================================== --- code/branches/federation/gazebo/worlds/bumper.world 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/worlds/bumper.world 2009-03-08 02:34:23 UTC (rev 7378) @@ -137,6 +137,7 @@ <!-- White Directional light --> <model:renderable name="directional_white"> + <enableGravity>false</enableGravity> <light> <type>directional</type> <direction>0 -0.6 -0.4</direction> Modified: code/branches/federation/gazebo/worlds/factory.world =================================================================== --- code/branches/federation/gazebo/worlds/factory.world 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/worlds/factory.world 2009-03-08 02:34:23 UTC (rev 7378) @@ -89,6 +89,7 @@ <!-- White Directional light --> <model:renderable name="directional_white"> + <enableGravity>false</enableGravity> <light> <type>directional</type> <direction>0 -0.5 -0.5</direction> Modified: code/branches/federation/gazebo/worlds/laser.world =================================================================== --- code/branches/federation/gazebo/worlds/laser.world 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/worlds/laser.world 2009-03-08 02:34:23 UTC (rev 7378) @@ -97,6 +97,7 @@ <!-- White Directional light --> <model:renderable name="directional_white"> + <enableGravity>false</enableGravity> <light> <type>directional</type> <direction>0 -0.5 -0.5</direction> Modified: code/branches/federation/gazebo/worlds/lights.world =================================================================== --- code/branches/federation/gazebo/worlds/lights.world 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/worlds/lights.world 2009-03-08 02:34:23 UTC (rev 7378) @@ -56,6 +56,7 @@ <model:renderable name="spot_red"> <xyz>0.0 0.0 2.0</xyz> <rpy>0.0 0.0 0.0</rpy> + <enableGravity>false</enableGravity> <light> <type>spot</type> @@ -70,6 +71,7 @@ <model:renderable name="spot_green"> <xyz>0.0 1.0 2.0</xyz> <rpy>0.0 0.0 0.0</rpy> + <enableGravity>false</enableGravity> <light> <type>spot</type> @@ -98,6 +100,7 @@ <model:renderable name="spot_blue"> <xyz>0.0 0 0</xyz> <rpy>0.0 0.0 0.0</rpy> + <enableGravity>false</enableGravity> <light> <type>spot</type> @@ -115,6 +118,8 @@ <!-- Yellow Point light --> <model:renderable name="point_yellow"> <xyz>5 5 5</xyz> + <enableGravity>false</enableGravity> + <light> <type>point</type> <diffuseColor>0.1 0.1 0.0</diffuseColor> @@ -125,6 +130,7 @@ <!-- White Directional light --> <model:renderable name="directional_white"> + <enableGravity>false</enableGravity> <light> <type>directional</type> <direction>0 -1 0</direction> Modified: code/branches/federation/gazebo/worlds/map.world =================================================================== --- code/branches/federation/gazebo/worlds/map.world 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/worlds/map.world 2009-03-08 02:34:23 UTC (rev 7378) @@ -77,6 +77,7 @@ </model:physical> <model:renderable name="directional_white"> + <enableGravity>false</enableGravity> <light> <type>directional</type> <direction>0 -0.6 -0.4</direction> Modified: code/branches/federation/gazebo/worlds/openal.world =================================================================== --- code/branches/federation/gazebo/worlds/openal.world 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/worlds/openal.world 2009-03-08 02:34:23 UTC (rev 7378) @@ -130,6 +130,8 @@ <!-- White Directional light --> <model:renderable name="point_white"> <xyz>0 2 2</xyz> + <enableGravity>false</enableGravity> + <light> <type>point</type> <diffuseColor>0.8 0.8 0.8</diffuseColor> Modified: code/branches/federation/gazebo/worlds/pioneer2at.world =================================================================== --- code/branches/federation/gazebo/worlds/pioneer2at.world 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/worlds/pioneer2at.world 2009-03-08 02:34:23 UTC (rev 7378) @@ -111,6 +111,7 @@ <!-- White Directional light --> <model:renderable name="directional_white"> + <enableGravity>false</enableGravity> <light> <type>directional</type> <direction>0 -0.8 -0.3</direction> Modified: code/branches/federation/gazebo/worlds/pioneer2dx.world =================================================================== --- code/branches/federation/gazebo/worlds/pioneer2dx.world 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/worlds/pioneer2dx.world 2009-03-08 02:34:23 UTC (rev 7378) @@ -94,9 +94,9 @@ --> <model:physical name="pioneer2dx_model1"> - <xyz>0 0 0.145</xyz> + <xyz>0 0 1.145</xyz> <rpy>0.0 0.0 0.0</rpy> - <disableGravity/> + <enableGravity>false</enableGravity> <controller:differential_position2d name="controller1"> <leftJoint>left_wheel_hinge</leftJoint> @@ -109,6 +109,7 @@ <model:physical name="laser"> <xyz>0.15 0 0.18</xyz> + <enableGravity>false</enableGravity> <attach> <parentBody>chassis_body</parentBody> @@ -128,6 +129,8 @@ <!-- White Point light --> <model:renderable name="point_white"> <xyz>0 2 2</xyz> + <enableGravity>false</enableGravity> + <light> <type>point</type> <diffuseColor>0.8 0.8 0.8</diffuseColor> Modified: code/branches/federation/gazebo/worlds/pioneer2dx_camera.world =================================================================== --- code/branches/federation/gazebo/worlds/pioneer2dx_camera.world 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/worlds/pioneer2dx_camera.world 2009-03-08 02:34:23 UTC (rev 7378) @@ -120,6 +120,8 @@ <!-- White Directional light --> <model:renderable name="directional_white"> <xyz>2.0 0 2.0</xyz> + <enableGravity>false</enableGravity> + <light> <type>spot</type> <range>1000 1000 100</range> Modified: code/branches/federation/gazebo/worlds/pioneer2dx_gripper.world =================================================================== --- code/branches/federation/gazebo/worlds/pioneer2dx_gripper.world 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/worlds/pioneer2dx_gripper.world 2009-03-08 02:34:23 UTC (rev 7378) @@ -104,6 +104,7 @@ <!-- White Directional light --> <model:renderable name="directional_white"> + <enableGravity>false</enableGravity> <light> <type>directional</type> <direction>0 -0.6 -0.4</direction> Modified: code/branches/federation/gazebo/worlds/simplecar.world =================================================================== --- code/branches/federation/gazebo/worlds/simplecar.world 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/worlds/simplecar.world 2009-03-08 02:34:23 UTC (rev 7378) @@ -114,6 +114,7 @@ <!-- White Directional light --> <model:renderable name="directional_white"> + <enableGravity>false</enableGravity> <light> <type>directional</type> <direction>0 -0.8 -0.3</direction> Modified: code/branches/federation/gazebo/worlds/simpleshapes.world =================================================================== --- code/branches/federation/gazebo/worlds/simpleshapes.world 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/worlds/simpleshapes.world 2009-03-08 02:34:23 UTC (rev 7378) @@ -115,6 +115,8 @@ <!-- White Directional light --> <model:renderable name="point_white"> <xyz>0 2 2</xyz> + <enableGravity>false</enableGravity> + <light> <type>point</type> <diffuseColor>0.8 0.8 0.8</diffuseColor> Modified: code/branches/federation/gazebo/worlds/test.world =================================================================== --- code/branches/federation/gazebo/worlds/test.world 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/worlds/test.world 2009-03-08 02:34:23 UTC (rev 7378) @@ -142,6 +142,7 @@ <!-- White Directional light --> <model:renderable name="directional_white"> + <enableGravity>false</enableGravity> <light> <type>directional</type> <direction>0 -0.6 -0.4</direction> Modified: code/branches/federation/gazebo/worlds/trimesh.world =================================================================== --- code/branches/federation/gazebo/worlds/trimesh.world 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/worlds/trimesh.world 2009-03-08 02:34:23 UTC (rev 7378) @@ -74,6 +74,7 @@ <!-- White Directional light --> <model:renderable name="directional_white"> + <enableGravity>false</enableGravity> <light> <type>directional</type> <direction>0 -0.5 -0.5</direction> Modified: code/branches/federation/gazebo/worlds/wizbot.world =================================================================== --- code/branches/federation/gazebo/worlds/wizbot.world 2009-03-08 01:56:07 UTC (rev 7377) +++ code/branches/federation/gazebo/worlds/wizbot.world 2009-03-08 02:34:23 UTC (rev 7378) @@ -163,6 +163,7 @@ <!-- White Directional light --> <model:renderable name="directional_white"> + <enableGravity>false</enableGravity> <light> <type>directional</type> <direction>0 -0.6 -0.4</direction> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-03-08 02:57:42
|
Revision: 7380 http://playerstage.svn.sourceforge.net/playerstage/?rev=7380&view=rev Author: natepak Date: 2009-03-08 02:57:32 +0000 (Sun, 08 Mar 2009) Log Message: ----------- Added flag to disable friction model wide Modified Paths: -------------- code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc code/branches/federation/gazebo/server/Model.cc code/branches/federation/gazebo/server/Model.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/physics/HeightmapGeom.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-08 02:35:03 UTC (rev 7379) +++ code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc 2009-03-08 02:57:32 UTC (rev 7380) @@ -40,15 +40,16 @@ { gazebo::Pose pose; - pose.pos.z = 1.0; - gazebo::Vec3 linearVel(0.1, 0, 0); + pose.pos.x = i+0.1; + pose.pos.z = .145; + gazebo::Vec3 linearVel(0.2, 0, 0); gazebo::Vec3 angularVel(0, 0, 0); gazebo::Vec3 linearAccel(0, 0, 0); gazebo::Vec3 angularAccel(0, 0, 0); simIface->SetState(name, pose, linearVel, angularVel, linearAccel, angularAccel ); - usleep(9000000); + usleep(900000); } Modified: code/branches/federation/gazebo/server/Model.cc =================================================================== --- code/branches/federation/gazebo/server/Model.cc 2009-03-08 02:35:03 UTC (rev 7379) +++ code/branches/federation/gazebo/server/Model.cc 2009-03-08 02:57:32 UTC (rev 7380) @@ -72,6 +72,9 @@ this->enableGravityP = new ParamT<bool>("enableGravity", true, 0); this->enableGravityP->Callback( &Model::SetGravityMode, this ); + this->enableFrictionP = new ParamT<bool>("enableFriction", true, 0); + this->enableFrictionP->Callback( &Model::SetFrictionMode, this ); + Param::End(); this->parentBodyNameP = NULL; @@ -123,6 +126,7 @@ this->xyzP->Load(node); this->rpyP->Load(node); this->enableGravityP->Load(node); + this->enableFrictionP->Load(node); this->xmlNode = node; this->type=node->GetName(); @@ -180,8 +184,10 @@ // This must be placed after creation of the bodies // Static variable overrides the gravity if (**this->staticP == false) - this->SetGravityMode( **(this->enableGravityP) ); + this->SetGravityMode( **this->enableGravityP ); + this->SetFrictionMode( **this->enableFrictionP ); + return 0; // Get the name of the python module @@ -235,6 +241,7 @@ stream << prefix << " " << *(this->xyzP) << "\n"; stream << prefix << " " << *(this->rpyP) << "\n"; stream << prefix << " " << *(this->enableGravityP) << "\n"; + stream << prefix << " " << *(this->enableFrictionP) << "\n"; if (this->type == "physical") { @@ -799,6 +806,22 @@ } //////////////////////////////////////////////////////////////////////////////// +/// Set the gravity mode of the model +void Model::SetFrictionMode( const bool &v ) +{ + Body *body; + + std::map<std::string, Body* >::iterator iter; + + for (iter=this->bodies.begin(); iter!=this->bodies.end(); iter++) + { + body = iter->second; + + body->SetFrictionMode( v ); + } +} + +//////////////////////////////////////////////////////////////////////////////// // Load a renderable model (like a light source). void Model::LoadRenderable(XMLConfigNode *node) { Modified: code/branches/federation/gazebo/server/Model.hh =================================================================== --- code/branches/federation/gazebo/server/Model.hh 2009-03-08 02:35:03 UTC (rev 7379) +++ code/branches/federation/gazebo/server/Model.hh 2009-03-08 02:57:32 UTC (rev 7380) @@ -158,6 +158,9 @@ /// \brief Set the gravity mode of the model public: void SetGravityMode( const bool &v ); + /// \brief Set the friction mode of the model + public: void SetFrictionMode( const bool &v ); + /// \brief Load a body helper function /// \param node XML Configuration node private: void LoadBody(XMLConfigNode *node); @@ -209,6 +212,7 @@ private: ParamT<std::string> *parentBodyNameP; private: ParamT<std::string> *myBodyNameP; private: ParamT<bool> *enableGravityP; + private: ParamT<bool> *enableFrictionP; // Name of a light (if the model is renderable:light) private: std::string lightName; Modified: code/branches/federation/gazebo/server/physics/Body.cc =================================================================== --- code/branches/federation/gazebo/server/physics/Body.cc 2009-03-08 02:35:03 UTC (rev 7379) +++ code/branches/federation/gazebo/server/physics/Body.cc 2009-03-08 02:57:32 UTC (rev 7380) @@ -202,6 +202,18 @@ } //////////////////////////////////////////////////////////////////////////////// +/// Set the friction mode of the body +void Body::SetFrictionMode( const bool &v ) +{ + std::map< std::string, Geom* >::iterator giter; + + for (giter = this->geoms.begin(); giter != this->geoms.end(); giter++) + { + giter->second->SetFrictionMode( v ); + } +} + +//////////////////////////////////////////////////////////////////////////////// // Initialize the body void Body::Init() { Modified: code/branches/federation/gazebo/server/physics/Body.hh =================================================================== --- code/branches/federation/gazebo/server/physics/Body.hh 2009-03-08 02:35:03 UTC (rev 7379) +++ code/branches/federation/gazebo/server/physics/Body.hh 2009-03-08 02:57:32 UTC (rev 7380) @@ -117,6 +117,9 @@ /// \brief Set whether gravity affects this body public: void SetGravityMode(bool mode); + /// \brief Set the friction mode of the body + public: void SetFrictionMode( const bool &v ); + /// \brief Set the linear velocity of the body public: void SetLinearVel(const Vector3 &vel); Modified: code/branches/federation/gazebo/server/physics/Geom.cc =================================================================== --- code/branches/federation/gazebo/server/physics/Geom.cc 2009-03-08 02:35:03 UTC (rev 7379) +++ code/branches/federation/gazebo/server/physics/Geom.cc 2009-03-08 02:57:32 UTC (rev 7380) @@ -562,3 +562,14 @@ { return this->body->GetModel(); } + +//////////////////////////////////////////////////////////////////////////////// +/// Set the friction mode of the geom +void Geom::SetFrictionMode( const bool &v ) +{ + this->contact->mu1 = 0; + this->contact->mu2 = 0; + this->contact->slip1 = 0; + this->contact->slip2 = 0; +} + Modified: code/branches/federation/gazebo/server/physics/Geom.hh =================================================================== --- code/branches/federation/gazebo/server/physics/Geom.hh 2009-03-08 02:35:03 UTC (rev 7379) +++ code/branches/federation/gazebo/server/physics/Geom.hh 2009-03-08 02:57:32 UTC (rev 7380) @@ -159,6 +159,9 @@ /// \brief Get the model this geom belongs to public: Model *GetModel() const; + /// \brief Set the friction mode of the geom + public: void SetFrictionMode( const bool &v ); + /// Contact parameters public: ContactParams *contact; Modified: code/branches/federation/gazebo/server/physics/HeightmapGeom.cc =================================================================== --- code/branches/federation/gazebo/server/physics/HeightmapGeom.cc 2009-03-08 02:35:03 UTC (rev 7379) +++ code/branches/federation/gazebo/server/physics/HeightmapGeom.cc 2009-03-08 02:57:32 UTC (rev 7380) @@ -227,9 +227,8 @@ stream << "MaxMipMapLevel=2\n"; // Create a data stream for loading the terrain into Ogre - char *mstr = new char[1024];//stream.str().size()]; - bzero (mstr, 1024); - sprintf(mstr, (char*)(stream.str().c_str())); + char *mstr = strdup(stream.str().c_str()); + Ogre::DataStreamPtr dataStream( new Ogre::MemoryDataStream(mstr,strlen(mstr)) ); @@ -287,7 +286,7 @@ pose.rot = pose.rot * quat; this->body->SetPose(pose); - delete [] mstr; + free(mstr); } //////////////////////////////////////////////////////////////////////////////// Modified: code/branches/federation/gazebo/worlds/pioneer2dx.world =================================================================== --- code/branches/federation/gazebo/worlds/pioneer2dx.world 2009-03-08 02:35:03 UTC (rev 7379) +++ code/branches/federation/gazebo/worlds/pioneer2dx.world 2009-03-08 02:57:32 UTC (rev 7380) @@ -94,9 +94,10 @@ --> <model:physical name="pioneer2dx_model1"> - <xyz>0 0 1.145</xyz> + <xyz>0 0 .145</xyz> <rpy>0.0 0.0 0.0</rpy> <enableGravity>false</enableGravity> + <enableFriction>false</enableFriction> <controller:differential_position2d name="controller1"> <leftJoint>left_wheel_hinge</leftJoint> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ge...@us...> - 2009-03-08 03:27:56
|
Revision: 7381 http://playerstage.svn.sourceforge.net/playerstage/?rev=7381&view=rev Author: gerkey Date: 2009-03-08 03:27:54 +0000 (Sun, 08 Mar 2009) Log Message: ----------- First cut at webgazebo Modified Paths: -------------- code/branches/federation/gazebo/SConstruct Added Paths: ----------- code/branches/federation/gazebo/webgazebo/ code/branches/federation/gazebo/webgazebo/README code/branches/federation/gazebo/webgazebo/SConscript code/branches/federation/gazebo/webgazebo/WebGazebo.cc code/branches/federation/gazebo/webgazebo/WebGazebo.hh code/branches/federation/gazebo/webgazebo/main.cc Modified: code/branches/federation/gazebo/SConstruct =================================================================== --- code/branches/federation/gazebo/SConstruct 2009-03-08 02:57:32 UTC (rev 7380) +++ code/branches/federation/gazebo/SConstruct 2009-03-08 03:27:54 UTC (rev 7381) @@ -143,8 +143,10 @@ # DEFAULT list of subdirectories to build ####### subdirs = ['libgazebo','server', 'player'] +# Disabled, pending test for <event.h> +if False: + subdirs.append('webgazebo') - ####### # Generate help text ####### Added: code/branches/federation/gazebo/webgazebo/README =================================================================== --- code/branches/federation/gazebo/webgazebo/README (rev 0) +++ code/branches/federation/gazebo/webgazebo/README 2009-03-08 03:27:54 UTC (rev 7381) @@ -0,0 +1,3 @@ +webgazebo is an HTTP interface to Gazebo. It uses libgazebo to talk to +a Gazebo simulation, and libevent to provide an HTTP socket to the outside +world. Added: code/branches/federation/gazebo/webgazebo/SConscript =================================================================== --- code/branches/federation/gazebo/webgazebo/SConscript (rev 0) +++ code/branches/federation/gazebo/webgazebo/SConscript 2009-03-08 03:27:54 UTC (rev 7381) @@ -0,0 +1,48 @@ +#Import variable +Import('*') + +env.Append(CPPPATH = '#webgazebo') + +libEnv = Environment ( + CC = 'g++', + CCFLAGS = Split ('-pipe -W -Wall -g'), + + #CXXCOMSTR = 'Compiling $TARGET', + #CCCOMSTR = 'Compiling $TARGET', + + #SHCXXCOMSTR = 'Compiling $TARGET', + #SHCCCOMSTR = 'Compiling $TARGET', + + #SHLINKCOMSTR = 'Linking $TARGET', + #LINKCOMSTR = 'Linking $TARGET', + + LIBS=Split('event'), +) + +sources = Split('WebGazebo.cc') +headers = Split('WebGazebo.hh') + +sharedLib = libEnv.SharedLibrary('webgazebo', sources) +staticLib = libEnv.StaticLibrary('webgazebo', sources) + +import os + +exeEnv = Environment ( + CC = 'g++', + CCFLAGS = Split ('-pipe -W -Wall -g'), + + #CXXCOMSTR = 'Compiling $TARGET', + #CCCOMSTR = 'Compiling $TARGET', + + #SHCXXCOMSTR = 'Compiling $TARGET', + #SHCCCOMSTR = 'Compiling $TARGET', + + #SHLINKCOMSTR = 'Linking $TARGET', + #LINKCOMSTR = 'Linking $TARGET', + + LIBS=Split('webgazebo event'), + LIBPATH=Split(os.getcwd()), + CPPPATH=Split('#.') +) + +webgazebo = exeEnv.Program('webgazebo', 'main.cc') Added: code/branches/federation/gazebo/webgazebo/WebGazebo.cc =================================================================== --- code/branches/federation/gazebo/webgazebo/WebGazebo.cc (rev 0) +++ code/branches/federation/gazebo/webgazebo/WebGazebo.cc 2009-03-08 03:27:54 UTC (rev 7381) @@ -0,0 +1,34 @@ +#include "WebGazebo.hh" + +#include <stdio.h> +#include <assert.h> +#include <stdlib.h> + +WebGazebo::WebGazebo(std::string _host, int _port) : + host(_host), port(_port) +{ + // Not sure whether it's safe to do this more that once in one process + event_init(); + + this->eh = evhttp_start(this->host.c_str(), this->port); + assert(eh); +} + +WebGazebo::~WebGazebo() +{ + // No event_fini() to call... +} + +void +WebGazebo::GenericURICallback(evhttp_request* req, void* arg) +{ + WebGazebo* obj = (WebGazebo*)arg; + + printf("in server cb for uri:%s:\n", req->uri); + struct evbuffer* eb = evbuffer_new(); + assert(eb); + evbuffer_add_printf(eb, "response for:%s:\n", req->uri); + evhttp_send_reply(req, 200, "foo", eb); + evbuffer_free(eb); +} + Added: code/branches/federation/gazebo/webgazebo/WebGazebo.hh =================================================================== --- code/branches/federation/gazebo/webgazebo/WebGazebo.hh (rev 0) +++ code/branches/federation/gazebo/webgazebo/WebGazebo.hh 2009-03-08 03:27:54 UTC (rev 7381) @@ -0,0 +1,26 @@ +#include <string> + +// These headers must be included prior to the libevent headers +#include <sys/types.h> +#include <sys/queue.h> + +// libevent +#include <event.h> +#include <evhttp.h> + +class WebGazebo +{ + public: + WebGazebo(std::string _host, int _port); + ~WebGazebo(); + + private: + std::string host; + int port; + + struct evhttp* eh; + + // Static, so that it can be passed as a callback to libevent + static void GenericURICallback(evhttp_request* req, void* arg); +}; + Added: code/branches/federation/gazebo/webgazebo/main.cc =================================================================== --- code/branches/federation/gazebo/webgazebo/main.cc (rev 0) +++ code/branches/federation/gazebo/webgazebo/main.cc 2009-03-08 03:27:54 UTC (rev 7381) @@ -0,0 +1,62 @@ +#include "webgazebo/WebGazebo.hh" + +#include <stdlib.h> + +#define USAGE "USAGE: webgazebo [-h <host>] [-p <port>]" + +int g_port; +std::string g_host; + +bool ParseArgs(int argc, char** argv); + +int +main(int argc, char** argv) +{ + //evhttp_set_gencb(eh, cb, NULL); + //evhttp_set_cb(eh, "/foo", cb, NULL); + + //event_dispatch(); + + if(!ParseArgs(argc, argv)) + { + fputs(USAGE, stderr); + exit(1); + } + + WebGazebo wg(g_host, g_port); + + return 0; +} + +bool +ParseArgs(int argc, char** argv) +{ + char *flags = (char*)("p:h:"); + int ch; + + while ((ch = getopt(argc, argv, flags)) != -1) + { + switch(ch) + { + // port + case 'p': + if(!optarg) + return false; + g_port = atoi(optarg); + break; + // host + case 'h': + if(!optarg) + return false; + g_host = optarg; + break; + default: + return false; + } + } + + if((g_host.size() == 0) || (g_port == 0)) + return false; + + return true; +} This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-03-08 03:31:20
|
Revision: 7382 http://playerstage.svn.sourceforge.net/playerstage/?rev=7382&view=rev Author: natepak Date: 2009-03-08 03:31:13 +0000 (Sun, 08 Mar 2009) Log Message: ----------- Set State velocities and accelerations are in the models local coord frame Modified Paths: -------------- code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc code/branches/federation/gazebo/server/Quatern.cc code/branches/federation/gazebo/server/Quatern.hh code/branches/federation/gazebo/server/World.cc code/branches/federation/gazebo/server/physics/ContactParams.cc code/branches/federation/gazebo/server/physics/ContactParams.hh 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-08 03:27:54 UTC (rev 7381) +++ code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc 2009-03-08 03:31:13 UTC (rev 7382) @@ -1,5 +1,6 @@ #include <string.h> #include <iostream> +#include <math.h> #include <gazebo/gazebo.h> int main() @@ -40,8 +41,9 @@ { gazebo::Pose pose; - pose.pos.x = i+0.1; + //pose.pos.x = i+0.1; pose.pos.z = .145; + pose.yaw = M_PI/2; gazebo::Vec3 linearVel(0.2, 0, 0); gazebo::Vec3 angularVel(0, 0, 0); gazebo::Vec3 linearAccel(0, 0, 0); @@ -49,7 +51,7 @@ simIface->SetState(name, pose, linearVel, angularVel, linearAccel, angularAccel ); - usleep(900000); + usleep(90000000); } Modified: code/branches/federation/gazebo/server/Quatern.cc =================================================================== --- code/branches/federation/gazebo/server/Quatern.cc 2009-03-08 03:27:54 UTC (rev 7381) +++ code/branches/federation/gazebo/server/Quatern.cc 2009-03-08 03:31:13 UTC (rev 7382) @@ -274,6 +274,28 @@ } //////////////////////////////////////////////////////////////////////////////// +/// Rotate a vector using the quaternion +Vector3 Quatern::RotateVector(Vector3 vec) const +{ + Quatern tmp; + Vector3 result; + + tmp.u = 0.0; + tmp.x = vec.x; + tmp.y = vec.y; + tmp.z = vec.z; + + tmp = (*this) * (tmp * this->GetInverse()); + + result.x = tmp.x; + result.y = tmp.y; + result.z = tmp.z; + + return result; +} + + +//////////////////////////////////////////////////////////////////////////////// // See if a quatern is finite (e.g., not nan) bool Quatern::IsFinite() const { Modified: code/branches/federation/gazebo/server/Quatern.hh =================================================================== --- code/branches/federation/gazebo/server/Quatern.hh 2009-03-08 03:27:54 UTC (rev 7381) +++ code/branches/federation/gazebo/server/Quatern.hh 2009-03-08 03:31:13 UTC (rev 7382) @@ -125,6 +125,10 @@ /// \return This quatern multiplied by the parameter public: Quatern operator*( const Quatern &qt ) const; + /// \brief Rotate a vector using the quaternion + /// \return The rotated vector + public: Vector3 RotateVector(Vector3 vec) const; + /// \brief See if a quatern is finite (e.g., not nan) /// \return True if quatern is finite public: bool IsFinite() const; Modified: code/branches/federation/gazebo/server/World.cc =================================================================== --- code/branches/federation/gazebo/server/World.cc 2009-03-08 03:27:54 UTC (rev 7381) +++ code/branches/federation/gazebo/server/World.cc 2009-03-08 03:31:13 UTC (rev 7382) @@ -611,11 +611,18 @@ // The the model's pose pose.rot.SetFromEuler( - Vector3(req->modelPose.roll, + Vector3( + req->modelPose.roll, req->modelPose.pitch, req->modelPose.yaw)); model->SetPose(pose); + linearVel = pose.rot.RotateVector(linearVel); + angularVel = pose.rot.RotateVector(angularVel); + + linearAccel = pose.rot.RotateVector(linearAccel); + angularAccel = pose.rot.RotateVector(angularAccel); + // Set the model's linear and angular velocity model->SetLinearVel(linearVel); model->SetAngularVel(angularVel); Modified: code/branches/federation/gazebo/server/physics/ContactParams.cc =================================================================== --- code/branches/federation/gazebo/server/physics/ContactParams.cc 2009-03-08 03:27:54 UTC (rev 7381) +++ code/branches/federation/gazebo/server/physics/ContactParams.cc 2009-03-08 03:31:13 UTC (rev 7382) @@ -45,6 +45,8 @@ this->mu2 = dInfinity; this->slip1 = 0.1; this->slip2 = 0.1; + + this->enabled = true; } ////////////////////////////////////////////////////////////////////////////// Modified: code/branches/federation/gazebo/server/physics/ContactParams.hh =================================================================== --- code/branches/federation/gazebo/server/physics/ContactParams.hh 2009-03-08 03:27:54 UTC (rev 7381) +++ code/branches/federation/gazebo/server/physics/ContactParams.hh 2009-03-08 03:31:13 UTC (rev 7382) @@ -78,6 +78,8 @@ /// \brief soft constraint force mixing public: double softCfm; + + public: bool enabled; public: boost::signal< void (Geom*, Geom*)> contactSignal; }; Modified: code/branches/federation/gazebo/worlds/pioneer2dx.world =================================================================== --- code/branches/federation/gazebo/worlds/pioneer2dx.world 2009-03-08 03:27:54 UTC (rev 7381) +++ code/branches/federation/gazebo/worlds/pioneer2dx.world 2009-03-08 03:31:13 UTC (rev 7382) @@ -95,7 +95,7 @@ <model:physical name="pioneer2dx_model1"> <xyz>0 0 .145</xyz> - <rpy>0.0 0.0 0.0</rpy> + <rpy>0.0 0.0 90.0</rpy> <enableGravity>false</enableGravity> <enableFriction>false</enableFriction> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-08 04:56:00
|
Revision: 7385 http://playerstage.svn.sourceforge.net/playerstage/?rev=7385&view=rev Author: rtv Date: 2009-03-08 04:55:48 +0000 (Sun, 08 Mar 2009) Log Message: ----------- added federation demo files Added Paths: ----------- code/branches/federation/gazebo/Media/materials/textures/cave.png code/branches/federation/gazebo/worlds/federation.world Added: code/branches/federation/gazebo/Media/materials/textures/cave.png =================================================================== (Binary files differ) Property changes on: code/branches/federation/gazebo/Media/materials/textures/cave.png ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/branches/federation/gazebo/worlds/federation.world =================================================================== --- code/branches/federation/gazebo/worlds/federation.world (rev 0) +++ code/branches/federation/gazebo/worlds/federation.world 2009-03-08 04:55:48 UTC (rev 7385) @@ -0,0 +1,159 @@ +<?xml version="1.0"?> + +<gazebo:world + xmlns:xi="http://www.w3.org/2001/XInclude" + xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" + xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" + xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" + xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window" + xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param" + xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" + xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" + xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" + xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" + xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui" + xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" + xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" + xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" > + + <verbosity>5</verbosity> + + <physics:ode> + <stepTime>0.03</stepTime> + <gravity>0 0 -9.8</gravity> + <cfm>10e-5</cfm> + <erp>0.3</erp> + </physics:ode> + + <rendering:gui> + <type>fltk</type> + <size>800 600</size> + <pos>0 0</pos> + <xyz>0 0 0</xyz> + <rpy>0 0 0</rpy> + </rendering:gui> + + <rendering:ogre> + <ambient>0.2 0.2 0.2 1.0</ambient> + <sky> + <material>Gazebo/CloudySky</material> + </sky> + + <fog> + <color>1.0 1.0 1.0</color> + <linearStart>10</linearStart> + <linearEnd>100</linearEnd> + </fog> + + <shadowTechnique>stencilAdditive</shadowTechnique> + </rendering:ogre> + + <!-- Ground Plane --> + <model:physical name="plane1_model"> + <xyz>0 0 0</xyz> + <rpy>0 0 0</rpy> + <static>true</static> + + <body:plane name="plane1_body"> + <geom:plane name="plane1_geom"> + <normal>0 0 1</normal> + <size>2000 2000</size> + <segments>10 10</segments> + <uvTile>100 100</uvTile> + <material>Gazebo/Grey</material> + </geom:plane> + </body:plane> + </model:physical> + + <model:physical name="sphere1_model"> + <xyz>2.15 -1.68 .3</xyz> + <rpy>0.0 0.0 0.0</rpy> + <static>true</static> + + <body:sphere name="sphere1_body"> + <geom:sphere name="sphere1_geom"> + <size>0.1</size> + + <visual> + <scale>0.1 0.1 0.1</scale> + <mesh>unit_sphere</mesh> + <material>Gazebo/Rocky</material> + </visual> + </geom:sphere> + </body:sphere> + </model:physical> + + + <!-- + Include the complete model described in the .model file + This assumes the root node is a <model:...> + --> + <!-- <include embedded="false"> + <xi:include href="pioneer2dx.model" /> + </include> + --> + + <model:physical name="pioneer2dx_model1"> + <xyz>0 0 .145</xyz> + <rpy>0.0 0.0 90.0</rpy> + <enableGravity>false</enableGravity> + <enableFriction>false</enableFriction> + + <controller:differential_position2d name="controller1"> + <leftJoint>left_wheel_hinge</leftJoint> + <rightJoint>right_wheel_hinge</rightJoint> + <wheelSeparation>0.39</wheelSeparation> + <wheelDiameter>0.15</wheelDiameter> + <torque>5</torque> + <interface:position name="position_iface_0"/> + </controller:differential_position2d> + + <model:physical name="laser"> + <xyz>0.15 0 0.18</xyz> + <enableGravity>false</enableGravity> + + <attach> + <parentBody>chassis_body</parentBody> + <myBody>laser_body</myBody> + </attach> + + <include embedded="true"> + <xi:include href="models/sicklms200.model" /> + </include> + </model:physical> + + <include embedded="true"> + <xi:include href="models/pioneer2dx.model" /> + </include> + </model:physical> + + <!-- White Point light --> + <model:renderable name="point_white"> + <xyz>0 2 2</xyz> + <enableGravity>false</enableGravity> + + <light> + <type>point</type> + <diffuseColor>0.8 0.8 0.8</diffuseColor> + <specularColor>0.1 0.1 0.1</specularColor> + <attenuation>20 0.5 0.1 0</attenuation> + </light> + </model:renderable> + + <model:physical name="map"> + <xyz>0 0 0</xyz> + <static>true</static> + <body:map name="map_body"> + <geom:map name="map_geom"> + <scale>0.032</scale> + <image>cave.png</image> + <threshold>200</threshold> + <granularity>2</granularity> + <negative>false</negative> + <scale>0.1</scale> + <material>Gazebo/Rocky</material> + </geom:map> + </body:map> + </model:physical> + +</gazebo:world> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-03-08 05:06:01
|
Revision: 7386 http://playerstage.svn.sourceforge.net/playerstage/?rev=7386&view=rev Author: natepak Date: 2009-03-08 05:05:57 +0000 (Sun, 08 Mar 2009) Log Message: ----------- Run for a specified time Modified Paths: -------------- code/branches/federation/gazebo/server/Global.hh code/branches/federation/gazebo/server/Model.cc code/branches/federation/gazebo/server/Model.hh 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/sensors/ir/IRSensor.cc code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc code/branches/federation/gazebo/worlds/pioneer2dx.world Modified: code/branches/federation/gazebo/server/Global.hh =================================================================== --- code/branches/federation/gazebo/server/Global.hh 2009-03-08 04:55:48 UTC (rev 7385) +++ code/branches/federation/gazebo/server/Global.hh 2009-03-08 05:05:57 UTC (rev 7386) @@ -52,7 +52,7 @@ #define GZ_ALL_COLLIDE 0xFFFFFFFF #define GZ_NONE_COLLIDE 0x00000000 #define GZ_FIXED_COLLIDE 0x00000001 -#define GZ_LASER_COLLIDE 0x00000002 +#define GZ_SENSOR_COLLIDE 0x00000002 #endif Modified: code/branches/federation/gazebo/server/Model.cc =================================================================== --- code/branches/federation/gazebo/server/Model.cc 2009-03-08 04:55:48 UTC (rev 7385) +++ code/branches/federation/gazebo/server/Model.cc 2009-03-08 05:05:57 UTC (rev 7386) @@ -75,6 +75,9 @@ this->enableFrictionP = new ParamT<bool>("enableFriction", true, 0); this->enableFrictionP->Callback( &Model::SetFrictionMode, this ); + this->collideP = new ParamT<std::string>("collide", "all", 0); + this->collideP->Callback( &Model::SetCollideMode, this ); + Param::End(); this->parentBodyNameP = NULL; @@ -127,6 +130,7 @@ this->rpyP->Load(node); this->enableGravityP->Load(node); this->enableFrictionP->Load(node); + this->collideP->Load(node); this->xmlNode = node; this->type=node->GetName(); @@ -188,6 +192,8 @@ this->SetFrictionMode( **this->enableFrictionP ); + this->SetCollideMode( **this->collideP ); + return 0; // Get the name of the python module @@ -242,6 +248,7 @@ stream << prefix << " " << *(this->rpyP) << "\n"; stream << prefix << " " << *(this->enableGravityP) << "\n"; stream << prefix << " " << *(this->enableFrictionP) << "\n"; + stream << prefix << " " << *(this->collideP) << "\n"; if (this->type == "physical") { @@ -822,6 +829,22 @@ } //////////////////////////////////////////////////////////////////////////////// +/// Set the collide mode of the model +void Model::SetCollideMode( const std::string &m ) +{ + /*Body *body; + + std::map<std::string, Body* >::iterator iter; + + for (iter=this->bodies.begin(); iter!=this->bodies.end(); iter++) + { + body = iter->second; + + body->SetFrictionMode( v ); + } + */ +} +//////////////////////////////////////////////////////////////////////////////// // Load a renderable model (like a light source). void Model::LoadRenderable(XMLConfigNode *node) { Modified: code/branches/federation/gazebo/server/Model.hh =================================================================== --- code/branches/federation/gazebo/server/Model.hh 2009-03-08 04:55:48 UTC (rev 7385) +++ code/branches/federation/gazebo/server/Model.hh 2009-03-08 05:05:57 UTC (rev 7386) @@ -161,6 +161,9 @@ /// \brief Set the friction mode of the model public: void SetFrictionMode( const bool &v ); + /// \brief Set the collide mode of the model + public: void SetCollideMode( const std::string &m ); + /// \brief Load a body helper function /// \param node XML Configuration node private: void LoadBody(XMLConfigNode *node); @@ -213,6 +216,7 @@ private: ParamT<std::string> *myBodyNameP; private: ParamT<bool> *enableGravityP; private: ParamT<bool> *enableFrictionP; + private: ParamT<std::string> *collideP; // Name of a light (if the model is renderable:light) private: std::string lightName; Modified: code/branches/federation/gazebo/server/Simulator.cc =================================================================== --- code/branches/federation/gazebo/server/Simulator.cc 2009-03-08 04:55:48 UTC (rev 7385) +++ code/branches/federation/gazebo/server/Simulator.cc 2009-03-08 05:05:57 UTC (rev 7386) @@ -290,11 +290,21 @@ this->prevPhysicsTime = this->GetRealTime(); this->prevRenderTime = this->GetRealTime(); - + + this->runLength = 1.0; + + this->lastUpdateTime = this->GetRealTime(); + while (!this->userQuit) { currTime = this->GetRealTime(); + if (currTime - this->lastUpdateTime > this->runLength) + { + World::Instance()->ProcessMessages(); + continue; + } + if (physicsUpdateRate == 0 || currTime - this->prevPhysicsTime >= physicsUpdatePeriod) { @@ -316,6 +326,7 @@ this->prevPhysicsTime = this->GetRealTime(); + std::cout << "Updating at[" << this->GetRealTime() << "]\n"; World::Instance()->Update(); } @@ -343,6 +354,7 @@ if (this->timeout > 0 && this->GetRealTime() > this->timeout) break; + } } @@ -536,3 +548,9 @@ 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-08 04:55:48 UTC (rev 7385) +++ code/branches/federation/gazebo/server/Simulator.hh 2009-03-08 05:05:57 UTC (rev 7386) @@ -159,6 +159,8 @@ /// \brief Get the model that currently selected public: Model *GetSelectedModel() const; + public: void Go(); + ///pointer to the XML Data private: XMLConfig *xmlFile; @@ -213,9 +215,16 @@ /// Length of time the simulation should run private: double timeout; + /// Length of time the simulator is allowed to run + private: double updateTime; + /// 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-08 04:55:48 UTC (rev 7385) +++ code/branches/federation/gazebo/server/World.cc 2009-03-08 05:05:57 UTC (rev 7386) @@ -214,28 +214,15 @@ this->physicsEngine->Update(); } + return 0; +} + +void World::ProcessMessages() +{ this->UpdateSimulationIface(); +} - // Copy the newly created models into the main model vector - std::copy(this->toAddModels.begin(), this->toAddModels.end(), - std::back_inserter(this->models)); - this->toAddModels.clear(); - - // Remove and delete all models that are marked for deletion - for (miter=this->toDeleteModels.begin(); - miter!=this->toDeleteModels.end(); miter++) - { - this->models.erase( - std::remove(this->models.begin(), this->models.end(), *miter) ); - delete *miter; - } - - this->toDeleteModels.clear(); - - return 0; -} - //////////////////////////////////////////////////////////////////////////////// // Finilize the world int World::Fini() @@ -630,6 +617,8 @@ // Set the model's linear and angular acceleration model->SetLinearAccel(linearAccel); model->SetAngularAccel(angularAccel); + + Simulator::Instance()->Go(); } else { @@ -724,5 +713,26 @@ } this->simIface->Unlock(); + + + std::vector< Model* >::iterator miter; + + // Copy the newly created models into the main model vector + std::copy(this->toAddModels.begin(), this->toAddModels.end(), + std::back_inserter(this->models)); + this->toAddModels.clear(); + + + // Remove and delete all models that are marked for deletion + for (miter=this->toDeleteModels.begin(); + miter!=this->toDeleteModels.end(); miter++) + { + this->models.erase( + std::remove(this->models.begin(), this->models.end(), *miter) ); + delete *miter; + } + + this->toDeleteModels.clear(); + } Modified: code/branches/federation/gazebo/server/World.hh =================================================================== --- code/branches/federation/gazebo/server/World.hh 2009-03-08 04:55:48 UTC (rev 7385) +++ code/branches/federation/gazebo/server/World.hh 2009-03-08 05:05:57 UTC (rev 7386) @@ -140,6 +140,8 @@ /// \brief Get whether to view as wireframe public: bool GetShowPhysics(); + public: void ProcessMessages(); + /// Set to true to show bounding boxes private: bool showBoundingBoxes; Modified: code/branches/federation/gazebo/server/sensors/ir/IRSensor.cc =================================================================== --- code/branches/federation/gazebo/server/sensors/ir/IRSensor.cc 2009-03-08 04:55:48 UTC (rev 7385) +++ code/branches/federation/gazebo/server/sensors/ir/IRSensor.cc 2009-03-08 05:05:57 UTC (rev 7386) @@ -126,8 +126,8 @@ this->raySpaceId = dSimpleSpaceCreate( this->superSpaceId ); // Set collision bits - dGeomSetCategoryBits((dGeomID) this->raySpaceId, GZ_LASER_COLLIDE); - dGeomSetCollideBits((dGeomID) this->raySpaceId, ~GZ_LASER_COLLIDE); + dGeomSetCategoryBits((dGeomID) this->raySpaceId, GZ_SENSOR_COLLIDE); + dGeomSetCollideBits((dGeomID) this->raySpaceId, ~GZ_SENSOR_COLLIDE); //this->body->spaceId = this->superSpaceId; this->body->spaceId = this->raySpaceId; Modified: code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc =================================================================== --- code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc 2009-03-08 04:55:48 UTC (rev 7385) +++ code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc 2009-03-08 05:05:57 UTC (rev 7386) @@ -108,8 +108,8 @@ this->raySpaceId = dSimpleSpaceCreate( this->superSpaceId ); // Set collision bits - dGeomSetCategoryBits((dGeomID) this->raySpaceId, GZ_LASER_COLLIDE); - dGeomSetCollideBits((dGeomID) this->raySpaceId, ~GZ_LASER_COLLIDE); + dGeomSetCategoryBits((dGeomID) this->raySpaceId, GZ_SENSOR_COLLIDE); + dGeomSetCollideBits((dGeomID) this->raySpaceId, ~GZ_SENSOR_COLLIDE); //this->body->spaceId = this->superSpaceId; this->body->spaceId = this->raySpaceId; @@ -158,8 +158,6 @@ ray = new RayGeom(this->body, displayRaysP->GetValue()); ray->SetPoints(start, end); -// ray->SetCategoryBits( GZ_LASER_COLLIDE ); - //ray->SetCollideBits( ~GZ_LASER_COLLIDE ); this->rays.push_back(ray); Modified: code/branches/federation/gazebo/worlds/pioneer2dx.world =================================================================== --- code/branches/federation/gazebo/worlds/pioneer2dx.world 2009-03-08 04:55:48 UTC (rev 7385) +++ code/branches/federation/gazebo/worlds/pioneer2dx.world 2009-03-08 05:05:57 UTC (rev 7386) @@ -98,6 +98,7 @@ <rpy>0.0 0.0 90.0</rpy> <enableGravity>false</enableGravity> <enableFriction>false</enableFriction> + <collide>sensors</collide> <controller:differential_position2d name="controller1"> <leftJoint>left_wheel_hinge</leftJoint> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-08 05:32:08
|
Revision: 7390 http://playerstage.svn.sourceforge.net/playerstage/?rev=7390&view=rev Author: rtv Date: 2009-03-08 05:31:58 +0000 (Sun, 08 Mar 2009) Log Message: ----------- increased hardcoded rate Modified Paths: -------------- code/branches/federation/gazebo/Media/materials/textures/cave.png code/branches/federation/gazebo/server/Simulator.cc code/branches/federation/gazebo/worlds/federation.world Modified: code/branches/federation/gazebo/Media/materials/textures/cave.png =================================================================== (Binary files differ) Modified: code/branches/federation/gazebo/server/Simulator.cc =================================================================== --- code/branches/federation/gazebo/server/Simulator.cc 2009-03-08 05:31:21 UTC (rev 7389) +++ code/branches/federation/gazebo/server/Simulator.cc 2009-03-08 05:31:58 UTC (rev 7390) @@ -291,7 +291,8 @@ this->prevPhysicsTime = this->GetRealTime(); this->prevRenderTime = this->GetRealTime(); - this->runLength = 1.0; + this->runLength = 0.1; + //this->runLength = 1.0; this->lastUpdateTime = this->GetRealTime(); Modified: code/branches/federation/gazebo/worlds/federation.world =================================================================== --- code/branches/federation/gazebo/worlds/federation.world 2009-03-08 05:31:21 UTC (rev 7389) +++ code/branches/federation/gazebo/worlds/federation.world 2009-03-08 05:31:58 UTC (rev 7390) @@ -94,7 +94,7 @@ --> <model:physical name="pioneer2dx_model1"> - <xyz>0 0 .145</xyz> + <xyz>2 2 .145</xyz> <rpy>0.0 0.0 90.0</rpy> <enableGravity>false</enableGravity> <enableFriction>false</enableFriction> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-03-08 08:46:31
|
Revision: 7391 http://playerstage.svn.sourceforge.net/playerstage/?rev=7391&view=rev Author: natepak Date: 2009-03-08 08:46:29 +0000 (Sun, 08 Mar 2009) Log Message: ----------- Added ghost objects Modified Paths: -------------- code/branches/federation/gazebo/server/Global.hh code/branches/federation/gazebo/server/Model.cc code/branches/federation/gazebo/server/Simulator.cc 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/worlds/pioneer2dx.world Modified: code/branches/federation/gazebo/server/Global.hh =================================================================== --- code/branches/federation/gazebo/server/Global.hh 2009-03-08 05:31:58 UTC (rev 7390) +++ code/branches/federation/gazebo/server/Global.hh 2009-03-08 08:46:29 UTC (rev 7391) @@ -49,10 +49,11 @@ #ifndef GZ_COLLIDE_BITS -#define GZ_ALL_COLLIDE 0xFFFFFFFF +#define GZ_ALL_COLLIDE 0x0FFFFFFF #define GZ_NONE_COLLIDE 0x00000000 #define GZ_FIXED_COLLIDE 0x00000001 #define GZ_SENSOR_COLLIDE 0x00000002 +#define GZ_GHOST_COLLIDE 0x10000000 #endif Modified: code/branches/federation/gazebo/server/Model.cc =================================================================== --- code/branches/federation/gazebo/server/Model.cc 2009-03-08 05:31:58 UTC (rev 7390) +++ code/branches/federation/gazebo/server/Model.cc 2009-03-08 08:46:29 UTC (rev 7391) @@ -832,7 +832,7 @@ /// Set the collide mode of the model void Model::SetCollideMode( const std::string &m ) { - /*Body *body; + Body *body; std::map<std::string, Body* >::iterator iter; @@ -840,10 +840,10 @@ { body = iter->second; - body->SetFrictionMode( v ); + body->SetCollideMode( m ); } - */ } + //////////////////////////////////////////////////////////////////////////////// // Load a renderable model (like a light source). void Model::LoadRenderable(XMLConfigNode *node) Modified: code/branches/federation/gazebo/server/Simulator.cc =================================================================== --- code/branches/federation/gazebo/server/Simulator.cc 2009-03-08 05:31:58 UTC (rev 7390) +++ code/branches/federation/gazebo/server/Simulator.cc 2009-03-08 08:46:29 UTC (rev 7391) @@ -303,6 +303,13 @@ if (currTime - this->lastUpdateTime > this->runLength) { World::Instance()->ProcessMessages(); + + // Update the gui + if (this->gui) + { + this->gui->Update(); + } + continue; } @@ -327,7 +334,6 @@ this->prevPhysicsTime = this->GetRealTime(); - std::cout << "Updating at[" << this->GetRealTime() << "]\n"; World::Instance()->Update(); } Modified: code/branches/federation/gazebo/server/physics/Body.cc =================================================================== --- code/branches/federation/gazebo/server/physics/Body.cc 2009-03-08 05:31:58 UTC (rev 7390) +++ code/branches/federation/gazebo/server/physics/Body.cc 2009-03-08 08:46:29 UTC (rev 7391) @@ -74,6 +74,7 @@ this->dampingFactorP = new ParamT<double>("dampingFactor", 0.03, 0); Param::End(); + } @@ -214,6 +215,35 @@ } //////////////////////////////////////////////////////////////////////////////// +/// Set the collide mode of the body +void Body::SetCollideMode( const std::string &m ) +{ + std::map< std::string, Geom* >::iterator giter; + + unsigned int collideBits; + + if (m == "all") + collideBits = GZ_ALL_COLLIDE; + else if (m == "none") + collideBits = GZ_NONE_COLLIDE; + else if (m == "sensors") + collideBits = GZ_SENSOR_COLLIDE; + else if (m == "ghost") + collideBits = GZ_GHOST_COLLIDE; + else + { + gzerr(0) << "Unknown collide mode[" << m << "]\n"; + return; + } + + for (giter = this->geoms.begin(); giter != this->geoms.end(); giter++) + { + giter->second->SetCategoryBits(collideBits); + giter->second->SetCollideBits(collideBits); + } +} + +//////////////////////////////////////////////////////////////////////////////// // Initialize the body void Body::Init() { Modified: code/branches/federation/gazebo/server/physics/Body.hh =================================================================== --- code/branches/federation/gazebo/server/physics/Body.hh 2009-03-08 05:31:58 UTC (rev 7390) +++ code/branches/federation/gazebo/server/physics/Body.hh 2009-03-08 08:46:29 UTC (rev 7391) @@ -120,6 +120,9 @@ /// \brief Set the friction mode of the body public: void SetFrictionMode( const bool &v ); + /// \brief Set the collide mode of the body + public: void SetCollideMode( const std::string &m ); + /// \brief Set the linear velocity of the body public: void SetLinearVel(const Vector3 &vel); Modified: code/branches/federation/gazebo/server/physics/Geom.cc =================================================================== --- code/branches/federation/gazebo/server/physics/Geom.cc 2009-03-08 05:31:58 UTC (rev 7390) +++ code/branches/federation/gazebo/server/physics/Geom.cc 2009-03-08 08:46:29 UTC (rev 7391) @@ -108,6 +108,7 @@ { XMLConfigNode *childNode = NULL; + this->xmlNode=node; this->typeName = node->GetName(); @@ -170,6 +171,7 @@ World::Instance()->RegisterGeom(this); this->ShowPhysics(false); } + } @@ -239,7 +241,13 @@ this->SetCategoryBits(GZ_FIXED_COLLIDE); this->SetCollideBits(~GZ_FIXED_COLLIDE); } + else + { + this->SetCategoryBits(GZ_ALL_COLLIDE); + //this->SetCollideBits(~GZ_FIXED_COLLIDE); + } + // Create a new name of the geom's mesh entity //std::ostringstream stream; //stream << "Entity[" << (int)this->geomId << "]"; Modified: code/branches/federation/gazebo/worlds/pioneer2dx.world =================================================================== --- code/branches/federation/gazebo/worlds/pioneer2dx.world 2009-03-08 05:31:58 UTC (rev 7390) +++ code/branches/federation/gazebo/worlds/pioneer2dx.world 2009-03-08 08:46:29 UTC (rev 7391) @@ -98,7 +98,7 @@ <rpy>0.0 0.0 90.0</rpy> <enableGravity>false</enableGravity> <enableFriction>false</enableFriction> - <collide>sensors</collide> + <collide>ghost</collide> <controller:differential_position2d name="controller1"> <leftJoint>left_wheel_hinge</leftJoint> @@ -112,6 +112,7 @@ <model:physical name="laser"> <xyz>0.15 0 0.18</xyz> <enableGravity>false</enableGravity> + <collide>ghost</collide> <attach> <parentBody>chassis_body</parentBody> @@ -128,6 +129,26 @@ </include> </model:physical> + <model:physical name="box1_model"> + <xyz>0 0.5 0.5</xyz> + <canonicalBody>box1_body</canonicalBody> + <static>false</static> + + <body:box name="box1_body"> + + <geom:box name="box1_geom"> + <size>1 .1 1</size> + <mass>0.1</mass> + <visual> + <size>1 .1 1</size> + <mesh>unit_box</mesh> + <material>Gazebo/Rocky</material> + </visual> + </geom:box> + </body:box> + </model:physical> + + <!-- White Point light --> <model:renderable name="point_white"> <xyz>0 2 2</xyz> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-03-09 02:15:44
|
Revision: 7393 http://playerstage.svn.sourceforge.net/playerstage/?rev=7393&view=rev Author: natepak Date: 2009-03-09 02:15:32 +0000 (Mon, 09 Mar 2009) Log Message: ----------- Geoms check for valid spaceid when setting collide bits Modified Paths: -------------- code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc code/branches/federation/gazebo/libgazebo/SConscript code/branches/federation/gazebo/libgazebo/SimIface.cc code/branches/federation/gazebo/libgazebo/gazebo.h code/branches/federation/gazebo/player/SimulationInterface.cc code/branches/federation/gazebo/server/World.cc code/branches/federation/gazebo/server/physics/Geom.cc code/branches/federation/gazebo/worlds/federation.world 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 00:06:43 UTC (rev 7392) +++ code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc 2009-03-09 02:15:32 UTC (rev 7393) @@ -35,7 +35,7 @@ //simIface->data->reset = 1; // Example of how to move a model (box1_model) - char name[512] = "pioneer2dx_model1"; + /*char name[512] = "pioneer2dx_model1"; for (int i=0; i< 10; i++) { @@ -52,9 +52,13 @@ simIface->SetState(name, pose, linearVel, angularVel, linearAccel, angularAccel ); usleep(90000000); - } + }*/ + printf("Go\n"); + simIface->Go(100); + printf("Go Done\n"); + // Example of resetting the simulator // simIface->Reset(); //usleep(1000000); Modified: code/branches/federation/gazebo/libgazebo/SConscript =================================================================== --- code/branches/federation/gazebo/libgazebo/SConscript 2009-03-09 00:06:43 UTC (rev 7392) +++ code/branches/federation/gazebo/libgazebo/SConscript 2009-03-09 02:15:32 UTC (rev 7393) @@ -25,6 +25,7 @@ myEnv = Environment ( CC = 'g++', CCFLAGS = Split ('-pthread -pipe -W -Wall -O2'), + LIBS=Split('boost_signals boost_thread'), CXXCOMSTR = 'Compiling $TARGET', CCCOMSTR = 'Compiling $TARGET', Modified: code/branches/federation/gazebo/libgazebo/SimIface.cc =================================================================== --- code/branches/federation/gazebo/libgazebo/SimIface.cc 2009-03-09 00:06:43 UTC (rev 7392) +++ code/branches/federation/gazebo/libgazebo/SimIface.cc 2009-03-09 02:15:32 UTC (rev 7393) @@ -1,3 +1,5 @@ +#include <boost/thread/thread.hpp> +#include <boost/bind.hpp> #include <string.h> #include "gazebo.h" @@ -3,5 +5,30 @@ using namespace gazebo; +void SimulationIface::BlockThread(unsigned int ms) +{ + printf("Run thread\n"); + boost::this_thread::sleep(boost::posix_time::milliseconds(ms)); + printf("Run thread done\n"); +} + //////////////////////////////////////////////////////////////////////////////// +/// Tell gazebo to execute for a specified amount of time +void SimulationIface::Go(unsigned int ms) +{ + this->Lock(1); + SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); + request->type = SimulationRequestData::GO; + request->runTime = ms; + this->Unlock(); + + printf("Create Thread\n"); + /*boost::thread *thread = new boost::thread( + boost::bind(&SimulationIface::BlockThread, this, boost::_1)(100) ); + */ + + printf("Thread created\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 00:06:43 UTC (rev 7392) +++ code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-09 02:15:32 UTC (rev 7393) @@ -395,7 +395,8 @@ GET_POSE2D, SET_POSE3D, SET_POSE2D, - SET_STATE + SET_STATE, + GO }; public: Type type; @@ -405,6 +406,7 @@ public: Vec3 modelAngularVel; public: Vec3 modelLinearAccel; public: Vec3 modelAngularAccel; + public: double runTime; }; /// \brief Simulation interface data @@ -461,6 +463,10 @@ this->data = (SimulationData*)((char*)this->mMap+sizeof(SimulationIface)); } + /// \brief Tell gazebo to execute for a specified amount of time + /// \param ms Number of milliseconds to run + public: void Go(unsigned int ms); + /// \brief Pause the simulation public: void Pause(); @@ -487,6 +493,8 @@ const Vec3 &linearVel, const Vec3 &angularVel, const Vec3 &linearAccel, const Vec3 &angularAccel ); + private: void BlockThread(unsigned int ms); + /// Pointer to the simulation data public: SimulationData *data; }; Modified: code/branches/federation/gazebo/player/SimulationInterface.cc =================================================================== --- code/branches/federation/gazebo/player/SimulationInterface.cc 2009-03-09 00:06:43 UTC (rev 7392) +++ code/branches/federation/gazebo/player/SimulationInterface.cc 2009-03-09 02:15:32 UTC (rev 7393) @@ -246,6 +246,7 @@ switch (response->type) { case gazebo::SimulationRequestData::SET_STATE: + case gazebo::SimulationRequestData::GO: case gazebo::SimulationRequestData::PAUSE: case gazebo::SimulationRequestData::RESET: case gazebo::SimulationRequestData::SAVE: Modified: code/branches/federation/gazebo/server/World.cc =================================================================== --- code/branches/federation/gazebo/server/World.cc 2009-03-09 00:06:43 UTC (rev 7392) +++ code/branches/federation/gazebo/server/World.cc 2009-03-09 02:15:32 UTC (rev 7393) @@ -682,8 +682,14 @@ break; } - case SimulationRequestData::SET_POSE2D: + case SimulationRequestData::GO: { + printf("World got a go\n"); + break; + } + + case SimulationRequestData::SET_POSE2D: + { Model *model = this->GetModelByName((char*)req->modelName); if (model) { Modified: code/branches/federation/gazebo/server/physics/Geom.cc =================================================================== --- code/branches/federation/gazebo/server/physics/Geom.cc 2009-03-09 00:06:43 UTC (rev 7392) +++ code/branches/federation/gazebo/server/physics/Geom.cc 2009-03-09 02:15:32 UTC (rev 7393) @@ -385,16 +385,20 @@ /// Set the category bits, used during collision detection void Geom::SetCategoryBits(unsigned int bits) { - dGeomSetCategoryBits(this->geomId, bits); - dGeomSetCategoryBits((dGeomID)this->spaceId, bits); + if (this->geomId) + dGeomSetCategoryBits(this->geomId, bits); + if (this->spaceId) + dGeomSetCategoryBits((dGeomID)this->spaceId, bits); } //////////////////////////////////////////////////////////////////////////////// /// Set the collide bits, used during collision detection void Geom::SetCollideBits(unsigned int bits) { - dGeomSetCollideBits(this->geomId, bits); - dGeomSetCollideBits((dGeomID)this->spaceId, bits); + if (this->geomId) + dGeomSetCollideBits(this->geomId, bits); + if (this->spaceId) + dGeomSetCollideBits((dGeomID)this->spaceId, bits); } //////////////////////////////////////////////////////////////////////////////// Modified: code/branches/federation/gazebo/worlds/federation.world =================================================================== --- code/branches/federation/gazebo/worlds/federation.world 2009-03-09 00:06:43 UTC (rev 7392) +++ code/branches/federation/gazebo/worlds/federation.world 2009-03-09 02:15:32 UTC (rev 7393) @@ -65,7 +65,7 @@ </body:plane> </model:physical> - <model:physical name="sphere1_model"> + <!--<model:physical name="sphere1_model"> <xyz>2.15 -1.68 .3</xyz> <rpy>0.0 0.0 0.0</rpy> <static>true</static> @@ -82,17 +82,9 @@ </geom:sphere> </body:sphere> </model:physical> - - - <!-- - Include the complete model described in the .model file - This assumes the root node is a <model:...> --> - <!-- <include embedded="false"> - <xi:include href="pioneer2dx.model" /> - </include> - --> + <!-- <model:physical name="pioneer2dx_model1"> <xyz>2 2 .145</xyz> <rpy>0.0 0.0 90.0</rpy> @@ -126,6 +118,7 @@ <xi:include href="models/pioneer2dx.model" /> </include> </model:physical> + --> <!-- White Point light --> <model:renderable name="point_white"> Modified: code/branches/federation/gazebo/worlds/pioneer2dx.world =================================================================== --- code/branches/federation/gazebo/worlds/pioneer2dx.world 2009-03-09 00:06:43 UTC (rev 7392) +++ code/branches/federation/gazebo/worlds/pioneer2dx.world 2009-03-09 02:15:32 UTC (rev 7393) @@ -98,7 +98,7 @@ <rpy>0.0 0.0 90.0</rpy> <enableGravity>false</enableGravity> <enableFriction>false</enableFriction> - <collide>ghost</collide> + <!--<collide>ghost</collide>--> <controller:differential_position2d name="controller1"> <leftJoint>left_wheel_hinge</leftJoint> @@ -112,7 +112,7 @@ <model:physical name="laser"> <xyz>0.15 0 0.18</xyz> <enableGravity>false</enableGravity> - <collide>ghost</collide> + <!-- <collide>ghost</collide>--> <attach> <parentBody>chassis_body</parentBody> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-03-09 03:56:27
|
Revision: 7395 http://playerstage.svn.sourceforge.net/playerstage/?rev=7395&view=rev Author: natepak Date: 2009-03-09 02:59:16 +0000 (Mon, 09 Mar 2009) Log Message: ----------- Added thread stuff to simiface 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 Modified: code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc =================================================================== --- code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc 2009-03-09 02:20:29 UTC (rev 7394) +++ code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc 2009-03-09 02:59:16 UTC (rev 7395) @@ -54,11 +54,10 @@ usleep(90000000); }*/ - printf("Go\n"); - simIface->Go(100); - printf("Go Done\n"); + simIface->Go(10e6); + usleep(100000000); // 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 02:20:29 UTC (rev 7394) +++ code/branches/federation/gazebo/libgazebo/SimIface.cc 2009-03-09 02:59:16 UTC (rev 7395) @@ -1,33 +1,31 @@ #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(unsigned int ms) +void SimulationIface::BlockThread() { - printf("Run thread\n"); - boost::this_thread::sleep(boost::posix_time::milliseconds(ms)); - printf("Run thread done\n"); + usleep(this->blockTimeUs); + //signal(); } //////////////////////////////////////////////////////////////////////////////// /// Tell gazebo to execute for a specified amount of time -void SimulationIface::Go(unsigned int ms) +void SimulationIface::Go(unsigned int us) { this->Lock(1); SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); request->type = SimulationRequestData::GO; - request->runTime = ms; + request->runTime = us; this->Unlock(); - printf("Create Thread\n"); -/* boost::thread *thread = new boost::thread( - boost::bind(&SimulationIface::BlockThread, this, boost::_1)(100) ); - */ + this->blockTimeUs = us; - printf("Thread created\n"); + boost::thread *thread = new boost::thread( + boost::bind(&SimulationIface::BlockThread, this)); } //////////////////////////////////////////////////////////////////////////////// Modified: code/branches/federation/gazebo/libgazebo/gazebo.h =================================================================== --- code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-09 02:20:29 UTC (rev 7394) +++ code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-09 02:59:16 UTC (rev 7395) @@ -493,7 +493,8 @@ const Vec3 &linearVel, const Vec3 &angularVel, const Vec3 &linearAccel, const Vec3 &angularAccel ); - private: void BlockThread(unsigned int ms); + private: void BlockThread(); + private: unsigned int blockTimeUs; /// Pointer to the simulation data public: SimulationData *data; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-03-09 07:14:17
|
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. |
From: <na...@us...> - 2009-03-09 07:21:55
|
Revision: 7398 http://playerstage.svn.sourceforge.net/playerstage/?rev=7398&view=rev Author: natepak Date: 2009-03-09 07:21:49 +0000 (Mon, 09 Mar 2009) Log Message: ----------- Remove boost semaphores Modified Paths: -------------- 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/libgazebo/SimIface.cc =================================================================== --- code/branches/federation/gazebo/libgazebo/SimIface.cc 2009-03-09 07:13:58 UTC (rev 7397) +++ code/branches/federation/gazebo/libgazebo/SimIface.cc 2009-03-09 07:21:49 UTC (rev 7398) @@ -76,15 +76,15 @@ while (true) { - boost::mutex::scoped_lock lock(*(this->mutex)); + 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); + 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(); + //this->data->sem.wait(); printf("Thread is sending a signal to the callback\n"); // Signal the callback function this->goAckSignal(); Modified: code/branches/federation/gazebo/libgazebo/gazebo.h =================================================================== --- code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-09 07:13:58 UTC (rev 7397) +++ code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-09 07:21:49 UTC (rev 7398) @@ -33,7 +33,7 @@ #include <stdlib.h> #include <stdint.h> #include <boost/signal.hpp> -#include <boost/interprocess/sync/interprocess_semaphore.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 +440,7 @@ public: SimulationRequestData responses[GAZEBO_SIMULATION_MAX_REQUESTS]; public: unsigned int responseCount; - public: boost::interprocess::interprocess_semaphore sem; +// public: boost::interprocess::interprocess_semaphore sem; }; /// \brief Common simulation interface @@ -474,19 +474,23 @@ request->runTime = us; this->Unlock(); - // Set the time the thread should block for - this->blockTimeUs = us; + { + boost::mutex::scoped_lock lock(this->mutex); - // Connect the callback. This is signaled when the thread - // (below) finishes waiting - this->goAckSignal.connect( subscriber ); + // 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(); + this->goAckCond.notify_one(); } /// \brief Pause the simulation @@ -519,8 +523,8 @@ private: unsigned int blockTimeUs; private: boost::signal<void (void)> goAckSignal; 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 07:13:58 UTC (rev 7397) +++ code/branches/federation/gazebo/server/World.cc 2009-03-09 07:21:49 UTC (rev 7398) @@ -209,7 +209,7 @@ Simulator::Instance()->GetSimTime(), this->simPauseTime); //this->simIface->Lock(1); printf("Sem Post\n"); - this->simIface->data->sem.post(); + //this->simIface->data->sem.post(); printf("Sem Post done\n"); //this->simIface->Unlock(); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-03-09 08:37:00
|
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. |
From: <ge...@us...> - 2009-03-09 19:15:05
|
Revision: 7404 http://playerstage.svn.sourceforge.net/playerstage/?rev=7404&view=rev Author: gerkey Date: 2009-03-09 19:14:49 +0000 (Mon, 09 Mar 2009) Log Message: ----------- Added duplicate model-checking to the Load step. When loaded from a world file, duplicates cause an abort; from factory, new model overwrites old one. Working on performance test client for webgazebo. Modified Paths: -------------- code/branches/federation/gazebo/server/Model.cc code/branches/federation/gazebo/server/Model.hh code/branches/federation/gazebo/server/World.cc code/branches/federation/gazebo/server/World.hh code/branches/federation/gazebo/server/controllers/factory/Factory.cc code/branches/federation/gazebo/webgazebo/SConscript code/branches/federation/gazebo/webgazebo/WebGazebo.cc code/branches/federation/gazebo/webgazebo/WebGazebo.hh code/branches/federation/gazebo/webgazebo/main.cc code/branches/federation/gazebo/worlds/federation.world Added Paths: ----------- code/branches/federation/gazebo/webgazebo/client.cc Modified: code/branches/federation/gazebo/server/Model.cc =================================================================== --- code/branches/federation/gazebo/server/Model.cc 2009-03-09 08:36:48 UTC (rev 7403) +++ code/branches/federation/gazebo/server/Model.cc 2009-03-09 19:14:49 UTC (rev 7404) @@ -117,12 +117,29 @@ //////////////////////////////////////////////////////////////////////////////// // Load the model -int Model::Load(XMLConfigNode *node) +int Model::Load(XMLConfigNode *node, bool removeDuplicate) { XMLConfigNode *childNode; Pose3d pose; + Model* dup; this->nameP->Load(node); + // Look for existing models by the same name + if((dup = World::Instance()->GetModelByName(this->nameP->GetValue())) != NULL) + { + if(!removeDuplicate) + { + gzthrow("Duplicate model name" + this->nameP->GetValue() + "\n"); + } + else + { + // Delete the existing one (this should only be reached when called + // via the factory interface). + printf("Queuing duplicate model %s (%p) for deletion\n", this->nameP->GetValue().c_str(), dup); + World::Instance()->DeleteEntity(this->nameP->GetValue().c_str()); + } + } + this->staticP->Load(node); this->canonicalBodyNameP->Load(node); @@ -203,18 +220,18 @@ // Import the python module if (this->pName) { - this->pModule.reset(PyImport_Import(this->pName)); - Py_DECREF(this->pName); + this->pModule.reset(PyImport_Import(this->pName)); + Py_DECREF(this->pName); } // Get the Update function from the module if (this->pModule) { - this->pFuncUpdate.reset(PyObject_GetAttrString(this->pModule, "Update")); - if (this->pFuncUpdate && !PyCallable_Check(this->pFuncUpdate)) - this->pFuncUpdate = NULL; + this->pFuncUpdate.reset(PyObject_GetAttrString(this->pModule, "Update")); + if (this->pFuncUpdate && !PyCallable_Check(this->pFuncUpdate)) + this->pFuncUpdate = NULL; } - */ + */ } //////////////////////////////////////////////////////////////////////////////// Modified: code/branches/federation/gazebo/server/Model.hh =================================================================== --- code/branches/federation/gazebo/server/Model.hh 2009-03-09 08:36:48 UTC (rev 7403) +++ code/branches/federation/gazebo/server/Model.hh 2009-03-09 19:14:49 UTC (rev 7404) @@ -63,7 +63,8 @@ public: virtual ~Model(); /// \brief Load the model - public: int Load(XMLConfigNode *node); + /// \param removeDuplicate Remove existing model of same name + public: int Load(XMLConfigNode *node, bool removeDuplicate); /// \brief Save the model public: void Save(std::string &prefix, std::ostream &stream); Modified: code/branches/federation/gazebo/server/World.cc =================================================================== --- code/branches/federation/gazebo/server/World.cc 2009-03-09 08:36:48 UTC (rev 7403) +++ code/branches/federation/gazebo/server/World.cc 2009-03-09 19:14:49 UTC (rev 7404) @@ -136,7 +136,7 @@ this->physicsEngine = new ODEPhysics(); //TODO: use exceptions here - this->LoadEntities(rootNode, NULL); + this->LoadEntities(rootNode, NULL, false); /*std::vector<Model*>::iterator miter; for (miter = this->models.begin(); miter != this->models.end(); miter++) @@ -302,7 +302,7 @@ /////////////////////////////////////////////////////////////////////////////// // Load a model -int World::LoadEntities(XMLConfigNode *node, Model *parent) +int World::LoadEntities(XMLConfigNode *node, Model *parent, bool removeDuplicate) { XMLConfigNode *cnode; Model *model = NULL; @@ -312,14 +312,14 @@ // Check for model nodes if (node->GetNSPrefix() == "model") { - model = this->LoadModel(node, parent); + model = this->LoadModel(node, parent, removeDuplicate); } } // Load children for (cnode = node->GetChild(); cnode != NULL; cnode = cnode->GetNext()) { - if (this->LoadEntities( cnode, model ) != 0) + if (this->LoadEntities( cnode, model, removeDuplicate ) != 0) return -1; } @@ -343,14 +343,14 @@ //////////////////////////////////////////////////////////////////////////////// // Load a model -Model *World::LoadModel(XMLConfigNode *node, Model *parent) +Model *World::LoadModel(XMLConfigNode *node, Model *parent, bool removeDuplicate) { Pose3d pose; Model *model = new Model(parent); //model->SetParent(parent); // Load the model - if (model->Load( node ) != 0) + if (model->Load( node, removeDuplicate ) != 0) return NULL; // Set the model's pose (relative to parent) Modified: code/branches/federation/gazebo/server/World.hh =================================================================== --- code/branches/federation/gazebo/server/World.hh 2009-03-09 08:36:48 UTC (rev 7403) +++ code/branches/federation/gazebo/server/World.hh 2009-03-09 19:14:49 UTC (rev 7404) @@ -95,8 +95,9 @@ /// \brief Load all entities /// \param node XMLConfg node pointer /// \param parent Parent of the model to load + /// \param removeDuplicate Remove existing model of same name /// \return 0 on success - public: int LoadEntities(XMLConfigNode *node, Model *parent); + public: int LoadEntities(XMLConfigNode *node, Model *parent, bool removeDuplicate); /// \brief Delete an entity by name /// \param name The name of the entity to delete @@ -156,8 +157,9 @@ /// \brief Load a model /// \param node Pointer to the XMLConfig node /// \param parent The parent model + /// \param removeDuplicate Remove existing model of same name /// \return The model that was created - private: Model *LoadModel(XMLConfigNode *node, Model *parent); + private: Model *LoadModel(XMLConfigNode *node, Model *parent, bool removeDuplicate); /// \brief Set the model pose and the pose of it's attached children /// \param model The model to set Modified: code/branches/federation/gazebo/server/controllers/factory/Factory.cc =================================================================== --- code/branches/federation/gazebo/server/controllers/factory/Factory.cc 2009-03-09 08:36:48 UTC (rev 7403) +++ code/branches/federation/gazebo/server/controllers/factory/Factory.cc 2009-03-09 19:14:49 UTC (rev 7404) @@ -112,7 +112,7 @@ } // Add the new models into the World - World::Instance()->LoadEntities( xmlConfig->GetRootNode(), NULL ); + World::Instance()->LoadEntities( xmlConfig->GetRootNode(), NULL, true ); // Cleanup delete xmlConfig; Modified: code/branches/federation/gazebo/webgazebo/SConscript =================================================================== --- code/branches/federation/gazebo/webgazebo/SConscript 2009-03-09 08:36:48 UTC (rev 7403) +++ code/branches/federation/gazebo/webgazebo/SConscript 2009-03-09 19:14:49 UTC (rev 7404) @@ -49,6 +49,7 @@ ) webgazebo = exeEnv.Program('webgazebo', 'main.cc') +client = exeEnv.Program('client', 'client.cc') env.Install(install_prefix+'/lib', sharedLib) #env.Install(install_prefix+'/lib', staticLib) Modified: code/branches/federation/gazebo/webgazebo/WebGazebo.cc =================================================================== --- code/branches/federation/gazebo/webgazebo/WebGazebo.cc 2009-03-09 08:36:48 UTC (rev 7403) +++ code/branches/federation/gazebo/webgazebo/WebGazebo.cc 2009-03-09 19:14:49 UTC (rev 7404) @@ -1,14 +1,50 @@ +/* + * Gazebo - Outdoor Multi-Robot Simulator + * Copyright (C) 2003 + * Nate Koenig & Andrew Howard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +/* Desc: HTTP portal to libgazebo + * Author: Brian Gerkey + * Date: 9 March 2009 + * SVN: $Id: gazebo.h 7398 2009-03-09 07:21:49Z natepak $ + */ + #include "WebGazebo.hh" +#include <fstream> + #include <stdio.h> #include <assert.h> #include <stdlib.h> #include <time.h> +#include <string.h> #include <boost/lexical_cast.hpp> #include "../server/Quatern.hh" +// TODO: +// - make ghost models not collide, fall, etc. +// - add SetState +// - expose URI command to run for 1 tick +// - do performance test + WebGazebo::WebGazebo(std::string _host, int _port, double dtol, double atol) : host(_host), port(_port), sq_dist_tol(dtol*dtol), sq_ang_tol(atol*atol) { @@ -29,9 +65,11 @@ fflush(stdout); this->client = new gazebo::Client(); this->simIface = new gazebo::SimulationIface(); + this->factoryIface = new gazebo::FactoryIface(); this->client->ConnectWait(0, GZ_CLIENT_ID_USER_FIRST); // Open the Simulation Interface; let exceptions leak out this->simIface->Open(this->client, "default"); + this->factoryIface->Open(this->client, "factory_iface"); // Get the list of models if(!GetModelList()) throw("Failed to get list of models from Gazebo"); @@ -58,23 +96,37 @@ bool WebGazebo::HasModel(const std::string& name) { - std::map<std::string,int>::const_iterator it = models.find(name); - return it != models.end(); + // TODO + //std::map<std::string,int>::const_iterator it = models.find(name); + //return it != models.end(); + return true; } bool WebGazebo::GetPose(std::string model, gazebo::Pose& pose) { - // Discard any leftover responses; + // Discard any leftover responses this->simIface->data->responseCount = 0; // Ask Gazebo this->simIface->GetPose3d(model.c_str()); // Wait for the response + double timeout = 3.0; + struct timeval t0, t1; + gettimeofday(&t0, NULL); struct timespec sleeptime = {0, 10000000}; while(this->simIface->data->responseCount == 0) + { + gettimeofday(&t1, NULL); + if(((t1.tv_sec + t1.tv_usec/1e6) - (t0.tv_sec + t0.tv_usec/1e6)) + > timeout) + { + puts("Timeout"); + return false; + } nanosleep(&sleeptime, NULL); + } assert(this->simIface->data->responseCount == 1); pose = this->simIface->data->responses[0].modelPose; @@ -152,170 +204,255 @@ } bool -WebGazebo::HandleURI(const std::string& model, - const std::string& prop, - const std::string& action, - struct evkeyvalq* kv, - std::string& response) +WebGazebo::GetModel(const std::string& name, + const std::string& type, + std::string& xmldata, + std::string& response) { - // The special simulation model - if(model == "sim") + // Search GAZEBO_MODEL_PATH for a matching .model file + const char* gmpath = getenv("GAZEBO_MODEL_PATH"); + if(!gmpath) { - // The special factory property - if(prop == "factory") + response = "ERROR: GAZEBO_MODEL_PATH is not set"; + return false; + } + + std::vector<std::string> gmpath_parts; + StringSplit(gmpath, gmpath_parts, ":"); + for(unsigned int i=0; i<gmpath_parts.size(); i++) + { + std::string p = gmpath_parts[0] + "/" + type + ".model"; + if(access(p.c_str(), F_OK) == 0) { - if(action == "create") - { - std::string name, type; - if(!GetValue(name, kv, "name") || - !GetValue(type, kv, "type")) - { - response = "ERROR: Missing name and/or type argument for sim/factor/create"; - return false; - } + std::ifstream ifs(p.c_str()); + xmldata = std::string((std::istreambuf_iterator<char>(ifs)), + std::istreambuf_iterator<char>()); - printf("[webgazebo] Creating model %s of type %s\n", - name.c_str(), type.c_str()); - response = "[webgazebo] Creating model " + name + " of type " + type; - return true; - } - else + // HACK: rename with a VERY crude textual replacement + std::string tag1 = "<model:physical"; + std::string tag2 = "name=\""; + int i = xmldata.find(tag1); + if(i < 0) + return false; + i = xmldata.find(tag2, i); + if(i < 0) + return false; + int j = xmldata.find("\"", i+tag2.length()); + xmldata.replace(i,j-i+1,std::string("name=\"" + name + "\"")); + return true; + } + } + + response = "ERROR: Could not find file " + type + ".model"; + return false; +} + +bool +WebGazebo::CreateModel(const std::string& xmldata, + std::string& response) +{ + struct timespec sleeptime = {0, 10000000}; + for(;;) + { + this->factoryIface->Lock(1); + if(!strcmp((const char*)this->factoryIface->data->newModel,"")) + { + strcpy((char*)this->factoryIface->data->newModel, xmldata.c_str()); + factoryIface->Unlock(); + return true; + } + else + { + factoryIface->Unlock(); + nanosleep(&sleeptime, NULL); + } + } +} + +bool +WebGazebo::HandleSimRequest(const std::string& prop, + const std::string& action, + struct evkeyvalq* kv, + std::string& response) +{ + // The special factory property + if(prop == "factory") + { + if(action == "create") + { + std::string name, type; + if(!GetValue(name, kv, "name") || + !GetValue(type, kv, "type")) { - response = "ERROR: Unknown action " + action + " for sim/factory"; + response = "ERROR: Missing name and/or type argument for sim/factor/create"; return false; } + + std::string xmldata; + if(!GetModel(name, type, xmldata, response)) + return false; + if(!CreateModel(xmldata, response)) + return false; + response = std::string("Created model of type ") + type; + return true; } else { - response = "ERROR: Unknown property " + prop + " for sim"; + response = "ERROR: Unknown action " + action + " for sim/factory"; return false; } } - // Everything else must be an existing model else { - printf("[webgazebo] got request for %s:%s:%s\n", - model.c_str(), - prop.c_str(), - action.c_str()); - if(HasModel(model)) + response = "ERROR: Unknown property " + prop + " for sim"; + return false; + } +} + +bool +WebGazebo::HandleModelRequest(const std::string& model, + const std::string& prop, + const std::string& action, + struct evkeyvalq* kv, + std::string& response) +{ + printf("[webgazebo] got request for %s:%s:%s\n", + model.c_str(), + prop.c_str(), + action.c_str()); + if(HasModel(model)) + { + if(action == "get") { - if(action == "get") + if(prop == "pose") { - if(prop == "pose") + gazebo::Pose p; + if(GetPose(model, p)) { - gazebo::Pose p; - if(GetPose(model, p)) - { - char buf[1024]; - snprintf(buf, sizeof(buf), - "Got %s's pose: (%.3f,%.3f,%.3f) (%.3f,%.3f,%.3f)", - model.c_str(), - p.pos.x, p.pos.y, p.pos.z, - p.roll, p.pitch, p.yaw); - response = buf; - return true; - } - else - { - response = "ERROR: Failed to get pose for model " + model; - return false; - } + char buf[1024]; + snprintf(buf, sizeof(buf), + "Got %s's pose: (%.3f,%.3f,%.3f) (%.3f,%.3f,%.3f)", + model.c_str(), + p.pos.x, p.pos.y, p.pos.z, + p.roll, p.pitch, p.yaw); + response = buf; + return true; } else { - response = "ERROR: Unknown property " + prop + " for model " + model; + response = "ERROR: Failed to get pose for model " + model; return false; } } - else if(action == "set") + else { - if(prop == "pose") + response = "ERROR: Unknown property " + prop + " for model " + model; + return false; + } + } + else if(action == "set") + { + if(prop == "pose") + { + std::string sx, sy, sz, sroll, spitch, syaw; + + // Get pose first, fill in what the caller provided + gazebo::Pose p, q; + if(!GetPose(model, p)) { - std::string sx, sy, sz, sroll, spitch, syaw; + response = "Failed to get pose before setting it"; + return false; + } + try + { + q = p; + if(GetValue(sx, kv, "x")) + p.pos.x = boost::lexical_cast<float>(sx); + if(GetValue(sy, kv, "y")) + p.pos.y = boost::lexical_cast<float>(sy); + if(GetValue(sz, kv, "z")) + p.pos.z = boost::lexical_cast<float>(sz); + if(GetValue(sroll, kv, "roll")) + p.roll = boost::lexical_cast<float>(sroll); + if(GetValue(spitch, kv, "pitch")) + p.pitch = boost::lexical_cast<float>(spitch); + if(GetValue(syaw, kv, "yaw")) + p.yaw = boost::lexical_cast<float>(syaw); + } + catch(boost::bad_lexical_cast e) + { + response = std::string("Failed to parse input value(s): ") + + e.what(); + return false; + } - // Get pose first, fill in what the caller provided - gazebo::Pose p, q; - if(!GetPose(model, p)) - { - response = "Failed to get pose before setting it"; - return false; - } - try - { - q = p; - if(GetValue(sx, kv, "x")) - p.pos.x = boost::lexical_cast<float>(sx); - /* - if(GetValue(sy, kv, "y")) - p.pos.y = boost::lexical_cast<float>(sy); - */ - if(GetValue(sz, kv, "z")) - p.pos.z = boost::lexical_cast<float>(sz); - if(GetValue(sroll, kv, "roll")) - p.roll = boost::lexical_cast<float>(sroll); - if(GetValue(spitch, kv, "pitch")) - p.pitch = boost::lexical_cast<float>(spitch); - if(GetValue(syaw, kv, "yaw")) - p.yaw = boost::lexical_cast<float>(syaw); - } - catch(boost::bad_lexical_cast e) - { - response = std::string("Failed to parse input value(s): ") + - e.what(); - return false; - } + if(SetPose(model, p)) + { + char buf[1024]; + snprintf(buf, sizeof(buf), + "Set %s's pose: (%.3f,%.3f,%.3f) (%.3f,%.3f,%.3f)", + model.c_str(), + p.pos.x, p.pos.y, p.pos.z, + p.roll, p.pitch, p.yaw); + response = buf; - if(SetPose(model, p)) - { - char buf[1024]; - snprintf(buf, sizeof(buf), - "Set %s's pose: (%.3f,%.3f,%.3f) (%.3f,%.3f,%.3f)", - model.c_str(), - p.pos.x, p.pos.y, p.pos.z, - p.roll, p.pitch, p.yaw); - response = buf; + // Check tolerances, and complain if they're exceeded + if(!CheckTolerances(p,q)) + response += "\nWARNING: Pose change exceeded tolerance"; - // Check tolerances - if(!CheckTolerances(p,q)) - response += "\nWARNING: Pose change exceeded tolerance"; - - return true; - } - else - { - response = "ERROR: Failed to get pose for model " + model; - return false; - } + return true; } else { - response = "ERROR: Unknown property " + prop + " for model " + model; + response = "ERROR: Failed to get pose for model " + model; return false; } } else { - response = "ERROR: Unknown action " + action; + response = "ERROR: Unknown property " + prop + " for model " + model; return false; } } else { - response = "ERROR: No such model " + model; + response = "ERROR: Unknown action " + action; return false; } } + else + { + response = "ERROR: No such model " + model; + return false; + } } +bool +WebGazebo::HandleURI(const std::string& model, + const std::string& prop, + const std::string& action, + struct evkeyvalq* kv, + std::string& response) +{ + // The special simulation model + if(model == "sim") + { + return HandleSimRequest(prop, action, kv, response); + } + // Everything else must be an existing model + else + { + return HandleModelRequest(model, prop, action, kv, response); + } +} + +// Handle all incoming URI requests void WebGazebo::EventCallback(evhttp_request* req, void* arg) { WebGazebo* obj = (WebGazebo*)arg; printf("[webgazebo] Got request:%s:\n", req->uri); - printf("[webgazebo] Decoded:%s:\n", evhttp_decode_uri(req->uri)); - // Pull out query args struct evkeyvalq query_args; @@ -324,36 +461,29 @@ struct evbuffer* eb = evbuffer_new(); assert(eb); + int response_code; + std::string response_string; std::string model, prop, action, response; if(!obj->ParseURI(model, prop, action, req->uri, response)) { - evbuffer_add_printf(eb, "%s\n", response.c_str()); - evhttp_send_reply(req, 400, "Invalid request", eb); - return; + response_code = 400; + response_string = "Invalid request"; } - - if(!obj->HandleURI(model, prop, action, &query_args, response)) + else if(!obj->HandleURI(model, prop, action, &query_args, response)) { - evbuffer_add_printf(eb, "%s\n", response.c_str()); - evhttp_send_reply(req, 400, "Invalid request", eb); - return; + response_code = 400; + response_string = "Invalid request"; } + else + { + response_code = 200; + response_string = "OK"; + } - /* - std::string model = "pioneer2dx_model1"; - gazebo::Pose p; - assert(obj->GetPose(&p, model)); - evbuffer_add_printf(eb, "<html><head><title>WebGazebo</title></head><body>"); - evbuffer_add_printf(eb, "<h1>WebGazebo!</h1>"); - evbuffer_add_printf(eb, "%s's pose: (%.3f,%.3f,%.3f) (%.3f,%.3f,%.3f)", - model.c_str(), - p.pos.x, p.pos.y, p.pos.z, - p.roll, p.pitch, p.yaw); - evbuffer_add_printf(eb, "</body></html>"); - */ - evbuffer_add_printf(eb, "%s\n", response.c_str()); - evhttp_send_reply(req, 200, "OK", eb); + printf("[webgazebo] Sending reply: %d %s\n", + response_code, response_string.c_str()); + evhttp_send_reply(req, response_code, response_string.c_str(), eb); evbuffer_free(eb); } Modified: code/branches/federation/gazebo/webgazebo/WebGazebo.hh =================================================================== --- code/branches/federation/gazebo/webgazebo/WebGazebo.hh 2009-03-09 08:36:48 UTC (rev 7403) +++ code/branches/federation/gazebo/webgazebo/WebGazebo.hh 2009-03-09 19:14:49 UTC (rev 7404) @@ -1,3 +1,30 @@ +/* + * Gazebo - Outdoor Multi-Robot Simulator + * Copyright (C) 2003 + * Nate Koenig & Andrew Howard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +/* Desc: HTTP portal to libgazebo + * Author: Brian Gerkey + * Date: 9 March 2009 + * SVN: $Id: gazebo.h 7398 2009-03-09 07:21:49Z natepak $ + */ + #include <string> #include <vector> #include <map> @@ -32,6 +59,7 @@ gazebo::Client *client; gazebo::SimulationIface *simIface; + gazebo::FactoryIface *factoryIface; // Available models std::map<std::string,int> models; @@ -42,6 +70,8 @@ bool CheckTolerances(gazebo::Pose p, gazebo::Pose q); bool GetModelList(); bool HasModel(const std::string& name); + bool CreateModel(const std::string& xmldata, + std::string& response); bool GetPose(std::string model, gazebo::Pose& pose); bool SetPose(std::string model, const gazebo::Pose& pose); void StringSplit(const std::string &s, @@ -50,11 +80,24 @@ bool GetValue(std::string& value, struct evkeyvalq* query_args, const std::string& key); + bool GetModel(const std::string& name, + const std::string& type, + std::string& xmldata, + std::string& response); bool HandleURI(const std::string& model, const std::string& prop, const std::string& action, struct evkeyvalq* kv, std::string& response); + bool HandleSimRequest(const std::string& prop, + const std::string& action, + struct evkeyvalq* kv, + std::string& response); + bool HandleModelRequest(const std::string& model, + const std::string& prop, + const std::string& action, + struct evkeyvalq* kv, + std::string& response); bool ParseURI(std::string& model, std::string& prop, std::string& action, Added: code/branches/federation/gazebo/webgazebo/client.cc =================================================================== --- code/branches/federation/gazebo/webgazebo/client.cc (rev 0) +++ code/branches/federation/gazebo/webgazebo/client.cc 2009-03-09 19:14:49 UTC (rev 7404) @@ -0,0 +1,89 @@ +/* + * Gazebo - Outdoor Multi-Robot Simulator + * Copyright (C) 2003 + * Nate Koenig & Andrew Howard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +/* Desc: Performance test for HTTP portal to libgazebo + * Author: Brian Gerkey + * Date: 9 March 2009 + * SVN: $Id: gazebo.h 7398 2009-03-09 07:21:49Z natepak $ + */ + +#include <stdio.h> +#include <assert.h> +#include <stdlib.h> +#include <sys/time.h> + +// These headers must be included prior to the libevent headers +#include <sys/types.h> +#include <sys/queue.h> + +// libevent +#include <event.h> +#include <evhttp.h> + +struct timeval g_t0, g_t1; + +// Callback that's invoked when the request is done and we have the +// response +void +cb(evhttp_request* req, void* arg) +{ + gettimeofday(&g_t1, NULL); + if(req->response_code != 200) + printf("Got non-OK response code: %d\n", req->response_code); + + printf("Response:\n%s", req->input_buffer->buffer); + + double dt = ((g_t1.tv_sec + g_t1.tv_usec/1e6) - + (g_t0.tv_sec + g_t0.tv_usec/1e6)); + printf("Elapsed time: %.6f\n", dt); + exit(0); +} + +#define USAGE "USAGE: client <URI>" + +int +main(int argc, char** argv) +{ + const char* uri; + + if(argc != 2) + { + puts(USAGE); + exit(1); + } + + uri = argv[1]; + + struct evhttp_connection* ec; + struct evhttp_request* er; + + event_init(); + + ec = evhttp_connection_new("localhost", 8000); + assert(ec); + + gettimeofday(&g_t0, NULL); + er = evhttp_request_new(cb, NULL); + int ret = evhttp_make_request(ec, er, EVHTTP_REQ_GET, uri); + event_dispatch(); + + return 0; +} Modified: code/branches/federation/gazebo/webgazebo/main.cc =================================================================== --- code/branches/federation/gazebo/webgazebo/main.cc 2009-03-09 08:36:48 UTC (rev 7403) +++ code/branches/federation/gazebo/webgazebo/main.cc 2009-03-09 19:14:49 UTC (rev 7404) @@ -1,3 +1,30 @@ +/* + * Gazebo - Outdoor Multi-Robot Simulator + * Copyright (C) 2003 + * Nate Koenig & Andrew Howard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +/* Desc: HTTP portal to libgazebo + * Author: Brian Gerkey + * Date: 9 March 2009 + * SVN: $Id: gazebo.h 7398 2009-03-09 07:21:49Z natepak $ + */ + #include "webgazebo/WebGazebo.hh" #include <stdlib.h> Modified: code/branches/federation/gazebo/worlds/federation.world =================================================================== --- code/branches/federation/gazebo/worlds/federation.world 2009-03-09 08:36:48 UTC (rev 7403) +++ code/branches/federation/gazebo/worlds/federation.world 2009-03-09 19:14:49 UTC (rev 7404) @@ -48,6 +48,12 @@ <shadowTechnique>stencilAdditive</shadowTechnique> </rendering:ogre> + <model:empty name="factory_model"> + <controller:factory name="factory_controller"> + <interface:factory name="factory_iface"/> + </controller:factory> + </model:empty> + <!-- Ground Plane --> <model:physical name="plane1_model"> <xyz>0 0 0</xyz> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ge...@us...> - 2009-03-10 05:48:03
|
Revision: 7427 http://playerstage.svn.sourceforge.net/playerstage/?rev=7427&view=rev Author: gerkey Date: 2009-03-10 05:48:01 +0000 (Tue, 10 Mar 2009) Log Message: ----------- removed svn:externals Modified Paths: -------------- code/branches/federation/gazebo/webgazebo/SConscript code/branches/federation/gazebo/webgazebo/WebGazebo.cc code/branches/federation/gazebo/webgazebo/WebGazebo.hh code/branches/federation/gazebo/webgazebo/main.cc Property Changed: ---------------- code/branches/federation/gazebo/ Property changes on: code/branches/federation/gazebo ___________________________________________________________________ Modified: svn:externals - websim https://playerstage.svn.sourceforge.net/svnroot/playerstage/code/websim + Modified: code/branches/federation/gazebo/webgazebo/SConscript =================================================================== --- code/branches/federation/gazebo/webgazebo/SConscript 2009-03-10 05:43:37 UTC (rev 7426) +++ code/branches/federation/gazebo/webgazebo/SConscript 2009-03-10 05:48:01 UTC (rev 7427) @@ -1,12 +1,6 @@ #Import variable Import('*') -# Build websim; it uses CMake -import os -print 'Building websim...' -os.system('mkdir -p ../websim/build && cd ../websim/build && cmake .. && make') -print '...done building websim.' - env.Append(CPPPATH = '#webgazebo') libEnv = Environment ( @@ -22,9 +16,9 @@ #SHLINKCOMSTR = 'Linking $TARGET', #LINKCOMSTR = 'Linking $TARGET', - LIBS=Split('event gazebo'), - LIBPATH=Split('#libgazebo'), - CPPPATH=Split('#.') + LIBS=Split('event gazebo websim'), + LIBPATH=Split('#libgazebo ' + install_prefix + '/lib'), + CPPPATH=Split('#. ' + install_prefix + '/include') ) # We need Gazebo's quaternion functionality @@ -49,9 +43,9 @@ #SHLINKCOMSTR = 'Linking $TARGET', #LINKCOMSTR = 'Linking $TARGET', - LIBS=Split('webgazebo event gazebo'), - LIBPATH=Split('#libgazebo ' + os.getcwd()), - CPPPATH=Split('#.') + LIBS=Split('websim webgazebo event gazebo'), + LIBPATH=Split('#libgazebo ' + os.getcwd() + ' ' + install_prefix + '/lib'), + CPPPATH=Split('#. ' + install_prefix + '/include') ) webgazebo = exeEnv.Program('webgazebo', 'main.cc') Modified: code/branches/federation/gazebo/webgazebo/WebGazebo.cc =================================================================== --- code/branches/federation/gazebo/webgazebo/WebGazebo.cc 2009-03-10 05:43:37 UTC (rev 7426) +++ code/branches/federation/gazebo/webgazebo/WebGazebo.cc 2009-03-10 05:48:01 UTC (rev 7427) @@ -35,8 +35,6 @@ #include <time.h> #include <string.h> -#include <boost/lexical_cast.hpp> - #include "../server/Quatern.hh" // TODO: @@ -45,21 +43,12 @@ // - expose URI command to run for 1 tick // - do performance test -WebGazebo::WebGazebo(std::string _host, int _port, double dtol, double atol) : - host(_host), port(_port), sq_dist_tol(dtol*dtol), sq_ang_tol(atol*atol) +WebGazebo::WebGazebo(const std::string& fedfile, + const std::string& host, int port, + double dtol, double atol) : + websim::WebSim(fedfile, host, port), + sq_dist_tol(dtol*dtol), sq_ang_tol(atol*atol) { - // Set up the HTTP server - // Not sure whether it's safe to do this more that once in one process - event_init(); - - printf("[webgazebo] Starting HTTP server listening on %s:%d...", - this->host.c_str(), this->port); - fflush(stdout); - this->eh = evhttp_start(this->host.c_str(), this->port); - assert(eh); - evhttp_set_gencb(eh, &WebGazebo::EventCallback, (void*)this); - puts("Done."); - // Hook up to Gazebo printf("[webgazebo] Opening Gazebo simulation interface..."); fflush(stdout); @@ -70,9 +59,6 @@ // Open the Simulation Interface; let exceptions leak out this->simIface->Open(this->client, "default"); this->factoryIface->Open(this->client, "factory_iface"); - // Get the list of models - if(!GetModelList()) - throw("Failed to get list of models from Gazebo"); puts("Done."); puts("[webgazebo] Ready"); @@ -82,34 +68,20 @@ { delete this->simIface; delete this->client; - - // No event_fini() to call... } bool -WebGazebo::GetModelList() +WebGazebo::GetModelPVA(const std::string& name, + websim::Pose& p, + websim::Velocity& v, + websim::Acceleration& a, + std::string& error) { - // TODO - this->models["pioneer2dx_model1"] = 0; - return true; -} - -bool WebGazebo::HasModel(const std::string& name) -{ - // TODO - //std::map<std::string,int>::const_iterator it = models.find(name); - //return it != models.end(); - return true; -} - -bool -WebGazebo::GetPose(std::string model, gazebo::Pose& pose) -{ // Discard any leftover responses this->simIface->data->responseCount = 0; // Ask Gazebo - this->simIface->GetPose3d(model.c_str()); + this->simIface->GetPose3d(name.c_str()); // Wait for the response double timeout = 3.0; @@ -122,55 +94,59 @@ if(((t1.tv_sec + t1.tv_usec/1e6) - (t0.tv_sec + t0.tv_usec/1e6)) > timeout) { - puts("Timeout"); + error= "Timeout"; return false; } nanosleep(&sleeptime, NULL); } assert(this->simIface->data->responseCount == 1); - pose = this->simIface->data->responses[0].modelPose; + p.x = this->simIface->data->responses[0].modelPose.pos.x; + p.y = this->simIface->data->responses[0].modelPose.pos.y; + p.z = this->simIface->data->responses[0].modelPose.pos.z; + p.r = this->simIface->data->responses[0].modelPose.roll; + p.p = this->simIface->data->responses[0].modelPose.pitch; + p.a = this->simIface->data->responses[0].modelPose.yaw; return true; } bool -WebGazebo::SetPose(std::string model, const gazebo::Pose& pose) +WebGazebo::SetModelPVA(const std::string& name, + const websim::Pose& p, + const websim::Velocity& v, + const websim::Acceleration& a, + std::string& error) { // TODO: get pose, check for too-large jump, and complain about it // Tell Gazebo the new pose - this->simIface->SetPose3d(model.c_str(), pose); + gazebo::Pose pose; + gazebo::Vec3 lv, av, la, aa; + pose.pos.x = p.x; + pose.pos.y = p.y; + pose.pos.z = p.z; + pose.roll = p.r; + pose.pitch = p.p; + pose.yaw = p.a; - return true; -} + lv.x = v.x; + lv.y = v.y; + lv.z = v.z; + av.x = v.r; + av.y = v.p; + av.z = v.a; -bool -WebGazebo::ParseURI(std::string& model, - std::string& prop, - std::string& action, - std::string uri, - std::string& response) -{ - // Remove the query args - std::vector<std::string> uri_parts; - StringSplit(uri, uri_parts, "?"); - assert(uri_parts.size() > 0); - std::string bare_uri = uri_parts[0]; + la.x = a.x; + la.y = a.y; + la.z = a.z; + aa.x = a.r; + aa.y = a.p; + aa.z = a.a; - // We require 3 path components: model/property/action - StringSplit(bare_uri, uri_parts, "/"); - // There should be 4 parts, with the first one empty - if(uri_parts.size() != 4) - { - response = "Must be 3 slash-separated parts in the URI"; - return false; - } + this->simIface->SetState(name.c_str(), + pose, lv, av, la, aa); - model = uri_parts[1]; - prop = uri_parts[2]; - action = uri_parts[3]; - return true; } @@ -248,9 +224,14 @@ } bool -WebGazebo::CreateModel(const std::string& xmldata, - std::string& response) +WebGazebo::CreateModel(const std::string& name, + const std::string& type, + std::string& error) { + std::string xmldata; + if(!GetModel(name,type,xmldata,error)) + return false; + struct timespec sleeptime = {0, 10000000}; for(;;) { @@ -270,279 +251,9 @@ } bool -WebGazebo::HandleSimRequest(const std::string& prop, - const std::string& action, - struct evkeyvalq* kv, - std::string& response) +WebGazebo::DeleteModel(const std::string& name, + std::string& error) { - // The special factory property - if(prop == "factory") - { - if(action == "create") - { - std::string name, type; - if(!GetValue(name, kv, "name") || - !GetValue(type, kv, "type")) - { - response = "ERROR: Missing name and/or type argument for sim/factor/create"; - return false; - } - - std::string xmldata; - if(!GetModel(name, type, xmldata, response)) - return false; - if(!CreateModel(xmldata, response)) - return false; - response = std::string("Created model of type ") + type; - return true; - } - else - { - response = "ERROR: Unknown action " + action + " for sim/factory"; - return false; - } - } - else - { - response = "ERROR: Unknown property " + prop + " for sim"; - return false; - } -} - -bool -WebGazebo::HandleModelRequest(const std::string& model, - const std::string& prop, - const std::string& action, - struct evkeyvalq* kv, - std::string& response) -{ - /* - printf("[webgazebo] got request for %s:%s:%s\n", - model.c_str(), - prop.c_str(), - action.c_str()); - */ - if(HasModel(model)) - { - if(action == "get") - { - if(prop == "pose") - { - gazebo::Pose p; - if(GetPose(model, p)) - { - char buf[1024]; - snprintf(buf, sizeof(buf), - "Got %s's pose: (%.3f,%.3f,%.3f) (%.3f,%.3f,%.3f)", - model.c_str(), - p.pos.x, p.pos.y, p.pos.z, - p.roll, p.pitch, p.yaw); - response = buf; - return true; - } - else - { - response = "ERROR: Failed to get pose for model " + model; - return false; - } - } - else - { - response = "ERROR: Unknown property " + prop + " for model " + model; - return false; - } - } - else if(action == "set") - { - if(prop == "pose") - { - std::string sx, sy, sz, sroll, spitch, syaw; - - // Get pose first, fill in what the caller provided - gazebo::Pose p, q; - if(!GetPose(model, p)) - { - response = "Failed to get pose before setting it"; - return false; - } - try - { - q = p; - if(GetValue(sx, kv, "x")) - p.pos.x = boost::lexical_cast<float>(sx); - if(GetValue(sy, kv, "y")) - p.pos.y = boost::lexical_cast<float>(sy); - if(GetValue(sz, kv, "z")) - p.pos.z = boost::lexical_cast<float>(sz); - if(GetValue(sroll, kv, "roll")) - p.roll = boost::lexical_cast<float>(sroll); - if(GetValue(spitch, kv, "pitch")) - p.pitch = boost::lexical_cast<float>(spitch); - if(GetValue(syaw, kv, "yaw")) - p.yaw = boost::lexical_cast<float>(syaw); - } - catch(boost::bad_lexical_cast e) - { - response = std::string("Failed to parse input value(s): ") + - e.what(); - return false; - } - - if(SetPose(model, p)) - { - char buf[1024]; - snprintf(buf, sizeof(buf), - "Set %s's pose: (%.3f,%.3f,%.3f) (%.3f,%.3f,%.3f)", - model.c_str(), - p.pos.x, p.pos.y, p.pos.z, - p.roll, p.pitch, p.yaw); - response = buf; - - // Check tolerances, and complain if they're exceeded - if(!CheckTolerances(p,q)) - response += "\nWARNING: Pose change exceeded tolerance"; - - return true; - } - else - { - response = "ERROR: Failed to get pose for model " + model; - return false; - } - } - else - { - response = "ERROR: Unknown property " + prop + " for model " + model; - return false; - } - } - else - { - response = "ERROR: Unknown action " + action; - return false; - } - } - else - { - response = "ERROR: No such model " + model; - return false; - } -} - -bool -WebGazebo::HandleURI(const std::string& model, - const std::string& prop, - const std::string& action, - struct evkeyvalq* kv, - std::string& response) -{ - // The special simulation model - if(model == "sim") - { - return HandleSimRequest(prop, action, kv, response); - } - // Everything else must be an existing model - else - { - return HandleModelRequest(model, prop, action, kv, response); - } -} - -// Handle all incoming URI requests -void -WebGazebo::EventCallback(evhttp_request* req, void* arg) -{ - WebGazebo* obj = (WebGazebo*)arg; - - /* - printf("[webgazebo] Got request:%s:\n", req->uri); - */ - - // Pull out query args - struct evkeyvalq query_args; - evhttp_parse_query(req->uri, &query_args); - - struct evbuffer* eb = evbuffer_new(); - assert(eb); - - int response_code; - std::string response_string; - std::string model, prop, action, response; - if(!obj->ParseURI(model, prop, action, req->uri, response)) - { - response_code = 400; - response_string = "Invalid request"; - } - else if(!obj->HandleURI(model, prop, action, &query_args, response)) - { - response_code = 400; - response_string = "Invalid request"; - } - else - { - response_code = 200; - response_string = "OK"; - } - - evbuffer_add_printf(eb, "%s\n", response.c_str()); - /* - printf("[webgazebo] Sending reply: %d %s\n", - response_code, response_string.c_str()); - */ - evhttp_send_reply(req, response_code, response_string.c_str(), eb); - evbuffer_free(eb); - - obj->DeleteKeyVal(&query_args); -} - -void -WebGazebo::Spin() -{ - event_dispatch(); -} - -void -WebGazebo::StringSplit(const std::string &s, - std::vector<std::string> &t, - const std::string &d) -{ - t.clear(); - size_t start = 0, end; - while ((end = s.find_first_of(d, start)) != std::string::npos) - { - t.push_back(s.substr(start, end-start)); - start = end + 1; - } - t.push_back(s.substr(start)); -} - -// Strange that libevent doesn't provide a way to free the evkeyvalq -// structure. -void -WebGazebo::DeleteKeyVal(struct evkeyvalq* query_args) -{ - struct evkeyval* kv; - TAILQ_FOREACH(kv, query_args, next) - { - free(kv->key); - free(kv->value); - TAILQ_REMOVE(query_args, kv, next); - } -} - -bool -WebGazebo::GetValue(std::string& value, - struct evkeyvalq* query_args, - const std::string& key) -{ - struct evkeyval* kv; - TAILQ_FOREACH(kv, query_args, next) - { - if(key == kv->key) - { - value = std::string(kv->value); - return true; - } - } + error = "DeleteModel not implemented"; return false; } Modified: code/branches/federation/gazebo/webgazebo/WebGazebo.hh =================================================================== --- code/branches/federation/gazebo/webgazebo/WebGazebo.hh 2009-03-10 05:43:37 UTC (rev 7426) +++ code/branches/federation/gazebo/webgazebo/WebGazebo.hh 2009-03-10 05:48:01 UTC (rev 7427) @@ -26,37 +26,46 @@ */ #include <string> -#include <vector> #include <map> // These headers must be included prior to the libevent headers #include <sys/types.h> #include <sys/queue.h> -// libevent -#include <event.h> -#include <evhttp.h> - // libgazebo; the libgazebo directory is used because we're building // against the source tree #include <libgazebo/gazebo.h> -class WebGazebo +#include "websim/websim.h" + +class WebGazebo : public websim::WebSim { public: - WebGazebo(std::string _host, int _port, + WebGazebo(const std::string& fedfile, + const std::string& host, int port, double dtol, double atol); - ~WebGazebo(); + virtual ~WebGazebo(); + + // Interface to be implemented by simulators + virtual bool CreateModel(const std::string& name, + const std::string& type, + std::string& error); + virtual bool DeleteModel(const std::string& name, + std::string& error); + virtual bool SetModelPVA(const std::string& name, + const websim::Pose& p, + const websim::Velocity& v, + const websim::Acceleration& a, + std::string& error); + virtual bool GetModelPVA(const std::string& name, + websim::Pose& p, + websim::Velocity& v, + websim::Acceleration& a, + std::string& error); - void Spin(); - private: - std::string host; - int port; double sq_dist_tol, sq_ang_tol; - struct evhttp* eh; - gazebo::Client *client; gazebo::SimulationIface *simIface; gazebo::FactoryIface *factoryIface; @@ -64,44 +73,9 @@ // Available models std::map<std::string,int> models; - // Static, so that it can be passed as a callback to libevent - static void EventCallback(evhttp_request* req, void* arg); - bool CheckTolerances(gazebo::Pose p, gazebo::Pose q); - bool GetModelList(); - bool HasModel(const std::string& name); - bool CreateModel(const std::string& xmldata, - std::string& response); - bool GetPose(std::string model, gazebo::Pose& pose); - bool SetPose(std::string model, const gazebo::Pose& pose); - void StringSplit(const std::string &s, - std::vector<std::string> &t, - const std::string &d); - bool GetValue(std::string& value, - struct evkeyvalq* query_args, - const std::string& key); bool GetModel(const std::string& name, const std::string& type, std::string& xmldata, std::string& response); - bool HandleURI(const std::string& model, - const std::string& prop, - const std::string& action, - struct evkeyvalq* kv, - std::string& response); - bool HandleSimRequest(const std::string& prop, - const std::string& action, - struct evkeyvalq* kv, - std::string& response); - bool HandleModelRequest(const std::string& model, - const std::string& prop, - const std::string& action, - struct evkeyvalq* kv, - std::string& response); - bool ParseURI(std::string& model, - std::string& prop, - std::string& action, - std::string uri, - std::string& response); - void DeleteKeyVal(struct evkeyvalq* query_args); }; Modified: code/branches/federation/gazebo/webgazebo/main.cc =================================================================== --- code/branches/federation/gazebo/webgazebo/main.cc 2009-03-10 05:43:37 UTC (rev 7426) +++ code/branches/federation/gazebo/webgazebo/main.cc 2009-03-10 05:48:01 UTC (rev 7427) @@ -29,13 +29,14 @@ #include <stdlib.h> -#define USAGE "USAGE: webgazebo [-h <host>] [-p <port>]\n" +#define USAGE "USAGE: webgazebo -f <file.fed> [-h <host>] [-p <port>] [-d <dtol>] [-a <atol>]\n" #define DEFAULT_PORT 8000 #define DEFAULT_HOST "localhost" int g_port = DEFAULT_PORT; std::string g_host = DEFAULT_HOST; +std::string g_fedfile; double g_dtol, g_atol; bool ParseArgs(int argc, char** argv); @@ -51,8 +52,8 @@ try { - WebGazebo wg(g_host, g_port, g_dtol, g_atol); - wg.Spin(); + WebGazebo wg(g_fedfile, g_host, g_port, g_dtol, g_atol); + wg.Update(); } catch(std::string e) { @@ -66,13 +67,19 @@ bool ParseArgs(int argc, char** argv) { - char *flags = (char*)("p:h:d:a:"); + char *flags = (char*)("f:p:h:d:a:"); int ch; while ((ch = getopt(argc, argv, flags)) != -1) { switch(ch) { + // federation file + case 'f': + if(!optarg) + return false; + g_fedfile = optarg; + break; // port case 'p': if(!optarg) @@ -102,7 +109,7 @@ } } - if((g_host.size() == 0) || (g_port == 0)) + if((g_fedfile.size() == 0) || (g_host.size() == 0) || (g_port == 0)) return false; return true; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ge...@us...> - 2009-03-10 08:22:33
|
Revision: 7438 http://playerstage.svn.sourceforge.net/playerstage/?rev=7438&view=rev Author: gerkey Date: 2009-03-10 08:22:13 +0000 (Tue, 10 Mar 2009) Log Message: ----------- Fixed deadlock on libgazebo tick system, got other stuff working Modified Paths: -------------- code/branches/federation/gazebo/libgazebo/SimIface.cc code/branches/federation/gazebo/libgazebo/gazebo.h code/branches/federation/gazebo/server/World.cc code/branches/federation/gazebo/webgazebo/WebGazebo.cc code/branches/federation/gazebo/webgazebo/WebGazebo.hh code/branches/federation/gazebo/webgazebo/main.cc Modified: code/branches/federation/gazebo/libgazebo/SimIface.cc =================================================================== --- code/branches/federation/gazebo/libgazebo/SimIface.cc 2009-03-10 07:45:57 UTC (rev 7437) +++ code/branches/federation/gazebo/libgazebo/SimIface.cc 2009-03-10 08:22:13 UTC (rev 7438) @@ -18,8 +18,6 @@ { this->goAckThread = NULL; - this->goAckCond = NULL; - this->mutex = NULL; } @@ -27,23 +25,10 @@ /// Destroy an interface SimulationIface::~SimulationIface() { - 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) - { - printf("Delete the condition\n"); - delete this->goAckCond; - printf("Done deleting the condition\n"); - }*/ - this->goAckCond = NULL; - this->data = NULL; } @@ -90,8 +75,6 @@ // 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); @@ -104,11 +87,6 @@ while (true) { - boost::mutex::scoped_lock lock(*this->mutex); - - // Wait on a condition that signals when the thread should run - this->goAckCond->wait(lock); - // Wait for Gazebo to send a Post this->GoAckWait(); Modified: code/branches/federation/gazebo/libgazebo/gazebo.h =================================================================== --- code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-10 07:45:57 UTC (rev 7437) +++ code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-10 08:22:13 UTC (rev 7438) @@ -476,11 +476,7 @@ this->Unlock(); { - boost::mutex::scoped_lock lock(*this->mutex); - - // Set the time the thread should block for - this->blockTimeUs = us; - + //boost::mutex::scoped_lock lock(*this->mutex); if (this->currentConnection.connected()) this->currentConnection.disconnect(); @@ -488,8 +484,6 @@ // (below) finishes waiting this->currentConnection = this->goAckSignal.connect( subscriber ); } - - this->goAckCond->notify_one(); } /// \brief Pause the simulation @@ -522,14 +516,10 @@ 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; - /// Pointer to the simulation data public: SimulationData *data; }; Modified: code/branches/federation/gazebo/server/World.cc =================================================================== --- code/branches/federation/gazebo/server/World.cc 2009-03-10 07:45:57 UTC (rev 7437) +++ code/branches/federation/gazebo/server/World.cc 2009-03-10 08:22:13 UTC (rev 7438) @@ -207,12 +207,13 @@ { //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(); - this->simPauseTime = 0; - Simulator::Instance()->SetPaused(true); } else { Modified: code/branches/federation/gazebo/webgazebo/WebGazebo.cc =================================================================== --- code/branches/federation/gazebo/webgazebo/WebGazebo.cc 2009-03-10 07:45:57 UTC (rev 7437) +++ code/branches/federation/gazebo/webgazebo/WebGazebo.cc 2009-03-10 08:22:13 UTC (rev 7438) @@ -39,9 +39,6 @@ // TODO: // - make ghost models not collide, fall, etc. -// - add SetState -// - expose URI command to run for 1 tick -// - do performance test WebGazebo::WebGazebo(const std::string& fedfile, const std::string& host, unsigned short port, @@ -262,13 +259,17 @@ void WebGazebo::GoCallback() { - puts("GoCallback"); + this->goCond.notify_one(); } bool WebGazebo::Go(double t) { - this->simIface->Go((unsigned int)rint(t/1e6), + unsigned int us = (unsigned int)rint(t*1e6); + this->simIface->Go(us, boost::bind(&WebGazebo::GoCallback, this)); + // Wait for the callback to fire + boost::mutex::scoped_lock lock(this->goMutex); + this->goCond.wait(lock); return true; } Modified: code/branches/federation/gazebo/webgazebo/WebGazebo.hh =================================================================== --- code/branches/federation/gazebo/webgazebo/WebGazebo.hh 2009-03-10 07:45:57 UTC (rev 7437) +++ code/branches/federation/gazebo/webgazebo/WebGazebo.hh 2009-03-10 08:22:13 UTC (rev 7438) @@ -67,7 +67,7 @@ private: double sq_dist_tol, sq_ang_tol; - boost::condition goMutex; + boost::mutex goMutex; boost::condition goCond; gazebo::Client *client; Modified: code/branches/federation/gazebo/webgazebo/main.cc =================================================================== --- code/branches/federation/gazebo/webgazebo/main.cc 2009-03-10 07:45:57 UTC (rev 7437) +++ code/branches/federation/gazebo/webgazebo/main.cc 2009-03-10 08:22:13 UTC (rev 7438) @@ -55,6 +55,7 @@ for(;;) { wg.Update(); + wg.Go(1.0); } return 0; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-03-10 09:37:22
|
Revision: 7443 http://playerstage.svn.sourceforge.net/playerstage/?rev=7443&view=rev Author: natepak Date: 2009-03-10 09:37:04 +0000 (Tue, 10 Mar 2009) Log Message: ----------- Improved graphics interface Modified Paths: -------------- code/branches/federation/gazebo/examples/libgazebo/position/SConstruct code/branches/federation/gazebo/examples/libgazebo/position/position.cc code/branches/federation/gazebo/libgazebo/SConscript code/branches/federation/gazebo/libgazebo/gazebo.h code/branches/federation/gazebo/server/Entity.cc code/branches/federation/gazebo/server/Entity.hh code/branches/federation/gazebo/server/Model.cc code/branches/federation/gazebo/server/Model.hh code/branches/federation/gazebo/server/SConscript code/branches/federation/gazebo/server/Simulator.cc code/branches/federation/gazebo/server/World.cc code/branches/federation/gazebo/server/World.hh code/branches/federation/gazebo/server/controllers/SConscript code/branches/federation/gazebo/server/controllers/factory/Factory.hh code/branches/federation/gazebo/server/controllers/position2d/differential/Differential_Position2d.cc code/branches/federation/gazebo/server/main.cc code/branches/federation/gazebo/server/physics/Body.cc code/branches/federation/gazebo/server/physics/Body.hh code/branches/federation/gazebo/server/physics/ContactParams.cc code/branches/federation/gazebo/server/physics/ContactParams.hh code/branches/federation/gazebo/server/physics/Geom.cc code/branches/federation/gazebo/server/physics/Geom.hh code/branches/federation/gazebo/server/physics/Joint.cc code/branches/federation/gazebo/server/physics/ode/ODEPhysics.cc code/branches/federation/gazebo/server/rendering/OgreAdaptor.cc code/branches/federation/gazebo/server/rendering/OgreCreator.cc code/branches/federation/gazebo/server/rendering/OgreCreator.hh code/branches/federation/gazebo/server/rendering/OgreVisual.cc code/branches/federation/gazebo/server/rendering/OgreVisual.hh code/branches/federation/gazebo/server/rendering/SConscript code/branches/federation/gazebo/server/sensors/Sensor.cc code/branches/federation/gazebo/server/sensors/Sensor.hh code/branches/federation/gazebo/server/sensors/ir/IRSensor.cc code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc code/branches/federation/gazebo/worlds/pioneer2dx.world Added Paths: ----------- code/branches/federation/gazebo/examples/libgazebo/graphics3d/ code/branches/federation/gazebo/examples/libgazebo/graphics3d/.graphics3d.cc.swo code/branches/federation/gazebo/examples/libgazebo/graphics3d/.sconsign.dblite code/branches/federation/gazebo/examples/libgazebo/graphics3d/SConstruct code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc Added: code/branches/federation/gazebo/examples/libgazebo/graphics3d/.graphics3d.cc.swo =================================================================== (Binary files differ) Property changes on: code/branches/federation/gazebo/examples/libgazebo/graphics3d/.graphics3d.cc.swo ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/branches/federation/gazebo/examples/libgazebo/graphics3d/.sconsign.dblite =================================================================== (Binary files differ) Property changes on: code/branches/federation/gazebo/examples/libgazebo/graphics3d/.sconsign.dblite ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/branches/federation/gazebo/examples/libgazebo/graphics3d/SConstruct =================================================================== --- code/branches/federation/gazebo/examples/libgazebo/graphics3d/SConstruct (rev 0) +++ code/branches/federation/gazebo/examples/libgazebo/graphics3d/SConstruct 2009-03-10 09:37:04 UTC (rev 7443) @@ -0,0 +1,14 @@ + +# 3rd party packages +parseConfigs=['pkg-config --cflags --libs libgazebo'] + +env = Environment ( + CC = 'g++', + CCFLAGS = Split ('-pthread -pipe -W -Wall -O2'), +) + +# Parse all the pacakge configurations +for cfg in parseConfigs: + env.ParseConfig(cfg) + +env.Program('graphics3d','graphics3d.cc') Added: code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc =================================================================== --- code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc (rev 0) +++ code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc 2009-03-10 09:37:04 UTC (rev 7443) @@ -0,0 +1,110 @@ +#include <string.h> +#include <string> +#include <iostream> +#include <gazebo/gazebo.h> + +int main() +{ + gazebo::Client *client = new gazebo::Client(); + gazebo::Graphics3dIface *g3dIface = new gazebo::Graphics3dIface(); + gazebo::Graphics3dIface *pioneerG3DIface = new gazebo::Graphics3dIface(); + + int serverId = 0; + + /// Connect to the libgazebo server + try + { + client->ConnectWait(serverId, GZ_CLIENT_ID_USER_FIRST); + } + catch (std::string e) + { + std::cout << "Gazebo error: Unable to connect\n" << e << "\n"; + return -1; + } + + /// Open the global graphics Interface + try + { + g3dIface->Open(client, "default"); + pioneerG3DIface->Open(client,"pioneer2dx_model1"); + } + catch (std::string e) + { + std::cerr << "Gazebo error: Unable to connect to the graphics3d interface\n" << e << "\n"; + return -1; + } + + float radius = 0.2; + float theta = 0; + + float size = 0.4; + float h = 0; + int dir = 1; + std::string name = "square"; + int count = 0; + + gazebo::Vec3 spherePos, sphereSize; + sphereSize.x = 0.1; + sphereSize.y = 0.1; + sphereSize.z = 0.1; + + gazebo::Vec3 vec[5]; + gazebo::Color clr; + while (true) + { + vec[0].x = size; + vec[0].y = size; + vec[0].z = h; + + vec[1].x = -size; + vec[1].y = size; + vec[1].z = h; + + vec[2].x = -size; + vec[2].y = -size; + vec[2].z = h; + + vec[3].x = size; + vec[3].y = -size; + vec[3].z = h; + + vec[4].x = size; + vec[4].y = size; + vec[4].z = h; + + clr.r = 1.0; + clr.g = 0.0; + clr.b = 0.0; + clr.a = 1.0; + + spherePos.x = radius * cos(theta); + spherePos.y = radius * sin(theta); + spherePos.z = 0.8; + + // Draw the bouncing square + pioneerG3DIface->DrawSimple(name.c_str(), + gazebo::Graphics3dDrawData::LINE_STRIP, vec, 5, clr); + + // Draw the sliding billboard + pioneerG3DIface->DrawBillboard("mybillboard", "heartpsg.png", + gazebo::Vec3(h,0,0.4), gazebo::Vec2(0.2, 0.2) ); + + + pioneerG3DIface->DrawShape("mysphere", + gazebo::Graphics3dDrawData::SPHERE, spherePos, sphereSize, clr); + + h += dir*0.01; + if (h >= 0.2 || h < 0) + dir *= -1; + + theta += 0.1; + usleep(10000); + } + + + g3dIface->Close(); + + delete g3dIface; + return 0; +} + Modified: code/branches/federation/gazebo/examples/libgazebo/position/SConstruct =================================================================== --- code/branches/federation/gazebo/examples/libgazebo/position/SConstruct 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/examples/libgazebo/position/SConstruct 2009-03-10 09:37:04 UTC (rev 7443) @@ -1,7 +1,6 @@ # 3rd party packages -parseConfigs=['pkg-config --cflags --libs libgazebo', - 'pkg-config --cflags --libs libgazeboServer'] +parseConfigs=['pkg-config --cflags --libs libgazebo'] env = Environment ( Modified: code/branches/federation/gazebo/examples/libgazebo/position/position.cc =================================================================== --- code/branches/federation/gazebo/examples/libgazebo/position/position.cc 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/examples/libgazebo/position/position.cc 2009-03-10 09:37:04 UTC (rev 7443) @@ -14,7 +14,7 @@ { client->ConnectWait(serverId, GZ_CLIENT_ID_USER_FIRST); } - catch (gazebo::GazeboError e) + catch (std::string e) { std::cout << "Gazebo error: Unable to connect\n" << e << "\n"; return -1; @@ -25,7 +25,7 @@ { simIface->Open(client, "default"); } - catch (gazebo::GazeboError e) + catch (std::string e) { std::cout << "Gazebo error: Unable to connect to the sim interface\n" << e << "\n"; return -1; Modified: code/branches/federation/gazebo/libgazebo/SConscript =================================================================== --- code/branches/federation/gazebo/libgazebo/SConscript 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/libgazebo/SConscript 2009-03-10 09:37:04 UTC (rev 7443) @@ -42,7 +42,7 @@ pkgconfig = env.PkgConfig(target='libgazebo.pc', source=Value(install_prefix)) env.Install(dir=install_prefix+'/lib/pkgconfig', source=pkgconfig) -sources = Split('Server.cc Client.cc Iface.cc IfaceFactory.cc SimIface.cc') +sources = Split('Server.cc Client.cc Iface.cc IfaceFactory.cc SimIface.cc Graphics3dIface.cc') headers = Split('gazebo.h IfaceFactory.hh') sharedLib = myEnv.SharedLibrary('gazebo', sources) Modified: code/branches/federation/gazebo/libgazebo/gazebo.h =================================================================== --- code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-10 09:37:04 UTC (rev 7443) @@ -59,6 +59,28 @@ /// \addtogroup libgazebo /// \{ + +/// \brief Vector 2 class +class Vec2 +{ + /// \brief Default Constructor + public: Vec2() : x(0), y(0) {} + + /// \brief Constructor + public: Vec2(float _x, float _y) : x(_x), y(_y) {} + + /// \brief Copy constructor + public: Vec2(const Vec2 &vec) : x(vec.x), y(vec.y) {} + + /// X value + public: float x; + + /// Y value + public: float y; + +}; + + /// \brief Vector 3 class class Vec3 { @@ -476,7 +498,6 @@ this->Unlock(); { - //boost::mutex::scoped_lock lock(*this->mutex); if (this->currentConnection.connected()) this->currentConnection.disconnect(); @@ -778,55 +799,81 @@ /// Maximum number of points that can be drawn #define GAZEBO_GRAPHICS3D_MAX_POINTS 1024 +#define GAZEBO_GRAPHICS3D_MAX_NAME 128 +#define GAZEBO_GRAPHICS3D_MAX_COMMANDS 128 -/// \brief Graphics3d interface data -class Graphics3dData +class Graphics3dDrawData { /// Type of drawing to perform - enum DrawMode {POINTS, LINES, LINE_STRIP, LINE_LOOP, TRIANGLES, TRIANGLE_STRIP, TRIANGLE_FAN, QUADS, QUAD_STRIP, POLYGON}; + public: enum DrawMode { POINTS, LINES, LINE_STRIP, TRIANGLES, TRIANGLE_STRIP, + TRIANGLE_FAN, PLANE, SPHERE, CUBE, CYLINDER, + BILLBOARD }; /// Drawing mode - public: DrawMode drawmode; + public: DrawMode drawMode; + public: char name[GAZEBO_GRAPHICS3D_MAX_NAME]; + /// Number of vertices - public: unsigned int point_count; + public: unsigned int pointCount; /// Vertices public: Vec3 points[GAZEBO_GRAPHICS3D_MAX_POINTS]; /// Drawing color public: Color color; + + /// Texture to apply to a billboard. Only applicable when + // drawMode == BILLBOARD + public: char billboardTexture[GAZEBO_GRAPHICS3D_MAX_NAME]; + + /// Pose at which to draw a shape + public: Pose pose; + + /// Size of the shape + public: Vec3 size; }; +/// \brief Graphics3d interface data +class Graphics3dData +{ + public: Graphics3dDrawData commands[GAZEBO_GRAPHICS3D_MAX_COMMANDS]; + public: unsigned int cmdCount; +}; + + /// \brief Graphics3d interface class Graphics3dIface : public Iface { /// \brief Constructor - public: Graphics3dIface():Iface("graphics3d", sizeof(Graphics3dIface)+sizeof(Graphics3dData)) {} + public: Graphics3dIface(); /// \brief Destructor - public: virtual ~Graphics3dIface() {this->data = NULL;} + public: virtual ~Graphics3dIface(); /// \brief Create the interface (used by Gazebo server) /// \param server Pointer to the server /// \param id Id of the interface - public: virtual void Create(Server *server, std::string id) - { - Iface::Create(server,id); - this->data = (Graphics3dData*)this->mMap; - } + public: virtual void Create(Server *server, std::string id); /// \brief Open an existing interface /// \param client Pointer to the client /// \param id Id of the interface - public: virtual void Open(Client *client, std::string id) - { - Iface::Open(client,id); - this->data = (Graphics3dData*)this->mMap; - } + public: virtual void Open(Client *client, std::string id); + /// \brief Draw a simple object, that is defined by a series of points + public: void DrawSimple(const char *name, Graphics3dDrawData::DrawMode mode, + Vec3 *point, unsigned int numPoints, Color clr ); + /// \brief Draw a shape + public: void DrawShape(const char *name, Graphics3dDrawData::DrawMode mode, + Vec3 pos, Vec3 size, Color clr); + + /// \brief Draw a billboard + public: void DrawBillboard(const char *name, const char *texture, + Vec3 pos, Vec2 size); + /// Pointer to the graphics3d data public: Graphics3dData *data; }; Modified: code/branches/federation/gazebo/server/Entity.cc =================================================================== --- code/branches/federation/gazebo/server/Entity.cc 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/Entity.cc 2009-03-10 09:37:04 UTC (rev 7443) @@ -29,6 +29,7 @@ #include "GazeboError.hh" #include "Global.hh" #include "OgreVisual.hh" +#include "OgreVisualManager.hh" #include "World.hh" #include "PhysicsEngine.hh" #include "Entity.hh" @@ -48,15 +49,20 @@ this->selected = false; + std::ostringstream visname; + visname << "Entity_" << this->GetId() << "_VISUAL"; + if (this->parent) { this->parent->AddChild(this); - this->visualNode=new OgreVisual(this->parent->GetVisualNode()); + this->visualNode = OgreVisualManager::Instance()->CreateVisual( + visname.str(), this->parent->GetVisualNode(), this); this->SetStatic(parent->IsStatic()); } else { - this->visualNode = new OgreVisual(NULL); + this->visualNode = OgreVisualManager::Instance()->CreateVisual( + visname.str(), NULL, this ); } // Add this to the phyic's engine @@ -180,8 +186,30 @@ } //////////////////////////////////////////////////////////////////////////////// +/// Set the space id +void Entity::SetSpaceId( dSpaceID spaceid ) +{ + this->spaceId = spaceid; +} + +//////////////////////////////////////////////////////////////////////////////// +/// Return the space id +dSpaceID Entity::GetSpaceId() const +{ + return this->spaceId; +} + +//////////////////////////////////////////////////////////////////////////////// /// Returns true if the entities are the same. Checks only the name bool Entity::operator==(const Entity &ent) const { return ent.GetName() == this->GetName(); } + +Pose3d Entity::GetPoseRelative() const +{ + if (this->parent) + return this->parent->GetPose() - this->GetPose(); + else + return this->GetPose(); +} Modified: code/branches/federation/gazebo/server/Entity.hh =================================================================== --- code/branches/federation/gazebo/server/Entity.hh 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/Entity.hh 2009-03-10 09:37:04 UTC (rev 7443) @@ -32,6 +32,7 @@ #include <ode/ode.h> #include "Common.hh" +#include "Pose3d.hh" #include "Param.hh" namespace gazebo @@ -97,7 +98,17 @@ /// \brief True if the entity is selected by the user public: bool IsSelected() const; - + + /// \brief Set the space id + public: void SetSpaceId( dSpaceID spaceid ); + + /// \brief Return the space id + public: dSpaceID GetSpaceId() const; + + /// \brief Get the pose of the entity + public: virtual Pose3d GetPose() const = 0;// { return Pose3d(); } + public: Pose3d GetPoseRelative() const; + /// \brief Returns true if the entities are the same. Checks only the name public: bool operator==(const Entity &ent) const; @@ -113,8 +124,8 @@ /// \brief Visual stuff protected: OgreVisual *visualNode; - /// \brief ODE Stuff (should be go somewhere else) - public: dSpaceID spaceId; + /// \brief ODE Stuff + protected: dSpaceID spaceId; private: bool selected; }; Modified: code/branches/federation/gazebo/server/Model.cc =================================================================== --- code/branches/federation/gazebo/server/Model.cc 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/Model.cc 2009-03-10 09:37:04 UTC (rev 7443) @@ -29,6 +29,8 @@ #include <sstream> #include <iostream> +#include "OgreVisual.hh" +#include "GraphicsIfaceHandler.hh" #include "Global.hh" #include "GazeboError.hh" #include "GazeboMessage.hh" @@ -80,6 +82,7 @@ Param::End(); + this->graphicsHandler = NULL; this->parentBodyNameP = NULL; this->myBodyNameP = NULL; } @@ -92,6 +95,9 @@ std::map< std::string, Joint* >::iterator jiter; std::map< std::string, Controller* >::iterator citer; + if (this->graphicsHandler) + delete this->graphicsHandler; + for (biter=this->bodies.begin(); biter != this->bodies.end(); biter++) { GZ_DELETE(biter->second); @@ -211,6 +217,10 @@ this->SetCollideMode( **this->collideP ); + // Create the graphics iface handler + this->graphicsHandler = new GraphicsIfaceHandler(); + this->graphicsHandler->Load(this->GetName(), this); + return 0; // Get the name of the python module @@ -330,6 +340,8 @@ std::map<std::string, Body* >::iterator biter; std::map<std::string, Controller* >::iterator contIter; + this->graphicsHandler->Init(); + for (biter = this->bodies.begin(); biter!=this->bodies.end(); biter++) biter->second->Init(); @@ -392,6 +404,12 @@ this->rpyP->SetValue(this->pose.rot); } + if (this->parent) + this->visualNode->SetPose(this->GetPose() - this->parent->GetPose()); + else + this->visualNode->SetPose(this->GetPose()); + + this->graphicsHandler->Update(); return this->UpdateChild(); } @@ -413,6 +431,9 @@ biter->second->Fini(); } + if (this->graphicsHandler) + delete this->graphicsHandler; + return this->FiniChild(); } @@ -509,6 +530,7 @@ childModel->SetPose(newPose); } } + } //////////////////////////////////////////////////////////////////////////////// @@ -592,11 +614,12 @@ } //////////////////////////////////////////////////////////////////////////////// // Get the current pose -const Pose3d &Model::GetPose() const +Pose3d Model::GetPose() const { return this->pose; } + //////////////////////////////////////////////////////////////////////////////// // Create and return a new body Body *Model::CreateBody() @@ -798,13 +821,6 @@ this->joint->SetAxis( Vector3(0,1,0) ); this->joint->SetParam( dParamHiStop, 0); this->joint->SetParam( dParamLoStop, 0); - - /* if (this->spaceId) - { - dSpaceDestroy(this->spaceId); - this->spaceId = parentModel->spaceId; - } - */ } //////////////////////////////////////////////////////////////////////////////// Modified: code/branches/federation/gazebo/server/Model.hh =================================================================== --- code/branches/federation/gazebo/server/Model.hh 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/Model.hh 2009-03-10 09:37:04 UTC (rev 7443) @@ -48,6 +48,7 @@ class Body; class Controller; class HingeJoint; + class GraphicsIfaceHandler; /// \addtogroup gazebo_server /// \brief A model @@ -122,7 +123,7 @@ public: void SetAngularAccel( const Vector3 &vel ); /// \brief Get the current pose - public: const Pose3d &GetPose() const; + public: virtual Pose3d GetPose() const; /// \brief Create and return a new body /// \return Pointer to a new body. @@ -222,6 +223,8 @@ // Name of a light (if the model is renderable:light) private: std::string lightName; + private: GraphicsIfaceHandler *graphicsHandler; + /* private: PyObject *pName; private: PyObject *pModule; private: PyObject *pFuncUpdate; Modified: code/branches/federation/gazebo/server/SConscript =================================================================== --- code/branches/federation/gazebo/server/SConscript 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/SConscript 2009-03-10 09:37:04 UTC (rev 7443) @@ -31,7 +31,8 @@ 'Model.cc', 'Simulator.cc', 'Angle.cc', - 'Param.cc' + 'Param.cc', + 'GraphicsIfaceHandler.cc' ] headers.append( @@ -53,7 +54,8 @@ '#/server/XMLConfig.hh', '#/server/GazeboConfig.hh', '#/server/Angle.hh', - '#/server/Param.hh' + '#/server/Param.hh', + '#/server/GraphicsIfaceHandler.hh' ] ) #staticObjs.append( env.StaticObject(sources) ) Modified: code/branches/federation/gazebo/server/Simulator.cc =================================================================== --- code/branches/federation/gazebo/server/Simulator.cc 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/Simulator.cc 2009-03-10 09:37:04 UTC (rev 7443) @@ -28,6 +28,7 @@ #include <fstream> #include <sys/time.h> +#include "OgreVisualManager.hh" #include "Body.hh" #include "Geom.hh" #include "Model.hh" @@ -330,6 +331,8 @@ //this->prevRenderTime = this->GetRealTime(); } + OgreVisualManager::Instance()->Update(); + // Update the gui if (this->gui) { Modified: code/branches/federation/gazebo/server/World.cc =================================================================== --- code/branches/federation/gazebo/server/World.cc 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/World.cc 2009-03-10 09:37:04 UTC (rev 7443) @@ -28,6 +28,7 @@ #include <sstream> #include <fstream> +#include "GraphicsIfaceHandler.hh" #include "Global.hh" #include "GazeboError.hh" #include "GazeboMessage.hh" @@ -60,6 +61,7 @@ this->showPhysics = false; this->physicsEngine = NULL; this->server = NULL; + this->graphics = NULL; } //////////////////////////////////////////////////////////////////////////////// @@ -74,7 +76,6 @@ void World::Close() { std::vector< Model* >::iterator miter; - for (miter = this->models.begin(); miter != this->models.end(); miter++) { if (*miter) @@ -97,13 +98,13 @@ { gzthrow(e); } + } //////////////////////////////////////////////////////////////////////////////// // Load the world void World::Load(XMLConfigNode *rootNode, unsigned int serverId) { - // Create the server object (needs to be done before models initialize) this->server = new Server(); @@ -116,7 +117,6 @@ gzthrow (err); } - // Create the simulator interface try { @@ -128,6 +128,10 @@ gzthrow(err); } + // Create the graphics iface handler + this->graphics = new GraphicsIfaceHandler(); + this->graphics->Load("default"); + #ifdef HAVE_OPENAL // Load OpenAL audio if (rootNode->GetChild("openal","audio")) @@ -145,6 +149,7 @@ }*/ this->physicsEngine->Load(rootNode); + } //////////////////////////////////////////////////////////////////////////////// @@ -191,6 +196,8 @@ this->toAddModels.clear(); this->toDeleteModels.clear(); + this->graphics->Init(); + return 0; } @@ -236,6 +243,8 @@ this->physicsEngine->Update(); } + this->graphics->Update(); + return 0; } @@ -251,6 +260,9 @@ { std::vector< Model* >::iterator miter; + if (this->graphics) + delete this->graphics; + // Finalize the models for (miter=this->models.begin(); miter!=this->models.end(); miter++) { @@ -324,6 +336,7 @@ return -1; } + return 0; } @@ -707,7 +720,6 @@ case SimulationRequestData::GO: { - this->simPauseTime = Simulator::Instance()->GetSimTime() + req->runTime * 10e-6; Modified: code/branches/federation/gazebo/server/World.hh =================================================================== --- code/branches/federation/gazebo/server/World.hh 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/World.hh 2009-03-10 09:37:04 UTC (rev 7443) @@ -46,6 +46,7 @@ class Geom; class PhysicsEngine; class XMLConfigNode; + class GraphicsIfaceHandler; /// \brief The World /* @@ -189,11 +190,11 @@ /// Simulation interface private: SimulationIface *simIface; + private: GraphicsIfaceHandler *graphics; /// 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/controllers/SConscript =================================================================== --- code/branches/federation/gazebo/server/controllers/SConscript 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/controllers/SConscript 2009-03-10 09:37:04 UTC (rev 7443) @@ -3,7 +3,18 @@ env.Append(CPPPATH = '#server/controllers') -dirs = Split('position2d laser camera factory gripper actarray ptz opaque bumper imu irarray') +dirs = ['position2d', + 'laser', + 'camera', + 'factory', + 'gripper', + 'actarray', + 'ptz', + 'opaque', + 'bumper', + 'imu', + 'irarray' + ] try: env["CCFLAGS"].index('-DHAVE_OPENAL') Modified: code/branches/federation/gazebo/server/controllers/factory/Factory.hh =================================================================== --- code/branches/federation/gazebo/server/controllers/factory/Factory.hh 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/controllers/factory/Factory.hh 2009-03-10 09:37:04 UTC (rev 7443) @@ -24,8 +24,8 @@ * Date: 29 July 2007 * SVN: $Id$ */ -#ifndef FACTORY_POSITION2D_HH -#define FACTORY_POSITION2D_HH +#ifndef FACTORY_HH +#define FACTORY_HH #include "Controller.hh" #include "Entity.hh" Modified: code/branches/federation/gazebo/server/controllers/position2d/differential/Differential_Position2d.cc =================================================================== --- code/branches/federation/gazebo/server/controllers/position2d/differential/Differential_Position2d.cc 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/controllers/position2d/differential/Differential_Position2d.cc 2009-03-10 09:37:04 UTC (rev 7443) @@ -197,6 +197,7 @@ this->joints[LEFT]->SetParam( dParamFMax, **(this->torqueP) ); this->joints[RIGHT]->SetParam( dParamFMax, **(this->torqueP) ); + //printf("Set Speed[%f %f]\n",this->wheelSpeed[LEFT], this->wheelSpeed[RIGHT]); } /*else { Modified: code/branches/federation/gazebo/server/main.cc =================================================================== --- code/branches/federation/gazebo/server/main.cc 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/main.cc 2009-03-10 09:37:04 UTC (rev 7443) @@ -277,7 +277,7 @@ } catch (gazebo::GazeboError e) { - std::cerr << "Loading Gazebo" << std::endl; + std::cerr << "Erro Loading Gazebo" << std::endl; std::cerr << e << std::endl; return -1; } Modified: code/branches/federation/gazebo/server/physics/Body.cc =================================================================== --- code/branches/federation/gazebo/server/physics/Body.cc 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/physics/Body.cc 2009-03-10 09:37:04 UTC (rev 7443) @@ -271,12 +271,12 @@ this->UpdatePose(); - if (!this->IsStatic()) +// if (!this->IsStatic()) { - // Set the pose of the scene node - this->visualNode->SetPose(this->pose); + this->visualNode->SetPose(this->GetPose() - this->parent->GetPose()); } + for (geomIter=this->geoms.begin(); geomIter!=this->geoms.end(); geomIter++) { @@ -327,16 +327,17 @@ //////////////////////////////////////////////////////////////////////////////// // Set the pose of the body -void Body::SetPose(const Pose3d &pose) +void Body::SetPose(const Pose3d &_pose) { - this->pose = pose; if (this->IsStatic()) { Pose3d oldPose = this->staticPose; Pose3d newPose; - this->staticPose = pose; + this->staticPose = _pose; + this->pose = this->staticPose; + std::map<std::string, Geom*>::iterator iter; // This loop doesn't work properly when rotating objects @@ -352,8 +353,9 @@ Pose3d localPose; // Compute pose of CoM - localPose = this->comPose + this->pose; + localPose = this->comPose + _pose; + this->pose = localPose; this->SetPosition(localPose.pos); this->SetRotation(localPose.rot); } @@ -363,9 +365,10 @@ // Return the pose of the body Pose3d Body::GetPose() const { - if (this->IsStatic()) + /*if (this->IsStatic()) return this->staticPose; else + */ return this->pose; } @@ -386,8 +389,9 @@ if (this->bodyId) dBodySetPosition(this->bodyId, pos.x, pos.y, pos.z); + // NATE // Set the position of the scene node - this->visualNode->SetPosition(pos); + //this->visualNode->SetPosition(pos); } //////////////////////////////////////////////////////////////////////////////// @@ -407,8 +411,9 @@ dBodySetQuaternion(this->bodyId, q); } + // NATE // Set the orientation of the scene node - this->visualNode->SetRotation(rot); + //this->visualNode->SetRotation(rot); } //////////////////////////////////////////////////////////////////////////////// Modified: code/branches/federation/gazebo/server/physics/Body.hh =================================================================== --- code/branches/federation/gazebo/server/physics/Body.hh 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/physics/Body.hh 2009-03-10 09:37:04 UTC (rev 7443) @@ -83,7 +83,7 @@ /// \brief Return the pose of the body /// \return Pose of the body - public: Pose3d GetPose() const; + public: virtual Pose3d GetPose() const; /// \brief Set the position of the body /// \param pos Vector position Modified: code/branches/federation/gazebo/server/physics/ContactParams.cc =================================================================== --- code/branches/federation/gazebo/server/physics/ContactParams.cc 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/physics/ContactParams.cc 2009-03-10 09:37:04 UTC (rev 7443) @@ -45,8 +45,6 @@ this->mu2 = dInfinity; this->slip1 = 0.1; this->slip2 = 0.1; - - this->enabled = true; } ////////////////////////////////////////////////////////////////////////////// Modified: code/branches/federation/gazebo/server/physics/ContactParams.hh =================================================================== --- code/branches/federation/gazebo/server/physics/ContactParams.hh 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/physics/ContactParams.hh 2009-03-10 09:37:04 UTC (rev 7443) @@ -79,8 +79,6 @@ /// \brief soft constraint force mixing public: double softCfm; - public: bool enabled; - public: boost::signal< void (Geom*, Geom*)> contactSignal; }; } Modified: code/branches/federation/gazebo/server/physics/Geom.cc =================================================================== --- code/branches/federation/gazebo/server/physics/Geom.cc 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/physics/Geom.cc 2009-03-10 09:37:04 UTC (rev 7443) @@ -26,6 +26,7 @@ #include <sstream> +#include "OgreVisualManager.hh" #include "OgreVisual.hh" #include "Global.hh" #include "GazeboMessage.hh" @@ -46,7 +47,7 @@ this->typeName = "unknown"; this->body = body; - this->spaceId = this->body->spaceId; + this->SetSpaceId(this->body->GetSpaceId()); // Create the contact parameters this->contact = new ContactParams(); @@ -89,11 +90,12 @@ if (this->transId) dGeomDestroy(this->transId); - for (iter = this->visuals.begin(); iter != this->visuals.end(); iter++) + /*for (iter = this->visuals.begin(); iter != this->visuals.end(); iter++) { GZ_DELETE (*iter) } this->visuals.clear(); + */ delete this->massP; delete this->xyzP; @@ -141,7 +143,12 @@ childNode = node->GetChild("visual"); while (childNode) { - OgreVisual *visual = new OgreVisual(this->visualNode, this); + std::ostringstream visname; + visname << this->GetName() << "_VISUAL_" << this->visuals.size(); + + OgreVisual *visual = OgreVisualManager::Instance()->CreateVisual( + visname.str(), this->visualNode, this); + visual->Load(childNode); this->visuals.push_back(visual); childNode = childNode->GetNext("visual"); @@ -161,7 +168,11 @@ Vector3 min(aabb[0], aabb[2], aabb[4]); Vector3 max(aabb[1], aabb[3], aabb[5]); - this->bbVisual = new OgreVisual(this->visualNode); + std::ostringstream visname; + visname << this->GetName() << "_BBVISUAL" ; + + this->bbVisual = OgreVisualManager::Instance()->CreateVisual( + visname.str(), this->visualNode); this->bbVisual->AttachBoundingBox(min,max); } @@ -258,8 +269,9 @@ // Update void Geom::Update() { + + this->visualNode->SetPose(this->GetPose()); this->UpdateChild(); - } //////////////////////////////////////////////////////////////////////////////// @@ -315,7 +327,8 @@ dGeomSetPosition(this->geomId, localPose.pos.x, localPose.pos.y, localPose.pos.z); dGeomSetQuaternion(this->geomId, q); - this->visualNode->SetPose(newPose); + // NATE + //this->visualNode->SetPose(this->GetPoseRelative()); if (updateCoM) { @@ -353,9 +366,11 @@ // Transform into body relative pose pose += this->body->GetCoMPose(); } - else + /*else pose = this->body->GetPose(); + */ + return pose; } @@ -579,9 +594,20 @@ /// Set the friction mode of the geom void Geom::SetFrictionMode( const bool &v ) { - this->contact->mu1 = 0; - this->contact->mu2 = 0; - this->contact->slip1 = 0; - this->contact->slip2 = 0; + if (v == false) + { + this->contact->mu1 = 0; + this->contact->mu2 = 0; + this->contact->slip1 = 0; + this->contact->slip2 = 0; + } + else + { + this->contact->mu1 = dInfinity; + this->contact->mu2 = dInfinity; + this->contact->slip1 = 0.1; + this->contact->slip2 = 0.1; + + } } Modified: code/branches/federation/gazebo/server/physics/Geom.hh =================================================================== --- code/branches/federation/gazebo/server/physics/Geom.hh 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/physics/Geom.hh 2009-03-10 09:37:04 UTC (rev 7443) @@ -99,7 +99,7 @@ public: void SetPose(const Pose3d &pose, bool updateCoM=true); /// \brief Return the pose of the geom - public: Pose3d GetPose() const; + public: virtual Pose3d GetPose() const; /// \brief Set the position /// \param pos Vector3 position @@ -164,7 +164,6 @@ /// Contact parameters public: ContactParams *contact; - /// The body this geom belongs to protected: Body *body; @@ -184,7 +183,7 @@ /// mass of the body protected: dMass bodyMass; - + private: ParamT<int> *laserFiducialIdP; private: ParamT<float> *laserRetroP; Modified: code/branches/federation/gazebo/server/physics/Joint.cc =================================================================== --- code/branches/federation/gazebo/server/physics/Joint.cc 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/physics/Joint.cc 2009-03-10 09:37:04 UTC (rev 7443) @@ -24,6 +24,7 @@ * CVS: $Id$ */ #include "OgreVisual.hh" +#include "OgreVisualManager.hh" #include "OgreDynamicLines.hh" #include "Global.hh" #include "Body.hh" @@ -115,8 +116,12 @@ this->Attach(body1,body2); + std::ostringstream visname; + visname << this->GetName() << "_VISUAL"; + /// Add a renderable for the joint - this->visual = new OgreVisual(this->model->GetVisualNode()); + this->visual = OgreVisualManager::Instance()->CreateVisual( + visname.str(), this->model->GetVisualNode()); this->visual->AttachMesh("joint_anchor"); this->visual->SetVisible(false); Modified: code/branches/federation/gazebo/server/physics/ode/ODEPhysics.cc =================================================================== --- code/branches/federation/gazebo/server/physics/ode/ODEPhysics.cc 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/physics/ode/ODEPhysics.cc 2009-03-10 09:37:04 UTC (rev 7443) @@ -158,12 +158,11 @@ // Only the top level parent should have a new space if (entity->GetParent() == NULL) { - //entity->spaceId = this->spaceId; - entity->spaceId = dSimpleSpaceCreate(this->spaceId); + entity->SetSpaceId( dSimpleSpaceCreate(this->spaceId) ); } else { - entity->spaceId = entity->GetParent()->spaceId; + entity->SetSpaceId( entity->GetParent()->GetSpaceId() ) ; } } Modified: code/branches/federation/gazebo/server/rendering/OgreAdaptor.cc =================================================================== --- code/branches/federation/gazebo/server/rendering/OgreAdaptor.cc 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/rendering/OgreAdaptor.cc 2009-03-10 09:37:04 UTC (rev 7443) @@ -489,9 +489,9 @@ { OgreVisual *vis = dynamic_cast<OgreVisual*>(iter->movable->getUserObject()); - if (vis && vis->GetEntity()) + if (vis && vis->GetOwner()) { - entity = vis->GetEntity(); + entity = vis->GetOwner(); entity->GetVisualNode()->ShowSelectionBox(true); Model *model = NULL; Modified: code/branches/federation/gazebo/server/rendering/OgreCreator.cc =================================================================== --- code/branches/federation/gazebo/server/rendering/OgreCreator.cc 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/rendering/OgreCreator.cc 2009-03-10 09:37:04 UTC (rev 7443) @@ -611,3 +611,41 @@ return window; } + +//////////////////////////////////////////////////////////////////////////////// +// Create a material from a color definition +std::string OgreCreator::CreateMaterial(float r, float g, float b, float a) +{ + std::ostringstream matNameStream; + + matNameStream << "Color[" << r << "," << g << "," << b << "," << a << "]"; + + if (!Ogre::MaterialManager::getSingleton().resourceExists(matNameStream.str())) + { + Ogre::MaterialPtr matPtr = Ogre::MaterialManager::getSingleton().create( + matNameStream.str(),"General"); + + matPtr->getTechnique(0)->setLightingEnabled(true); + matPtr->getTechnique(0)->getPass(0)->setDiffuse(r,g,b,a); + matPtr->getTechnique(0)->getPass(0)->setAmbient(r,g,b); + matPtr->getTechnique(0)->getPass(0)->setSelfIllumination(r,g,b); + // matPtr->setReceiveShadows(false); + } + + return matNameStream.str(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Create a material from a texture file +std::string OgreCreator::CreateMaterialFromTexFile(const std::string &filename) +{ + if (!Ogre::MaterialManager::getSingleton().resourceExists(filename)) + { + Ogre::MaterialPtr matPtr = Ogre::MaterialManager::getSingleton().create( + filename, "General"); + matPtr->setLightingEnabled(true); + matPtr->getTechnique(0)->getPass(0)->createTextureUnitState(filename); + } + + return filename; +} Modified: code/branches/federation/gazebo/server/rendering/OgreCreator.hh =================================================================== --- code/branches/federation/gazebo/server/rendering/OgreCreator.hh 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/rendering/OgreCreator.hh 2009-03-10 09:37:04 UTC (rev 7443) @@ -116,6 +116,14 @@ /// \brief Remove a mesh by name public: static void RemoveMesh(const std::string &name); + /// \brief Create a material from a color definition + /// \return The name of the material + public: static std::string CreateMaterial(float r, float g, + float b, float a); + + /// \brief Create a material from a texture file + public: static std::string CreateMaterialFromTexFile(const std::string &filename); + private: static unsigned int lightCounter; private: static unsigned int windowCounter; Modified: code/branches/federation/gazebo/server/rendering/OgreVisual.cc =================================================================== --- code/branches/federation/gazebo/server/rendering/OgreVisual.cc 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/rendering/OgreVisual.cc 2009-03-10 09:37:04 UTC (rev 7443) @@ -39,7 +39,7 @@ //////////////////////////////////////////////////////////////////////////////// // Constructor -OgreVisual::OgreVisual(OgreVisual *node, Entity *owner) +OgreVisual::OgreVisual(OgreVisual *node, Entity *_owner) : Common() { std::ostringstream stream; @@ -58,7 +58,7 @@ // Create the scene node this->sceneNode = this->parentNode->createChildSceneNode( stream.str() ); - this->entity = owner; + this->owner = _owner; Param::Begin(&this->parameters); this->xyzP = new ParamT<Vector3>("xyz", Vector3(0,0,0), 0); @@ -239,6 +239,27 @@ } //////////////////////////////////////////////////////////////////////////////// +/// Detach all objects +void OgreVisual::DetachObjects() +{ + this->sceneNode->detachAllObjects(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Get the number of attached objects +unsigned short OgreVisual::GetNumAttached() +{ + return this->sceneNode->numAttachedObjects(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Get an attached object +Ogre::MovableObject *OgreVisual::GetAttached(unsigned short num) +{ + return this->sceneNode->getAttachedObject(num); +} + +//////////////////////////////////////////////////////////////////////////////// // Attach a static object void OgreVisual::MakeStatic() { @@ -360,6 +381,9 @@ } } + + + //////////////////////////////////////////////////////////////////////////////// /// Set the transparency void OgreVisual::SetTransparency( float trans ) @@ -595,9 +619,9 @@ //////////////////////////////////////////////////////////////////////////////// /// Get the entity that manages this visual -Entity *OgreVisual::GetEntity() const +Entity *OgreVisual::GetOwner() const { - return this->entity; + return this->owner; } //////////////////////////////////////////////////////////////////////////////// Modified: code/branches/federation/gazebo/server/rendering/OgreVisual.hh =================================================================== --- code/branches/federation/gazebo/server/rendering/OgreVisual.hh 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/rendering/OgreVisual.hh 2009-03-10 09:37:04 UTC (rev 7443) @@ -57,6 +57,15 @@ /// \brief Attach a renerable object to the visual public: void AttachObject( Ogre::MovableObject *obj); + /// \brief Detach all objects + public: void DetachObjects(); + + /// \brief Get the number of attached objects + public: unsigned short GetNumAttached(); + + /// \brief Get an attached object + public: Ogre::MovableObject *GetAttached(unsigned short num); + /// \brief Attach a mesh to this visual by name public: void AttachMesh( const std::string &meshName ); @@ -118,7 +127,7 @@ public: void MakeStatic(); /// \brief Get the entity that manages this visual - public: Entity *GetEntity() const; + public: Entity *GetOwner() const; /// \brief Set to true to show a white bounding box, used to indicate // user selection @@ -138,7 +147,7 @@ private: static unsigned int visualCounter; - private: Entity *entity; + private: Entity *owner; private: ParamT<Vector3> *xyzP; private: ParamT<Quatern> *rpyP; Modified: code/branches/federation/gazebo/server/rendering/SConscript =================================================================== --- code/branches/federation/gazebo/server/rendering/SConscript 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/rendering/SConscript 2009-03-10 09:37:04 UTC (rev 7443) @@ -13,6 +13,7 @@ 'OgreSimpleShape.cc', 'OgreHUD.cc', 'OgreVisual.cc', + 'OgreVisualManager.cc', 'OgreCamera.cc', 'CameraManager.cc', 'UserCamera.cc' @@ -28,6 +29,7 @@ '#/server/rendering/OgreHUD.hh', '#/server/rendering/OgreSimpleShape.hh', '#/server/rendering/OgreVisual.hh', + '#/server/rendering/OgreVisualManager.hh', '#/server/rendering/OgreCamera.hh', '#/server/rendering/CameraManager.hh', '#/server/rendering/UserCamera.hh' Modified: code/branches/federation/gazebo/server/sensors/Sensor.cc =================================================================== --- code/branches/federation/gazebo/server/sensors/Sensor.cc 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/sensors/Sensor.cc 2009-03-10 09:37:04 UTC (rev 7443) @@ -190,7 +190,7 @@ //////////////////////////////////////////////////////////////////////////////// /// Get the current pose -const Pose3d Sensor::GetPose() const +Pose3d Sensor::GetPose() const { return this->body->GetPose(); } Modified: code/branches/federation/gazebo/server/sensors/Sensor.hh =================================================================== --- code/branches/federation/gazebo/server/sensors/Sensor.hh 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/sensors/Sensor.hh 2009-03-10 09:37:04 UTC (rev 7443) @@ -70,7 +70,7 @@ public: void Fini(); /// \brief Get the current pose - public: const Pose3d GetPose() const; + public: virtual Pose3d GetPose() const; /// \brief Set whether the sensor is active or not public: void SetActive(bool value); Modified: code/branches/federation/gazebo/server/sensors/ir/IRSensor.cc =================================================================== --- code/branches/federation/gazebo/server/sensors/ir/IRSensor.cc 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/sensors/ir/IRSensor.cc 2009-03-10 09:37:04 UTC (rev 7443) @@ -129,9 +129,7 @@ dGeomSetCategoryBits((dGeomID) this->raySpaceId, GZ_SENSOR_COLLIDE); dGeomSetCollideBits((dGeomID) this->raySpaceId, ~GZ_SENSOR_COLLIDE); - //this->body->spaceId = this->superSpaceId; - this->body->spaceId = this->raySpaceId; - + this->body->SetSpaceId( this->raySpaceId ); } ////////////////////////////////////////////////////////////////////////////// Modified: code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc =================================================================== --- code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc 2009-03-10 09:37:04 UTC (rev 7443) @@ -111,9 +111,7 @@ dGeomSetCategoryBits((dGeomID) this->raySpaceId, GZ_SENSOR_COLLIDE); dGeomSetCollideBits((dGeomID) this->raySpaceId, ~GZ_SENSOR_COLLIDE); - //this->body->spaceId = this->superSpaceId; - this->body->spaceId = this->raySpaceId; - + this->body->SetSpaceId( this->raySpaceId ); } ////////////////////////////////////////////////////////////////////////////// Modified: code/branches/federation/gazebo/worlds/pioneer2dx.world =================================================================== --- code/branches/federation/gazebo/worlds/pioneer2dx.world 2009-03-10 09:07:34 UTC (rev 7442) +++ code/branches/federation/gazebo/worlds/pioneer2dx.world 2009-03-10 09:37:04 UTC (rev 7443) @@ -96,9 +96,7 @@ <model:physical name="pioneer2dx_model1"> <xyz>0 0 .145</xyz> <rpy>0.0 0.0 90.0</rpy> - <enableGravity>false</enableGravity> - <enableFriction>false</enableFriction> - <!--<collide>ghost</collide>--> + <collide>all</collide> <controller:differential_position2d name="controller1"> <leftJoint>left_wheel_hinge</leftJoint> @@ -109,10 +107,8 @@ <interface:position name="position_iface_0"/> </controller:differential_position2d> - <model:physical name="laser"> + <!--<model:physical name="laser"> <xyz>0.15 0 0.18</xyz> - <enableGravity>false</enableGravity> - <!-- <collide>ghost</collide>--> <attach> <parentBody>chassis_body</parentBody> @@ -123,32 +119,13 @@ <xi:include href="models/sicklms200.model" /> </include> </model:physical> + --> <include embedded="true"> <xi:include href="models/pioneer2dx.model" /> </include> </model:physical> - <model:physical name="box1_model"> - <xyz>0 0.5 0.5</xyz> - <canonicalBody>box1_body</canonicalBody> - <static>false</static> - - <body:box name="box1_body"> - - <geom:box name="box1_geom"> - <size>1 .1 1</size> - <mass>0.1</mass> - <visual> - <size>1 .1 1</size> - <mesh>unit_box</mesh> - <material>Gazebo/Rocky</material> - </visual> - </geom:box> - </body:box> - </model:physical> - - <!-- White Point light --> <model:renderable name="point_white"> <xyz>0 2 2</xyz> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-03-10 09:40:30
|
Revision: 7446 http://playerstage.svn.sourceforge.net/playerstage/?rev=7446&view=rev Author: natepak Date: 2009-03-10 09:40:23 +0000 (Tue, 10 Mar 2009) Log Message: ----------- Added missing files Added Paths: ----------- code/branches/federation/gazebo/Media/materials/textures/heartpsg.png code/branches/federation/gazebo/libgazebo/Graphics3dIface.cc Added: code/branches/federation/gazebo/Media/materials/textures/heartpsg.png =================================================================== (Binary files differ) Property changes on: code/branches/federation/gazebo/Media/materials/textures/heartpsg.png ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/branches/federation/gazebo/libgazebo/Graphics3dIface.cc =================================================================== --- code/branches/federation/gazebo/libgazebo/Graphics3dIface.cc (rev 0) +++ code/branches/federation/gazebo/libgazebo/Graphics3dIface.cc 2009-03-10 09:40:23 UTC (rev 7446) @@ -0,0 +1,130 @@ +#include "gazebo.h" + +using namespace gazebo; + +//////////////////////////////////////////////////////////////////////////////// +/// Constructor +Graphics3dIface::Graphics3dIface() + :Iface("graphics3d", sizeof(Graphics3dIface)+sizeof(Graphics3dData)) +{ +} + +//////////////////////////////////////////////////////////////////////////////// +/// Destructor +Graphics3dIface::~Graphics3dIface() +{ + this->data = NULL; +} + +//////////////////////////////////////////////////////////////////////////////// +/// Create the interface (used by Gazebo server) +void Graphics3dIface::Create(Server *server, std::string id) +{ + Iface::Create(server,id); + this->data = (Graphics3dData*)this->mMap; +} + +//////////////////////////////////////////////////////////////////////////////// +/// Open an existing interface +void Graphics3dIface::Open(Client *client, std::string id) +{ + Iface::Open(client,id); + this->data = (Graphics3dData*)this->mMap; +} + +//////////////////////////////////////////////////////////////////////////////// +/// Draw a simple object, that is defined by a series of points +void Graphics3dIface::DrawSimple(const char *name, + Graphics3dDrawData::DrawMode mode, + Vec3 *points, unsigned int numPoints, + Color clr ) +{ + this->Lock(1); + Graphics3dDrawData *cmd = &(this->data->commands[this->data->cmdCount++]); + + cmd->pointCount = numPoints; + + cmd->drawMode = mode; + + // Set the name of the graphics drawable + memset( cmd->name, 0, GAZEBO_GRAPHICS3D_MAX_NAME); + strcpy( cmd->name, name); + + memcpy(cmd->points, points, numPoints * sizeof(Vec3)); + + cmd->color.r = clr.r; + cmd->color.g = clr.g; + cmd->color.b = clr.b; + cmd->color.a = clr.a; + + this->Unlock(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Draw a shape +void Graphics3dIface::DrawShape(const char *name, + Graphics3dDrawData::DrawMode mode, + Vec3 pos, Vec3 size, Color clr) +{ + this->Lock(1); + Graphics3dDrawData *cmd = &(this->data->commands[this->data->cmdCount++]); + + if (mode != Graphics3dDrawData::SPHERE && + mode != Graphics3dDrawData::CUBE && + mode != Graphics3dDrawData::CYLINDER) + { + std::cerr << "Invalid shape draw mode[" << mode << "]\n"; + return; + } + + cmd->drawMode = mode; + + // Set the name of the graphics drawable + memset( cmd->name, 0, GAZEBO_GRAPHICS3D_MAX_NAME); + strcpy( cmd->name, name); + + cmd->pose.pos.x = pos.x; + cmd->pose.pos.y = pos.y; + cmd->pose.pos.z = pos.z; + + cmd->size.x = size.x; + cmd->size.y = size.y; + cmd->size.z = size.z; + + + cmd->color.r = clr.r; + cmd->color.g = clr.g; + cmd->color.b = clr.b; + cmd->color.a = clr.a; + + this->Unlock(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Draw a billboard +void Graphics3dIface::DrawBillboard(const char *name, const char *texture, + Vec3 pos, Vec2 size) +{ + this->Lock(1); + Graphics3dDrawData *cmd = &(this->data->commands[this->data->cmdCount++]); + + cmd->drawMode = Graphics3dDrawData::BILLBOARD; + + // Set the name of the graphics drawable + memset( cmd->name, 0, GAZEBO_GRAPHICS3D_MAX_NAME); + strcpy( cmd->name, name); + + // Set the material to draw on the billboard + memset( cmd->billboardTexture, 0, GAZEBO_GRAPHICS3D_MAX_NAME); + strcpy( cmd->billboardTexture, texture); + + cmd->pose.pos.x = pos.x; + cmd->pose.pos.y = pos.y; + cmd->pose.pos.z = pos.z; + + cmd->size.x = size.x; + cmd->size.y = size.y; + cmd->size.z = 0; + + this->Unlock(); +} This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-03-10 17:41:53
|
Revision: 7448 http://playerstage.svn.sourceforge.net/playerstage/?rev=7448&view=rev Author: natepak Date: 2009-03-10 17:41:45 +0000 (Tue, 10 Mar 2009) Log Message: ----------- Updated the graphics3d example. Added drawing of text Modified Paths: -------------- code/branches/federation/gazebo/SConstruct code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc code/branches/federation/gazebo/examples/libgazebo/position/position.cc code/branches/federation/gazebo/libgazebo/Graphics3dIface.cc code/branches/federation/gazebo/libgazebo/gazebo.h code/branches/federation/gazebo/server/GraphicsIfaceHandler.cc code/branches/federation/gazebo/server/GraphicsIfaceHandler.hh code/branches/federation/gazebo/server/rendering/OgreCreator.cc Modified: code/branches/federation/gazebo/SConstruct =================================================================== --- code/branches/federation/gazebo/SConstruct 2009-03-10 09:40:49 UTC (rev 7447) +++ code/branches/federation/gazebo/SConstruct 2009-03-10 17:41:45 UTC (rev 7448) @@ -144,8 +144,8 @@ ####### subdirs = ['libgazebo','server', 'player'] # Need test for <event.h> -if True: - subdirs.append('webgazebo') +#if True: +# subdirs.append('webgazebo') ####### # Generate help text Modified: code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc =================================================================== --- code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc 2009-03-10 09:40:49 UTC (rev 7447) +++ code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc 2009-03-10 17:41:45 UTC (rev 7448) @@ -1,13 +1,184 @@ #include <string.h> +#include <stdio.h> #include <string> #include <iostream> +#include <vector> +#include <math.h> + #include <gazebo/gazebo.h> +// All the interfaces +gazebo::Client *client; +gazebo::SimulationIface *simIface; +gazebo::PositionIface *posIface; +gazebo::Graphics3dIface *g3dIface; +gazebo::Graphics3dIface *pioneerG3DIface; + +// Stuff to draw the square +std::string squareName = "square"; +gazebo::Vec3 squareSize; +int dir = 1; + +// Stuff to draw the sphere +std::string sphereName = "sphere"; +gazebo::Vec3 spherePos, sphereSize; +float radius = 0.2; +float theta = 0; + +// Stuff to draw the robot's path +std::string pathName = "path"; +std::vector<gazebo::Vec3> positions; + +// Stuff to draw the text +std::string textName = "velocities"; + +void UpdateSquare() +{ + gazebo::Color clr; + gazebo::Vec3 vec[5]; + + vec[0].x = squareSize.x; + vec[0].y = squareSize.y; + vec[0].z = squareSize.z; + + vec[1].x = -squareSize.x; + vec[1].y = squareSize.y; + vec[1].z = squareSize.z; + + vec[2].x = -squareSize.x; + vec[2].y = -squareSize.y; + vec[2].z = squareSize.z; + + vec[3].x = squareSize.x; + vec[3].y = -squareSize.y; + vec[3].z = squareSize.z; + + vec[4].x = squareSize.x; + vec[4].y = squareSize.y; + vec[4].z = squareSize.z; + + squareSize.z += dir*0.01; + if (squareSize.z >= 0.2 || squareSize.z < 0) + dir *= -1; + + clr.r = 1.0; + clr.g = 0.0; + clr.b = 0.0; + clr.a = 1.0; + + // Draw the bouncing square + pioneerG3DIface->DrawSimple(sphereName.c_str(), + gazebo::Graphics3dDrawData::LINE_STRIP, vec, 5, clr); + +} + +void UpdateSphere() +{ + gazebo::Color clr; + + clr.r = 1.0; + clr.g = 0.0; + clr.b = 0.0; + clr.a = 1.0; + + spherePos.x = radius * cos(theta); + spherePos.y = radius * sin(theta); + spherePos.z = 0.8; + + pioneerG3DIface->DrawShape("mysphere", + gazebo::Graphics3dDrawData::SPHERE, spherePos, sphereSize, clr); + + theta += 0.1; + +} + +void UpdatePath() +{ + gazebo::Vec3 rPos; + gazebo::Vec3 blockSize; + gazebo::Color clr; + std::ostringstream blockName; + + blockSize.x = 0.05; + blockSize.y = 0.05; + blockSize.z = 0.05; + + // Get the simulation pose of the robot + simIface->Lock(1); + simIface->data->requestCount = 0; + simIface->Unlock(); + simIface->GetPose2d("pioneer2dx_model1"); + while (simIface->data->requestCount == 0) + usleep(1000); + + rPos.x = simIface->data->responses[0].modelPose.pos.x; + rPos.y = simIface->data->responses[0].modelPose.pos.y; + rPos.z = 0.15; + + // Draw the robot's path + if (positions.size() == 0 || + sqrt( pow(positions[positions.size()-1].x - rPos.x,2) + + pow(positions[positions.size()-1].y - rPos.y,2)) > 0.5) + { + + // Store the new position + positions.push_back(rPos); + + gazebo::Vec3 tmpvec[positions.size()]; + for (unsigned int j =0; j < positions.size(); j++) + { + tmpvec[j].x = positions[j].x; + tmpvec[j].y = positions[j].y; + tmpvec[j].z = positions[j].z; + } + + clr.r = 0.0; + clr.g = 1.0; + clr.b = 0.0; + + // Draw the line + g3dIface->DrawSimple(pathName.c_str(), + gazebo::Graphics3dDrawData::LINE_STRIP, + tmpvec, positions.size(), clr); + + blockName << "path_block:" << positions.size(); + + clr.r = 1.0; + clr.g = 1.0; + clr.b = 0.0; + clr.a = 1.0; + + g3dIface->DrawShape(blockName.str().c_str(), + gazebo::Graphics3dDrawData::CUBE, tmpvec[positions.size()-1], + blockSize, clr); + } +} + +void UpdateText() +{ + gazebo::Vec3 pos; + char vel[50]; + float fontSize = 0.1; + + pos.x = 0; + pos.y = 0; + pos.z = 0.2; + + sprintf(vel,"Linear %4.2f Angular %4.2f", + posIface->data->velocity.pos.x, + posIface->data->velocity.yaw); + + // Draw some text on the robot + pioneerG3DIface->DrawText(textName.c_str(), vel, pos, fontSize); +} + int main() { - gazebo::Client *client = new gazebo::Client(); - gazebo::Graphics3dIface *g3dIface = new gazebo::Graphics3dIface(); - gazebo::Graphics3dIface *pioneerG3DIface = new gazebo::Graphics3dIface(); + client = new gazebo::Client(); + simIface = new gazebo::SimulationIface(); + posIface = new gazebo::PositionIface(); + g3dIface = new gazebo::Graphics3dIface(); + pioneerG3DIface = new gazebo::Graphics3dIface(); int serverId = 0; @@ -25,86 +196,54 @@ /// Open the global graphics Interface try { + simIface->Open(client, "default"); + g3dIface->Open(client, "default"); + posIface->Open(client, "position_iface_0"); pioneerG3DIface->Open(client,"pioneer2dx_model1"); } catch (std::string e) { - std::cerr << "Gazebo error: Unable to connect to the graphics3d interface\n" << e << "\n"; + std::cerr << "Gazebo error: Unable to connect to an interface\n" + << e << "\n"; return -1; } - float radius = 0.2; - float theta = 0; + // Set the size of the square + squareSize.x = 0.25; + squareSize.y = 0.25; + squareSize.z = 0.0; - float size = 0.4; - float h = 0; - int dir = 1; - std::string name = "square"; - int count = 0; - - gazebo::Vec3 spherePos, sphereSize; + // Set the size of the sphere sphereSize.x = 0.1; sphereSize.y = 0.1; sphereSize.z = 0.1; - gazebo::Vec3 vec[5]; - gazebo::Color clr; + // Draw a billboard + pioneerG3DIface->DrawBillboard("mybillboard", "heartpsg.png", + gazebo::Vec3(0.4,0.0,0.1), gazebo::Vec2(0.2, 0.2) ); + + // Update all the drawables while (true) { - vec[0].x = size; - vec[0].y = size; - vec[0].z = h; + UpdateSquare(); + UpdateSphere(); + UpdatePath(); + UpdateText(); - vec[1].x = -size; - vec[1].y = size; - vec[1].z = h; - - vec[2].x = -size; - vec[2].y = -size; - vec[2].z = h; - - vec[3].x = size; - vec[3].y = -size; - vec[3].z = h; - - vec[4].x = size; - vec[4].y = size; - vec[4].z = h; - - clr.r = 1.0; - clr.g = 0.0; - clr.b = 0.0; - clr.a = 1.0; - - spherePos.x = radius * cos(theta); - spherePos.y = radius * sin(theta); - spherePos.z = 0.8; - - // Draw the bouncing square - pioneerG3DIface->DrawSimple(name.c_str(), - gazebo::Graphics3dDrawData::LINE_STRIP, vec, 5, clr); - - // Draw the sliding billboard - pioneerG3DIface->DrawBillboard("mybillboard", "heartpsg.png", - gazebo::Vec3(h,0,0.4), gazebo::Vec2(0.2, 0.2) ); - - - pioneerG3DIface->DrawShape("mysphere", - gazebo::Graphics3dDrawData::SPHERE, spherePos, sphereSize, clr); - - h += dir*0.01; - if (h >= 0.2 || h < 0) - dir *= -1; - - theta += 0.1; usleep(10000); } - + simIface->Close(); + posIface->Close(); + pioneerG3DIface->Close(); g3dIface->Close(); + delete simIface; + delete posIface; + delete pioneerG3DIface; delete g3dIface; + delete client; return 0; } Modified: code/branches/federation/gazebo/examples/libgazebo/position/position.cc =================================================================== --- code/branches/federation/gazebo/examples/libgazebo/position/position.cc 2009-03-10 09:40:49 UTC (rev 7447) +++ code/branches/federation/gazebo/examples/libgazebo/position/position.cc 2009-03-10 17:41:45 UTC (rev 7448) @@ -48,6 +48,7 @@ posIface->data->cmdEnableMotors = 1; posIface->Unlock(); + posIface->data->cmdVelocity.yaw = -0.1; while (true) { posIface->Lock(1); Modified: code/branches/federation/gazebo/libgazebo/Graphics3dIface.cc =================================================================== --- code/branches/federation/gazebo/libgazebo/Graphics3dIface.cc 2009-03-10 09:40:49 UTC (rev 7447) +++ code/branches/federation/gazebo/libgazebo/Graphics3dIface.cc 2009-03-10 17:41:45 UTC (rev 7448) @@ -128,3 +128,30 @@ this->Unlock(); } + +//////////////////////////////////////////////////////////////////////////////// +/// Draw text +void Graphics3dIface::DrawText(const char *name, const char *text, Vec3 pos, + float fontSize) +{ + this->Lock(1); + Graphics3dDrawData *cmd = &(this->data->commands[this->data->cmdCount++]); + + cmd->drawMode = Graphics3dDrawData::TEXT; + + // Set the name of the graphics drawable + memset( cmd->name, 0, GAZEBO_GRAPHICS3D_MAX_NAME); + strcpy( cmd->name, name); + + // Set the text to draw + memset( cmd->text, 0, GAZEBO_GRAPHICS3D_MAX_NAME); + strcpy( cmd->text, text); + + cmd->pose.pos.x = pos.x; + cmd->pose.pos.y = pos.y; + cmd->pose.pos.z = pos.z; + + cmd->size.x = fontSize; + + this->Unlock(); +} Modified: code/branches/federation/gazebo/libgazebo/gazebo.h =================================================================== --- code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-10 09:40:49 UTC (rev 7447) +++ code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-10 17:41:45 UTC (rev 7448) @@ -807,13 +807,17 @@ /// Type of drawing to perform public: enum DrawMode { POINTS, LINES, LINE_STRIP, TRIANGLES, TRIANGLE_STRIP, TRIANGLE_FAN, PLANE, SPHERE, CUBE, CYLINDER, - BILLBOARD }; + BILLBOARD, TEXT }; /// Drawing mode public: DrawMode drawMode; + /// Unique name of the operation public: char name[GAZEBO_GRAPHICS3D_MAX_NAME]; + /// Text to display + public: char text[GAZEBO_GRAPHICS3D_MAX_NAME]; + /// Number of vertices public: unsigned int pointCount; @@ -874,6 +878,11 @@ public: void DrawBillboard(const char *name, const char *texture, Vec3 pos, Vec2 size); + /// \brief Draw text + public: void DrawText(const char *name, const char *text, Vec3 pos, + float fontSize); + + /// Pointer to the graphics3d data public: Graphics3dData *data; }; Modified: code/branches/federation/gazebo/server/GraphicsIfaceHandler.cc =================================================================== --- code/branches/federation/gazebo/server/GraphicsIfaceHandler.cc 2009-03-10 09:40:49 UTC (rev 7447) +++ code/branches/federation/gazebo/server/GraphicsIfaceHandler.cc 2009-03-10 17:41:45 UTC (rev 7448) @@ -34,6 +34,7 @@ #include "OgreCreator.hh" #include "OgreVisualManager.hh" #include "OgreDynamicLines.hh" +#include "MovableText.hh" #include "GraphicsIfaceHandler.hh" #include "OgreAdaptor.hh" @@ -129,6 +130,10 @@ this->DrawShape(vis, &this->threeDIface->data->commands[i] ); break; + case Graphics3dDrawData::TEXT: + this->DrawText(vis, &this->threeDIface->data->commands[i] ); + break; + default: gzerr(0) << "Unknown draw mode[" << this->threeDIface->data->commands[i].drawMode << "\n"; @@ -194,7 +199,7 @@ { pos.Set( data->points[i].x, data->points[i].y, data->points[i].z); - if (attached) + if (attached && i < line->GetNumPoints()) line->SetPoint(i,pos); else line->AddPoint(pos); @@ -215,8 +220,12 @@ case Graphics3dDrawData::CUBE: { if (vis->GetNumAttached() <= 0) - vis->AttachMesh("unit_box"); + vis->AttachMesh("unit_box_U1V1"); + vis->SetMaterial( OgreCreator::CreateMaterial( data->color.r, + data->color.g, + data->color.b, + data->color.a )); vis->SetScale(Vector3(data->size.x, data->size.y, data->size.z) ); vis->SetPosition(Vector3(data->pose.pos.x, data->pose.pos.y, @@ -229,6 +238,12 @@ if (vis->GetNumAttached() <= 0) vis->AttachMesh("unit_cylinder"); + vis->SetMaterial( OgreCreator::CreateMaterial( data->color.r, + data->color.g, + data->color.b, + data->color.a )); + + vis->SetScale(Vector3(data->size.x, data->size.y, data->size.z) ); vis->SetPosition(Vector3(data->pose.pos.x, data->pose.pos.y, @@ -241,6 +256,11 @@ if (vis->GetNumAttached() <= 0) vis->AttachMesh("unit_sphere"); + vis->SetMaterial( OgreCreator::CreateMaterial( data->color.r, + data->color.g, + data->color.b, + data->color.a )); + vis->SetScale(Vector3(data->size.x, data->size.y, data->size.z) ); vis->SetPosition(Vector3(data->pose.pos.x, data->pose.pos.y, @@ -290,3 +310,40 @@ } } +//////////////////////////////////////////////////////////////////////////////// +/// Helper funciton used to draw text +void GraphicsIfaceHandler::DrawText(OgreVisual *vis, Graphics3dDrawData *data) +{ + bool attached = false; + MovableText* msg = NULL; + + if (vis->GetNumAttached() > 0) + { + attached = true; + msg = (MovableText*)(vis->GetAttached(0)); + } + else + msg = new MovableText(); + + try + { + msg->Load(data->name, data->text, "Arial", data->size.x); + } + catch (Ogre::Exception e) + { + std::ostringstream stream; + stream << "Unable to create the text. " << e.getDescription() <<std::endl; + gzthrow(stream.str() ); + } + + msg->SetTextAlignment(MovableText::H_CENTER, MovableText::V_ABOVE); + + vis->SetPosition(Vector3(data->pose.pos.x, + data->pose.pos.y, + data->pose.pos.z)); + + if (!attached) + vis->AttachObject( msg ); + +} + Modified: code/branches/federation/gazebo/server/GraphicsIfaceHandler.hh =================================================================== --- code/branches/federation/gazebo/server/GraphicsIfaceHandler.hh 2009-03-10 09:40:49 UTC (rev 7447) +++ code/branches/federation/gazebo/server/GraphicsIfaceHandler.hh 2009-03-10 17:41:45 UTC (rev 7448) @@ -61,6 +61,9 @@ /// \brief Helper funciton used to draw shapes private: void DrawShape(OgreVisual *vis, Graphics3dDrawData *data); + /// \brief Helper funciton used to draw text + private: void DrawText(OgreVisual *vis, Graphics3dDrawData *data); + private: std::string name; private: std::map<std::string, OgreVisual* > visuals; private: Graphics3dIface *threeDIface; Modified: code/branches/federation/gazebo/server/rendering/OgreCreator.cc =================================================================== --- code/branches/federation/gazebo/server/rendering/OgreCreator.cc 2009-03-10 09:40:49 UTC (rev 7447) +++ code/branches/federation/gazebo/server/rendering/OgreCreator.cc 2009-03-10 17:41:45 UTC (rev 7448) @@ -628,7 +628,7 @@ matPtr->getTechnique(0)->setLightingEnabled(true); matPtr->getTechnique(0)->getPass(0)->setDiffuse(r,g,b,a); matPtr->getTechnique(0)->getPass(0)->setAmbient(r,g,b); - matPtr->getTechnique(0)->getPass(0)->setSelfIllumination(r,g,b); +// matPtr->getTechnique(0)->getPass(0)->setSelfIllumination(r,g,b); // matPtr->setReceiveShadows(false); } @@ -643,8 +643,13 @@ { Ogre::MaterialPtr matPtr = Ogre::MaterialManager::getSingleton().create( filename, "General"); - matPtr->setLightingEnabled(true); - matPtr->getTechnique(0)->getPass(0)->createTextureUnitState(filename); + Ogre::Pass *pass = matPtr->getTechnique(0)->createPass(); + + matPtr->setLightingEnabled(false); + + //pass->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + //pass->setDepthWriteEnabled(false); + pass->createTextureUnitState(filename); } return filename; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-03-11 16:19:42
|
Revision: 7461 http://playerstage.svn.sourceforge.net/playerstage/?rev=7461&view=rev Author: natepak Date: 2009-03-11 16:19:22 +0000 (Wed, 11 Mar 2009) Log Message: ----------- added threads Modified Paths: -------------- code/branches/federation/gazebo/build.py code/branches/federation/gazebo/server/Simulator.cc Modified: code/branches/federation/gazebo/build.py =================================================================== --- code/branches/federation/gazebo/build.py 2009-03-11 16:14:35 UTC (rev 7460) +++ code/branches/federation/gazebo/build.py 2009-03-11 16:19:22 UTC (rev 7461) @@ -170,7 +170,7 @@ # If valid so far, apply any flags to the environment if valid: for flag in flags: - env.Append(CCFLAGS = " " + flag) + env.Append(CCFLAGS = flag) # Check for trimesh support in ODE #if not conf.CheckODELib(): Modified: code/branches/federation/gazebo/server/Simulator.cc =================================================================== --- code/branches/federation/gazebo/server/Simulator.cc 2009-03-11 16:14:35 UTC (rev 7460) +++ code/branches/federation/gazebo/server/Simulator.cc 2009-03-11 16:19:22 UTC (rev 7461) @@ -295,11 +295,11 @@ this->prevPhysicsTime = this->GetRealTime(); this->prevRenderTime = this->GetRealTime(); - //this->guiThread = new boost::thread( boost::bind(&Simulator::GuiLoop, this)); + this->guiThread = new boost::thread( boost::bind(&Simulator::GuiLoop, this)); while (!this->userQuit) { - currTime = this->GetRealTime(); + /*currTime = this->GetRealTime(); if (physicsUpdateRate == 0 || currTime - this->prevPhysicsTime >= physicsUpdatePeriod) @@ -332,6 +332,7 @@ //this->GetRenderEngine()->Render(); //this->prevRenderTime = this->GetRealTime(); } + */ // Update the gui if (this->gui) @@ -339,21 +340,22 @@ this->gui->Update(); } - world->ProcessMessages(); + /*world->ProcessMessages(); elapsedTime = (this->GetRealTime() - currTime); + */ // Wait if we're going too fast - /*if ( elapsedTime < 1.0/MAX_FRAME_RATE ) - { - usleep( (int)((1.0/MAX_FRAME_RATE - elapsedTime) * 1e6) ); - }*/ + //if ( elapsedTime < 1.0/MAX_FRAME_RATE ) + //{ + // usleep( (int)((1.0/MAX_FRAME_RATE - elapsedTime) * 1e6) ); + //} if (this->timeout > 0 && this->GetRealTime() > this->timeout) break; } - //this->guiThread->join(); + this->guiThread->join(); } //////////////////////////////////////////////////////////////////////////////// @@ -609,7 +611,7 @@ world->ProcessMessages(); - usleep(1000); + usleep(10); } } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-03-12 18:50:40
|
Revision: 7465 http://playerstage.svn.sourceforge.net/playerstage/?rev=7465&view=rev Author: natepak Date: 2009-03-12 18:50:11 +0000 (Thu, 12 Mar 2009) Log Message: ----------- Added more thread safety Modified Paths: -------------- code/branches/federation/gazebo/Media/materials/scripts/Gazebo.material code/branches/federation/gazebo/SConstruct code/branches/federation/gazebo/build.py code/branches/federation/gazebo/examples/libgazebo/bandit/SConstruct code/branches/federation/gazebo/examples/libgazebo/bandit/bandit.cc code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc code/branches/federation/gazebo/server/Entity.cc code/branches/federation/gazebo/server/GraphicsIfaceHandler.cc 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/audio_video/AudioDecoder.hh code/branches/federation/gazebo/server/gui/Gui.cc code/branches/federation/gazebo/server/gui/StatusBar.cc code/branches/federation/gazebo/server/gui/StatusBar.hh code/branches/federation/gazebo/server/physics/Body.cc code/branches/federation/gazebo/server/physics/Geom.cc code/branches/federation/gazebo/server/physics/Joint.cc code/branches/federation/gazebo/server/physics/RayGeom.cc code/branches/federation/gazebo/server/physics/RayGeom.hh code/branches/federation/gazebo/server/rendering/OgreAdaptor.cc code/branches/federation/gazebo/server/rendering/OgreCreator.cc code/branches/federation/gazebo/server/rendering/OgreCreator.hh code/branches/federation/gazebo/server/rendering/SConscript code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc code/branches/federation/gazebo/server/sensors/ray/RaySensor.hh code/branches/federation/gazebo/webgazebo/SConscript code/branches/federation/gazebo/worlds/bandit.world code/branches/federation/gazebo/worlds/models/sicklms200.model code/branches/federation/gazebo/worlds/pioneer2dx.world code/branches/federation/gazebo/worlds/simpleshapes.world Added Paths: ----------- code/branches/federation/gazebo/server/rendering/OgreMovableText.cc code/branches/federation/gazebo/server/rendering/OgreMovableText.hh Removed Paths: ------------- code/branches/federation/gazebo/server/rendering/MovableText.cc code/branches/federation/gazebo/server/rendering/MovableText.hh Modified: code/branches/federation/gazebo/Media/materials/scripts/Gazebo.material =================================================================== --- code/branches/federation/gazebo/Media/materials/scripts/Gazebo.material 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/Media/materials/scripts/Gazebo.material 2009-03-12 18:50:11 UTC (rev 7465) @@ -192,6 +192,24 @@ } } +material Gazebo/BlueLaser +{ + receive_shadows off + + technique + { + pass + { + scene_blend alpha_blend + ambient 0.000000 0.000000 0.900000 0.2 + diffuse 0.000000 0.000000 0.900000 0.2 + specular 0.000000 0.000000 0.100000 0.1 + emissive 0.000000 0.000000 1.000000 0.2 + lighting on + } + } +} + material Gazebo/BlueEmissive { receive_shadows off Modified: code/branches/federation/gazebo/SConstruct =================================================================== --- code/branches/federation/gazebo/SConstruct 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/SConstruct 2009-03-12 18:50:11 UTC (rev 7465) @@ -5,11 +5,11 @@ exec(open('build.py')) - PKG_CONFIG_VERSION = '0.22' OGRE_VERSION = '>= 1.6.0' ODE_VERSION = '>= 0.10.1' + ####### # Create a release ####### @@ -17,6 +17,7 @@ CreateRelease() exit() + ####### # Setup the Options ####### @@ -107,6 +108,34 @@ 'flags' : ['-DHAVE_PLAYER'], 'web' : 'http://playerstage.sourceforge.net' }, + 'event' : { + 'header' : 'event.h', + 'lib' : 'event', + 'msg' : 'Warning: libevent not found. Webgazebo will no be built.', + 'flags' : ['-DHAVE_EVENT'], + 'web' : 'http://www.monkey.org/~provos/libevent/' + }, + 'yaml' : { + 'header' : 'yaml.h', + 'lib' : 'yaml', + 'msg' : 'Warning: yaml not found. Webgazebo will not be built.', + 'flags' : ['-DHAVE_YAML'], + 'web' : 'http://www.yaml.org' + }, + 'websim' : { + 'pkgcfg' : 'pkg-config --cflags --libs websim', + 'check' : 'websim', + 'msg' : 'Warning: websim not found. Webgazebo will not be built.', + 'flags' : ['-DHAVE_WEBSIM'], + 'web' : 'http://www.playerstage.sourceforge.net' + }, + 'glib' : { + 'pkgcfg' : 'pkg-config --cflags --libs glib-2.0', + 'check' : 'glib-2.0', + 'msg' : 'Warning: glib not found. Webgazebo will not be built.', + 'flags' : '', + 'web' : 'http://www.gtk.org' + }, } ####### @@ -144,8 +173,13 @@ ####### subdirs = ['libgazebo','server', 'player'] # Need test for <event.h> -if True: +try: + env["CCFLAGS"].index("-DHAVE_WEBSIM") + env["CCFLAGS"].index("-DHAVE_YAML") + env["CCFLAGS"].index("-DHAVE_EVENT") subdirs.append('webgazebo') +except ValueError, e: + garbage = 1 ####### # Generate help text @@ -190,6 +224,7 @@ ####### Config(env, packages) + ####### # Export the environment ####### Modified: code/branches/federation/gazebo/build.py =================================================================== --- code/branches/federation/gazebo/build.py 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/build.py 2009-03-12 18:50:11 UTC (rev 7465) @@ -116,8 +116,10 @@ def Config(env, packages): + simpleEnv = Environment(CC="g++") + # Setup the configuration environment. - conf = Configure(env, custom_tests = {'CheckODELib' : CheckODELib, + conf = Configure(simpleEnv, custom_tests = {'CheckODELib' : CheckODELib, 'CheckPkgConfig' : CheckPkgConfig, 'CheckPkg' : CheckPkg}) @@ -132,11 +134,27 @@ # Parse all the pacakge configurations # for key in packages: - pkgcfg = packages[key]['pkgcfg'] - check = packages[key]['check'] + pkgcfg ='' + header='' + lib='' + check='' + + if packages[key].has_key('pkgcfg'): + pkgcfg = packages[key]['pkgcfg'] + + if packages[key].has_key('header'): + header = packages[key]['header'] + + if packages[key].has_key('lib'): + lib = packages[key]['lib'] + + if packages[key].has_key('check'): + check = packages[key]['check'] + msg = packages[key]['msg'] web = packages[key]['web'] flags = packages[key]['flags'] + docfg = True valid = True @@ -148,9 +166,9 @@ print ' See: ' + web if msg.find('Error') > 0: Exit(1) - + # Try parsing the pkg-config - if docfg: + if docfg and pkgcfg: try: if not check: print "Checking for "+key+"...", @@ -166,6 +184,13 @@ print ' See: ' + web if msg.find('Error') > 0: Exit(1) + elif header and lib: + if not conf.CheckLibWithHeader(lib, header, 'c'): + valid = False + print msg + else: + env.Append(LIBS = lib) + valid = True # If valid so far, apply any flags to the environment if valid: @@ -176,9 +201,22 @@ #if not conf.CheckODELib(): # print ' Error: ODE not compiled with trimesh support.' # Exit(1) + + if not conf.CheckCHeader('avformat.h'): + if conf.CheckCHeader('libavformat/avformat.h'): + env.Append( CCFLAGS = '-DSPECIAL_LIBAVFORMAT') + else: + print "Unable to find libavformat" + + if not conf.CheckCHeader('avcodec.h'): + if conf.CheckCHeader('libavcodec/avcodec.h'): + env.Append( CCFLAGS = '-DSPECIAL_LIBAVCODEC') + else: + print "Unable to find libavcodec" + + + simpleEnv = conf.Finish() - env = conf.Finish() - #simpleenv = Environment(CPPPATH="/usr/include/AL") #simpleconf = Configure(simpleenv) # Modified: code/branches/federation/gazebo/examples/libgazebo/bandit/SConstruct =================================================================== --- code/branches/federation/gazebo/examples/libgazebo/bandit/SConstruct 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/examples/libgazebo/bandit/SConstruct 2009-03-12 18:50:11 UTC (rev 7465) @@ -1,7 +1,6 @@ # 3rd party packages -parseConfigs=['pkg-config --cflags --libs libgazebo', - 'pkg-config --cflags --libs libgazeboServer'] +parseConfigs=['pkg-config --cflags --libs libgazebo'] env = Environment ( Modified: code/branches/federation/gazebo/examples/libgazebo/bandit/bandit.cc =================================================================== --- code/branches/federation/gazebo/examples/libgazebo/bandit/bandit.cc 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/examples/libgazebo/bandit/bandit.cc 2009-03-12 18:50:11 UTC (rev 7465) @@ -1,5 +1,4 @@ #include <gazebo/gazebo.h> -#include <gazebo/GazeboError.hh> #include <math.h> @@ -25,7 +24,7 @@ { client->ConnectWait(serverId, GZ_CLIENT_ID_USER_FIRST); } - catch (gazebo::GazeboError e) + catch (std::string e) { std::cout << "Gazebo error: Unable to connect\n" << e << "\n"; return -1; @@ -36,7 +35,7 @@ { simIface->Open(client, "default"); } - catch (gazebo::GazeboError e) + catch (std::string e) { std::cout << "Gazebo error: Unable to connect to the sim interface\n" << e << "\n"; return -1; Modified: code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc =================================================================== --- code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc 2009-03-12 18:50:11 UTC (rev 7465) @@ -231,7 +231,7 @@ UpdatePath(); UpdateText(); - usleep(10000); + usleep(20000); } simIface->Close(); Modified: code/branches/federation/gazebo/server/Entity.cc =================================================================== --- code/branches/federation/gazebo/server/Entity.cc 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/server/Entity.cc 2009-03-12 18:50:11 UTC (rev 7465) @@ -29,7 +29,7 @@ #include "GazeboError.hh" #include "Global.hh" #include "OgreVisual.hh" -#include "OgreVisualManager.hh" +#include "OgreCreator.hh" #include "World.hh" #include "PhysicsEngine.hh" #include "Entity.hh" @@ -55,13 +55,13 @@ if (this->parent) { this->parent->AddChild(this); - this->visualNode = OgreVisualManager::Instance()->CreateVisual( + this->visualNode = OgreCreator::Instance()->CreateVisual( visname.str(), this->parent->GetVisualNode(), this); this->SetStatic(parent->IsStatic()); } else { - this->visualNode = OgreVisualManager::Instance()->CreateVisual( + this->visualNode = OgreCreator::Instance()->CreateVisual( visname.str(), NULL, this ); } @@ -77,7 +77,7 @@ World::Instance()->GetPhysicsEngine()->RemoveEntity(this); - OgreVisualManager::Instance()->DeleteVisual(this->visualNode); + OgreCreator::Instance()->DeleteVisual(this->visualNode); //GZ_DELETE(this->visualNode); } Modified: code/branches/federation/gazebo/server/GraphicsIfaceHandler.cc =================================================================== --- code/branches/federation/gazebo/server/GraphicsIfaceHandler.cc 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/server/GraphicsIfaceHandler.cc 2009-03-12 18:50:11 UTC (rev 7465) @@ -32,9 +32,8 @@ #include "IfaceFactory.hh" #include "OgreVisual.hh" #include "OgreCreator.hh" -#include "OgreVisualManager.hh" #include "OgreDynamicLines.hh" -#include "MovableText.hh" +#include "OgreMovableText.hh" #include "GraphicsIfaceHandler.hh" #include "OgreAdaptor.hh" @@ -108,10 +107,10 @@ << this->visuals.size(); if (this->parent) - vis = OgreVisualManager::Instance()->CreateVisual(nodeName.str(), + vis = OgreCreator::Instance()->CreateVisual(nodeName.str(), this->parent->GetVisualNode()); else - vis = OgreVisualManager::Instance()->CreateVisual(nodeName.str()); + vis = OgreCreator::Instance()->CreateVisual(nodeName.str()); this->visuals[visName] = vis; } @@ -190,7 +189,7 @@ attached = true; } else - line = new OgreDynamicLines(opType); + line = OgreCreator::Instance()->CreateDynamicLine(opType); line->setMaterial(OgreCreator::CreateMaterial( data->color.r, data->color.g, @@ -208,8 +207,6 @@ line->AddPoint(pos); } - line->Update(); - if (!attached) vis->AttachObject(line); } @@ -318,15 +315,15 @@ void GraphicsIfaceHandler::DrawText(OgreVisual *vis, Graphics3dDrawData *data) { bool attached = false; - MovableText* msg = NULL; + OgreMovableText* msg = NULL; if (vis->GetNumAttached() > 0) { attached = true; - msg = (MovableText*)(vis->GetAttached(0)); + msg = (OgreMovableText*)(vis->GetAttached(0)); } else - msg = new MovableText(); + msg = OgreCreator::Instance()->CreateMovableText(); try { @@ -339,7 +336,7 @@ gzthrow(stream.str() ); } - msg->SetTextAlignment(MovableText::H_CENTER, MovableText::V_ABOVE); + msg->SetTextAlignment(OgreMovableText::H_CENTER, OgreMovableText::V_ABOVE); vis->SetPosition(Vector3(data->pose.pos.x, data->pose.pos.y, @@ -347,6 +344,5 @@ if (!attached) vis->AttachObject( msg ); - } Modified: code/branches/federation/gazebo/server/Model.cc =================================================================== --- code/branches/federation/gazebo/server/Model.cc 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/server/Model.cc 2009-03-12 18:50:11 UTC (rev 7465) @@ -404,10 +404,11 @@ this->rpyP->SetValue(this->pose.rot); } - if (this->parent) + /*if (this->parent) this->visualNode->SetPose(this->GetPose() - this->parent->GetPose()); else this->visualNode->SetPose(this->GetPose()); + */ if (this->graphicsHandler) this->graphicsHandler->Update(); Modified: code/branches/federation/gazebo/server/Simulator.cc =================================================================== --- code/branches/federation/gazebo/server/Simulator.cc 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/server/Simulator.cc 2009-03-12 18:50:11 UTC (rev 7465) @@ -29,7 +29,6 @@ #include <sys/time.h> #include <boost/bind.hpp> -#include "OgreVisualManager.hh" #include "Body.hh" #include "Geom.hh" #include "Model.hh" @@ -281,81 +280,18 @@ /// Main simulation loop, when this loop ends the simulation finish void Simulator::MainLoop() { - World *world = World::Instance(); + this->physicsThread = new boost::thread( boost::bind(&Simulator::PhysicsLoop, this)); - 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; - double elapsedTime; - - this->prevPhysicsTime = this->GetRealTime(); - this->prevRenderTime = this->GetRealTime(); - - this->guiThread = new boost::thread( boost::bind(&Simulator::GuiLoop, this)); - - while (!this->userQuit) + if (this->gui) { - /*currTime = this->GetRealTime(); - - if (physicsUpdateRate == 0 || - currTime - this->prevPhysicsTime >= physicsUpdatePeriod) + // Update the gui + while (!this->userQuit) { - - // Update the physics engine - //if (!this->GetUserPause() && !this->IsPaused() || - // (this->GetUserPause() && this->GetUserStepInc())) - if (!this->IsPaused()) - { - this->simTime += step; - this->iterations++; - this->SetUserStepInc(!this->GetUserStepInc()); - } - else - { - this->pauseTime += step; - // this->pause=true; - } - - this->prevPhysicsTime = this->GetRealTime(); - - world->Update(); - } - - // Update the rendering - if (renderUpdateRate == 0 || - currTime - this->prevRenderTime >= renderUpdatePeriod) - { - //this->GetRenderEngine()->Render(); - //this->prevRenderTime = this->GetRealTime(); - } - */ - - // Update the gui - if (this->gui) - { this->gui->Update(); } - - /*world->ProcessMessages(); - - elapsedTime = (this->GetRealTime() - currTime); - */ - - // Wait if we're going too fast - //if ( elapsedTime < 1.0/MAX_FRAME_RATE ) - //{ - // usleep( (int)((1.0/MAX_FRAME_RATE - elapsedTime) * 1e6) ); - //} - - if (this->timeout > 0 && this->GetRealTime() > this->timeout) - break; - } - this->guiThread->join(); + + this->physicsThread->join(); } //////////////////////////////////////////////////////////////////////////////// @@ -556,8 +492,8 @@ } //////////////////////////////////////////////////////////////////////////////// -/// Function to run gui. Used by guiThread -void Simulator::GuiLoop() +/// Function to run physics. Used by physicsThread +void Simulator::PhysicsLoop() { World *world = World::Instance(); @@ -565,10 +501,9 @@ double physicsUpdateRate = world->GetPhysicsEngine()->GetUpdateRate(); double renderUpdateRate = OgreAdaptor::Instance()->GetUpdateRate(); double physicsUpdatePeriod = 1.0 / physicsUpdateRate; - double renderUpdatePeriod = 1.0 / renderUpdateRate; + //double renderUpdatePeriod = 1.0 / renderUpdateRate; double currTime; - double elapsedTime; this->prevPhysicsTime = this->GetRealTime(); this->prevRenderTime = this->GetRealTime(); @@ -601,17 +536,14 @@ world->Update(); } - // Update the rendering - if (renderUpdateRate == 0 || - currTime - this->prevRenderTime >= renderUpdatePeriod) - { - //this->GetRenderEngine()->Render(); - //this->prevRenderTime = this->GetRealTime(); - } - + // Process all incoming messages from simiface world->ProcessMessages(); - usleep(10); + if (this->timeout > 0 && this->GetRealTime() > this->timeout) + { + this->userQuit = true; + break; + } } } Modified: code/branches/federation/gazebo/server/Simulator.hh =================================================================== --- code/branches/federation/gazebo/server/Simulator.hh 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/server/Simulator.hh 2009-03-12 18:50:11 UTC (rev 7465) @@ -164,7 +164,7 @@ public: Model *GetSelectedModel() const; /// \brief Function to run gui. Used by guiThread - private: void GuiLoop(); + private: void PhysicsLoop(); ///pointer to the XML Data private: XMLConfig *xmlFile; @@ -227,7 +227,7 @@ private: Entity *selectedEntity; /// Thread in which to run the gui - private: boost::thread *guiThread; + private: boost::thread *physicsThread; //Singleton implementation private: friend class DestroyerT<Simulator>; Modified: code/branches/federation/gazebo/server/audio_video/AudioDecoder.hh =================================================================== --- code/branches/federation/gazebo/server/audio_video/AudioDecoder.hh 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/server/audio_video/AudioDecoder.hh 2009-03-12 18:50:11 UTC (rev 7465) @@ -2,8 +2,18 @@ #define AUDIODECODER_HH extern "C" { + +#ifdef SPECIAL_LIBAVFORMAT +#include <libavformat/avformat.h> +#else #include <avformat.h> +#endif + +#ifdef SPECIAL_LIBAVCODEC +#include <libavcodec/avcodec.h> +#else #include <avcodec.h> +#endif } #include <string> Modified: code/branches/federation/gazebo/server/gui/Gui.cc =================================================================== --- code/branches/federation/gazebo/server/gui/Gui.cc 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/server/gui/Gui.cc 2009-03-12 18:50:11 UTC (rev 7465) @@ -148,7 +148,11 @@ this->toolbar->Update(); this->statusbar->Update(); this->frameMgr->Update(); + Fl::check(); + + OgreAdaptor::Instance()->UpdateCameras(); + //Fl::wait(0.3); } Modified: code/branches/federation/gazebo/server/gui/StatusBar.cc =================================================================== --- code/branches/federation/gazebo/server/gui/StatusBar.cc 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/server/gui/StatusBar.cc 2009-03-12 18:50:11 UTC (rev 7465) @@ -51,7 +51,11 @@ this->realTime = new Fl_Value_Output(x,y,55,20,"Real Time"); this->realTime->precision(2); - x = this->realTime->x() + this->realTime->w() + 75; + x = this->realTime->x() + this->realTime->w() + 80; + this->percent = new Fl_Value_Output(x,y,55,20,"% Real Time"); + this->percent->precision(2); + + x = this->percent->x() + this->percent->w() + 75; this->simTime = new Fl_Value_Output(x,y,55,20,"Sim Time"); this->simTime->precision(2); @@ -88,6 +92,7 @@ //this->iterations->value(Simulator::Instance()->GetIterations()); this->fps->value(avgFPS); + this->percent->value(Simulator::Instance()->GetSimTime() / Simulator::Instance()->GetRealTime()); if (Simulator::Instance()->GetRealTime() - this->realTime->value() > 0.1) { Modified: code/branches/federation/gazebo/server/gui/StatusBar.hh =================================================================== --- code/branches/federation/gazebo/server/gui/StatusBar.hh 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/server/gui/StatusBar.hh 2009-03-12 18:50:11 UTC (rev 7465) @@ -54,6 +54,7 @@ public: static void StepButtonCB( Fl_Widget *w, void *data ); private: Fl_Value_Output *iterations; + private: Fl_Value_Output *percent; private: Fl_Value_Output *fps; private: Fl_Value_Output *realTime; private: Fl_Value_Output *pauseTime; Modified: code/branches/federation/gazebo/server/physics/Body.cc =================================================================== --- code/branches/federation/gazebo/server/physics/Body.cc 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/server/physics/Body.cc 2009-03-12 18:50:11 UTC (rev 7465) @@ -271,12 +271,8 @@ this->UpdatePose(); -// if (!this->IsStatic()) - { - this->visualNode->SetPose(this->GetPose() - this->parent->GetPose()); - } +// this->visualNode->SetPose(this->GetPose() - this->parent->GetPose()); - for (geomIter=this->geoms.begin(); geomIter!=this->geoms.end(); geomIter++) { @@ -369,7 +365,7 @@ return this->staticPose; else */ - return this->pose; + return this->pose; } //////////////////////////////////////////////////////////////////////////////// @@ -388,10 +384,6 @@ { if (this->bodyId) dBodySetPosition(this->bodyId, pos.x, pos.y, pos.z); - - // NATE - // Set the position of the scene node - //this->visualNode->SetPosition(pos); } //////////////////////////////////////////////////////////////////////////////// @@ -410,10 +402,6 @@ // Set the rotation of the ODE body dBodySetQuaternion(this->bodyId, q); } - - // NATE - // Set the orientation of the scene node - //this->visualNode->SetRotation(rot); } //////////////////////////////////////////////////////////////////////////////// Modified: code/branches/federation/gazebo/server/physics/Geom.cc =================================================================== --- code/branches/federation/gazebo/server/physics/Geom.cc 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/server/physics/Geom.cc 2009-03-12 18:50:11 UTC (rev 7465) @@ -26,8 +26,8 @@ #include <sstream> -#include "OgreVisualManager.hh" #include "OgreVisual.hh" +#include "OgreCreator.hh" #include "Global.hh" #include "GazeboMessage.hh" #include "ContactParams.hh" @@ -146,8 +146,8 @@ std::ostringstream visname; visname << this->GetName() << "_VISUAL_" << this->visuals.size(); - OgreVisual *visual = OgreVisualManager::Instance()->CreateVisual( - visname.str(), this->visualNode, this); + OgreVisual *visual = OgreCreator::Instance()->CreateVisual( + visname.str(), this->visualNode, NULL); visual->Load(childNode); this->visuals.push_back(visual); @@ -171,7 +171,7 @@ std::ostringstream visname; visname << this->GetName() << "_BBVISUAL" ; - this->bbVisual = OgreVisualManager::Instance()->CreateVisual( + this->bbVisual = OgreCreator::Instance()->CreateVisual( visname.str(), this->visualNode); this->bbVisual->AttachBoundingBox(min,max); } @@ -269,8 +269,7 @@ // Update void Geom::Update() { - - this->visualNode->SetPose(this->GetPose()); + //this->visualNode->SetPose(this->GetPose()); this->UpdateChild(); } @@ -327,9 +326,6 @@ dGeomSetPosition(this->geomId, localPose.pos.x, localPose.pos.y, localPose.pos.z); dGeomSetQuaternion(this->geomId, q); - // NATE - //this->visualNode->SetPose(this->GetPoseRelative()); - if (updateCoM) { this->body->UpdateCoM(); @@ -366,11 +362,7 @@ // Transform into body relative pose pose += this->body->GetCoMPose(); } - /*else - pose = this->body->GetPose(); - */ - return pose; } Modified: code/branches/federation/gazebo/server/physics/Joint.cc =================================================================== --- code/branches/federation/gazebo/server/physics/Joint.cc 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/server/physics/Joint.cc 2009-03-12 18:50:11 UTC (rev 7465) @@ -24,7 +24,7 @@ * CVS: $Id$ */ #include "OgreVisual.hh" -#include "OgreVisualManager.hh" +#include "OgreCreator.hh" #include "OgreDynamicLines.hh" #include "Global.hh" #include "Body.hh" @@ -120,13 +120,13 @@ visname << this->GetName() << "_VISUAL"; /// Add a renderable for the joint - this->visual = OgreVisualManager::Instance()->CreateVisual( + this->visual = OgreCreator::Instance()->CreateVisual( visname.str(), this->model->GetVisualNode()); this->visual->AttachMesh("joint_anchor"); this->visual->SetVisible(false); - this->line1 = new OgreDynamicLines(OgreDynamicRenderable::OT_LINE_LIST); - this->line2 = new OgreDynamicLines(OgreDynamicRenderable::OT_LINE_LIST); + this->line1 = OgreCreator::Instance()->CreateDynamicLine(OgreDynamicRenderable::OT_LINE_LIST); + this->line2 = OgreCreator::Instance()->CreateDynamicLine(OgreDynamicRenderable::OT_LINE_LIST); this->line1->setMaterial("Gazebo/BlueEmissive"); this->line2->setMaterial("Gazebo/BlueEmissive"); @@ -202,14 +202,14 @@ { start = this->body1->GetPose().pos - this->GetAnchor(); this->line1->SetPoint(0, start); - this->line1->Update(); + //this->line1->Update(); } if (this->body2) { start = this->body2->GetPose().pos - this->GetAnchor(); this->line2->SetPoint(0, start); - this->line2->Update(); + //this->line2->Update(); } } Modified: code/branches/federation/gazebo/server/physics/RayGeom.cc =================================================================== --- code/branches/federation/gazebo/server/physics/RayGeom.cc 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/server/physics/RayGeom.cc 2009-03-12 18:50:11 UTC (rev 7465) @@ -30,6 +30,7 @@ #include "OgreVisual.hh" #include "OgreDynamicLines.hh" +#include "OgreCreator.hh" #include "Body.hh" #include "Global.hh" #include "RayGeom.hh" @@ -49,7 +50,8 @@ if (displayRays) { - this->line = new OgreDynamicLines(OgreDynamicRenderable::OT_LINE_LIST); + //this->line = new OgreDynamicLines(OgreDynamicRenderable::OT_LINE_LIST); + this->line = OgreCreator::Instance()->CreateDynamicLine(OgreDynamicRenderable::OT_LINE_LIST); // Add two points this->line->AddPoint(Vector3(0,0,0)); @@ -78,6 +80,8 @@ } } +////////////////////////////////////////////////////////////////////////////// +// Update the ray geom void RayGeom::Update() { Vector3 dir; @@ -124,7 +128,6 @@ // Set the line's position relative to it's parent scene node this->line->SetPoint(0, this->relativeStartPos); this->line->SetPoint(1, this->relativeEndPos); - this->line->Update(); } } @@ -151,13 +154,14 @@ //dGeomRaySetLength( this->geomId, len ); this->contactLen=len; + Vector3 dir = this->relativeEndPos - this->relativeStartPos; + dir.Normalize(); + + this->relativeEndPos = dir * len + this->relativeStartPos; + if (this->line) { - Vector3 dir = this->relativeEndPos - this->relativeStartPos; - dir.Normalize(); - - this->line->SetPoint(1, dir * len + this->relativeStartPos); - this->line->Update(); + this->line->SetPoint(1, this->relativeEndPos); } } Modified: code/branches/federation/gazebo/server/physics/RayGeom.hh =================================================================== --- code/branches/federation/gazebo/server/physics/RayGeom.hh 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/server/physics/RayGeom.hh 2009-03-12 18:50:11 UTC (rev 7465) @@ -114,9 +114,6 @@ /// Start and end positions of the ray in global cs private: Vector3 globalStartPos; private: Vector3 globalEndPos; - - private: OgreVisual *visual; - }; /// \} Deleted: code/branches/federation/gazebo/server/rendering/MovableText.cc =================================================================== --- code/branches/federation/gazebo/server/rendering/MovableText.cc 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/server/rendering/MovableText.cc 2009-03-12 18:50:11 UTC (rev 7465) @@ -1,754 +0,0 @@ -/** - * File: OgreMovableText.cpp - * - * description: This create create a billboarding object that display a text. - * - * @author 2003 by cTh see gav...@ra... - * @update 2006 by barraq see no...@ba... - * @update 2007 by independentCreations see ind...@gm... - */ - -#include "OgreMovableText.hh" - -#include <boost/thread/recursive_mutex.hpp> -#include <OgreFontManager.h> -#include <OgrePrerequisites.h> - -#define POS_TEX_BINDING 0 -#define COLOUR_BINDING 1 - -using namespace gazebo; - -//////////////////////////////////////////////////////////////////////////////// -// Constructor -OgreMovableText::OgreMovableText() - : camera(NULL), - renderWindow(NULL) , - viewportAspectCoef (0.75), - font(NULL) , - spaceWidth(0) , - updateColors(true) , - vertAlign(V_BELOW) , - horizAlign(H_LEFT) , - onTop(false) , - baseline(0.0) -{ - this->renderOp.vertexData = NULL; - - this->dirty = true; - this->mutex = new boost::recursive_mutex(); -} - -//////////////////////////////////////////////////////////////////////////////// -// Destructor -OgreMovableText::~OgreMovableText() -{ - if (this->renderOp.vertexData) - delete this->renderOp.vertexData; - - delete this->mutex; -} - -//////////////////////////////////////////////////////////////////////////////// -//Loads the text to display and select the font -void OgreMovableText::Load(const std::string &name, - const Ogre::UTFString &text, - const std::string &fontName, - float charHeight, - const Ogre::ColourValue &color) -{ - { - boost::recursive_mutex::scoped_lock lock(*this->mutex); - - this->text=text; - this->color=color; - this->fontName=fontName; - this->charHeight=charHeight; - this->mName = name; - - if (this->mName == "") - throw Ogre::Exception(Ogre::Exception::ERR_INVALIDPARAMS, - "Trying to create OgreMovableText without name", - "OgreMovableText::OgreMovableText"); - - if (this->text == "") - throw Ogre::Exception(Ogre::Exception::ERR_INVALIDPARAMS, - "Trying to create OgreMovableText without text", - "OgreMovableText::OgreMovableText"); - - this->dirty = true; - } - - this->SetFontName(this->fontName); - - - // NATE - //this->_setupGeometry(); -} - -//////////////////////////////////////////////////////////////////////////////// -// Update the text -void OgreMovableText::Update() -{ - if (this->dirty) - { - this->_setupGeometry(); - this->dirty = false; - } -} - -//////////////////////////////////////////////////////////////////////////////// -// Set the font name -void OgreMovableText::SetFontName(const std::string &newFontName) -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - - if ((Ogre::MaterialManager::getSingletonPtr()->resourceExists(this->mName + "Material"))) - { - Ogre::MaterialManager::getSingleton().remove(this->mName + "Material"); - } - - if (this->fontName != newFontName || this->material.isNull() || !this->font) - { - this->fontName = newFontName; - - this->font = (Ogre::Font*)Ogre::FontManager::getSingleton().getByName(this->fontName).getPointer(); - - if (!this->font) - { - throw Ogre::Exception(Ogre::Exception::ERR_ITEM_NOT_FOUND, - "Could not find font " + fontName, - "OgreMovableText::setFontName"); - } - - this->font->load(); - - if (!this->material.isNull()) - { - Ogre::MaterialManager::getSingletonPtr()->remove(this->material->getName()); - this->material.setNull(); - } - - this->material = this->font->getMaterial()->clone(this->mName + "Material"); - - if (!this->material->isLoaded()) - this->material->load(); - - this->material->setDepthCheckEnabled(!this->onTop); - this->material->setDepthBias(!this->onTop, 0); - this->material->setDepthWriteEnabled(this->onTop); - this->material->setLightingEnabled(false); - - this->needUpdate = true; - } -} - -//////////////////////////////////////////////////////////////////////////////// -// Set the caption -void OgreMovableText::SetText(const Ogre::UTFString &newText) -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - - if (this->text != newText) - { - this->text = newText; - this->needUpdate = true; - } -} - -//////////////////////////////////////////////////////////////////////////////// -// Set the color -void OgreMovableText::SetColor(const Ogre::ColourValue &newColor) -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - - if (this->color != newColor) - { - this->color = newColor; - this->updateColors = true; - } -} - -//////////////////////////////////////////////////////////////////////////////// -// Set the character height -void OgreMovableText::SetCharHeight(float height) -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - - if (this->charHeight != height) - { - this->charHeight = height; - this->needUpdate = true; - } -} - -//////////////////////////////////////////////////////////////////////////////// -// Set the width of the space between characters -void OgreMovableText::SetSpaceWidth(float width) -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - - if (this->spaceWidth != width) - { - this->spaceWidth = width; - this->needUpdate = true; - } -} - -//////////////////////////////////////////////////////////////////////////////// -// Set alignment of the text -void OgreMovableText::SetTextAlignment(const HorizAlign &h, const VertAlign &v) -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - if (this->horizAlign != h) - { - this->horizAlign = h; - this->needUpdate = true; - } - - if (this->vertAlign != v) - { - this->vertAlign = v; - this->needUpdate = true; - } -} - -//////////////////////////////////////////////////////////////////////////////// -// Set additional height -void OgreMovableText::SetBaseline( float base ) -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - if ( this->baseline != base ) - { - this->baseline = base; - this->needUpdate = true; - } -} - -//////////////////////////////////////////////////////////////////////////////// -// Set whether the text should be shown on top -void OgreMovableText::SetShowOnTop(bool show) -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - if (this->onTop != show && !this->material.isNull()) - { - this->onTop = show; - - this->material->setDepthBias(!this->onTop, 0); - this->material->setDepthCheckEnabled(!this->onTop); - this->material->setDepthWriteEnabled(this->onTop); - } -} - -//////////////////////////////////////////////////////////////////////////////// -// True=text is displayed on top -bool OgreMovableText::GetShowOnTop() const -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - return this->onTop; -} - -//////////////////////////////////////////////////////////////////////////////// -// Get the axis aligned bounding box -Ogre::AxisAlignedBox OgreMovableText::GetAABB(void) -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - return this->aabb; -} - -//////////////////////////////////////////////////////////////////////////////// -// Setup the billboard that renders the text -void OgreMovableText::_setupGeometry() -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - - assert(this->font); - assert(!this->material.isNull()); - - Ogre::VertexDeclaration *decl = NULL; - Ogre::VertexBufferBinding *bind = NULL; - Ogre::HardwareVertexBufferSharedPtr ptbuf; - Ogre::HardwareVertexBufferSharedPtr cbuf; - float *pVert = NULL; - float largestWidth = 0; - float left = 0; - float top = 0; - size_t offset = 0; - float maxSquaredRadius = 0.0f; - bool first = true; - Ogre::UTFString::iterator i; - bool newLine = true; - float len = 0.0f; - - // for calculation of AABB - Ogre::Vector3 min, max, currPos; - - unsigned int vertexCount = static_cast<unsigned int>(this->text.size() * 6); - - - if (this->renderOp.vertexData) - { - // Removed this test as it causes problems when replacing a caption - // of the same size: replacing "Hello" with "hello" - // as well as when changing the text alignment - //if (this->renderOp.vertexData->vertexCount != vertexCount) - { - delete this->renderOp.vertexData; - this->renderOp.vertexData = NULL; - this->updateColors = true; - } - } - - if (!this->renderOp.vertexData) - this->renderOp.vertexData = new Ogre::VertexData(); - - this->renderOp.indexData = 0; - this->renderOp.vertexData->vertexStart = 0; - this->renderOp.vertexData->vertexCount = vertexCount; - this->renderOp.operationType = Ogre::RenderOperation::OT_TRIANGLE_LIST; - this->renderOp.useIndexes = false; - - decl = this->renderOp.vertexData->vertexDeclaration; - bind = this->renderOp.vertexData->vertexBufferBinding; - - // create/bind positions/tex.ccord. buffer - if (!decl->findElementBySemantic(Ogre::VES_POSITION)) - decl->addElement(POS_TEX_BINDING, offset, Ogre::VET_FLOAT3, - Ogre::VES_POSITION); - - offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3); - - if (!decl->findElementBySemantic(Ogre::VES_TEXTURE_COORDINATES)) - decl->addElement(POS_TEX_BINDING, offset, Ogre::VET_FLOAT2, - Ogre::VES_TEXTURE_COORDINATES, 0); - - ptbuf = Ogre::HardwareBufferManager::getSingleton().createVertexBuffer( - decl->getVertexSize(POS_TEX_BINDING), - this->renderOp.vertexData->vertexCount, - Ogre::HardwareBuffer::HBU_DYNAMIC_WRITE_ONLY); - - bind->setBinding(POS_TEX_BINDING, ptbuf); - - // Colours - store these in a separate buffer because they change less often - if (!decl->findElementBySemantic(Ogre::VES_DIFFUSE)) - decl->addElement(COLOUR_BINDING, 0, Ogre::VET_COLOUR, Ogre::VES_DIFFUSE); - - cbuf = Ogre::HardwareBufferManager::getSingleton().createVertexBuffer( - decl->getVertexSize(COLOUR_BINDING), - this->renderOp.vertexData->vertexCount, - Ogre::HardwareBuffer::HBU_DYNAMIC_WRITE_ONLY); - - bind->setBinding(COLOUR_BINDING, cbuf); - - pVert = static_cast<float*>(ptbuf->lock(Ogre::HardwareBuffer::HBL_DISCARD)); - - // Derive space width from a capital A - if (this->spaceWidth == 0) - this->spaceWidth = this->font->getGlyphAspectRatio('A') * this->charHeight * 2.0; - - if (this->vertAlign == OgreMovableText::V_ABOVE) - { - // Raise the first line of the caption - top += this->charHeight; - - for (i = this->text.begin(); i != this->text.end(); ++i) - { - if (*i == '\n') - top += this->charHeight * 2.0; - } - } - - for ( i = this->text.begin(); i != this->text.end(); ++i ) - { - if ( newLine ) - { - len = 0.0; - for ( Ogre::UTFString::iterator j = i; j != this->text.end(); j++ ) - { - Ogre::Font::CodePoint character = j.getCharacter(); - if (character == 0x000D // CR - || character == 0x0085) // NEL - { - break; - } - else if (character == 0x0020) // space - { - len += this->spaceWidth; - } - else - { - len += this->font->getGlyphAspectRatio(character) * this->charHeight * 2.0 * this->viewportAspectCoef; - } - } - - newLine = false; - } - - Ogre::Font::CodePoint character = i.getCharacter(); - - if (character == 0x000D // CR - || character == 0x0085) // NEL - { - top -= this->charHeight * 2.0; - newLine = true; - - // Also reduce tri count - this->renderOp.vertexData->vertexCount -= 6; - continue; - } - else if (character == 0x0020) // space - { - // Just leave a gap, no tris - left += this->spaceWidth; - - // Also reduce tri count - this->renderOp.vertexData->vertexCount -= 6; - continue; - } - - float horiz_height = this->font->getGlyphAspectRatio(character) * this->viewportAspectCoef ; - - const Ogre::Font::UVRect& uvRect = this->font->getGlyphTexCoords(character); - - // each vert is (x, y, z, u, v) - //------------------------------------------------------------------------ - // First tri - // - // Upper left - if (this->horizAlign == OgreMovableText::H_LEFT) - *pVert++ = left; - else - *pVert++ = left - (len/2.0); - - *pVert++ = top; - *pVert++ = 0; - *pVert++ = uvRect.left; - *pVert++ = uvRect.top; - - // Deal with bounds - if (this->horizAlign == OgreMovableText::H_LEFT) - currPos = Ogre::Vector3(left,top,0); - else - currPos = Ogre::Vector3(left - (len/2.0), top, 0); - - if (first) - { - min = max = currPos; - maxSquaredRadius = currPos.squaredLength(); - first = false; - } - else - { - min.makeFloor(currPos); - max.makeCeil(currPos); - maxSquaredRadius = std::max(maxSquaredRadius, currPos.squaredLength()); - } - - top -= this->charHeight * 2.0; - - // Bottom left - if (this->horizAlign == OgreMovableText::H_LEFT) - *pVert++ = left; - else - *pVert++ = left - (len / 2.0); - - *pVert++ = top; - *pVert++ = 0; - *pVert++ = uvRect.left; - *pVert++ = uvRect.bottom; - - // Deal with bounds - if (this->horizAlign == OgreMovableText::H_LEFT) - currPos = Ogre::Vector3(left,top,0); - else - currPos = Ogre::Vector3(left - (len/2), top, 0); - - min.makeFloor(currPos); - max.makeCeil(currPos); - maxSquaredRadius = std::max(maxSquaredRadius, currPos.squaredLength()); - - - top += this->charHeight * 2.0; - left += horiz_height * this->charHeight * 2.0; - - // Top right - if (this->horizAlign == OgreMovableText::H_LEFT) - *pVert++ = left; - else - *pVert++ = left - (len/2.0); - - *pVert++ = top; - *pVert++ = 0; - *pVert++ = uvRect.right; - *pVert++ = uvRect.top; - - // Deal with bounds - if (this->horizAlign == OgreMovableText::H_LEFT) - currPos = Ogre::Vector3(left,top,0); - else - currPos = Ogre::Vector3(left - (len/2), top, 0); - - min.makeFloor(currPos); - max.makeFloor(currPos); - maxSquaredRadius = std::max(maxSquaredRadius, currPos.squaredLength()); - - - //------------------------------------------------------------------------ - - //------------------------------------------------------------------------ - // Second tri - // - // Top right (again) - if (this->horizAlign == OgreMovableText::H_LEFT) - *pVert++ = left; - else - *pVert++ = left - (len/2.0); - *pVert++ = top; - *pVert++ = 0; - *pVert++ = uvRect.right; - *pVert++ = uvRect.top; - - // Deal with bounds - currPos = Ogre::Vector3(left,top,0); - min.makeFloor(currPos); - max.makeFloor(currPos); - maxSquaredRadius = std::max(maxSquaredRadius, currPos.squaredLength()); - - - top -= this->charHeight * 2.0; - left -= horiz_height * this->charHeight * 2.0; - - // Bottom left (again) - if (this->horizAlign == OgreMovableText::H_LEFT) - *pVert++ = left; - else - *pVert++ = left - (len/2.0); - *pVert++ = top; - *pVert++ = 0; - *pVert++ = uvRect.left; - *pVert++ = uvRect.bottom; - - // Deal with bounds - currPos = Ogre::Vector3(left,top,0); - min.makeFloor(currPos); - max.makeFloor(currPos); - maxSquaredRadius = std::max(maxSquaredRadius, currPos.squaredLength()); - - - left += horiz_height * this->charHeight * 2.0; - - // Bottom right - if (this->horizAlign == OgreMovableText::H_LEFT) - *pVert++ = left; - else - *pVert++ = left - (len/2.0); - *pVert++ = top; - *pVert++ = 0; - *pVert++ = uvRect.right; - *pVert++ = uvRect.bottom; - - // Deal with bounds - currPos = Ogre::Vector3(left,top,0); - min.makeFloor(currPos); - max.makeFloor(currPos); - maxSquaredRadius = std::max(maxSquaredRadius, currPos.squaredLength()); - - //------------------------------------------------------------------------- - - - // Go back up with top - top += this->charHeight * 2.0; - - float currentWidth = (left + 1.0)/2.0; - if (currentWidth > largestWidth) - { - largestWidth = currentWidth; - } - } - - // Unlock vertex buffer - ptbuf->unlock(); - - - /* min.x=min.y=min.z=-10000; - max.x=max.y=max.z = 10000; - */ - - // update AABB/Sphere radius - this->aabb = Ogre::AxisAlignedBox(min, max); - this->radius = Ogre::Math::Sqrt(maxSquaredRadius); - - if (this->updateColors) - this->_updateColors(); - - this->needUpdate = false; -} - -//////////////////////////////////////////////////////////////////////////////// -// Update the colors -void OgreMovableText::_updateColors(void) -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - - Ogre::RGBA color; - Ogre::HardwareVertexBufferSharedPtr vbuf; - Ogre::RGBA *pDest=NULL; - unsigned int i; - - assert(this->font); - assert(!this->material.isNull()); - - // Convert to system-specific - Ogre::Root::getSingleton().convertColourValue(this->color, & color); - - vbuf = this->renderOp.vertexData->vertexBufferBinding->getBuffer(COLOUR_BINDING); - - pDest = static_cast<Ogre::RGBA*>(vbuf->lock(Ogre::HardwareBuffer::HBL_DISCARD)); - - for (i = 0; i < this->renderOp.vertexData->vertexCount; ++i) - { - *pDest++ = color; - } - - vbuf->unlock(); - this->updateColors = false; -} - -//////////////////////////////////////////////////////////////////////////////// -const Ogre::Quaternion & OgreMovableText::getWorldOrientation(void) const -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - assert(this->camera); - return const_cast<Ogre::Quaternion&>(this->camera->getDerivedOrientation()); - //return mParentNode->_getDerivedOrientation(); -} - -//////////////////////////////////////////////////////////////////////////////// -const Ogre::Vector3 & OgreMovableText::getWorldPosition(void) const -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - assert(mParentNode); - return mParentNode->_getDerivedPosition(); -} - -//////////////////////////////////////////////////////////////////////////////// -const Ogre::AxisAlignedBox &OgreMovableText::getBoundingBox(void) const -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - return this->aabb; -} - -//////////////////////////////////////////////////////////////////////////////// -const Ogre::String &OgreMovableText::getMovableType() const -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - static Ogre::String movType = "OgreMovableText"; - return movType; -} - -//////////////////////////////////////////////////////////////////////////////// -void OgreMovableText::getWorldTransforms(Ogre::Matrix4 * xform) const -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - if (this->isVisible() && this->camera) - { - Ogre::Matrix3 rot3x3, scale3x3 = Ogre::Matrix3::IDENTITY; - - // store rotation in a matrix - this->camera->getDerivedOrientation().ToRotationMatrix(rot3x3); - //mParentNode->_getDerivedOrientation().ToRotationMatrix(rot3x3); - - // parent node position - Ogre::Vector3 ppos = mParentNode->_getDerivedPosition() + Ogre::Vector3::UNIT_Y * this->baseline; - -// std::cout << "Parent Pos[" << ppos << "]\n"; - - // apply scale - scale3x3[0][0] = mParentNode->_getDerivedScale().x / 2; - scale3x3[1][1] = mParentNode->_getDerivedScale().y / 2; - scale3x3[2][2] = mParentNode->_getDerivedScale().z / 2; - - // apply all transforms to xform - *xform = (rot3x3 * scale3x3); - xform->setTrans(ppos); - } -} - -//////////////////////////////////////////////////////////////////////////////// -float OgreMovableText::getBoundingRadius() const -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - return this->radius; -} - -//////////////////////////////////////////////////////////////////////////////// -float OgreMovableText::getSquaredViewDepth(const Ogre::Camera *cam) const -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - return 0; -} - -//////////////////////////////////////////////////////////////////////////////// -void OgreMovableText::getRenderOperation(Ogre::RenderOperation & op) -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - - if (this->isVisible()) - { - if (this->needUpdate) - this->_setupGeometry(); - if (this->updateColors) - this->_updateColors(); - op = this->renderOp; - } -} - -//////////////////////////////////////////////////////////////////////////////// -const Ogre::MaterialPtr &OgreMovableText::getMaterial(void) const -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - assert(!this->material.isNull()); - return this->material; -} - -//////////////////////////////////////////////////////////////////////////////// -// -const Ogre::LightList &OgreMovableText::getLights(void) const -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - return this->lightList; -} - -//////////////////////////////////////////////////////////////////////////////// -void OgreMovableText::_notifyCurrentCamera(Ogre::Camera *cam) -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - this->camera = cam; -} - -//////////////////////////////////////////////////////////////////////////////// -void OgreMovableText::_updateRenderQueue(Ogre::RenderQueue* queue) -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - if (this->isVisible()) - { - if (this->needUpdate) - this->_setupGeometry(); - - if (this->updateColors) - this->_updateColors(); - - queue->addRenderable(this, mRenderQueueID, OGRE_RENDERABLE_DEFAULT_PRIORITY); - } -} - -//////////////////////////////////////////////////////////////////////////////// -/// Method to allow a caller to abstractly iterate over the Renderable instances -void OgreMovableText::visitRenderables( Ogre::Renderable::Visitor* /*visitor*/, - bool /*debug*/) -{ - boost::recursive_mutex::scoped_lock lock(*this->mutex); - return; -} - - Deleted: code/branches/federation/gazebo/server/rendering/MovableText.hh =================================================================== --- code/branches/federation/gazebo/server/rendering/MovableText.hh 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/server/rendering/MovableText.hh 2009-03-12 18:50:11 UTC (rev 7465) @@ -1,187 +0,0 @@ -/* - * Gazebo - Outdoor Multi-Robot Simulator - * Copyright (C) 2003 - * Nate Koenig & Andrew Howard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - */ -/* Desc: Middleman between OGRE and Gazebo - * Author: ind...@gm... - * Date: 13 Feb 2006 - * CVS: $Id$ - */ - -#ifndef MOVABLETEXT_HH -#define MOVABLETEXT_HH - -#include <Ogre.h> -#include <OgreMovableObject.h> -#include <OgreRenderable.h> -#include <OgreUserDefinedObject.h> -#include <string> - -namespace boost -{ - class recursive_mutex; -} - -namespace gazebo -{ - /// \brief Movable text - class OgreMovableText : public Ogre::MovableObject, public Ogre::Renderable - { - /// \brief Horizontal alignment - public: enum HorizAlign {H_LEFT, H_CENTER}; - - /// \brief vertical alignment - public: enum VertAlign {V_BELOW, V_ABOVE}; - - /// \brief Constructor - public: OgreMovableText(); - - /// \brief Destructor - public: virtual ~OgreMovableText(); - - /// \brief Loads text and font info - public: void Load(const std::string &name, - const Ogre::UTFString &text, - const std::string &fontName = "Arial", - float charHeight = 1.0, - const Ogre::ColourValue &color = Ogre::ColourValue::White); - - /// \brief Set the font - public: void SetFontName(const std::string &font); - - /// \brief Get the font - public: const std::string &GetFont() const; - - /// \brief Set the text to display - public: void SetText(const Ogre::UTFString & caption); - - /// \brief Get the displayed text - public: const Ogre::UTFString & GetText() const; - - /// \brief Set the text color - public: void SetColor(const Ogre::ColourValue & color); - - /// \brief Get the text color - public: const Ogre::ColourValue & GetColor() const; - - /// \brief Set the height of a character - public: void SetCharHeight(float height); - - /// \brief Set the height of a characters - public: float GetCharHeight() const; - - /// \brief Set the width of a space - public:void SetSpaceWidth(float width); - - /// \brief Get the width of a space - public: float GetSpaceWidth() const; - - /// \brief Set the alignment of the text - public: void SetTextAlignment(const HorizAlign &hAlign, - const VertAlign &vAlign); - - /// \brief Set the baseline height of the text - public: void SetBaseline(float height); - - /// \brief Get the baseline height - public: float GetBaseline() const; - - /// \brief True=text always is displayed ontop - public: void SetShowOnTop(bool show); - - /// \brief True=text is displayed on top - public: bool GetShowOnTop() const; - - /// \brief Get the axis aligned bounding box of the text - public: Ogre::AxisAlignedBox GetAABB(); - - /// \brief Method to allow a caller to abstractly iterate over the - // Renderable instances - public: virtual void visitRenderables( Ogre::Renderable::Visitor* visitor, - bool debug = false ); - - /// \brief Update the text - public: void Update(); - - /// \brief setup the geometry (from MovableText) - protected: void _setupGeometry(); - - /// \brief update the color(from MovableText) - protected: void _updateColors(); - - /// \brief Get the world transform (from MovableObject) - protected: void getWorldTransforms(Ogre::Matrix4 *xform) const; - - /// \brief Get the bounding radiu (from MovableObject) - protected: float getBoundingRadius() const; - - /// \brief Get the squared view depth (from MovableObject) - protected: float getSquaredViewDepth(const Ogre::Camera *cam) const; - - private: std::string fontName; - private: Ogre::UTFString text; - - private: Ogre::ColourValue color; - private: Ogre::RenderOperation renderOp; - private: Ogre::AxisAlignedBox aabb; - private: Ogre::LightList lightList; - - private: float charHeight; - - private: bool needUpdate; - - private: float radius; - - private: Ogre::Camera *camera; - private: Ogre::RenderWindow *renderWindow; - private: float viewportAspectCoef; - private: Ogre::Font *font; - private: float spaceWidth; - private: bool updateColors; - private: VertAlign vertAlign; - private: HorizAlign horizAlign; - private: bool onTop; - private: float baseline; - private: Ogre::MaterialPtr material; - private: Ogre::MaterialPtr backgroundMaterial; - - private: const Ogre::Quaternion &getWorldOrientation(void) const; - private: const Ogre::Vector3 &getWorldPosition(void) const; - private: const Ogre::AxisAlignedBox &getBoundingBox(void) const; - - private: const Ogre::String &getMovableType() const; - - private: void _notifyCurrentCamera(Ogre::Camera *cam); - private: void _updateRenderQueue(Ogre::RenderQueue* queue); - - /// \brief Get the render operation - protected: void getRenderOperation(Ogre::RenderOperation &op); - - /// \brief Get the material - protected: const Ogre::MaterialPtr &getMaterial(void) const; - - /// \brief Get the lights - protected: const Ogre::LightList &getLights(void) const; //{return mLList;}; - private: bool dirty; - - private: boost::recursive_mutex *mutex; - }; -} - -#endif Modified: code/branches/federation/gazebo/server/rendering/OgreAdaptor.cc =================================================================== --- code/branches/federation/gazebo/server/rendering/OgreAdaptor.cc 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/server/rendering/OgreAdaptor.cc 2009-03-12 18:50:11 UTC (rev 7465) @@ -37,7 +37,7 @@ #include "Model.hh" #include "OgreVisual.hh" #include "UserCamera.hh" -#include "MovableText.hh" +#include "OgreMovableText.hh" #include "OgreHUD.hh" #include "Entity.hh" #include "GazeboError.hh" Modified: code/branches/federation/gazebo/server/rendering/OgreCreator.cc =================================================================== --- code/branches/federation/gazebo/server/rendering/OgreCreator.cc 2009-03-12 17:47:22 UTC (rev 7464) +++ code/branches/federation/gazebo/server/rendering/OgreCreator.cc 2009-03-12 18:50:11 UTC (rev 7465) @@ -38,7 +38,7 @@ #include "XMLConfig.hh" #include "GazeboError.hh" #include "GazeboMessage.hh" -#include "MovableText.hh" +#include "OgreMovableText.hh" #include "OgreAdaptor.hh" #include "OgreVisual.hh" #include "OgreDynamicLines.hh" @@ -466,7 +466,7 @@ sprintf(name,"(%d %d)_yaxis",0,y); sprintf(text,"%d",y); - MovableText* msg = new MovableText(); + OgreMovableText* msg = new OgreMovableText(); try { msg->Load(name, text,"Arial",0.08); @@ -477,7 +477,7 @@ stream << "Unable to create the text. " << e.getDescription() <<std::endl; gzthrow(stream.str() ); } - msg->SetTextAlignment(MovableText::H_CENTER, MovableText::V_ABOVE... [truncated message content] |
From: <na...@us...> - 2009-03-12 23:37:17
|
Revision: 7467 http://playerstage.svn.sourceforge.net/playerstage/?rev=7467&view=rev Author: natepak Date: 2009-03-12 23:37:08 +0000 (Thu, 12 Mar 2009) Log Message: ----------- Updates to the threading Modified Paths: -------------- code/branches/federation/gazebo/SConstruct code/branches/federation/gazebo/build.py code/branches/federation/gazebo/libgazebo/gazebo.h code/branches/federation/gazebo/player/GazeboTime.cc code/branches/federation/gazebo/server/gui/StatusBar.cc code/branches/federation/gazebo/server/physics/Body.cc code/branches/federation/gazebo/server/physics/Geom.cc code/branches/federation/gazebo/server/rendering/OgreCreator.cc Modified: code/branches/federation/gazebo/SConstruct =================================================================== --- code/branches/federation/gazebo/SConstruct 2009-03-12 21:52:35 UTC (rev 7466) +++ code/branches/federation/gazebo/SConstruct 2009-03-12 23:37:08 UTC (rev 7467) @@ -37,7 +37,8 @@ 'msg' : 'Error: Ogre3d and development files are required, but not\ found. ', 'flags' : '', - 'web' : 'http://www.ogre3d.org' + 'web' : 'http://www.ogre3d.org', + 'required' : True }, 'ODE' : { @@ -46,7 +47,8 @@ 'msg' : 'Error: ODE and development files are required, but not\ found.', 'flags' : '', - 'web' : 'http://www.ode.org' + 'web' : 'http://www.ode.org', + 'required' : True }, 'OpenAL' : { @@ -54,7 +56,8 @@ 'check' : 'openal', 'msg' : 'Warning: OpenAL not found. 3D audio will be disbled', 'flags' : ['-DHAVE_OPENAL'], - 'web' : 'http://www.openal.org' + 'web' : 'http://www.openal.org', + 'required' : False }, 'XFT' : { @@ -62,7 +65,8 @@ 'check' : 'xft', 'msg' : 'Error: ', 'flags' : '', - 'web' : 'http://www.xft.org' + 'web' : 'http://www.xft.org', + 'required' : True }, 'avformat' : { @@ -71,7 +75,8 @@ 'msg' : 'Warning: FFMpeg pkg-config not found. MP3 decoding\ is disabled', 'flags' : '', - 'web' : 'http://ffmpeg.mplayerhq.hu/' + 'web' : 'http://ffmpeg.mplayerhq.hu/', + 'required' : False }, 'avcodec' : { @@ -80,7 +85,8 @@ 'msg' : 'Warning: FFMpeg pkg-config not found. MP3 decoding is\ disabled', 'flags' : '', - 'web' : 'http://ffmpeg.mplayerhq.hu/' + 'web' : 'http://ffmpeg.mplayerhq.hu/', + 'required' : False }, 'XML2' : { @@ -89,7 +95,8 @@ 'msg' : 'Error: libxml2 and development files are required, but\ not found.', 'flags' : '', - 'web' : 'http://www.xmlsoft.org' + 'web' : 'http://www.xmlsoft.org', + 'required' : True }, 'FLTK' : { @@ -99,42 +106,48 @@ 'msg' : 'Error: libfltk & development files are required, but not\ found.', 'flags' : '', - 'web' : 'http://www.fltk.org' + 'web' : 'http://www.fltk.org', + 'required' : True }, 'Player' : { 'pkgcfg' : 'pkg-config --cflags --libs playerc++', 'check' : 'playerc++', 'msg' : 'Warning: Player not found, bindings will not be built.', 'flags' : ['-DHAVE_PLAYER'], - 'web' : 'http://playerstage.sourceforge.net' + 'web' : 'http://playerstage.sourceforge.net', + 'required' : False }, 'event' : { 'header' : 'event.h', 'lib' : 'event', 'msg' : 'Warning: libevent not found. Webgazebo will no be built.', 'flags' : ['-DHAVE_EVENT'], - 'web' : 'http://www.monkey.org/~provos/libevent/' + 'web' : 'http://www.monkey.org/~provos/libevent/', + 'required' : False }, 'yaml' : { 'header' : 'yaml.h', 'lib' : 'yaml', 'msg' : 'Warning: yaml not found. Webgazebo will not be built.', 'flags' : ['-DHAVE_YAML'], - 'web' : 'http://www.yaml.org' + 'web' : 'http://www.yaml.org', + 'required' : False }, 'websim' : { 'pkgcfg' : 'pkg-config --cflags --libs websim', 'check' : 'websim', 'msg' : 'Warning: websim not found. Webgazebo will not be built.', 'flags' : ['-DHAVE_WEBSIM'], - 'web' : 'http://www.playerstage.sourceforge.net' + 'web' : 'http://www.playerstage.sourceforge.net', + 'required' : False }, 'glib' : { 'pkgcfg' : 'pkg-config --cflags --libs glib-2.0', 'check' : 'glib-2.0', 'msg' : 'Warning: glib not found. Webgazebo will not be built.', 'flags' : '', - 'web' : 'http://www.gtk.org' + 'web' : 'http://www.gtk.org', + 'required' : False }, } @@ -165,6 +178,8 @@ SHCXXCOMSTR = 'Compiling $TARGET', SHCCCOMSTR = 'Compiling $TARGET', SHLINKCOMSTR = 'Linking $TARGET', + + HAS_ERROR = False, ) @@ -225,6 +240,10 @@ Config(env, packages) +if env['HAS_ERROR'] == True: + print "Build failed due to missing packages. See above errors" + Exit() + ####### # Export the environment ####### Modified: code/branches/federation/gazebo/build.py =================================================================== --- code/branches/federation/gazebo/build.py 2009-03-12 21:52:35 UTC (rev 7466) +++ code/branches/federation/gazebo/build.py 2009-03-12 23:37:08 UTC (rev 7467) @@ -138,6 +138,7 @@ header='' lib='' check='' + requred = True if packages[key].has_key('pkgcfg'): pkgcfg = packages[key]['pkgcfg'] @@ -151,6 +152,10 @@ if packages[key].has_key('check'): check = packages[key]['check'] + if packages[key].has_key('required'): + required = packages[key]['required'] + + msg = packages[key]['msg'] web = packages[key]['web'] flags = packages[key]['flags'] @@ -164,8 +169,8 @@ valid = False print ' !!' + msg + key + ' not found.' print ' See: ' + web - if msg.find('Error') > 0: - Exit(1) + if required: + env['HAS_ERROR'] = True; # Try parsing the pkg-config if docfg and pkgcfg: @@ -182,8 +187,8 @@ print "Unable to parse config ["+pkgcfg+"]" print ' !!' + msg + key + ' not found.' print ' See: ' + web - if msg.find('Error') > 0: - Exit(1) + if required: + env['HAS_ERROR'] = True; elif header and lib: if not conf.CheckLibWithHeader(lib, header, 'c'): valid = False Modified: code/branches/federation/gazebo/libgazebo/gazebo.h =================================================================== --- code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-12 21:52:35 UTC (rev 7466) +++ code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-12 23:37:08 UTC (rev 7467) @@ -379,7 +379,7 @@ /// Number of times the interface has been opened public: int openCount; - public: float time; + public: double time; public: int version; Modified: code/branches/federation/gazebo/player/GazeboTime.cc =================================================================== --- code/branches/federation/gazebo/player/GazeboTime.cc 2009-03-12 21:52:35 UTC (rev 7466) +++ code/branches/federation/gazebo/player/GazeboTime.cc 2009-03-12 23:37:08 UTC (rev 7467) @@ -48,7 +48,7 @@ int GazeboTime::GetTime(struct timeval* time) { time->tv_sec = (int) floor(GazeboClient::sim->data->simTime); - time->tv_usec = (int) floor(fmod(GazeboClient::sim->data->simTime, 1.0) * 1e6); + time->tv_usec = (int) floor(fmod((double)GazeboClient::sim->data->simTime, (double)1.0) * 1e6); return 0; } @@ -60,7 +60,7 @@ struct timeval ts; ts.tv_sec = (int) floor(GazeboClient::sim->data->simTime); - ts.tv_usec = (int) floor(fmod(GazeboClient::sim->data->simTime, 1.0) * 1e6); + ts.tv_usec = (int) floor(fmod((double)GazeboClient::sim->data->simTime, (double)1.0) * 1e6); *time = ts.tv_sec + ts.tv_usec/1e6; Modified: code/branches/federation/gazebo/server/gui/StatusBar.cc =================================================================== --- code/branches/federation/gazebo/server/gui/StatusBar.cc 2009-03-12 21:52:35 UTC (rev 7466) +++ code/branches/federation/gazebo/server/gui/StatusBar.cc 2009-03-12 23:37:08 UTC (rev 7467) @@ -44,22 +44,22 @@ y += 5; this->box(FL_UP_BOX); - this->fps = new Fl_Value_Output(x,y,40,20,"FPS"); + this->fps = new Fl_Value_Output(x,y,35,20,"FPS"); this->fps->precision(0); - x = this->fps->x() + this->fps->w() + 80; - this->realTime = new Fl_Value_Output(x,y,55,20,"Real Time"); + x = this->fps->x() + this->fps->w() + 95; + this->percent = new Fl_Value_Output(x,y,65,20,"% Real Time"); + this->percent->precision(2); + + x = this->percent->x() + this->percent->w() + 80; + this->realTime = new Fl_Value_Output(x,y,55,20,"Real Time (sec)"); this->realTime->precision(2); - x = this->realTime->x() + this->realTime->w() + 80; - this->percent = new Fl_Value_Output(x,y,55,20,"% Real Time"); - this->percent->precision(2); - - x = this->percent->x() + this->percent->w() + 75; - this->simTime = new Fl_Value_Output(x,y,55,20,"Sim Time"); + x = this->realTime->x() + this->realTime->w() + 85; + this->simTime = new Fl_Value_Output(x,y,65,20,"Sim Time (sec)"); this->simTime->precision(2); - x = this->simTime->x() + this->simTime->w() + 90; + x = this->simTime->x() + this->simTime->w() + 100; this->pauseTime = new Fl_Value_Output(x,y,55,20,"Pause Time"); this->pauseTime->precision(2); @@ -87,19 +87,29 @@ void StatusBar::Update() { float avgFPS = 0; + float percent = 0; + float sim = 0; avgFPS = this->gui->GetAvgFPS(); + percent = 100. * (Simulator::Instance()->GetSimTime() / Simulator::Instance()->GetRealTime()); + sim = Simulator::Instance()->GetSimTime(); + if (sim > 9999) + { + sim /= 60; + this->simTime->lable("Sim Time (min)"); + } + //this->iterations->value(Simulator::Instance()->GetIterations()); this->fps->value(avgFPS); - this->percent->value(Simulator::Instance()->GetSimTime() / Simulator::Instance()->GetRealTime()); + this->percent->value(percent); if (Simulator::Instance()->GetRealTime() - this->realTime->value() > 0.1) { this->realTime->value(Simulator::Instance()->GetRealTime()); } - this->simTime->value(Simulator::Instance()->GetSimTime()); + this->simTime->value(); this->pauseTime->value(Simulator::Instance()->GetPauseTime()); } Modified: code/branches/federation/gazebo/server/physics/Body.cc =================================================================== --- code/branches/federation/gazebo/server/physics/Body.cc 2009-03-12 21:52:35 UTC (rev 7466) +++ code/branches/federation/gazebo/server/physics/Body.cc 2009-03-12 23:37:08 UTC (rev 7467) @@ -328,7 +328,7 @@ // Set the pose of the body void Body::SetPose(const Pose3d &_pose) { - boost::recursive_mutex::scoped_lock lock(*this->mutex); + //boost::recursive_mutex::scoped_lock lock(*this->mutex); if (this->IsStatic()) { @@ -365,7 +365,7 @@ // Return the pose of the body Pose3d Body::GetPose() const { - boost::recursive_mutex::scoped_lock lock(*this->mutex); + //boost::recursive_mutex::scoped_lock lock(*this->mutex); /*if (this->IsStatic()) return this->staticPose; @@ -378,7 +378,7 @@ // Update the pose of the body void Body::UpdatePose() { - boost::recursive_mutex::scoped_lock lock(*this->mutex); + //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/Geom.cc =================================================================== --- code/branches/federation/gazebo/server/physics/Geom.cc 2009-03-12 21:52:35 UTC (rev 7466) +++ code/branches/federation/gazebo/server/physics/Geom.cc 2009-03-12 23:37:08 UTC (rev 7467) @@ -313,7 +313,7 @@ // Set the pose relative to the body void Geom::SetPose(const Pose3d &newPose, bool updateCoM) { - boost::recursive_mutex::scoped_lock lock(*this->mutex); + //boost::recursive_mutex::scoped_lock lock(*this->mutex); if (this->placeable && this->geomId) { @@ -346,7 +346,7 @@ { Pose3d pose; - boost::recursive_mutex::scoped_lock lock(*this->mutex); + //boost::recursive_mutex::scoped_lock lock(*this->mutex); if (this->placeable && this->geomId) { @@ -380,7 +380,7 @@ void Geom::SetPosition(const Vector3 &pos) { Pose3d pose; - boost::recursive_mutex::scoped_lock lock(*this->mutex); + //boost::recursive_mutex::scoped_lock lock(*this->mutex); pose = this->GetPose(); pose.pos = pos; @@ -392,7 +392,7 @@ void Geom::SetRotation(const Quatern &rot) { Pose3d pose; - boost::recursive_mutex::scoped_lock lock(*this->mutex); + //boost::recursive_mutex::scoped_lock lock(*this->mutex); pose = this->GetPose(); pose.rot = rot; Modified: code/branches/federation/gazebo/server/rendering/OgreCreator.cc =================================================================== --- code/branches/federation/gazebo/server/rendering/OgreCreator.cc 2009-03-12 21:52:35 UTC (rev 7466) +++ code/branches/federation/gazebo/server/rendering/OgreCreator.cc 2009-03-12 23:37:08 UTC (rev 7467) @@ -703,9 +703,6 @@ 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 This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-03-17 00:18:33
|
Revision: 7491 http://playerstage.svn.sourceforge.net/playerstage/?rev=7491&view=rev Author: natepak Date: 2009-03-17 00:18:30 +0000 (Tue, 17 Mar 2009) Log Message: ----------- Throttle the gui update to 30hz Modified Paths: -------------- code/branches/federation/gazebo/SConstruct code/branches/federation/gazebo/server/Simulator.cc code/branches/federation/gazebo/worlds/bandit.world Modified: code/branches/federation/gazebo/SConstruct =================================================================== --- code/branches/federation/gazebo/SConstruct 2009-03-17 00:01:48 UTC (rev 7490) +++ code/branches/federation/gazebo/SConstruct 2009-03-17 00:18:30 UTC (rev 7491) @@ -173,11 +173,13 @@ CXXCOMSTR = 'Compiling $TARGET', CCCOMSTR = 'Compiling $TARGET', LINKCOMSTR = 'Linking $TARGET', + ARCOMSTR = 'Linking $TARGET', # Shared object compile strings SHCXXCOMSTR = 'Compiling $TARGET', SHCCCOMSTR = 'Compiling $TARGET', SHLINKCOMSTR = 'Linking $TARGET', + SHARCOMSTR = 'Linking $TARGET', HAS_ERROR = False, ) Modified: code/branches/federation/gazebo/server/Simulator.cc =================================================================== --- code/branches/federation/gazebo/server/Simulator.cc 2009-03-17 00:01:48 UTC (rev 7490) +++ code/branches/federation/gazebo/server/Simulator.cc 2009-03-17 00:18:30 UTC (rev 7491) @@ -286,6 +286,9 @@ /// Main simulation loop, when this loop ends the simulation finish void Simulator::MainLoop() { + double currTime, lastTime; + double freq = 30.0; + this->physicsThread = new boost::thread( boost::bind(&Simulator::PhysicsLoop, this)); if (this->gui) @@ -293,8 +296,24 @@ // Update the gui while (!this->userQuit) { - this->gui->Update(); - usleep(1000); + currTime = this->GetWallTime(); + if ( currTime - lastTime > 1/freq) + { + lastTime = this->GetWallTime(); + this->gui->Update(); + currTime = this->GetWallTime(); + + //printf("Curr Time[%f] LastTime[%f] Period[%f] %f]\n",currTime, lastTime, 1/freq, currTime-lastTime); + if (currTime - lastTime < 1/freq) + { + usleep((1/freq - (currTime - lastTime)) * 1e6); + } + } + else + { + //printf("Now Here[%f]\n",1/freq - (currTime - lastTime)); + usleep((1/freq - currTime - lastTime) * 1e6); + } } } @@ -545,8 +564,7 @@ boost::recursive_mutex::scoped_lock lock(*this->mutex); world->Update(); } - - usleep(100); + usleep(1); } // Process all incoming messages from simiface Modified: code/branches/federation/gazebo/worlds/bandit.world =================================================================== --- code/branches/federation/gazebo/worlds/bandit.world 2009-03-17 00:01:48 UTC (rev 7490) +++ code/branches/federation/gazebo/worlds/bandit.world 2009-03-17 00:18:30 UTC (rev 7491) @@ -16,7 +16,7 @@ <verbosity>5</verbosity> <physics:ode> - <stepTime>0.003</stepTime> + <stepTime>0.03</stepTime> <gravity>0 0 -9.8</gravity> <cfm>10e-5</cfm> <erp>0.3</erp> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-03-24 19:40:43
|
Revision: 7542 http://playerstage.svn.sourceforge.net/playerstage/?rev=7542&view=rev Author: natepak Date: 2009-03-24 19:40:33 +0000 (Tue, 24 Mar 2009) Log Message: ----------- Updated the gripper Modified Paths: -------------- code/branches/federation/gazebo/server/Angle.hh code/branches/federation/gazebo/server/Model.cc code/branches/federation/gazebo/server/Model.hh code/branches/federation/gazebo/server/Simulator.cc code/branches/federation/gazebo/server/World.cc code/branches/federation/gazebo/server/controllers/Controller.cc code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh code/branches/federation/gazebo/server/main.cc code/branches/federation/gazebo/server/physics/Body.cc code/branches/federation/gazebo/server/physics/Body.hh code/branches/federation/gazebo/server/physics/Joint.cc code/branches/federation/gazebo/server/physics/RayGeom.cc code/branches/federation/gazebo/server/rendering/OgreDynamicLines.cc code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc code/branches/federation/gazebo/server/sensors/ray/RaySensor.hh code/branches/federation/gazebo/worlds/models/pioneer2gripper.model code/branches/federation/gazebo/worlds/models/sicklms200.model Modified: code/branches/federation/gazebo/server/Angle.hh =================================================================== --- code/branches/federation/gazebo/server/Angle.hh 2009-03-24 18:50:06 UTC (rev 7541) +++ code/branches/federation/gazebo/server/Angle.hh 2009-03-24 19:40:33 UTC (rev 7542) @@ -28,6 +28,7 @@ #define ANGLE_HH #include <iostream> +#include <math.h> namespace gazebo { Modified: code/branches/federation/gazebo/server/Model.cc =================================================================== --- code/branches/federation/gazebo/server/Model.cc 2009-03-24 18:50:06 UTC (rev 7541) +++ code/branches/federation/gazebo/server/Model.cc 2009-03-24 19:40:33 UTC (rev 7542) @@ -96,29 +96,53 @@ std::map< std::string, Controller* >::iterator citer; if (this->graphicsHandler) + { delete this->graphicsHandler; + this->graphicsHandler = NULL; + } for (biter=this->bodies.begin(); biter != this->bodies.end(); biter++) { - GZ_DELETE(biter->second); + if (biter->second) + { + delete biter->second; + biter->second = NULL; + } } this->bodies.clear(); for (jiter = this->joints.begin(); jiter != this->joints.end(); jiter++) { - GZ_DELETE( jiter->second ); + if (jiter->second) + { + delete jiter->second; + jiter->second=NULL; + } } this->joints.clear(); for (citer = this->controllers.begin(); citer != this->controllers.end(); citer++) { - GZ_DELETE( citer->second ); + if (citer->second) + { + delete citer->second; + citer->second = NULL; + } } this->controllers.clear(); - GZ_DELETE(this->parentBodyNameP); - GZ_DELETE(this->myBodyNameP); + if (this->parentBodyNameP) + { + delete this->parentBodyNameP; + this->parentBodyNameP = NULL; + } + + if (this->myBodyNameP) + { + delete this->myBodyNameP; + this->myBodyNameP = NULL; + } } //////////////////////////////////////////////////////////////////////////////// @@ -741,6 +765,7 @@ std::cerr << "Error Loading Controller[" << controllerName << "]\n" << e << std::endl; delete controller; + controller = NULL; return; } @@ -766,8 +791,40 @@ { return &(this->bodies); } + +//////////////////////////////////////////////////////////////////////////////// +/// Get a sensor by name +Sensor *Model::GetSensor(const std::string &name) const +{ + Sensor *sensor = NULL; + std::map< std::string, Body* >::const_iterator biter; + + for (biter=this->bodies.begin(); biter != this->bodies.end(); biter++) + { + if ( (sensor = biter->second->GetSensor(name)) != NULL) + break; + } + + return sensor; +} //////////////////////////////////////////////////////////////////////////////// +/// Get a geom by name +Geom *Model::GetGeom(const std::string &name) const +{ + Geom *geom = NULL; + std::map< std::string, Body* >::const_iterator biter; + + for (biter=this->bodies.begin(); biter != this->bodies.end(); biter++) + { + if ( (geom = biter->second->GetGeom(name)) != NULL) + break; + } + + return geom; +} + +//////////////////////////////////////////////////////////////////////////////// /// Get a body by name Body *Model::GetBody(const std::string &name) { Modified: code/branches/federation/gazebo/server/Model.hh =================================================================== --- code/branches/federation/gazebo/server/Model.hh 2009-03-24 18:50:06 UTC (rev 7541) +++ code/branches/federation/gazebo/server/Model.hh 2009-03-24 19:40:33 UTC (rev 7542) @@ -49,6 +49,8 @@ class Controller; class HingeJoint; class GraphicsIfaceHandler; + class Sensor; + class Geom; /// \addtogroup gazebo_server /// \brief A model @@ -149,6 +151,12 @@ /// \brief Get a map of all the bodies public: const std::map<std::string, Body*> *GetBodies() const; + + /// \brief Get a sensor by name + public: Sensor *GetSensor(const std::string &name) const; + + /// \brief Get a geom by name + public: Geom *GetGeom(const std::string &name) const; /// \brief Attach this model to its parent public: void Attach(XMLConfigNode *node); Modified: code/branches/federation/gazebo/server/Simulator.cc =================================================================== --- code/branches/federation/gazebo/server/Simulator.cc 2009-03-24 18:50:06 UTC (rev 7541) +++ code/branches/federation/gazebo/server/Simulator.cc 2009-03-24 19:40:33 UTC (rev 7542) @@ -84,10 +84,42 @@ // Destructor Simulator::~Simulator() { - this->Close(); - delete this->mutex; - this->mutex = NULL; + if (this->gazeboConfig) + { + delete this->gazeboConfig; + this->gazeboConfig = NULL; + } + + if (this->xmlFile) + { + delete this->xmlFile; + this->xmlFile = NULL; + } + + if (this->mutex) + { + delete this->mutex; + this->mutex = NULL; + } + + if (this->gui) + { + delete this->gui; + this->gui = NULL; + } + + if (this->physicsThread) + { + delete this->physicsThread; + this->physicsThread = NULL; + } + + if (this->mutex) + { + delete this->mutex; + this->mutex = NULL; + } } //////////////////////////////////////////////////////////////////////////////// @@ -97,13 +129,8 @@ if (!this->loaded) return; - GZ_DELETE (this->gui) - GZ_DELETE (this->xmlFile) - GZ_DELETE (this->gazeboConfig) gazebo::World::Instance()->Close(); gazebo::OgreAdaptor::Instance()->Close(); - - //GZ_DELETE(this->renderEngine); } //////////////////////////////////////////////////////////////////////////////// @@ -280,6 +307,8 @@ void Simulator::Fini( ) { gazebo::World::Instance()->Fini(); + + this->Close(); } //////////////////////////////////////////////////////////////////////////////// Modified: code/branches/federation/gazebo/server/World.cc =================================================================== --- code/branches/federation/gazebo/server/World.cc 2009-03-24 18:50:06 UTC (rev 7541) +++ code/branches/federation/gazebo/server/World.cc 2009-03-24 19:40:33 UTC (rev 7542) @@ -87,12 +87,25 @@ this->models.clear(); this->geometries.clear(); - GZ_DELETE (this->physicsEngine) - GZ_DELETE (this->server) + if (this->physicsEngine) + { + delete this->physicsEngine; + this->physicsEngine = NULL; + } + if (this->server) + { + delete this->server; + this->server =NULL; + } + try { - GZ_DELETE (this->simIface) + if (this->simIface) + { + delete this->simIface; + this->simIface = NULL; + } } catch (std::string e) { Modified: code/branches/federation/gazebo/server/controllers/Controller.cc =================================================================== --- code/branches/federation/gazebo/server/controllers/Controller.cc 2009-03-24 18:50:06 UTC (rev 7541) +++ code/branches/federation/gazebo/server/controllers/Controller.cc 2009-03-24 19:40:33 UTC (rev 7542) @@ -58,6 +58,7 @@ /// Destructor Controller::~Controller() { + std::cout << "Deleting Controller[" << **this->nameP << "]\n"; delete this->nameP; delete this->updatePeriodP; } Modified: code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc =================================================================== --- code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc 2009-03-24 18:50:06 UTC (rev 7541) +++ code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc 2009-03-24 19:40:33 UTC (rev 7542) @@ -25,6 +25,13 @@ * SVN info: $Id$ */ +#include <boost/bind.hpp> + +#include "PhysicsEngine.hh" +#include "Geom.hh" +#include "ContactParams.hh" +#include "Simulator.hh" +#include "RaySensor.hh" #include "Global.hh" #include "XMLConfig.hh" #include "Model.hh" @@ -57,12 +64,35 @@ // Destructor Pioneer2_Gripper::~Pioneer2_Gripper() { + if (this->holdJoint) + delete this->holdJoint; + this->holdJoint = NULL; + for (int i=0; i<3; i++) { - GZ_DELETE(this->jointNamesP[i]); - GZ_DELETE(this->gainsP[i]); - GZ_DELETE(this->forcesP[i]); + if (this->jointNamesP[i]) + delete this->jointNamesP[i]; + this->jointNamesP[i] = NULL; + + if (this->gainsP[i]) + delete this->gainsP[i]; + this->gainsP[i] = NULL; + + if (this->forcesP[i]) + delete this->forcesP[i]; + this->forcesP[i] = NULL; } + + for (int i =0; i<2; i++) + { + if (this->breakBeamNamesP[i]) + delete this->breakBeamNamesP[i]; + this->breakBeamNamesP[i] = NULL; + + if (this->paddleNamesP[i]) + delete this->paddleNamesP[i]; + this->paddleNamesP[i] = NULL; + } } //////////////////////////////////////////////////////////////////////////////// @@ -133,8 +163,26 @@ this->joints[LIFT] = dynamic_cast<SliderJoint*>(this->myParent->GetJoint(this->jointNamesP[LIFT]->GetValue())); } + this->breakBeamNamesP[0] = new ParamT<std::string>("name","",1); + this->breakBeamNamesP[1] = new ParamT<std::string>("name","",1); + + this->breakBeamNamesP[0]->Load(node->GetChild("outerBreakBeam")); + this->breakBeamNamesP[1]->Load(node->GetChild("innerBreakBeam")); + + this->paddleNamesP[LEFT] = new ParamT<std::string>("name","",1); + this->paddleNamesP[RIGHT] = new ParamT<std::string>("name","",1); + + this->paddleNamesP[LEFT]->Load(node->GetChild("leftPaddle")); + this->paddleNamesP[RIGHT]->Load(node->GetChild("rightPaddle")); + Param::End(); + this->breakBeams[0] = dynamic_cast<RaySensor*>(this->myParent->GetSensor(**this->breakBeamNamesP[0])); + this->breakBeams[1] = dynamic_cast<RaySensor*>(this->myParent->GetSensor(**this->breakBeamNamesP[1])); + + this->paddles[LEFT] = dynamic_cast<Geom*>(this->myParent->GetGeom(**this->paddleNamesP[LEFT])); + this->paddles[RIGHT] = dynamic_cast<Geom*>(this->myParent->GetGeom(**this->paddleNamesP[RIGHT])); + if (!this->joints[LEFT]) gzthrow("couldn't get left slider joint"); @@ -143,6 +191,24 @@ if (!this->joints[LIFT]) gzthrow("couldn't get lift slider joint"); + + if (!this->breakBeams[0]) + gzthrow("Couldn't get outer breakbeam sensor"); + + if (!this->breakBeams[1]) + gzthrow("Couldn't get inner breakbeam sensor"); + + if (!this->paddles[LEFT]) + gzthrow("Couldn't get the left paddle geom"); + + if (!this->paddles[RIGHT]) + gzthrow("Couldn't get the right paddle geom"); + + this->holdJoint = (SliderJoint*)World::Instance()->GetPhysicsEngine()->CreateJoint(Joint::SLIDER); + this->holdJoint->SetName(this->GetName() + "_Hold_Joint"); + + this->paddles[LEFT]->contact->Callback(&Pioneer2_Gripper::LeftPaddleCB, this); + this->paddles[RIGHT]->contact->Callback(&Pioneer2_Gripper::RightPaddleCB, this); } //////////////////////////////////////////////////////////////////////////////// @@ -163,23 +229,48 @@ // Initialize the controller void Pioneer2_Gripper::InitChild() { + // Initially keep the gripper closed + this->joints[RIGHT]->SetParam(dParamVel,-0.1); + this->joints[LEFT]->SetParam(dParamVel,0.1); + this->joints[LEFT]->SetParam(dParamFMax, **(this->forcesP[LEFT])); + this->joints[RIGHT]->SetParam(dParamFMax, **(this->forcesP[RIGHT])); + + + // Initially lower the lift + this->joints[LIFT]->SetParam(dParamFMax, **(this->forcesP[LIFT])); + this->joints[LIFT]->SetParam(dParamFMax, **(this->forcesP[LIFT])); + + this->contactGeoms[LEFT] = this->contactGeoms[RIGHT] = NULL; } //////////////////////////////////////////////////////////////////////////////// // Update the controller void Pioneer2_Gripper::UpdateChild() { - /*double leftPaddleHiStop = this->joints[LEFT]->GetParam(dParamHiStop); - double leftPaddleLoStop = this->joints[LEFT]->GetParam(dParamLoStop); - double rightPaddleHiStop = this->joints[RIGHT]->GetParam(dParamHiStop); - double rightPaddleLoStop = this->joints[RIGHT]->GetParam(dParamLoStop); + // Create a hold joint, if both paddles are touching the same geom + if (this->contactGeoms[LEFT] && this->contactGeoms[RIGHT] && + this->contactGeoms[LEFT]->GetName() == + this->contactGeoms[RIGHT]->GetName()) + { + if (!this->holdJoint->AreConnected(this->myParent->GetBody(), + this->contactGeoms[LEFT]->GetBody())) + { + this->holdJoint->Attach(this->myParent->GetBody(), + this->contactGeoms[LEFT]->GetBody()); + this->holdJoint->SetAxis(Vector3(0,0,1)); - double leftPaddlePos = this->joints[LEFT]->GetPosition(); - double rightPaddlePos = this->joints[RIGHT]->GetPosition(); - */ + } + } + // Otherwise disconnect the joint if it has been created + else if (this->holdJoint->GetJointBody(0)) + { + this->holdJoint->Detach(); + this->contactGeoms[LEFT] = this->contactGeoms[RIGHT] = NULL; + } this->gripIface->Lock(1); + // Move the paddles switch( this->gripIface->data->cmd) { case GAZEBO_GRIPPER_CMD_OPEN: @@ -192,58 +283,54 @@ this->joints[LEFT]->SetParam(dParamVel,0.1); break; -/* case GAZEBO_GRIPPER_CMD_STORE: - this->joints[LIFT]->SetParam(dParamVel, 0.2); - break; - - case GAZEBO_GRIPPER_CMD_RETRIEVE: - this->joints[LIFT]->SetParam(dParamVel, -0.2); - break; - */ - case GAZEBO_GRIPPER_CMD_STOP: this->joints[RIGHT]->SetParam(dParamVel,0); this->joints[LEFT]->SetParam(dParamVel,0); - // this->joints[LIFT]->SetParam(dParamVel,0); break; - - - /*default: - this->joints[RIGHT]->SetParam(dParamVel,0.0); - this->joints[LEFT]->SetParam(dParamVel,0.0); - this->joints[LIFT]->SetParam(dParamVel,0.0); - break; - */ } // Move the lift if (this->actIface->data->cmd_pos[0] > 0.5) - this->joints[LIFT]->SetParam(dParamVel, 0.2); + { + this->joints[LIFT]->SetParam(dParamVel, 0.2); + } else if (this->actIface->data->cmd_pos[0] < 0.5) - this->joints[LIFT]->SetParam(dParamVel, -0.2); + { + this->joints[LIFT]->SetParam(dParamVel, -0.2); + } - this->joints[LEFT]->SetParam(dParamFMax, **(this->forcesP[LEFT])); this->joints[RIGHT]->SetParam(dParamFMax, **(this->forcesP[RIGHT])); this->joints[LIFT]->SetParam(dParamFMax, **(this->forcesP[LIFT])); + // DEBUG Statements /*printf("Left Pos[%f] High[%f] Low[%f]\n",this->joints[LEFT]->GetPosition(), this->joints[LEFT]->GetHighStop(),this->joints[LEFT]->GetLowStop() ); printf("Right Pos[%f] High[%f] Low[%f]\n",this->joints[RIGHT]->GetPosition(), this->joints[RIGHT]->GetHighStop(),this->joints[RIGHT]->GetLowStop() ); - */ printf("Lift Pos[%f] High[%f] Low[%f]\n", this->joints[LIFT]->GetPosition(), this->joints[LIFT]->GetHighStop(), this->joints[LIFT]->GetLowStop()); - + */ - // Set the state of the paddles + // Set the state of the left paddle pressure sensor + if (this->contactGeoms[LEFT]) + this->gripIface->data->left_paddle_open = 0; + else + this->gripIface->data->left_paddle_open = 1; + + // Set the state of the right paddle pressure sensor + if (this->contactGeoms[RIGHT]) + this->gripIface->data->right_paddle_open = 0; + else + this->gripIface->data->right_paddle_open = 1; + + + // Set the OPEN/CLOSED/MOVING state of the gripper if (fabs(this->joints[LEFT]->GetPosition() - this->joints[LEFT]->GetHighStop()) < 0.01 && fabs(this->joints[RIGHT]->GetPosition() - this->joints[RIGHT]->GetLowStop()) < 0.01) { this->gripIface->data->state = GAZEBO_GRIPPER_STATE_CLOSED; - this->gripIface->data->left_paddle_open = 0; - this->gripIface->data->right_paddle_open = 0; } else if (fabs(this->joints[LEFT]->GetPosition() - this->joints[LEFT]->GetLowStop()) < 0.01 && @@ -251,26 +338,47 @@ this->joints[RIGHT]->GetHighStop()) < 0.01) { this->gripIface->data->state = GAZEBO_GRIPPER_STATE_OPEN; - this->gripIface->data->left_paddle_open = 1; - this->gripIface->data->right_paddle_open = 1; } else { this->gripIface->data->state = GAZEBO_GRIPPER_STATE_MOVING; - this->gripIface->data->left_paddle_open = 0; - this->gripIface->data->right_paddle_open = 0; } - // Set the state of the lift + // Set the UP/DOWN state of the lift if (fabs(this->joints[LIFT]->GetPosition() - this->joints[LIFT]->GetHighStop()) < 0.01) + { this->actIface->data->actuators[0].position = 1; + } else if (fabs(this->joints[LIFT]->GetPosition() - this->joints[LIFT]->GetLowStop()) < 0.01) + { this->actIface->data->actuators[0].position = 0; + } + + // Check the break beams + if (this->gripIface->data->state == GAZEBO_GRIPPER_STATE_OPEN) + { + if (this->breakBeams[0]->GetRange(0) < 0.22) + this->gripIface->data->outer_beam_obstruct = 1; + else + this->gripIface->data->outer_beam_obstruct = 0; + + if (this->breakBeams[1]->GetRange(0) < 0.22) + this->gripIface->data->inner_beam_obstruct = 1; + else + this->gripIface->data->inner_beam_obstruct = 0; + } + this->actIface->data->actuators_count = 1; + + this->gripIface->data->head.time = Simulator::Instance()->GetSimTime(); + this->gripIface->Post(); this->gripIface->Unlock(); + + // Reset these flags. + this->contactGeoms[LEFT] = this->contactGeoms[RIGHT] = NULL; } //////////////////////////////////////////////////////////////////////////////// @@ -278,3 +386,23 @@ void Pioneer2_Gripper::FiniChild() { } + +//////////////////////////////////////////////////////////////////////////////// +// Left paddle contact callback +void Pioneer2_Gripper::LeftPaddleCB(Geom *g1, Geom *g2) +{ + if (g1->GetName() != this->paddles[LEFT]->GetName()) + this->contactGeoms[LEFT] = g1; + else + this->contactGeoms[LEFT] = g2; +} + +//////////////////////////////////////////////////////////////////////////////// +// Right paddle contact callback +void Pioneer2_Gripper::RightPaddleCB(Geom *g1, Geom *g2) +{ + if (g1->GetName() != this->paddles[RIGHT]->GetName()) + this->contactGeoms[RIGHT] = g1; + else + this->contactGeoms[RIGHT] = g2; +} Modified: code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh =================================================================== --- code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh 2009-03-24 18:50:06 UTC (rev 7541) +++ code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh 2009-03-24 19:40:33 UTC (rev 7542) @@ -37,6 +37,7 @@ class SliderJoint; class GripperIface; class ActarrayIface; + class RaySensor; /// \addtogroup gazebo_controller /// \{ @@ -61,13 +62,19 @@ /// This is a controller that simulates a Pioneer 2DX motion class Pioneer2_Gripper : public Controller { - /// Constructor + /// \brief Constructor public: Pioneer2_Gripper(Entity *parent); - /// Destructor + /// \brief Destructor public: virtual ~Pioneer2_Gripper(); - /// Load the controller + /// \brief Left paddle contact callback + public: void LeftPaddleCB(Geom *g1, Geom *g2); + + /// \brief Right paddle contact callback + public: void RightPaddleCB(Geom *g1, Geom *g2); + + /// \brief Load the controller /// \param node XML config node /// \return 0 on success protected: virtual void LoadChild(XMLConfigNode *node); @@ -94,15 +101,22 @@ // The interface for the lift private: ActarrayIface *actIface; + private: RaySensor *breakBeams[2]; + /// The parent Model private: Model *myParent; private: SliderJoint *joints[3]; + private: SliderJoint *holdJoint; + private: Geom *paddles[2]; + private: Geom *contactGeoms[2]; + private: ParamT<std::string> *jointNamesP[3]; private: ParamT<float> *gainsP[3]; private: ParamT<float> *forcesP[3]; - + private: ParamT<std::string> *breakBeamNamesP[2]; + private: ParamT<std::string> *paddleNamesP[2]; }; /** \} */ Modified: code/branches/federation/gazebo/server/main.cc =================================================================== --- code/branches/federation/gazebo/server/main.cc 2009-03-24 18:50:06 UTC (rev 7541) +++ code/branches/federation/gazebo/server/main.cc 2009-03-24 19:40:33 UTC (rev 7542) @@ -277,8 +277,9 @@ } catch (gazebo::GazeboError e) { - std::cerr << "Erro Loading Gazebo" << std::endl; + std::cerr << "Error Loading Gazebo" << std::endl; std::cerr << e << std::endl; + gazebo::Simulator::Instance()->Fini(); return -1; } @@ -291,6 +292,7 @@ { std::cerr << "Initialization failed" << std::endl; std::cerr << e << std::endl; + gazebo::Simulator::Instance()->Fini(); return -1; } @@ -303,6 +305,7 @@ { std::cerr << "Main simulation loop failed" << std::endl; std::cerr << e << std::endl; + gazebo::Simulator::Instance()->Fini(); return -1; } Modified: code/branches/federation/gazebo/server/physics/Body.cc =================================================================== --- code/branches/federation/gazebo/server/physics/Body.cc 2009-03-24 18:50:06 UTC (rev 7541) +++ code/branches/federation/gazebo/server/physics/Body.cc 2009-03-24 19:40:33 UTC (rev 7542) @@ -793,19 +793,33 @@ } //////////////////////////////////////////////////////////////////////////////// +/// Get a sensor by name +Sensor *Body::GetSensor( const std::string &name ) const +{ + Sensor *sensor = NULL; + std::vector< Sensor* >::const_iterator iter; + + for (iter = this->sensors.begin(); iter != this->sensors.end(); iter++) + { + if ((*iter)->GetName() == name) + { + sensor = (*iter); + break; + } + } + + return sensor; +} + +//////////////////////////////////////////////////////////////////////////////// /// Get a geom by name Geom *Body::GetGeom(const std::string &name) const { std::map<std::string, Geom*>::const_iterator iter = this->geoms.find(name); if (iter != this->geoms.end()) - { return iter->second; - } else - { - gzerr(0) << "Unknown geom[" << name << "]\n"; return NULL; - } } Modified: code/branches/federation/gazebo/server/physics/Body.hh =================================================================== --- code/branches/federation/gazebo/server/physics/Body.hh 2009-03-24 18:50:06 UTC (rev 7541) +++ code/branches/federation/gazebo/server/physics/Body.hh 2009-03-24 19:40:33 UTC (rev 7542) @@ -175,6 +175,9 @@ /// \brief Get the mass of the body public: float GetMass() const { return mass.mass; } + /// \brief Get a sensor by name + public: Sensor *GetSensor( const std::string &name ) const; + /// Load a new geom helper function /// \param node XMLConfigNode used to load the geom private: void LoadGeom(XMLConfigNode *node); Modified: code/branches/federation/gazebo/server/physics/Joint.cc =================================================================== --- code/branches/federation/gazebo/server/physics/Joint.cc 2009-03-24 18:50:06 UTC (rev 7541) +++ code/branches/federation/gazebo/server/physics/Joint.cc 2009-03-24 19:40:33 UTC (rev 7542) @@ -52,6 +52,9 @@ this->provideFeedbackP = new ParamT<bool>("provideFeedback", false, 0); this->fudgeFactorP = new ParamT<double>( "fudgeFactor", 1.0, 0 ); Param::End(); + + this->body1 = NULL; + this->body2 = NULL; } @@ -236,9 +239,10 @@ if ( index==0 || index==1 ) { - if (dJointGetBody( this->jointId, index ) == this->body1->GetId()) + if (this->body1 && + dJointGetBody( this->jointId, index ) == this->body1->GetId()) result = this->body1; - else + else if (this->body2) result = this->body2; } @@ -298,6 +302,8 @@ // Detach this joint from all bodies void Joint::Detach() { + this->body1 = NULL; + this->body2 = NULL; dJointAttach( this->jointId, 0, 0 ); return; } Modified: code/branches/federation/gazebo/server/physics/RayGeom.cc =================================================================== --- code/branches/federation/gazebo/server/physics/RayGeom.cc 2009-03-24 18:50:06 UTC (rev 7541) +++ code/branches/federation/gazebo/server/physics/RayGeom.cc 2009-03-24 19:40:33 UTC (rev 7542) @@ -50,7 +50,6 @@ if (displayRays) { - //this->line = new OgreDynamicLines(OgreDynamicRenderable::OT_LINE_LIST); this->line = OgreCreator::Instance()->CreateDynamicLine(OgreDynamicRenderable::OT_LINE_LIST); // Add two points Modified: code/branches/federation/gazebo/server/rendering/OgreDynamicLines.cc =================================================================== --- code/branches/federation/gazebo/server/rendering/OgreDynamicLines.cc 2009-03-24 18:50:06 UTC (rev 7541) +++ code/branches/federation/gazebo/server/rendering/OgreDynamicLines.cc 2009-03-24 19:40:33 UTC (rev 7542) @@ -27,6 +27,7 @@ #include <Ogre.h> #include <cassert> #include <cmath> +#include <math.h> #include <sstream> #include "GazeboError.hh" @@ -90,7 +91,7 @@ void OgreDynamicLines::Update() { - if (this->dirty) + if (this->dirty && this->points.size() > 1) this->FillHardwareBuffers(); } @@ -156,8 +157,26 @@ if ((float)vaabMin.z >= (float)vaabMax.z) vaabMin.z = vaabMax.z - 10; + if (!finite(vaabMin.x)) + vaabMin.x = 0; + if (!finite(vaabMin.y)) + vaabMin.y = 0; + if (!finite(vaabMin.z)) + vaabMin.z = 0; + + if (!finite(vaabMax.x)) + vaabMax.x = 0; + if (!finite(vaabMax.y)) + vaabMax.y = 0; + if (!finite(vaabMax.z)) + vaabMax.z = 0; + + /*printf("Min[%f %f %f] Max[%f %f %f]\n",vaabMin.x, vaabMin.y, vaabMin.z, + vaabMax.x, vaabMax.y, vaabMax.z); + */ + this->mBox.setExtents(Ogre::Vector3(vaabMin.x, vaabMin.y, vaabMin.z), - Ogre::Vector3(vaabMax.x, vaabMax.y, vaabMax.z) ); + Ogre::Vector3(vaabMax.x, vaabMax.y, vaabMax.z) ); this->dirty = false; } Modified: code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc =================================================================== --- code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc 2009-03-24 18:50:06 UTC (rev 7541) +++ code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc 2009-03-24 19:40:33 UTC (rev 7542) @@ -73,7 +73,7 @@ this->minRangeP = new ParamT<double>("minRange",0,1); this->maxRangeP = new ParamT<double>("maxRange",0,1); this->originP = new ParamT<Vector3>("origin", Vector3(0,0,0), 0); - this->displayRaysP = new ParamT<bool>("displayRays", true, 0); + this->displayRaysP = new ParamT<std::string>("displayRays", "off", 0); Param::End(); } @@ -82,8 +82,12 @@ // Destructor RaySensor::~RaySensor() { - delete this->rayFan; + if (this->rayFan) + delete this->rayFan; + if (this->rayFanOutline) + delete this->rayFanOutline; + delete this->rayCountP; delete this->rangeCountP; delete this->minAngleP; @@ -151,43 +155,50 @@ bodyPose = this->body->GetPose(); 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++) { double diff = (**(this->maxAngleP) - **(this->minAngleP)).GetAsRadian(); - angle = i * diff / (rayCountP->GetValue() - 1) + (**(this->minAngleP)).GetAsRadian(); + angle = i * diff / (rayCountP->GetValue()) + (**(this->minAngleP)).GetAsRadian(); axis.Set(cos(angle), sin(angle),0); start = (axis * this->minRangeP->GetValue()) + this->originP->GetValue(); end = (axis * this->maxRangeP->GetValue()) + this->originP->GetValue(); - ray = new RayGeom(this->body, false); + ray = new RayGeom(this->body, (**this->displayRaysP) == "lines"); - this->rayFan->AddPoint(end); - this->rayFanOutline->AddPoint(end); + if ((**this->displayRaysP) == "fan") + { + if (i == 0) + { + this->rayFan->AddPoint(start); + this->rayFanOutline->AddPoint(start); + } + this->rayFan->AddPoint(end); + this->rayFanOutline->AddPoint(end); + } + + ray->SetPoints(start, end); this->rays.push_back(ray); } - this->rayFan->AddPoint(Vector3(0,0,0)); - this->rayFan->setMaterial("Gazebo/BlueLaser"); + if ((**this->displayRaysP) == "fan") + { + this->rayFan->AddPoint(this->rayFan->GetPoint(0)); + this->rayFan->setMaterial("Gazebo/BlueLaser"); - this->rayFanOutline->AddPoint(Vector3(0,0,0)); - this->rayFanOutline->setMaterial("Gazebo/BlueEmissive"); + this->rayFanOutline->AddPoint(this->rayFanOutline->GetPoint(0)); + this->rayFanOutline->setMaterial("Gazebo/BlueEmissive"); - if (**displayRaysP) - { this->visualNode->AttachObject(this->rayFan); this->visualNode->AttachObject(this->rayFanOutline); } + } ////////////////////////////////////////////////////////////////////////////// @@ -295,7 +306,7 @@ // Update the sensor information void RaySensor::UpdateChild() { -// if (this->active) + // if (this->active) { std::vector<RayGeom*>::iterator iter; Pose3d poseDelta; @@ -329,18 +340,21 @@ // Do collision detection dSpaceCollide2( ( dGeomID ) ( this->superSpaceId ), - ( dGeomID ) ( ode->GetSpaceId() ), - this, &UpdateCallback ); - - i = 1; - for (iter = this->rays.begin(); iter != this->rays.end(); iter++, i++) - { - (*iter)->Update(); + ( dGeomID ) ( ode->GetSpaceId() ), + this, &UpdateCallback ); - (*iter)->GetRelativePoints(a,b); + if ((**this->displayRaysP) == "fan") + { + i = 1; + for (iter = this->rays.begin(); iter != this->rays.end(); iter++, i++) + { + (*iter)->Update(); - this->rayFan->SetPoint(i,b); - this->rayFanOutline->SetPoint(i,b); + (*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-24 18:50:06 UTC (rev 7541) +++ code/branches/federation/gazebo/server/sensors/ray/RaySensor.hh 2009-03-24 19:40:33 UTC (rev 7542) @@ -142,7 +142,7 @@ private: ParamT<int> *rangeCountP; /// Display rays when rendering images - private: ParamT<bool> *displayRaysP; + private: ParamT<std::string> *displayRaysP; private: OgreDynamicLines *rayFan; private: OgreDynamicLines *rayFanOutline; Modified: code/branches/federation/gazebo/worlds/models/pioneer2gripper.model =================================================================== --- code/branches/federation/gazebo/worlds/models/pioneer2gripper.model 2009-03-24 18:50:06 UTC (rev 7541) +++ code/branches/federation/gazebo/worlds/models/pioneer2gripper.model 2009-03-24 19:40:33 UTC (rev 7542) @@ -77,6 +77,14 @@ <material>Gazebo/Black</material> </visual> </geom:box> + + <sensor:ray name="breakbeam1"> + <rayCount>1</rayCount> + <origin>0 0 0</origin> + <displayRays>true</displayRays> + <minRange>0.01</minRange> + <maxRange>0.5</maxRange> + </sensor:ray> </body:box> Modified: code/branches/federation/gazebo/worlds/models/sicklms200.model =================================================================== --- code/branches/federation/gazebo/worlds/models/sicklms200.model 2009-03-24 18:50:06 UTC (rev 7541) +++ code/branches/federation/gazebo/worlds/models/sicklms200.model 2009-03-24 19:40:33 UTC (rev 7542) @@ -46,7 +46,6 @@ <displayRays>false</displayRays> - <minAngle>-90</minAngle> <maxAngle>90</maxAngle> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |