From: <na...@us...> - 2008-09-22 02:50:36
|
Revision: 7034 http://playerstage.svn.sourceforge.net/playerstage/?rev=7034&view=rev Author: natepak Date: 2008-09-22 02:50:29 +0000 (Mon, 22 Sep 2008) Log Message: ----------- Fixed missing directories Modified Paths: -------------- code/gazebo/trunk/server/Simulator.cc code/gazebo/trunk/server/Simulator.hh code/gazebo/trunk/server/gui/Toolbar.cc code/gazebo/trunk/server/physics/Body.cc code/gazebo/trunk/server/physics/Body.hh code/gazebo/trunk/server/physics/Geom.cc code/gazebo/trunk/server/physics/Geom.hh Added Paths: ----------- code/gazebo/trunk/server/controllers/imu/ code/gazebo/trunk/server/controllers/imu/Generic_Imu.cc code/gazebo/trunk/server/controllers/imu/Generic_Imu.hh code/gazebo/trunk/server/controllers/imu/SConscript code/gazebo/trunk/server/sensors/imu/ code/gazebo/trunk/server/sensors/imu/ImuSensor.cc code/gazebo/trunk/server/sensors/imu/ImuSensor.hh code/gazebo/trunk/server/sensors/imu/SConscript code/gazebo/trunk/worlds/models/imu.model Modified: code/gazebo/trunk/server/Simulator.cc =================================================================== --- code/gazebo/trunk/server/Simulator.cc 2008-09-21 22:55:45 UTC (rev 7033) +++ code/gazebo/trunk/server/Simulator.cc 2008-09-22 02:50:29 UTC (rev 7034) @@ -28,6 +28,8 @@ #include <fstream> #include <sys/time.h> +#include "Body.hh" +#include "Geom.hh" #include "Model.hh" #include "Entity.hh" #include "OgreVisual.hh" @@ -507,4 +509,26 @@ return this->selectedEntity; } +//////////////////////////////////////////////////////////////////////////////// +/// Get the model that currently selected +Model *Simulator::GetSelectedModel() const +{ + Model *model = NULL; + Body *body = NULL; + Geom *geom = NULL; + if (!this->selectedEntity) + return NULL; + + if ( (model = dynamic_cast<Model*>(this->selectedEntity)) != NULL ) + return model; + else + { + if ( (body = dynamic_cast<Body*>(this->selectedEntity)) != NULL ) + model = body->GetModel(); + else if ( (geom = dynamic_cast<Geom*>(this->selectedEntity)) != NULL ) + model = geom->GetModel(); + else + gzerr(0) << "Unknown type\n"; + } +} Modified: code/gazebo/trunk/server/Simulator.hh =================================================================== --- code/gazebo/trunk/server/Simulator.hh 2008-09-21 22:55:45 UTC (rev 7033) +++ code/gazebo/trunk/server/Simulator.hh 2008-09-22 02:50:29 UTC (rev 7034) @@ -45,6 +45,7 @@ class GazeboConfig; class OgreAdaptor; class Entity; + class Model; /// \brief The World /* @@ -155,6 +156,9 @@ /// \brief Get the selected entity public: Entity *GetSelectedEntity() const; + /// \brief Get the model that currently selected + public: Model *GetSelectedModel() const; + ///pointer to the XML Data private: XMLConfig *xmlFile; @@ -162,7 +166,8 @@ private: Gui *gui; private: OgreAdaptor *renderEngine; -/// Pointer to the selected Gui + + /// Pointer to the selected Gui private: GazeboConfig *gazeboConfig; /// Flag to know if we have a simulation loaded Added: code/gazebo/trunk/server/controllers/imu/Generic_Imu.cc =================================================================== --- code/gazebo/trunk/server/controllers/imu/Generic_Imu.cc (rev 0) +++ code/gazebo/trunk/server/controllers/imu/Generic_Imu.cc 2008-09-22 02:50:29 UTC (rev 7034) @@ -0,0 +1,143 @@ +/* + * 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: Generic IMU controller. + * Author: Matt Thompson + * Date: 07 September 2008 + * SVN info: $Id: $ + */ + +#include <algorithm> +#include <assert.h> + +#include "Global.hh" +#include "XMLConfig.hh" +#include "Model.hh" +#include "World.hh" +#include "gazebo.h" +#include "GazeboError.hh" +#include "ControllerFactory.hh" +#include "Generic_Imu.hh" +#include "ImuSensor.hh" + +using namespace gazebo; + +GZ_REGISTER_STATIC_CONTROLLER("generic_imu", Generic_Imu); + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +Generic_Imu::Generic_Imu(Entity *parent) + : Controller(parent) +{ + this->myParent = dynamic_cast<ImuSensor*>(this->parent); + + if (!this->myParent) + gzthrow("GenericImu controller requires an IMU Sensor as its parent"); + + this->imuIface = NULL; +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +Generic_Imu::~Generic_Imu() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void Generic_Imu::LoadChild(XMLConfigNode *node) +{ + std::vector<Iface*>::iterator iter; + + for (iter = this->ifaces.begin(); iter != this->ifaces.end(); iter++) + { + if ((*iter)->GetType() == "imu") + this->imuIface = dynamic_cast<ImuIface*>(*iter); + } + + if (!this->imuIface) gzthrow("GenericImu controller requires a ImuIface"); + +} + +//////////////////////////////////////////////////////////////////////////////// +// Initialize the controller +void Generic_Imu::InitChild() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void Generic_Imu::UpdateChild() +{ + bool imuOpened = false; + + if (this->imuIface->Lock(1)) + { + imuOpened = this->imuIface->GetOpenCount() > 0; + this->imuIface->Unlock(); + } + + if (imuOpened) + { + this->myParent->SetActive(true); + this->PutImuData(); + } + + if (!imuOpened) + { + this->myParent->SetActive(false); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Finalize the controller +void Generic_Imu::FiniChild() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Put IMU data to the interface +void Generic_Imu::PutImuData() +{ + + Pose3d velocity; + velocity = this->myParent->GetVelocity(); + + if (this->imuIface->Lock(1)) + { + // Data timestamp + //this->imuIface->data->head.time = Simulator::Instance()->GetSimTime(); + + this->imuIface->data->velocity.pos.x = velocity.pos.x; + this->imuIface->data->velocity.pos.y = velocity.pos.y; + this->imuIface->data->velocity.pos.z = velocity.pos.z; + this->imuIface->data->velocity.roll = velocity.rot.x; + this->imuIface->data->velocity.pitch = velocity.rot.y; + this->imuIface->data->velocity.yaw = velocity.rot.z; + + this->imuIface->Unlock(); + + // New data is available + this->imuIface->Post(); + } +} + +} Added: code/gazebo/trunk/server/controllers/imu/Generic_Imu.hh =================================================================== --- code/gazebo/trunk/server/controllers/imu/Generic_Imu.hh (rev 0) +++ code/gazebo/trunk/server/controllers/imu/Generic_Imu.hh 2008-09-22 02:50:29 UTC (rev 7034) @@ -0,0 +1,81 @@ +/* + * 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: generic IMU controller. + * Author: Matt Thompson + * Date: 07 Sep 2008 + * SVN: $Id: $ + */ + +#ifndef GENERIC_IMU_HH +#define GENERIC_IMU_HH + +#include "Controller.hh" + +namespace gazebo +{ + class ImuIface; + class ImuSensor; + +class Generic_Imu : public Controller +{ + /// \brief Constructor + /// \param parent The parent entity, must be a Model or a Sensor + public: Generic_Imu(Entity *parent); + + /// \brief Destructor + public: virtual ~Generic_Imu(); + + /// \brief Load the controller + /// \param node XML config node + /// \return 0 on success + protected: virtual void LoadChild(XMLConfigNode *node); + + /// \brief Init the controller + /// \return 0 on success + protected: virtual void InitChild(); + + /// \brief Update the controller + /// \return 0 on success + protected: virtual void UpdateChild(); + + /// \brief Finalize the controller + /// \return 0 on success + protected: virtual void FiniChild(); + + /// \brief Put IMU data to the iface + private: void PutImuData(); + + /// The IMU interface + private: ImuIface *imuIface; + + /// The parent sensor + private: ImuSensor *myParent; + +}; + +/** /} */ +/// @} + +} + +#endif + Added: code/gazebo/trunk/server/controllers/imu/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/imu/SConscript (rev 0) +++ code/gazebo/trunk/server/controllers/imu/SConscript 2008-09-22 02:50:29 UTC (rev 7034) @@ -0,0 +1,6 @@ +#Import variable +Import('env sharedObjs') + +sources = Split('Generic_Imu.cc') + +sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/gui/Toolbar.cc =================================================================== --- code/gazebo/trunk/server/gui/Toolbar.cc 2008-09-21 22:55:45 UTC (rev 7033) +++ code/gazebo/trunk/server/gui/Toolbar.cc 2008-09-22 02:50:29 UTC (rev 7034) @@ -98,49 +98,43 @@ if (this->entityBrowser->size() == 0) this->UpdateEntityBrowser(); - Entity *entity = Simulator::Instance()->GetSelectedEntity(); + Model *model = Simulator::Instance()->GetSelectedModel(); this->paramCount = 0; - if (entity) + if (model) { std::string value = "@b@B52@s@cModel "; this->AddToParamBrowser(value); - this->AddEntityToParamBrowser(entity, ""); + this->AddEntityToParamBrowser(model, ""); - Model *model = dynamic_cast<Model*>(entity); - - if (model) + const std::map<std::string, Body *> *bodies = model->GetBodies(); + const std::map<std::string, Geom *> *geoms; + std::map<std::string, Body*>::const_iterator iter; + std::map<std::string, Geom*>::const_iterator giter; + + for (iter = bodies->begin(); iter != bodies->end(); iter++) { - const std::map<std::string, Body *> *bodies = model->GetBodies(); - const std::map<std::string, Geom *> *geoms; - std::map<std::string, Body*>::const_iterator iter; - std::map<std::string, Geom*>::const_iterator giter; - std::string value; + value = "@b@B52@s-Body:~@b@B52@s" + iter->second->GetName(); + this->AddToParamBrowser(value); + this->AddEntityToParamBrowser( iter->second, " " ); - for (iter = bodies->begin(); iter != bodies->end(); iter++) + geoms = iter->second->GetGeoms(); + + for (giter = geoms->begin(); giter != geoms->end(); giter++) { - value = "@b@B52@s-Body:~@b@B52@s" + iter->second->GetName(); + value = "@b@B52@s -Geom:~@b@B52@s" + giter->second->GetName(); this->AddToParamBrowser(value); - this->AddEntityToParamBrowser( iter->second, " " ); + this->AddEntityToParamBrowser( giter->second, " " ); - geoms = iter->second->GetGeoms(); - - for (giter = geoms->begin(); giter != geoms->end(); giter++) + for (unsigned int i=0; i < giter->second->GetVisualCount(); i++) { - value = "@b@B52@s -Geom:~@b@B52@s" + giter->second->GetName(); + OgreVisual *vis = giter->second->GetVisual(i); + std::ostringstream stream; + stream << vis->GetId(); + value = "@b@B52@s -Visual:~@b@B52@s" + stream.str(); this->AddToParamBrowser(value); - this->AddEntityToParamBrowser( giter->second, " " ); - - for (unsigned int i=0; i < giter->second->GetVisualCount(); i++) - { - OgreVisual *vis = giter->second->GetVisual(i); - std::ostringstream stream; - stream << vis->GetId(); - value = "@b@B52@s -Visual:~@b@B52@s" + stream.str(); - this->AddToParamBrowser(value); - this->AddEntityToParamBrowser( vis, " " ); - } + this->AddEntityToParamBrowser( vis, " " ); } } } @@ -168,10 +162,32 @@ int endLbl = 0; int beginValue = 0; - if (lineText.find("-Body") != std::string::npos || - lineText.find("-Geom") != std::string::npos) + if (lineText.find("-Body") != std::string::npos) { + /*beginLbl = lineText.rfind("@") + 2; + + std::string bodyName = lineText.substr(beginLbl, lineText.size()-beginLbl); + std::cout << "Body Name[" << bodyName << "]\n"; + + Model *model = Simulator::Instance()->GetSelectedModel(); + Body *body = model->GetBody(bodyName); + Simulator::Instance()->SetSelectedEntity(body); + */ + } + else if (lineText.find("-Geom") != std::string::npos) + { + /*beginLbl = lineText.rfind("@") + 2; + + std::string geomName = lineText.substr(beginLbl, lineText.size()-beginLbl); + std::cout << "Geom Name[" << geomName << "]\n"; + + Model *model = Simulator::Instance()->GetSelectedModel(); + Geom *geom = model->GetGeom(geomName); + Simulator::Instance()->SetSelectedEntity(geom); + toolbar->paramInput->deactivate(); + Simulator::Instance()->SetSelectedEntity( ); + */ return; } else Modified: code/gazebo/trunk/server/physics/Body.cc =================================================================== --- code/gazebo/trunk/server/physics/Body.cc 2008-09-21 22:55:45 UTC (rev 7033) +++ code/gazebo/trunk/server/physics/Body.cc 2008-09-22 02:50:29 UTC (rev 7034) @@ -697,6 +697,13 @@ } //////////////////////////////////////////////////////////////////////////////// +/// Get the model that this body belongs to +Model *Body::GetModel() const +{ + return dynamic_cast<Model*>(this->GetParent()); +} + +//////////////////////////////////////////////////////////////////////////////// /// Get a geom by name Geom *Body::GetGeom(const std::string &name) const { Modified: code/gazebo/trunk/server/physics/Body.hh =================================================================== --- code/gazebo/trunk/server/physics/Body.hh 2008-09-21 22:55:45 UTC (rev 7033) +++ code/gazebo/trunk/server/physics/Body.hh 2008-09-22 02:50:29 UTC (rev 7034) @@ -39,6 +39,7 @@ namespace gazebo { + class Model; class Geom; class Sensor; @@ -145,6 +146,9 @@ /// \brief Get a geom by name public: Geom *GetGeom(const std::string &name) const; + + /// \brief Get the model that this body belongs to + public: Model *GetModel() const; /// Load a new geom helper function /// \param node XMLConfigNode used to load the geom Modified: code/gazebo/trunk/server/physics/Geom.cc =================================================================== --- code/gazebo/trunk/server/physics/Geom.cc 2008-09-21 22:55:45 UTC (rev 7033) +++ code/gazebo/trunk/server/physics/Geom.cc 2008-09-22 02:50:29 UTC (rev 7034) @@ -546,3 +546,17 @@ return NULL; } + +//////////////////////////////////////////////////////////////////////////////// +/// Get the body this geom belongs to +Body *Geom::GetBody() const +{ + return this->body; +} + +//////////////////////////////////////////////////////////////////////////////// +/// Get the model this geom belongs to +Model *Geom::GetModel() const +{ + return this->body->GetModel(); +} Modified: code/gazebo/trunk/server/physics/Geom.hh =================================================================== --- code/gazebo/trunk/server/physics/Geom.hh 2008-09-21 22:55:45 UTC (rev 7033) +++ code/gazebo/trunk/server/physics/Geom.hh 2008-09-22 02:50:29 UTC (rev 7034) @@ -37,6 +37,7 @@ namespace gazebo { + class Model; class Body; class ContactParams; class XMLConfigNode; @@ -152,8 +153,15 @@ /// \brief Get a visual by id public: OgreVisual *GetVisualById( int id ) const; + /// \brief Get the body this geom belongs to + public: Body *GetBody() const; + + /// \brief Get the model this geom belongs to + public: Model *GetModel() const; + /// Contact parameters public: ContactParams *contact; + /// The body this geom belongs to protected: Body *body; Added: code/gazebo/trunk/server/sensors/imu/ImuSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/imu/ImuSensor.cc (rev 0) +++ code/gazebo/trunk/server/sensors/imu/ImuSensor.cc 2008-09-22 02:50:29 UTC (rev 7034) @@ -0,0 +1,145 @@ +/* + * 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: IMU sensor + * Author: Matt Thompson + * Date: 6 September 2008 + * SVN: $Id: $ +*/ + +#include <assert.h> +#include <float.h> +#include <sstream> + +#include "SensorFactory.hh" +#include "XMLConfig.hh" +#include "Global.hh" +#include "World.hh" +#include "PhysicsEngine.hh" +#include "GazeboError.hh" +#include "ODEPhysics.hh" +#include "XMLConfig.hh" +#include "Controller.hh" +#include "ImuSensor.hh" + +#include "Vector3.hh" + +using namespace gazebo; + +GZ_REGISTER_STATIC_SENSOR("imu", ImuSensor); + +////////////////////////////////////////////////////////////////////////////// +// Constructor +ImuSensor::ImuSensor(Body *body) + : Sensor(body) +{ + this->active = false; + + this->typeName = "imu"; +} + + +////////////////////////////////////////////////////////////////////////////// +// Destructor +ImuSensor::~ImuSensor() +{ +} + +////////////////////////////////////////////////////////////////////////////// +/// Load the ray using parameter from an XMLConfig node +void ImuSensor::LoadChild(XMLConfigNode *node) +{ + if (this->body == NULL) + { + gzthrow("Null body in the IMU sensor"); + } + +} + +////////////////////////////////////////////////////////////////////////////// +/// Save the sensor info in XML format +void ImuSensor::SaveChild(std::string &prefix, std::ostream &stream) +{ +} + +////////////////////////////////////////////////////////////////////////////// +// Init the IMU +void ImuSensor::InitChild() +{ + Pose3d bodyPose; + bodyPose = this->body->GetPose(); + this->prevPose = bodyPose; +} + +void ImuSensor::FiniChild() +{ +} + +Pose3d ImuSensor::GetVelocity() +{ + return this->imuVel; +} + +////////////////////////////////////////////////////////////////////////////// +// Update the sensor information +void ImuSensor::UpdateChild() +{ +// if (this->active) + { + Vector3 velocity; + Pose3d poseDelta; + double heading; + double v1; + + double vlong, vlat; + + //Quatern rot; + Vector3 rot; + Vector3 pose; + + // Get the pose of the sensor body (global cs) + poseDelta = this->body->GetPose() - this->prevPose; + this->prevPose = this->body->GetPose(); + + velocity = this->body->GetLinearVel(); + rot = this->body->GetRotation().GetAsEuler(); + pose = this->body->GetPosition(); + + heading = atan2(velocity.y, velocity.x); + + v1 = sqrt(pow(velocity.x,2) + pow(velocity.y,2)); + + vlong = v1 * cos(heading - rot.z); + vlat = v1 * sin(heading - rot.z); + + this->imuVel.pos.x = vlong; + this->imuVel.pos.y = vlat; + + this->imuVel.pos.z = 0; + + velocity = this->body->GetAngularVel(); + this->imuVel.rot.x = velocity.x; + this->imuVel.rot.y = velocity.y; + this->imuVel.rot.z = velocity.z; + + } +} + +} Added: code/gazebo/trunk/server/sensors/imu/ImuSensor.hh =================================================================== --- code/gazebo/trunk/server/sensors/imu/ImuSensor.hh (rev 0) +++ code/gazebo/trunk/server/sensors/imu/ImuSensor.hh 2008-09-22 02:50:29 UTC (rev 7034) @@ -0,0 +1,70 @@ +/* + * 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 + * + */ + +#ifndef IMUSENSOR_HH +#define IMUSENSOR_HH + +#include <vector> + +#include "Sensor.hh" +#include "Body.hh" + +namespace gazebo +{ + +class XMLConfigNode; +class ImuSensor: public Sensor +{ + /// \brief Constructor + /// \param body The IMU sensor must be attached to a body. + public: ImuSensor(Body *body); + + /// \brief Destructor + public: virtual ~ImuSensor(); + + /// \param node The XMLConfig node + protected: virtual void LoadChild(XMLConfigNode *node); + + /// \brief Save the sensor info in XML format + protected: virtual void SaveChild(std::string &prefix, std::ostream &stream); + + /// Initialize the ray + protected: virtual void InitChild(); + + /// Update sensed values + protected: virtual void UpdateChild(); + + /// Finalize the ray + protected: virtual void FiniChild(); + + public: Pose3d GetVelocity(); + + private: Pose3d prevPose; + private: Pose3d imuVel; +/* + private: ParamT<int> *rayCountP; + private: ParamT<int> *rangeCountP; +*/ + + +}; + +#endif Added: code/gazebo/trunk/server/sensors/imu/SConscript =================================================================== --- code/gazebo/trunk/server/sensors/imu/SConscript (rev 0) +++ code/gazebo/trunk/server/sensors/imu/SConscript 2008-09-22 02:50:29 UTC (rev 7034) @@ -0,0 +1,6 @@ +#Import variable +Import('env sharedObjs') + +sources = Split('ImuSensor.cc') + +sharedObjs.append(env.SharedObject(sources)) Added: code/gazebo/trunk/worlds/models/imu.model =================================================================== --- code/gazebo/trunk/worlds/models/imu.model (rev 0) +++ code/gazebo/trunk/worlds/models/imu.model 2008-09-22 02:50:29 UTC (rev 7034) @@ -0,0 +1,41 @@ +<?xml version="1.0"?> + +<model:physical name="default_generic_imu__model" + xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" + xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" + xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" + xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" + 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" + > + + <xyz>0 0.0 0.0</xyz> + <rpy>0 0 0</rpy> + <canonicalBody>imu_body</canonicalBody> + + <body:box name="imu_body"> + <xyz>0.0 0.0 0.0</xyz> + <rpy>0.0 0.0 0.0</rpy> + + <geom:box name="imu_geom1"> + <xyz>0.0 0.0 0.0</xyz> + <rpy>0 0 90</rpy> + <size>0.1 0.1 0.1</size> + <mass>0.01</mass> + + <visual> + <scale>0.07 0.07 0.1</scale> + <mesh>unit_box</mesh> + <material>Gazebo/Green</material> + </visual> + </geom:box> + + <sensor:imu name="imu_1"> + <controller:generic_imu name="imu_controller_1"> + <interface:imu name="imu_iface_0"/> + </controller:generic_imu> + </sensor:imu> + </body:box> + +</model:physical> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-09-29 03:01:44
|
Revision: 7051 http://playerstage.svn.sourceforge.net/playerstage/?rev=7051&view=rev Author: natepak Date: 2008-09-29 03:01:33 +0000 (Mon, 29 Sep 2008) Log Message: ----------- Give the stereo iface the names of the two camera ifaces Modified Paths: -------------- code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc code/gazebo/trunk/server/rendering/OgreCamera.cc code/gazebo/trunk/server/rendering/OgreCamera.hh code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-09-28 21:14:00 UTC (rev 7050) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-09-29 03:01:33 UTC (rev 7051) @@ -1398,6 +1398,8 @@ /// Right depth map (float) public: float right_depth[GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE]; + public: char left_camera_iface_name[256]; + public: char right_camera_iface_name[256]; }; Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-09-28 21:14:00 UTC (rev 7050) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-09-29 03:01:33 UTC (rev 7051) @@ -142,10 +142,18 @@ this->stereoIface->Lock(1); if (this->stereoIface->data->head.openCount > 0) this->PutStereoData(); + + strcpy( this->stereoIface->data->left_camera_iface_name, + (**this->leftCameraNameP).c_str() ); + + strcpy( this->stereoIface->data->right_camera_iface_name, + (**this->rightCameraNameP).c_str() ); + this->stereoIface->Unlock(); this->stereoIface->Post(); } + } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/rendering/OgreCamera.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreCamera.cc 2008-09-28 21:14:00 UTC (rev 7050) +++ code/gazebo/trunk/server/rendering/OgreCamera.cc 2008-09-29 03:01:33 UTC (rev 7051) @@ -66,8 +66,7 @@ this->farClipP = new ParamT<double>("farClip",100,0); this->saveFramesP = new ParamT<bool>("saveFrames",false,0); this->savePathnameP = new ParamT<std::string>("saveFramePath","",0); - this->imageWidthP = new ParamT<unsigned int>("imageSize",640,0); - this->imageHeightP = new ParamT<unsigned int>("imageSize",480,0); + this->imageSizeP = new ParamT< Vector2<int> >("imageSize", Vector2<int>(640,480),0); this->visMaskP = new ParamT<std::string>("mask","none",0); this->hfovP = new ParamT<Angle>("hfov", Angle(DTOR(60)),0); Param::End(); @@ -88,8 +87,7 @@ delete this->farClipP; delete this->saveFramesP; delete this->savePathnameP; - delete this->imageWidthP; - delete this->imageHeightP; + delete this->imageSizeP; delete this->visMaskP; delete this->hfovP; } @@ -107,8 +105,7 @@ this->farClipP->Load(node); this->saveFramesP->Load(node); this->savePathnameP->Load(node); - this->imageWidthP->Load(node); - this->imageHeightP->Load(node); + this->imageSizeP->Load(node); this->visMaskP->Load(node); this->hfovP->Load(node); @@ -146,8 +143,7 @@ stream << prefix << (*this->farClipP) << "\n"; stream << prefix << (*this->saveFramesP) << "\n"; stream << prefix << (*this->savePathnameP) << "\n"; - stream << prefix << (*this->imageWidthP) << "\n"; - stream << prefix << (*this->imageHeightP) << "\n"; + stream << prefix << (*this->imageSizeP) << "\n"; stream << prefix << (*this->visMaskP) << "\n"; stream << prefix << (*this->hfovP) << "\n"; } @@ -275,14 +271,14 @@ /// Get the width of the image unsigned int OgreCamera::GetImageWidth() const { - return this->imageWidthP->GetValue(); + return this->imageSizeP->GetValue().x; } ////////////////////////////////////////////////////////////////////////////// /// \brief Get the height of the image unsigned int OgreCamera::GetImageHeight() const { - return this->imageHeightP->GetValue(); + return this->imageSizeP->GetValue().y; } ////////////////////////////////////////////////////////////////////////////// @@ -304,7 +300,7 @@ // Get the image size in bytes size_t OgreCamera::GetImageByteSize() const { - return this->imageHeightP->GetValue() * this->imageWidthP->GetValue() * 3; + return this->imageSizeP->GetValue().y * this->imageSizeP->GetValue().x * 3; } Modified: code/gazebo/trunk/server/rendering/OgreCamera.hh =================================================================== --- code/gazebo/trunk/server/rendering/OgreCamera.hh 2008-09-28 21:14:00 UTC (rev 7050) +++ code/gazebo/trunk/server/rendering/OgreCamera.hh 2008-09-29 03:01:33 UTC (rev 7051) @@ -174,7 +174,7 @@ protected: ParamT<Angle> *hfovP; protected: ParamT<double> *nearClipP, *farClipP; - protected: ParamT<unsigned int> *imageWidthP, *imageHeightP; + protected: ParamT< Vector2<int> > *imageSizeP; protected: unsigned int textureWidth, textureHeight; private: Ogre::Camera *camera; Modified: code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc 2008-09-28 21:14:00 UTC (rev 7050) +++ code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc 2008-09-29 03:01:33 UTC (rev 7051) @@ -67,7 +67,8 @@ this->LoadCam( node ); // Do some sanity checks - if (this->imageWidthP->GetValue() == 0 || this->imageHeightP->GetValue() == 0) + if (this->imageSizeP->GetValue().x == 0 || + this->imageSizeP->GetValue().y == 0) { gzthrow("image has zero size"); } @@ -82,8 +83,8 @@ this->ogreTextureName, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, - this->imageWidthP->GetValue(), - this->imageHeightP->GetValue(), 0, + this->imageSizeP->GetValue().x, + this->imageSizeP->GetValue().y, 0, Ogre::PF_R8G8B8, Ogre::TU_RENDERTARGET); @@ -147,7 +148,7 @@ // Get access to the buffer and make an image and write it to file mBuffer = this->renderTexture->getBuffer(0, 0); - size = this->imageWidthP->GetValue() * this->imageHeightP->GetValue() * 3; + size = this->imageSizeP->GetValue().x * this->imageSizeP->GetValue().y * 3; // Allocate buffer if (!this->saveFrameBuffer) @@ -155,18 +156,18 @@ mBuffer->lock(Ogre::HardwarePixelBuffer::HBL_READ_ONLY); - int top = (int)((mBuffer->getHeight() - this->imageHeightP->GetValue()) / 2.0); - int left = (int)((mBuffer->getWidth() - this->imageWidthP->GetValue()) / 2.0); - int right = left + this->imageWidthP->GetValue(); - int bottom = top + this->imageHeightP->GetValue(); + int top = (int)((mBuffer->getHeight() - this->imageSizeP->GetValue().y) / 2.0); + int left = (int)((mBuffer->getWidth() - this->imageSizeP->GetValue().x) / 2.0); + int right = left + this->imageSizeP->GetValue().x; + int bottom = top + this->imageSizeP->GetValue().y; // Get the center of the texture in RGB 24 bit format mBuffer->blitToMemory( Ogre::Box(left, top, right, bottom), Ogre::PixelBox( - this->imageWidthP->GetValue(), - this->imageHeightP->GetValue(), + this->imageSizeP->GetValue().x, + this->imageSizeP->GetValue().y, 1, Ogre::PF_B8G8R8, this->saveFrameBuffer) @@ -215,8 +216,8 @@ // Create image data structure imgData = new Ogre::ImageCodec::ImageData(); - imgData->width = this->imageWidthP->GetValue(); - imgData->height = this->imageHeightP->GetValue(); + imgData->width = this->imageSizeP->GetValue().x; + imgData->height = this->imageSizeP->GetValue().y; imgData->depth = 1; imgData->format = Ogre::PF_B8G8R8; size = this->GetImageByteSize(); Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-09-28 21:14:00 UTC (rev 7050) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-09-29 03:01:33 UTC (rev 7051) @@ -178,8 +178,8 @@ //this->renderTargets[0]->addListener(&this->leftCameraListener); //this->renderTargets[1]->addListener(&this->rightCameraListener); - this->rgbBufferSize = this->imageWidthP->GetValue()*this->imageHeightP->GetValue() * 3; - this->depthBufferSize = this->imageWidthP->GetValue()*this->imageHeightP->GetValue(); + this->rgbBufferSize = this->imageSizeP->GetValue().x * this->imageSizeP->GetValue().y * 3; + this->depthBufferSize = this->imageSizeP->GetValue().x*this->imageSizeP->GetValue().y; // Allocate buffers this->depthBuffer[0] = new float[this->depthBufferSize]; @@ -338,22 +338,22 @@ hardwareBuffer->lock(Ogre::HardwarePixelBuffer::HBL_NORMAL); - top = (int)((hardwareBuffer->getHeight() - this->imageHeightP->GetValue()) / 2.0); - left = (int)((hardwareBuffer->getWidth() - this->imageWidthP->GetValue()) / 2.0); - right = left + this->imageWidthP->GetValue(); - bottom = top + this->imageHeightP->GetValue(); + top = (int)((hardwareBuffer->getHeight() - this->imageSizeP->GetValue().y) / 2.0); + left = (int)((hardwareBuffer->getWidth() - this->imageSizeP->GetValue().x) / 2.0); + right = left + this->imageSizeP->GetValue().x; + bottom = top + this->imageSizeP->GetValue().y; if (i < 2) { hardwareBuffer->blitToMemory ( Ogre::Box(left,top,right,bottom), - Ogre::PixelBox( this->imageWidthP->GetValue(), this->imageHeightP->GetValue(), + Ogre::PixelBox( this->imageSizeP->GetValue().x, this->imageSizeP->GetValue().y, 1, PF_RGB, this->rgbBuffer[i]) ); } else { hardwareBuffer->blitToMemory (Ogre::Box(left,top,right,bottom), - Ogre::PixelBox( this->imageWidthP->GetValue(), this->imageHeightP->GetValue(), + Ogre::PixelBox( this->imageSizeP->GetValue().x, this->imageSizeP->GetValue().y, 1, PF_FLOAT, this->depthBuffer[i-2]) ); } @@ -379,13 +379,13 @@ return; } - fprintf( fp, "P6\n# Gazebo\n%d %d\n255\n", this->imageWidthP->GetValue(), this->imageHeightP->GetValue()); + fprintf( fp, "P6\n# Gazebo\n%d %d\n255\n", this->imageSizeP->GetValue().x, this->imageSizeP->GetValue().y); - for (unsigned int i = 0; i<this->imageHeightP->GetValue(); i++) + for (unsigned int i = 0; i<this->imageSizeP->GetValue().x; i++) { - for (unsigned int j =0; j<this->imageWidthP->GetValue(); j++) + for (unsigned int j =0; j<this->imageSizeP->GetValue().y; j++) { - double f = this->depthBuffer[0][i*this->imageWidthP->GetValue()+j]; + double f = this->depthBuffer[0][i*this->imageSizeP->GetValue().x+j]; unsigned char value = static_cast<unsigned char>(f * 255); fwrite( &value, 1, 1, fp ); @@ -538,7 +538,7 @@ name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, - this->imageWidthP->GetValue(), this->imageHeightP->GetValue(), 0, + this->imageSizeP->GetValue().x, this->imageSizeP->GetValue().y, 0, pf, Ogre::TU_RENDERTARGET); } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-11-07 16:41:41
|
Revision: 7127 http://playerstage.svn.sourceforge.net/playerstage/?rev=7127&view=rev Author: natepak Date: 2008-11-07 16:41:20 +0000 (Fri, 07 Nov 2008) Log Message: ----------- Added pioneerarm patch #2144853 Modified Paths: -------------- code/gazebo/trunk/SConstruct code/gazebo/trunk/worlds/models/bandit.model Added Paths: ----------- code/gazebo/trunk/Media/models/PioneerArm/ code/gazebo/trunk/Media/models/PioneerArm/Base.mesh code/gazebo/trunk/Media/models/PioneerArm/Base.mesh.xml code/gazebo/trunk/Media/models/PioneerArm/Gripper.mesh code/gazebo/trunk/Media/models/PioneerArm/Gripper.mesh.xml code/gazebo/trunk/Media/models/PioneerArm/Link01.mesh code/gazebo/trunk/Media/models/PioneerArm/Link01.mesh.xml code/gazebo/trunk/Media/models/PioneerArm/Link02.mesh code/gazebo/trunk/Media/models/PioneerArm/Link02.mesh.xml code/gazebo/trunk/Media/models/PioneerArm/Link03.mesh code/gazebo/trunk/Media/models/PioneerArm/Link03.mesh.xml code/gazebo/trunk/Media/models/PioneerArm/Link04.mesh code/gazebo/trunk/Media/models/PioneerArm/Link04.mesh.xml code/gazebo/trunk/Media/models/PioneerArm/Link05.mesh code/gazebo/trunk/Media/models/PioneerArm/Link05.mesh.xml code/gazebo/trunk/Media/models/PioneerArm/OgreXMLConverter.log code/gazebo/trunk/Media/models/PioneerArm/PioneerArm.blend code/gazebo/trunk/Media/models/PioneerArm/Scene.material code/gazebo/trunk/player_cfgs/ code/gazebo/trunk/player_cfgs/gazebo.cfg code/gazebo/trunk/player_cfgs/pioneerarm.cfg code/gazebo/trunk/worlds/models/pioneerarm.model code/gazebo/trunk/worlds/pioneerarm.world Added: code/gazebo/trunk/Media/models/PioneerArm/Base.mesh =================================================================== (Binary files differ) Property changes on: code/gazebo/trunk/Media/models/PioneerArm/Base.mesh ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/gazebo/trunk/Media/models/PioneerArm/Base.mesh.xml =================================================================== --- code/gazebo/trunk/Media/models/PioneerArm/Base.mesh.xml (rev 0) +++ code/gazebo/trunk/Media/models/PioneerArm/Base.mesh.xml 2008-11-07 16:41:20 UTC (rev 7127) @@ -0,0 +1,1420 @@ +<mesh> + <submeshes> + <submesh material="LinkMaterial" usesharedvertices="false"> + <faces count="128"> + <face v1="0" v2="1" v3="3"/> + <face v1="1" v2="2" v3="3"/> + <face v1="4" v2="5" v3="6"/> + <face v1="7" v2="8" v3="9"/> + <face v1="7" v2="9" v3="10"/> + <face v1="11" v2="12" v3="13"/> + <face v1="14" v2="15" v3="17"/> + <face v1="15" v2="16" v3="17"/> + <face v1="18" v2="19" v3="20"/> + <face v1="21" v2="22" v3="24"/> + <face v1="22" v2="23" v3="24"/> + <face v1="25" v2="26" v3="27"/> + <face v1="28" v2="29" v3="31"/> + <face v1="29" v2="30" v3="31"/> + <face v1="32" v2="33" v3="34"/> + <face v1="35" v2="36" v3="38"/> + <face v1="36" v2="37" v3="38"/> + <face v1="39" v2="40" v3="41"/> + <face v1="42" v2="43" v3="45"/> + <face v1="43" v2="44" v3="45"/> + <face v1="46" v2="47" v3="48"/> + <face v1="49" v2="50" v3="52"/> + <face v1="50" v2="51" v3="52"/> + <face v1="53" v2="54" v3="55"/> + <face v1="56" v2="57" v3="59"/> + <face v1="57" v2="58" v3="59"/> + <face v1="60" v2="61" v3="62"/> + <face v1="63" v2="64" v3="66"/> + <face v1="64" v2="65" v3="66"/> + <face v1="67" v2="68" v3="69"/> + <face v1="70" v2="71" v3="72"/> + <face v1="73" v2="74" v3="75"/> + <face v1="76" v2="77" v3="78"/> + <face v1="79" v2="80" v3="81"/> + <face v1="82" v2="83" v3="84"/> + <face v1="82" v2="84" v3="85"/> + <face v1="86" v2="87" v3="88"/> + <face v1="89" v2="90" v3="91"/> + <face v1="89" v2="91" v3="92"/> + <face v1="93" v2="94" v3="95"/> + <face v1="96" v2="97" v3="98"/> + <face v1="96" v2="98" v3="99"/> + <face v1="100" v2="101" v3="102"/> + <face v1="103" v2="104" v3="105"/> + <face v1="103" v2="105" v3="106"/> + <face v1="107" v2="108" v3="109"/> + <face v1="110" v2="111" v3="112"/> + <face v1="110" v2="112" v3="113"/> + <face v1="114" v2="115" v3="116"/> + <face v1="117" v2="118" v3="119"/> + <face v1="117" v2="119" v3="120"/> + <face v1="121" v2="122" v3="123"/> + <face v1="124" v2="125" v3="126"/> + <face v1="124" v2="126" v3="127"/> + <face v1="128" v2="129" v3="130"/> + <face v1="131" v2="132" v3="133"/> + <face v1="131" v2="133" v3="134"/> + <face v1="135" v2="136" v3="137"/> + <face v1="138" v2="139" v3="140"/> + <face v1="138" v2="140" v3="141"/> + <face v1="142" v2="143" v3="144"/> + <face v1="145" v2="146" v3="147"/> + <face v1="145" v2="147" v3="148"/> + <face v1="149" v2="150" v3="151"/> + <face v1="152" v2="153" v3="154"/> + <face v1="152" v2="154" v3="155"/> + <face v1="156" v2="157" v3="158"/> + <face v1="159" v2="160" v3="161"/> + <face v1="159" v2="161" v3="162"/> + <face v1="163" v2="164" v3="165"/> + <face v1="166" v2="167" v3="168"/> + <face v1="166" v2="168" v3="169"/> + <face v1="170" v2="171" v3="172"/> + <face v1="173" v2="174" v3="175"/> + <face v1="173" v2="175" v3="176"/> + <face v1="177" v2="178" v3="179"/> + <face v1="180" v2="181" v3="183"/> + <face v1="181" v2="182" v3="183"/> + <face v1="184" v2="185" v3="186"/> + <face v1="187" v2="188" v3="190"/> + <face v1="188" v2="189" v3="190"/> + <face v1="191" v2="192" v3="193"/> + <face v1="194" v2="195" v3="197"/> + <face v1="195" v2="196" v3="197"/> + <face v1="198" v2="199" v3="200"/> + <face v1="201" v2="202" v3="204"/> + <face v1="202" v2="203" v3="204"/> + <face v1="205" v2="206" v3="207"/> + <face v1="208" v2="209" v3="210"/> + <face v1="211" v2="212" v3="213"/> + <face v1="214" v2="215" v3="216"/> + <face v1="217" v2="218" v3="219"/> + <face v1="220" v2="221" v3="222"/> + <face v1="223" v2="224" v3="225"/> + <face v1="226" v2="227" v3="228"/> + <face v1="229" v2="230" v3="231"/> + <face v1="232" v2="233" v3="234"/> + <face v1="235" v2="236" v3="237"/> + <face v1="238" v2="239" v3="240"/> + <face v1="241" v2="242" v3="243"/> + <face v1="244" v2="245" v3="246"/> + <face v1="247" v2="248" v3="249"/> + <face v1="250" v2="251" v3="252"/> + <face v1="253" v2="254" v3="255"/> + <face v1="256" v2="257" v3="258"/> + <face v1="259" v2="260" v3="261"/> + <face v1="262" v2="263" v3="264"/> + <face v1="265" v2="266" v3="267"/> + <face v1="268" v2="269" v3="270"/> + <face v1="271" v2="272" v3="273"/> + <face v1="274" v2="275" v3="276"/> + <face v1="277" v2="278" v3="279"/> + <face v1="280" v2="281" v3="282"/> + <face v1="283" v2="284" v3="285"/> + <face v1="286" v2="287" v3="288"/> + <face v1="289" v2="290" v3="291"/> + <face v1="292" v2="293" v3="295"/> + <face v1="293" v2="294" v3="295"/> + <face v1="296" v2="297" v3="299"/> + <face v1="297" v2="298" v3="299"/> + <face v1="300" v2="301" v3="302"/> + <face v1="303" v2="304" v3="305"/> + <face v1="303" v2="305" v3="306"/> + <face v1="307" v2="308" v3="309"/> + <face v1="310" v2="311" v3="312"/> + <face v1="310" v2="312" v3="313"/> + <face v1="314" v2="315" v3="316"/> + <face v1="317" v2="318" v3="319"/> + </faces> + <geometry vertexcount="320"> + <vertexbuffer positions="true" normals="true"> + <vertex> + <position x="46.394463" y="-45.594971" z="-12.500005"/> + <normal x="0.541224" y="-0.663545" z="0.516512"/> + </vertex> + <vertex> + <position x="35.355141" y="-35.138901" z="12.500006"/> + <normal x="0.541224" y="-0.663545" z="0.516512"/> + </vertex> + <vertex> + <position x="27.778328" y="-41.318970" z="12.500006"/> + <normal x="0.541224" y="-0.663545" z="0.516512"/> + </vertex> + <vertex> + <position x="36.544624" y="-53.629028" z="-12.500005"/> + <normal x="0.541224" y="-0.663545" z="0.516512"/> + </vertex> + <vertex> + <position x="46.394463" y="-45.594971" z="-12.500005"/> + <normal x="-0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="36.544624" y="-53.629028" z="-12.500005"/> + <normal x="-0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="-0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="46.394463" y="-45.594971" z="-12.500005"/> + <normal x="0.659223" y="-0.544343" z="0.518763"/> + </vertex> + <vertex> + <position x="54.477962" y="-35.805489" z="-12.500005"/> + <normal x="0.659223" y="-0.544343" z="0.518763"/> + </vertex> + <vertex> + <position x="41.573563" y="-27.608063" z="12.500006"/> + <normal x="0.659223" y="-0.544343" z="0.518763"/> + </vertex> + <vertex> + <position x="35.355141" y="-35.138901" z="12.500006"/> + <normal x="0.659223" y="-0.544343" z="0.518763"/> + </vertex> + <vertex> + <position x="46.394463" y="-45.594971" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="54.477962" y="-35.805489" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="36.544624" y="-53.629028" z="-12.500005"/> + <normal x="0.402402" y="-0.757485" z="0.514090"/> + </vertex> + <vertex> + <position x="27.778328" y="-41.318970" z="12.500006"/> + <normal x="0.402402" y="-0.757485" z="0.514090"/> + </vertex> + <vertex> + <position x="19.134010" y="-45.911133" z="12.500006"/> + <normal x="0.402402" y="-0.757485" z="0.514090"/> + </vertex> + <vertex> + <position x="25.307022" y="-59.598881" z="-12.500005"/> + <normal x="0.402402" y="-0.757485" z="0.514090"/> + </vertex> + <vertex> + <position x="36.544624" y="-53.629028" z="-12.500005"/> + <normal x="-0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="25.307022" y="-59.598881" z="-12.500005"/> + <normal x="-0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="-0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="25.307022" y="-59.598881" z="-12.500005"/> + <normal x="0.248015" y="-0.822628" z="0.511637"/> + </vertex> + <vertex> + <position x="19.134010" y="-45.911133" z="12.500006"/> + <normal x="0.248015" y="-0.822628" z="0.511637"/> + </vertex> + <vertex> + <position x="9.754382" y="-48.739006" z="12.500008"/> + <normal x="0.248015" y="-0.822628" z="0.511637"/> + </vertex> + <vertex> + <position x="13.113531" y="-63.275097" z="-12.499999"/> + <normal x="0.248015" y="-0.822628" z="0.511637"/> + </vertex> + <vertex> + <position x="25.307022" y="-59.598881" z="-12.500005"/> + <normal x="-0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="13.113531" y="-63.275097" z="-12.499999"/> + <normal x="-0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="-0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="13.113531" y="-63.275097" z="-12.499999"/> + <normal x="0.083839" y="-0.856509" z="0.509277"/> + </vertex> + <vertex> + <position x="9.754382" y="-48.739006" z="12.500008"/> + <normal x="0.083839" y="-0.856509" z="0.509277"/> + </vertex> + <vertex> + <position x="-0.000111" y="-49.693817" z="12.500008"/> + <normal x="0.083839" y="-0.856509" z="0.509277"/> + </vertex> + <vertex> + <position x="0.432706" y="-64.516403" z="-12.499999"/> + <normal x="0.083839" y="-0.856509" z="0.509277"/> + </vertex> + <vertex> + <position x="13.113531" y="-63.275097" z="-12.499999"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.432706" y="-64.516403" z="-12.499999"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.432706" y="-64.516403" z="-12.499999"/> + <normal x="-0.083967" y="-0.857774" z="0.507123"/> + </vertex> + <vertex> + <position x="-0.000111" y="-49.693817" z="12.500008"/> + <normal x="-0.083967" y="-0.857774" z="0.507123"/> + </vertex> + <vertex> + <position x="-9.754560" y="-48.738960" z="12.500008"/> + <normal x="-0.083967" y="-0.857774" z="0.507123"/> + </vertex> + <vertex> + <position x="-12.248115" y="-63.275097" z="-12.499999"/> + <normal x="-0.083967" y="-0.857774" z="0.507123"/> + </vertex> + <vertex> + <position x="0.432706" y="-64.516403" z="-12.499999"/> + <normal x="-0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-12.248115" y="-63.275097" z="-12.499999"/> + <normal x="-0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="-0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-12.248115" y="-63.275097" z="-12.499999"/> + <normal x="-0.249103" y="-0.826235" z="0.505256"/> + </vertex> + <vertex> + <position x="-9.754560" y="-48.738960" z="12.500008"/> + <normal x="-0.249103" y="-0.826235" z="0.505256"/> + </vertex> + <vertex> + <position x="-19.134163" y="-45.911087" z="12.500006"/> + <normal x="-0.249103" y="-0.826235" z="0.505256"/> + </vertex> + <vertex> + <position x="-24.441614" y="-59.598892" z="-12.500005"/> + <normal x="-0.249103" y="-0.826235" z="0.505256"/> + </vertex> + <vertex> + <position x="-12.248115" y="-63.275097" z="-12.499999"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-24.441614" y="-59.598892" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-24.441614" y="-59.598892" z="-12.500005"/> + <normal x="-0.405281" y="-0.762890" z="0.503732"/> + </vertex> + <vertex> + <position x="-19.134163" y="-45.911087" z="12.500006"/> + <normal x="-0.405281" y="-0.762890" z="0.503732"/> + </vertex> + <vertex> + <position x="-27.778427" y="-41.318878" z="12.500006"/> + <normal x="-0.405281" y="-0.762890" z="0.503732"/> + </vertex> + <vertex> + <position x="-35.679214" y="-53.629028" z="-12.500005"/> + <normal x="-0.405281" y="-0.762890" z="0.503732"/> + </vertex> + <vertex> + <position x="-24.441614" y="-59.598892" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-35.679214" y="-53.629028" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-35.679214" y="-53.629028" z="-12.500005"/> + <normal x="-0.546442" y="-0.669940" z="0.502575"/> + </vertex> + <vertex> + <position x="-27.778427" y="-41.318878" z="12.500006"/> + <normal x="-0.546442" y="-0.669940" z="0.502575"/> + </vertex> + <vertex> + <position x="-35.355209" y="-35.138817" z="12.500006"/> + <normal x="-0.546442" y="-0.669940" z="0.502575"/> + </vertex> + <vertex> + <position x="-45.529041" y="-45.594936" z="-12.500005"/> + <normal x="-0.546442" y="-0.669940" z="0.502575"/> + </vertex> + <vertex> + <position x="-35.679214" y="-53.629028" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-45.529041" y="-45.594936" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-45.529041" y="-45.594936" z="-12.500005"/> + <normal x="-0.666992" y="-0.550757" z="0.501785"/> + </vertex> + <vertex> + <position x="-35.355209" y="-35.138817" z="12.500006"/> + <normal x="-0.666992" y="-0.550757" z="0.501785"/> + </vertex> + <vertex> + <position x="-41.573318" y="-27.608402" z="12.500006"/> + <normal x="-0.666992" y="-0.550757" z="0.501785"/> + </vertex> + <vertex> + <position x="-53.612591" y="-35.805386" z="-12.500005"/> + <normal x="-0.666992" y="-0.550757" z="0.501785"/> + </vertex> + <vertex> + <position x="-45.529041" y="-45.594936" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-53.612591" y="-35.805386" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-53.612591" y="-35.805386" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-59.619240" y="-24.636547" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-59.619240" y="-24.636547" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-63.318077" y="-12.517648" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-63.318077" y="-12.517648" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-64.567047" y="0.085564" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-64.567047" y="0.085564" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-63.318077" y="12.688783" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-63.318077" y="12.688783" z="-12.500005"/> + <normal x="-0.826907" y="0.252383" z="0.502522"/> + </vertex> + <vertex> + <position x="-49.039070" y="9.694705" z="12.500006"/> + <normal x="-0.826907" y="0.252383" z="0.502522"/> + </vertex> + <vertex> + <position x="-46.193810" y="19.016918" z="12.500004"/> + <normal x="-0.826907" y="0.252383" z="0.502522"/> + </vertex> + <vertex> + <position x="-59.619202" y="24.807665" z="-12.500005"/> + <normal x="-0.826907" y="0.252383" z="0.502522"/> + </vertex> + <vertex> + <position x="-63.318077" y="12.688783" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-59.619202" y="24.807665" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-59.619202" y="24.807665" z="-12.500005"/> + <normal x="-0.760963" y="0.409246" z="0.503442"/> + </vertex> + <vertex> + <position x="-46.193810" y="19.016918" z="12.500004"/> + <normal x="-0.760963" y="0.409246" z="0.503442"/> + </vertex> + <vertex> + <position x="-41.573357" y="27.608318" z="12.500004"/> + <normal x="-0.760963" y="0.409246" z="0.503442"/> + </vertex> + <vertex> + <position x="-53.612583" y="35.976486" z="-12.500005"/> + <normal x="-0.760963" y="0.409246" z="0.503442"/> + </vertex> + <vertex> + <position x="-59.619202" y="24.807665" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-53.612583" y="35.976486" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-53.612583" y="35.976486" z="-12.500005"/> + <normal x="-0.665730" y="0.549713" z="0.504598"/> + </vertex> + <vertex> + <position x="-41.573357" y="27.608318" z="12.500004"/> + <normal x="-0.665730" y="0.549713" z="0.504598"/> + </vertex> + <vertex> + <position x="-35.355278" y="35.138714" z="12.500004"/> + <normal x="-0.665730" y="0.549713" z="0.504598"/> + </vertex> + <vertex> + <position x="-45.529030" y="45.766037" z="-12.500005"/> + <normal x="-0.665730" y="0.549713" z="0.504598"/> + </vertex> + <vertex> + <position x="-53.612583" y="35.976486" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-45.529030" y="45.766037" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-45.529030" y="45.766037" z="-12.500005"/> + <normal x="-0.545183" y="0.668393" z="0.505991"/> + </vertex> + <vertex> + <position x="-35.355278" y="35.138714" z="12.500004"/> + <normal x="-0.545183" y="0.668393" z="0.505991"/> + </vertex> + <vertex> + <position x="-27.778515" y="41.318794" z="12.500004"/> + <normal x="-0.545183" y="0.668393" z="0.505991"/> + </vertex> + <vertex> + <position x="-35.679184" y="53.800095" z="-12.500005"/> + <normal x="-0.545183" y="0.668393" z="0.505991"/> + </vertex> + <vertex> + <position x="-45.529030" y="45.766037" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-35.679184" y="53.800095" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-35.679184" y="53.800095" z="-12.500005"/> + <normal x="-0.404213" y="0.760881" z="0.507614"/> + </vertex> + <vertex> + <position x="-27.778515" y="41.318794" z="12.500004"/> + <normal x="-0.404213" y="0.760881" z="0.507614"/> + </vertex> + <vertex> + <position x="-19.134243" y="45.911011" z="12.500004"/> + <normal x="-0.404213" y="0.760881" z="0.507614"/> + </vertex> + <vertex> + <position x="-24.441574" y="59.769951" z="-12.500005"/> + <normal x="-0.404213" y="0.760881" z="0.507614"/> + </vertex> + <vertex> + <position x="-35.679184" y="53.800095" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-24.441574" y="59.769951" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-24.441574" y="59.769951" z="-12.500005"/> + <normal x="-0.248392" y="0.823870" z="0.509450"/> + </vertex> + <vertex> + <position x="-19.134243" y="45.911011" z="12.500004"/> + <normal x="-0.248392" y="0.823870" z="0.509450"/> + </vertex> + <vertex> + <position x="-9.754655" y="48.738899" z="12.500004"/> + <normal x="-0.248392" y="0.823870" z="0.509450"/> + </vertex> + <vertex> + <position x="-12.248065" y="63.446190" z="-12.500009"/> + <normal x="-0.248392" y="0.823870" z="0.509450"/> + </vertex> + <vertex> + <position x="-24.441574" y="59.769951" z="-12.500005"/> + <normal x="-0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-12.248065" y="63.446190" z="-12.500009"/> + <normal x="-0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="-0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-12.248065" y="63.446190" z="-12.500009"/> + <normal x="-0.083720" y="0.855216" z="0.511466"/> + </vertex> + <vertex> + <position x="-9.754655" y="48.738899" z="12.500004"/> + <normal x="-0.083720" y="0.855216" z="0.511466"/> + </vertex> + <vertex> + <position x="-0.000191" y="49.693794" z="12.500004"/> + <normal x="-0.083720" y="0.855216" z="0.511466"/> + </vertex> + <vertex> + <position x="0.432755" y="64.687485" z="-12.500009"/> + <normal x="-0.083720" y="0.855216" z="0.511466"/> + </vertex> + <vertex> + <position x="-12.248065" y="63.446190" z="-12.500009"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.432755" y="64.687485" z="-12.500009"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.432755" y="64.687485" z="-12.500009"/> + <normal x="0.083587" y="0.853948" z="0.513601"/> + </vertex> + <vertex> + <position x="-0.000191" y="49.693794" z="12.500004"/> + <normal x="0.083587" y="0.853948" z="0.513601"/> + </vertex> + <vertex> + <position x="9.754275" y="48.738995" z="12.500004"/> + <normal x="0.083587" y="0.853948" z="0.513601"/> + </vertex> + <vertex> + <position x="13.113601" y="63.446156" z="-12.500009"/> + <normal x="0.083587" y="0.853948" z="0.513601"/> + </vertex> + <vertex> + <position x="0.432755" y="64.687485" z="-12.500009"/> + <normal x="-0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="13.113601" y="63.446156" z="-12.500009"/> + <normal x="-0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="-0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="13.113601" y="63.446156" z="-12.500009"/> + <normal x="0.247297" y="0.820256" z="0.515775"/> + </vertex> + <vertex> + <position x="9.754275" y="48.738995" z="12.500004"/> + <normal x="0.247297" y="0.820256" z="0.515775"/> + </vertex> + <vertex> + <position x="19.133881" y="45.911156" z="12.500004"/> + <normal x="0.247297" y="0.820256" z="0.515775"/> + </vertex> + <vertex> + <position x="25.307087" y="59.769913" z="-12.500005"/> + <normal x="0.247297" y="0.820256" z="0.515775"/> + </vertex> + <vertex> + <position x="13.113601" y="63.446156" z="-12.500009"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="25.307087" y="59.769913" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="25.307087" y="59.769913" z="-12.500005"/> + <normal x="0.401327" y="0.755464" z="0.517891"/> + </vertex> + <vertex> + <position x="19.133881" y="45.911156" z="12.500004"/> + <normal x="0.401327" y="0.755464" z="0.517891"/> + </vertex> + <vertex> + <position x="27.778191" y="41.319019" z="12.500004"/> + <normal x="0.401327" y="0.755464" z="0.517891"/> + </vertex> + <vertex> + <position x="36.544685" y="53.800045" z="-12.500005"/> + <normal x="0.401327" y="0.755464" z="0.517891"/> + </vertex> + <vertex> + <position x="25.307087" y="59.769913" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="36.544685" y="53.800045" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="36.544685" y="53.800045" z="-12.500005"/> + <normal x="0.539950" y="0.661989" z="0.519831"/> + </vertex> + <vertex> + <position x="27.778191" y="41.319019" z="12.500004"/> + <normal x="0.539950" y="0.661989" z="0.519831"/> + </vertex> + <vertex> + <position x="35.355003" y="35.139008" z="12.500004"/> + <normal x="0.539950" y="0.661989" z="0.519831"/> + </vertex> + <vertex> + <position x="46.394520" y="45.765949" z="-12.500005"/> + <normal x="0.539950" y="0.661989" z="0.519831"/> + </vertex> + <vertex> + <position x="36.544685" y="53.800045" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="46.394520" y="45.765949" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="46.394520" y="45.765949" z="-12.500005"/> + <normal x="0.657945" y="0.543294" z="0.521478"/> + </vertex> + <vertex> + <position x="35.355003" y="35.139008" z="12.500004"/> + <normal x="0.657945" y="0.543294" z="0.521478"/> + </vertex> + <vertex> + <position x="41.573151" y="27.608650" z="12.500004"/> + <normal x="0.657945" y="0.543294" z="0.521478"/> + </vertex> + <vertex> + <position x="54.478062" y="35.976383" z="-12.500005"/> + <normal x="0.657945" y="0.543294" z="0.521478"/> + </vertex> + <vertex> + <position x="46.394520" y="45.765949" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="54.478062" y="35.976383" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="54.478062" y="35.976383" z="-12.500005"/> + <normal x="0.750810" y="0.403794" z="0.522719"/> + </vertex> + <vertex> + <position x="41.573151" y="27.608650" z="12.500004"/> + <normal x="0.750810" y="0.403794" z="0.522719"/> + </vertex> + <vertex> + <position x="46.193680" y="19.017292" z="12.500004"/> + <normal x="0.750810" y="0.403794" z="0.522719"/> + </vertex> + <vertex> + <position x="60.484673" y="24.807543" z="-12.500005"/> + <normal x="0.750810" y="0.403794" z="0.522719"/> + </vertex> + <vertex> + <position x="54.478062" y="35.976383" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="60.484673" y="24.807543" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="60.484673" y="24.807543" z="-12.500005"/> + <normal x="0.814936" y="0.248737" z="0.523459"/> + </vertex> + <vertex> + <position x="46.193680" y="19.017292" z="12.500004"/> + <normal x="0.814936" y="0.248737" z="0.523459"/> + </vertex> + <vertex> + <position x="49.039017" y="9.695115" z="12.500006"/> + <normal x="0.814936" y="0.248737" z="0.523459"/> + </vertex> + <vertex> + <position x="64.183502" y="12.688662" z="-12.500005"/> + <normal x="0.814936" y="0.248737" z="0.523459"/> + </vertex> + <vertex> + <position x="60.484673" y="24.807543" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="64.183502" y="12.688662" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="64.183502" y="12.688662" z="-12.500005"/> + <normal x="0.847790" y="0.084020" z="0.523634"/> + </vertex> + <vertex> + <position x="49.039017" y="9.695115" z="12.500006"/> + <normal x="0.847790" y="0.084020" z="0.523634"/> + </vertex> + <vertex> + <position x="49.999817" y="0.000360" z="12.500006"/> + <normal x="0.847790" y="0.084020" z="0.523634"/> + </vertex> + <vertex> + <position x="65.432449" y="0.085437" z="-12.500005"/> + <normal x="0.847790" y="0.084020" z="0.523634"/> + </vertex> + <vertex> + <position x="64.183502" y="12.688662" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="65.432449" y="0.085437" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="-0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="65.432449" y="0.085437" z="-12.500005"/> + <normal x="0.848046" y="-0.084034" z="0.523217"/> + </vertex> + <vertex> + <position x="49.999817" y="0.000360" z="12.500006"/> + <normal x="0.848046" y="-0.084034" z="0.523217"/> + </vertex> + <vertex> + <position x="49.039146" y="-9.694413" z="12.500006"/> + <normal x="0.848046" y="-0.084034" z="0.523217"/> + </vertex> + <vertex> + <position x="64.183487" y="-12.517772" z="-12.500005"/> + <normal x="0.848046" y="-0.084034" z="0.523217"/> + </vertex> + <vertex> + <position x="65.432449" y="0.085437" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="64.183487" y="-12.517772" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="64.183487" y="-12.517772" z="-12.500005"/> + <normal x="0.815665" y="-0.248945" z="0.522223"/> + </vertex> + <vertex> + <position x="49.039146" y="-9.694413" z="12.500006"/> + <normal x="0.815665" y="-0.248945" z="0.522223"/> + </vertex> + <vertex> + <position x="46.193962" y="-19.016628" z="12.500006"/> + <normal x="0.815665" y="-0.248945" z="0.522223"/> + </vertex> + <vertex> + <position x="60.484600" y="-24.636671" z="-12.500005"/> + <normal x="0.815665" y="-0.248945" z="0.522223"/> + </vertex> + <vertex> + <position x="64.183487" y="-12.517772" z="-12.500005"/> + <normal x="-0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="60.484600" y="-24.636671" z="-12.500005"/> + <normal x="-0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="-0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="60.484600" y="-24.636671" z="-12.500005"/> + <normal x="0.751900" y="-0.404365" z="0.520707"/> + </vertex> + <vertex> + <position x="46.193962" y="-19.016628" z="12.500006"/> + <normal x="0.751900" y="-0.404365" z="0.520707"/> + </vertex> + <vertex> + <position x="41.573563" y="-27.608063" z="12.500006"/> + <normal x="0.751900" y="-0.404365" z="0.520707"/> + </vertex> + <vertex> + <position x="54.477962" y="-35.805489" z="-12.500005"/> + <normal x="0.751900" y="-0.404365" z="0.520707"/> + </vertex> + <vertex> + <position x="60.484600" y="-24.636671" z="-12.500005"/> + <normal x="-0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="54.477962" y="-35.805489" z="-12.500005"/> + <normal x="-0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="0.000006" y="0.000000" z="-12.500005"/> + <normal x="-0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="35.355141" y="-35.138901" z="12.500006"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="27.778328" y="-41.318970" z="12.500006"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="35.355141" y="-35.138901" z="12.500006"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="41.573563" y="-27.608063" z="12.500006"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="27.778328" y="-41.318970" z="12.500006"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="19.134010" y="-45.911133" z="12.500006"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="19.134010" y="-45.911133" z="12.500006"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="9.754382" y="-48.739006" z="12.500008"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="9.754382" y="-48.739006" z="12.500008"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000111" y="-49.693817" z="12.500008"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000111" y="-49.693817" z="12.500008"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-9.754560" y="-48.738960" z="12.500008"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-9.754560" y="-48.738960" z="12.500008"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-19.134163" y="-45.911087" z="12.500006"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-19.134163" y="-45.911087" z="12.500006"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-27.778427" y="-41.318878" z="12.500006"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-27.778427" y="-41.318878" z="12.500006"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-35.355209" y="-35.138817" z="12.500006"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-35.355209" y="-35.138817" z="12.500006"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-41.573318" y="-27.608402" z="12.500006"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-49.039070" y="9.694705" z="12.500006"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-46.193810" y="19.016918" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-46.193810" y="19.016918" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-41.573357" y="27.608318" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-41.573357" y="27.608318" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-35.355278" y="35.138714" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-35.355278" y="35.138714" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-27.778515" y="41.318794" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-27.778515" y="41.318794" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-19.134243" y="45.911011" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-19.134243" y="45.911011" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-9.754655" y="48.738899" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-9.754655" y="48.738899" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000191" y="49.693794" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000191" y="49.693794" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="9.754275" y="48.738995" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="9.754275" y="48.738995" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="19.133881" y="45.911156" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="19.133881" y="45.911156" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="27.778191" y="41.319019" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="27.778191" y="41.319019" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="35.355003" y="35.139008" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="35.355003" y="35.139008" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="41.573151" y="27.608650" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="41.573151" y="27.608650" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="46.193680" y="19.017292" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="46.193680" y="19.017292" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="49.039017" y="9.695115" z="12.500006"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="49.039017" y="9.695115" z="12.500006"/> + <normal x="-0.000000" y="-0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="-0.000000" y="-0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="49.999817" y="0.000360" z="12.500006"/> + <normal x="-0.000000" y="-0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="49.999817" y="0.000360" z="12.500006"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="49.039146" y="-9.694413" z="12.500006"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="49.039146" y="-9.694413" z="12.500006"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="46.193962" y="-19.016628" z="12.500006"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="46.193962" y="-19.016628" z="12.500006"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="41.573563" y="-27.608063" z="12.500006"/> + <normal x="-0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-64.567047" y="0.085564" z="-12.500005"/> + <normal x="-0.860997" y="-0.085324" z="0.501402"/> + </vertex> + <vertex> + <position x="-63.318077" y="-12.517648" z="-12.500005"/> + <normal x="-0.860997" y="-0.085324" z="0.501402"/> + </vertex> + <vertex> + <position x="-49.039055" y="-9.694806" z="12.500006"/> + <normal x="-0.860997" y="-0.085324" z="0.501402"/> + </vertex> + <vertex> + <position x="-49.999802" y="-0.000056" z="12.500006"/> + <normal x="-0.860997" y="-0.085324" z="0.501402"/> + </vertex> + <vertex> + <position x="-53.612591" y="-35.805386" z="-12.500005"/> + <normal x="-0.762062" y="-0.409776" z="0.501344"/> + </vertex> + <vertex> + <position x="-41.573318" y="-27.608402" z="12.500006"/> + <normal x="-0.762062" y="-0.409776" z="0.501344"/> + </vertex> + <vertex> + <position x="-46.192574" y="-19.017956" z="12.500006"/> + <normal x="-0.762062" y="-0.409776" z="0.501344"/> + </vertex> + <vertex> + <position x="-59.619240" y="-24.636547" z="-12.500005"/> + <normal x="-0.762062" y="-0.409776" z="0.501344"/> + </vertex> + <vertex> + <position x="-41.573318" y="-27.608402" z="12.500006"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-46.192574" y="-19.017956" z="12.500006"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-59.619240" y="-24.636547" z="-12.500005"/> + <normal x="-0.827586" y="-0.252673" z="0.501256"/> + </vertex> + <vertex> + <position x="-46.192574" y="-19.017956" z="12.500006"/> + <normal x="-0.827586" y="-0.252673" z="0.501256"/> + </vertex> + <vertex> + <position x="-49.039055" y="-9.694806" z="12.500006"/> + <normal x="-0.827586" y="-0.252673" z="0.501256"/> + </vertex> + <vertex> + <position x="-63.318077" y="-12.517648" z="-12.500005"/> + <normal x="-0.827586" y="-0.252673" z="0.501256"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-49.039055" y="-9.694806" z="12.500006"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-46.192574" y="-19.017956" z="12.500006"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-64.567047" y="0.085564" z="-12.500005"/> + <normal x="-0.860745" y="0.085298" z="0.501839"/> + </vertex> + <vertex> + <position x="-49.999802" y="-0.000056" z="12.500006"/> + <normal x="-0.860745" y="0.085298" z="0.501839"/> + </vertex> + <vertex> + <position x="-49.039070" y="9.694705" z="12.500006"/> + <normal x="-0.860745" y="0.085298" z="0.501839"/> + </vertex> + <vertex> + <position x="-63.318077" y="12.688783" z="-12.500005"/> + <normal x="-0.860745" y="0.085298" z="0.501839"/> + </vertex> + <vertex> + <position x="-49.039070" y="9.694705" z="12.500006"/> + <normal x="0.000000" y="-0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-49.999802" y="-0.000056" z="12.500006"/> + <normal x="0.000000" y="-0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="0.000000" y="-0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-0.000000" y="0.000003" z="12.500004"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-49.999802" y="-0.000056" z="12.500006"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + <vertex> + <position x="-49.039055" y="-9.694806" z="12.500006"/> + <normal x="0.000000" y="0.000000" z="1.000000"/> + </vertex> + </vertexbuffer> + </geometry> + </submesh> + </submeshes> +</mesh> Added: code/gazebo/trunk/Media/models/PioneerArm/Gripper.mesh =================================================================== (Binary files differ) Property changes on: code/gazebo/trunk/Media/models/PioneerArm/Gripper.mesh ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/gazebo/trunk/Media/models/PioneerArm/Gripper.mesh.xml =================================================================== --- code/gazebo/trunk/Media/models/PioneerArm/Gripper.mesh.xml (rev 0) +++ code/gazebo/trunk/Media/models/PioneerArm/Gripper.mesh.xml 2008-11-07 16:41:20 UTC (rev 7127) @@ -0,0 +1,164 @@ +<mesh> + <submeshes> + <submesh material="LinkMaterial" usesharedvertices="false"> + <faces count="16"> + <face v1="0" v2="1" v3="2"/> + <face v1="0" v2="2" v3="3"/> + <face v1="4" v2="5" v3="6"/> + <face v1="7" v2="8" v3="10"/> + <face v1="8" v2="9" v3="10"/> + <face v1="11" v2="12" v3="14"/> + <face v1="12" v2="13" v3="14"/> + <face v1="15" v2="16" v3="18"/> + <face v1="16" v2="17" v3="18"/> + <face v1="19" v2="20" v3="22"/> + <face v1="20" v2="21" v3="22"/> + <face v1="23" v2="24" v3="26"/> + <face v1="24" v2="25" v3="26"/> + <face v1="27" v2="28" v3="29"/> + <face v1="30" v2="31" v3="33"/> + <face v1="31" v2="32" v3="33"/> + </faces> + <geometry vertexcount="34"> + <vertexbuffer positions="true" normals="true"> + <vertex> + <position x="-12.000003" y="-10.600002" z="-2.500000"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-12.000001" y="7.400000" z="-2.500000"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="28.000000" y="-0.599999" z="-2.500000"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="28.000000" y="-10.600001" z="-2.500000"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-12.000003" y="-10.600002" z="-2.500000"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-28.000000" y="10.600002" z="-2.500000"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="-12.000001" y="7.400000" z="-2.500000"/> + <normal x="0.000000" y="0.000000" z="-1.000000"/> + </vertex> + <vertex> + <position x="28.000000" y="-0.599999" z="-2.500000"/> + <normal x="1.000000" y="0.000000" z="-0.000000"/> + </vertex> + <vertex> + <position x="28.000000" y="-0.599999" z="2.500000"/> + <normal x="1.000000" y="0.000000" z="-0.000000"/> + </vertex> + <vertex> + <position x="28.000000" y="-10.600001" z="2.500000"/> + <normal x="1.000000" y="0.000000" z="-0.000000"/> + </vertex> + <vertex> + <position x="28.000000" y="-10.600001" z="-2.500000"/> + <normal x="1.000000" y="0.000000" z="-0.000000"/> + </vertex> + <vertex> + <position x="-28.000000" y="10.600002" z="-2.500000"/> + <normal x="0.196116" y="0.980581" z="-0.000000"/> + </vertex> + <vertex> + <position x="-28.000000" y="10.600002" z="2.500000"/> + <normal x="0.196116" y="0.980581" z="-0.000000"/> + </vertex> + <vertex> + <position x="-12.000001" y="7.400000" z="2.500000"/> + <normal x="0.196116" y="0.980581" z="-0.000000"/> + </vertex> + <vertex> + <position x="-12.000001" y="7.400000" z="-2.500000"/> + <normal x="0.196116" y="0.980581" z="-0.000000"/> + </vertex> + <vertex> + <position x="-12.000001" y="7.400000" z="-2.500000"/> + <normal x="0.196116" y="0.980581" z="-0.000000"/> + </vertex> + <vertex> + <position x="-12.000001" y="7.400000" z="2.500000"/> + <normal x="0.196116" y="0.980581" z="-0.000000"/> + </vertex> + <vertex> + <position x="28.000000" y="-0.599999" z="2.500000"/> + <normal x="0.196116" y="0.980581" z="-0.000000"/> + </vertex> + <vertex> + <position x="28.000000" y="-0.599999" z="-2.500000"/> + <normal x="0.196116" y="0.980581" z="-0.000000"/> + </vertex> + <vertex> + <position x="-12.000003" y="-10.600002" z="2.500000"/> + <normal x="0.000000" y="-1.000000" z="0.000000"/> + </vertex> + <vertex> + <position x="-12.000003" y="-10.600002" z="-2.500000"/> + <normal x="0.000000" y="-1.000000" z="0.000000"/> + </vertex> + <vertex> + <position x="28.000000" y="-10.600001" z="-2.500000"/> + <normal x="0.000000" y="-1.000000" z="0.000000"/> + </vertex> + <vertex> + <position x="28.000000" y="-10.600001" z="2.500000"/> + <normal x="0.000000" y="-1.000000" z="0.000000"/> + </vertex> + <vertex> + <position x="-12.000003" y="-10.600002" z="-2.500000"/> + <normal x="-0.798189" y="-0.602407" z="0.000000"/> + </vertex> + <vertex> + <position x="-12.000003" y="-10.600002" z="2.500000"/> + ... [truncated message content] |
From: <na...@us...> - 2008-11-13 20:46:00
|
Revision: 7140 http://playerstage.svn.sourceforge.net/playerstage/?rev=7140&view=rev Author: natepak Date: 2008-11-13 20:45:50 +0000 (Thu, 13 Nov 2008) Log Message: ----------- Fixed the initial rendering of the GL Frame, and added some debug code to StereoCameraSensor Modified Paths: -------------- code/gazebo/trunk/server/gui/GLFrame.cc code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc code/gazebo/trunk/worlds/models/bandit.model Modified: code/gazebo/trunk/server/gui/GLFrame.cc =================================================================== --- code/gazebo/trunk/server/gui/GLFrame.cc 2008-11-12 23:54:19 UTC (rev 7139) +++ code/gazebo/trunk/server/gui/GLFrame.cc 2008-11-13 20:45:50 UTC (rev 7140) @@ -129,6 +129,9 @@ this->glWindow->GetCamera()->SetSaveFramePathname(this->savePathname); CameraManager::Instance()->ConnectAddCameraSignal( boost::bind(&GLFrame::CameraAddedSlot, this, _1) ); + + this->show(); + this->redraw(); } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-11-12 23:54:19 UTC (rev 7139) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-11-13 20:45:50 UTC (rev 7140) @@ -186,6 +186,42 @@ this->depthBuffer[1] = new float[this->depthBufferSize]; this->rgbBuffer[0] = new unsigned char[this->rgbBufferSize]; this->rgbBuffer[1] = new unsigned char[this->rgbBufferSize]; + + Ogre::MaterialPtr debugMat = Ogre::MaterialManager::getSingleton().create( + "Ogre/DebugShadowMap0", + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + + debugMat->getTechnique(0)->getPass(0)->setLightingEnabled(false); + Ogre::TexturePtr shadowTex = OgreAdaptor::Instance()->sceneMgr->getShadowTexture(0); + Ogre::TextureUnitState *t = debugMat->getTechnique(0)->getPass(0)->createTextureUnitState(shadowTex->getName()); + t->setTextureAddressingMode(Ogre::TextureUnitState::TAM_CLAMP); + + /*debugMat = Ogre::MaterialManager::getSingleton().create( + "Ogre/DebugShadowMap1", + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + debugMat->getTechnique(0)->getPass(0)->setLightingEnabled(false); + shadowTex = OgreAdaptor::Instance()->sceneMgr->getShadowTexture(1); + t = debugMat->getTechnique(0)->getPass(0)->createTextureUnitState(shadowTex->getName()); + t->setTextureAddressingMode(Ogre::TextureUnitState::TAM_CLAMP); + + + Ogre::OverlayContainer* debugPanel = (Ogre::OverlayContainer*) + (Ogre::OverlayManager::getSingleton().createOverlayElement("Panel", "Ogre/DebugShadowPanel0")); + debugPanel->_setPosition(0.8, 0); + debugPanel->_setDimensions(0.2, 0.2); + debugPanel->setMaterialName("Ogre/DebugShadowMap0"); + Ogre::Overlay* debugOverlay = Ogre::OverlayManager::getSingleton().getByName("Core/DebugOverlay"); + if (!debugOverlay) + printf("NULL\n"); + debugOverlay->add2D(debugPanel); + + debugPanel = (Ogre::OverlayContainer*) + (Ogre::OverlayManager::getSingleton().createOverlayElement("Panel", "Ogre/DebugShadowPanel1")); + debugPanel->_setPosition(0.8, 0.2); + debugPanel->_setDimensions(0.2, 0.2); + debugPanel->setMaterialName("Ogre/DebugShadowMap1"); + debugOverlay->add2D(debugPanel); + */ } ////////////////////////////////////////////////////////////////////////////// @@ -281,10 +317,10 @@ sceneMgr->_suppressRenderStateChanges(false); // Render the image texture - for (i=0; i<2; i++) + /*for (i=0; i<2; i++) { this->renderTargets[i]->update(); - } + }*/ if (gridNode) gridNode->setVisible(true); Modified: code/gazebo/trunk/worlds/models/bandit.model =================================================================== --- code/gazebo/trunk/worlds/models/bandit.model 2008-11-12 23:54:19 UTC (rev 7139) +++ code/gazebo/trunk/worlds/models/bandit.model 2008-11-13 20:45:50 UTC (rev 7140) @@ -74,7 +74,7 @@ <hfov>60</hfov> <nearClip>0.1</nearClip> <farClip>100</farClip> - <saveFrames>false</saveFrames> + <saveFrames>true</saveFrames> <saveFramePath>frames</saveFramePath> <baseline>0.2</baseline> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-11-14 01:53:28
|
Revision: 7141 http://playerstage.svn.sourceforge.net/playerstage/?rev=7141&view=rev Author: natepak Date: 2008-11-14 01:53:24 +0000 (Fri, 14 Nov 2008) Log Message: ----------- Fixed the stereo camera to output correct depth maps Modified Paths: -------------- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc code/gazebo/trunk/worlds/bandit.world code/gazebo/trunk/worlds/models/bandit.model Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-11-13 20:45:50 UTC (rev 7140) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-11-14 01:53:24 UTC (rev 7141) @@ -128,16 +128,12 @@ for (i = 2; i<4; i++) { this->renderTexture[i] = this->CreateRTT(this->textureName[i], true); + printf("RTT WH[%d %d]\n",this->renderTexture[i]->getWidth(),this->renderTexture[i]->getHeight()); this->renderTargets[i] = this->renderTexture[i]->getBuffer()->getRenderTarget(); this->renderTargets[i]->setAutoUpdated(false); } - // Create the camera - /*this->camera = OgreCreator::CreateCamera(this->GetName(), - this->nearClip, this->farClip, this->hfov, - this->renderTargets[D_LEFT]); - */ - this->renderTarget = this->renderTargets[D_LEFT]; + this->renderTarget = this->renderTargets[D_LEFT]; this->InitCam(); @@ -158,6 +154,10 @@ matPtr = Ogre::MaterialManager::getSingleton().create( this->materialName[i], Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + matPtr->getTechnique(0)->getPass(0)->setDepthCheckEnabled(false); + matPtr->getTechnique(0)->getPass(0)->setDepthWriteEnabled(false); + matPtr->getTechnique(0)->getPass(0)->setLightingEnabled(false); + matPtr->getTechnique(0)->getPass(0)->createTextureUnitState( this->textureName[i] ); @@ -172,12 +172,6 @@ this->textureHeight = mBuffer->getHeight(); - //this->leftCameraListener.Init(this, this->renderTargets[0], true); - //this->rightCameraListener.Init(this, this->renderTargets[1], false); - - //this->renderTargets[0]->addListener(&this->leftCameraListener); - //this->renderTargets[1]->addListener(&this->rightCameraListener); - this->rgbBufferSize = this->imageSizeP->GetValue().x * this->imageSizeP->GetValue().y * 3; this->depthBufferSize = this->imageSizeP->GetValue().x*this->imageSizeP->GetValue().y; @@ -187,41 +181,19 @@ this->rgbBuffer[0] = new unsigned char[this->rgbBufferSize]; this->rgbBuffer[1] = new unsigned char[this->rgbBufferSize]; - Ogre::MaterialPtr debugMat = Ogre::MaterialManager::getSingleton().create( - "Ogre/DebugShadowMap0", - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + // Uncomment this section to create a debug overaly + /*{ + Ogre::Overlay *overlay = Ogre::OverlayManager::getSingletonPtr()->create("__GAZEBO_STEREO_DEBUG_OVERLAY__"); - debugMat->getTechnique(0)->getPass(0)->setLightingEnabled(false); - Ogre::TexturePtr shadowTex = OgreAdaptor::Instance()->sceneMgr->getShadowTexture(0); - Ogre::TextureUnitState *t = debugMat->getTechnique(0)->getPass(0)->createTextureUnitState(shadowTex->getName()); - t->setTextureAddressingMode(Ogre::TextureUnitState::TAM_CLAMP); + Ogre::OverlayContainer *overlayPanel = (Ogre::OverlayContainer*)(Ogre::OverlayManager::getSingletonPtr()->createOverlayElement("Panel", "__GAZEBO_PANEL")); + overlayPanel->setPosition(0.5, 0); + overlayPanel->_setDimensions(0.5, 0.5); + overlayPanel->setMaterialName(this->materialName[D_LEFT]); - /*debugMat = Ogre::MaterialManager::getSingleton().create( - "Ogre/DebugShadowMap1", - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - debugMat->getTechnique(0)->getPass(0)->setLightingEnabled(false); - shadowTex = OgreAdaptor::Instance()->sceneMgr->getShadowTexture(1); - t = debugMat->getTechnique(0)->getPass(0)->createTextureUnitState(shadowTex->getName()); - t->setTextureAddressingMode(Ogre::TextureUnitState::TAM_CLAMP); + overlay->add2D(overlayPanel); - - Ogre::OverlayContainer* debugPanel = (Ogre::OverlayContainer*) - (Ogre::OverlayManager::getSingleton().createOverlayElement("Panel", "Ogre/DebugShadowPanel0")); - debugPanel->_setPosition(0.8, 0); - debugPanel->_setDimensions(0.2, 0.2); - debugPanel->setMaterialName("Ogre/DebugShadowMap0"); - Ogre::Overlay* debugOverlay = Ogre::OverlayManager::getSingleton().getByName("Core/DebugOverlay"); - if (!debugOverlay) - printf("NULL\n"); - debugOverlay->add2D(debugPanel); - - debugPanel = (Ogre::OverlayContainer*) - (Ogre::OverlayManager::getSingleton().createOverlayElement("Panel", "Ogre/DebugShadowPanel1")); - debugPanel->_setPosition(0.8, 0.2); - debugPanel->_setDimensions(0.2, 0.2); - debugPanel->setMaterialName("Ogre/DebugShadowMap1"); - debugOverlay->add2D(debugPanel); - */ + overlay->show(); + }*/ } ////////////////////////////////////////////////////////////////////////////// @@ -262,8 +234,6 @@ sceneMgr->_suppressRenderStateChanges(true); - //prev_pass = sceneMgr->getPass(); - // Get pointer to the material pass pass = this->depthMaterial->getBestTechnique()->getPass(0); @@ -292,7 +262,10 @@ autoParamDataSource.setCurrentSceneManager(sceneMgr); autoParamDataSource.setCurrentCamera(this->GetOgreCamera(), true); pass->_updateAutoParamsNoLights(&autoParamDataSource); - + + renderSys->setLightingEnabled(false); + renderSys->_setFog(Ogre::FOG_NONE); + // These two lines don't seem to do anything useful renderSys->_setProjectionMatrix(this->GetOgreCamera()->getProjectionMatrixRS()); renderSys->_setViewMatrix(this->GetOgreCamera()->getViewMatrix(true)); @@ -310,22 +283,24 @@ renderSys->bindGpuProgram( pass->getFragmentProgram()->_getBindingDelegate() ); renderSys->bindGpuProgramParameters(Ogre::GPT_FRAGMENT_PROGRAM, pass->getFragmentProgramParameters()); - } + } + + this->renderTargets[i]->update(); } sceneMgr->_suppressRenderStateChanges(false); // Render the image texture - /*for (i=0; i<2; i++) + for (i=0; i<2; i++) { this->renderTargets[i]->update(); - }*/ + } if (gridNode) gridNode->setVisible(true); - this->FillBuffers(); + this->FillBuffers(); if (this->saveFramesP->GetValue()) this->SaveFrame(); @@ -389,7 +364,8 @@ else { hardwareBuffer->blitToMemory (Ogre::Box(left,top,right,bottom), - Ogre::PixelBox( this->imageSizeP->GetValue().x, this->imageSizeP->GetValue().y, + Ogre::PixelBox( this->imageSizeP->GetValue().x, + this->imageSizeP->GetValue().y, 1, PF_FLOAT, this->depthBuffer[i-2]) ); } @@ -417,9 +393,9 @@ fprintf( fp, "P6\n# Gazebo\n%d %d\n255\n", this->imageSizeP->GetValue().x, this->imageSizeP->GetValue().y); - for (unsigned int i = 0; i< (unsigned int)this->imageSizeP->GetValue().x; i++) + for (unsigned int i = 0; i< (unsigned int)this->imageSizeP->GetValue().y; i++) { - for (unsigned int j =0; j<(unsigned int)this->imageSizeP->GetValue().y; j++) + for (unsigned int j =0; j<(unsigned int)this->imageSizeP->GetValue().x; j++) { double f = this->depthBuffer[0][i*this->imageSizeP->GetValue().x+j]; Modified: code/gazebo/trunk/worlds/bandit.world =================================================================== --- code/gazebo/trunk/worlds/bandit.world 2008-11-13 20:45:50 UTC (rev 7140) +++ code/gazebo/trunk/worlds/bandit.world 2008-11-14 01:53:24 UTC (rev 7141) @@ -124,8 +124,9 @@ </model:physical> <model:physical name="cylinder1_model"> - <xyz>10 0 0.5</xyz> + <xyz>5 0 0.5</xyz> <rpy>0.0 0.0 0.0</rpy> + <static>true</static> <body:cylinder name="cylinder1_body"> <geom:cylinder name="cylinder1_geom"> @@ -139,15 +140,33 @@ </body:cylinder> </model:physical> + + <model:physical name="cylinder2_model"> + <xyz>10 0 2.5</xyz> + <rpy>0.0 0.0 0.0</rpy> + <static>true</static> + + <body:cylinder name="cylinder2_body"> + <geom:cylinder name="cylinder2_geom"> + <size>0.5 1</size> + <mass>1.0</mass> + <visual> + <mesh>unit_cylinder</mesh> + <material>Gazebo/RustyBarrel</material> + </visual> + </geom:cylinder> + </body:cylinder> + </model:physical> + <!-- White Directional light --> - <model:renderable name="directional_white"> + <model:renderable name="point_white"> + <xyz>0 2 2</xyz> <light> - <type>directional</type> - <direction>-1 1.0 -0.5</direction> - <diffuseColor>0.2 0.2 0.2</diffuseColor> - <specularColor>0.0 0.0 0.0</specularColor> - <attenuation>100 0.0 1.0 0.4</attenuation> + <type>point</type> + <diffuseColor>0.8 0.8 0.8</diffuseColor> + <specularColor>0.1 0.1 0.1</specularColor> + <attenuation>10 0.8 0.1 0.0</attenuation> </light> </model:renderable> - + </gazebo:world> Modified: code/gazebo/trunk/worlds/models/bandit.model =================================================================== --- code/gazebo/trunk/worlds/models/bandit.model 2008-11-13 20:45:50 UTC (rev 7140) +++ code/gazebo/trunk/worlds/models/bandit.model 2008-11-14 01:53:24 UTC (rev 7141) @@ -73,8 +73,8 @@ <imageSize>320 240</imageSize> <hfov>60</hfov> <nearClip>0.1</nearClip> - <farClip>100</farClip> - <saveFrames>true</saveFrames> + <farClip>10</farClip> + <saveFrames>false</saveFrames> <saveFramePath>frames</saveFramePath> <baseline>0.2</baseline> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-11-14 01:59:39
|
Revision: 7143 http://playerstage.svn.sourceforge.net/playerstage/?rev=7143&view=rev Author: natepak Date: 2008-11-14 01:59:33 +0000 (Fri, 14 Nov 2008) Log Message: ----------- Removed a bad printf, and update stereocamera.world to have a realistic farclip plane Modified Paths: -------------- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc code/gazebo/trunk/worlds/stereocamera.world Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-11-14 01:54:08 UTC (rev 7142) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-11-14 01:59:33 UTC (rev 7143) @@ -128,7 +128,6 @@ for (i = 2; i<4; i++) { this->renderTexture[i] = this->CreateRTT(this->textureName[i], true); - printf("RTT WH[%d %d]\n",this->renderTexture[i]->getWidth(),this->renderTexture[i]->getHeight()); this->renderTargets[i] = this->renderTexture[i]->getBuffer()->getRenderTarget(); this->renderTargets[i]->setAutoUpdated(false); } Modified: code/gazebo/trunk/worlds/stereocamera.world =================================================================== --- code/gazebo/trunk/worlds/stereocamera.world 2008-11-14 01:54:08 UTC (rev 7142) +++ code/gazebo/trunk/worlds/stereocamera.world 2008-11-14 01:59:33 UTC (rev 7143) @@ -100,7 +100,7 @@ <imageSize>320 240</imageSize> <hfov>60</hfov> <nearClip>0.1</nearClip> - <farClip>1000</farClip> + <farClip>10</farClip> <saveFrames>false</saveFrames> <saveFramePath>frames</saveFramePath> <baseline>0.2</baseline> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-11-17 02:00:16
|
Revision: 7154 http://playerstage.svn.sourceforge.net/playerstage/?rev=7154&view=rev Author: natepak Date: 2008-11-17 02:00:11 +0000 (Mon, 17 Nov 2008) Log Message: ----------- Fixed libgazebo to work with 64bit systems. Improved debugging output Modified Paths: -------------- code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/player/CameraInterface.cc code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-11-15 03:14:43 UTC (rev 7153) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-11-17 02:00:11 UTC (rev 7154) @@ -58,13 +58,13 @@ class Vec3 { /// X value - public: double x; + public: float x; /// Y value - public: double y; + public: float y; /// Z value - public: double z; + public: float z; }; @@ -75,13 +75,13 @@ public: Vec3 pos; /// Rotation information. Roll Euler angle - public: double roll; + public: float roll; /// Rotation information. Pitch Euler angle - public: double pitch; + public: float pitch; /// Rotation information. Yaw Euler angle - public: double yaw; + public: float yaw; }; /// \brief Color class @@ -332,7 +332,7 @@ /// Number of times the interface has been opened public: int openCount; - public: double time; + public: float time; public: int version; @@ -389,13 +389,13 @@ public: GazeboData head; /// Elapsed simulation time - public: double simTime; + public: float simTime; /// Accumpated pause time (this interface may be updated with the server is paused). - public: double pauseTime; + public: float pauseTime; /// Elapsed real time since start of simulation (from system clock) - public: double realTime; + public: float realTime; /// state of the simulation : 0 paused, 1 running -1 not_started/exiting public: int state; @@ -467,7 +467,7 @@ public: GazeboData head; /// Data timestamp - public: double time; + public: float time; /// Play in a loop? public: bool loop; @@ -576,10 +576,10 @@ public: unsigned char image[GAZEBO_CAMERA_MAX_IMAGE_SIZE]; /// Horizontal field of view of the camera in radians - public: double hfov; + public: float hfov; /// Vertical field of view of the camera in radians - public: double vfov; + public: float vfov; /// Pose of the camera public: Pose camera_pose; @@ -812,25 +812,25 @@ public: GazeboData head; /// Range scan min angle - public: double min_angle; + public: float min_angle; /// Range scan max angle - public: double max_angle; + public: float max_angle; /// Angular resolution - public: double res_angle; + public: float res_angle; /// Range resolution - public: double res_range; + public: float res_range; /// Max range value - public: double max_range; + public: float max_range; /// Number of range readings public: int range_count; /// Range readings - public: double ranges[GZ_LASER_MAX_RANGES]; + public: float ranges[GZ_LASER_MAX_RANGES]; /// Intensity readings public: int intensity[GZ_LASER_MAX_RANGES]; @@ -842,13 +842,13 @@ public: int cmd_new_length; /// Commanded range value - public: double cmd_max_range; + public: float cmd_max_range; /// Commaned min angle - public: double cmd_min_angle; + public: float cmd_min_angle; /// Commaned max angle - public: double cmd_max_angle; + public: float cmd_max_angle; /// Commaned range count public: int cmd_range_count; @@ -1299,22 +1299,22 @@ public: GazeboData head; /// Measured pan angle (radians) - public: double pan; + public: float pan; /// Measured tilt angle (radians) - public: double tilt; + public: float tilt; /// Measured field of view (radians) - public: double zoom; + public: float zoom; /// Commanded pan angle (radians) - public: double cmd_pan; + public: float cmd_pan; /// Commanded tilt angle (radians) - public: double cmd_tilt; + public: float cmd_tilt; /// Commanded field of view (radians) - public: double cmd_zoom; + public: float cmd_zoom; }; @@ -1382,10 +1382,10 @@ public: float nearClip; /// Horizontal field of view of the camera in radians - public: double hfov; + public: float hfov; /// Vertical field of view of the camera in radians - public: double vfov; + public: float vfov; /// Left depth map size public: unsigned int left_depth_size; Modified: code/gazebo/trunk/player/CameraInterface.cc =================================================================== --- code/gazebo/trunk/player/CameraInterface.cc 2008-11-15 03:14:43 UTC (rev 7153) +++ code/gazebo/trunk/player/CameraInterface.cc 2008-11-17 02:00:11 UTC (rev 7154) @@ -146,8 +146,8 @@ catch (std::string e) { //std::ostringstream stream; - std::cout << "Error Subscribing to Gazebo Camera Interface\n" - << e << "\n"; + std::cout << "Error Subscribing to Gazebo Camera Interface[" + << this->gz_id << "]\n" << e << "\n"; //gzthrow(stream.str()); exit(0); } Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-11-15 03:14:43 UTC (rev 7153) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-11-17 02:00:11 UTC (rev 7154) @@ -242,7 +242,6 @@ // Render the depth texture for (i=2; i<4; i++) { - // OgreSceneManager::_render function automatically sets farClip to 0. // Which normally equates to infinite distance. We don't want this. So // we have to set the distance every time. @@ -283,8 +282,7 @@ renderSys->bindGpuProgramParameters(Ogre::GPT_FRAGMENT_PROGRAM, pass->getFragmentProgramParameters()); } - - + this->renderTargets[i]->update(); } @@ -299,7 +297,7 @@ if (gridNode) gridNode->setVisible(true); - this->FillBuffers(); + this->FillBuffers(); if (this->saveFramesP->GetValue()) this->SaveFrame(); @@ -371,6 +369,7 @@ hardwareBuffer->unlock(); } + } //////////////////////////////////////////////////////////////////////////////// This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-11-21 01:39:25
|
Revision: 7162 http://playerstage.svn.sourceforge.net/playerstage/?rev=7162&view=rev Author: natepak Date: 2008-11-21 01:34:37 +0000 (Fri, 21 Nov 2008) Log Message: ----------- Fixed the factory example, and fixed deleting models via a factory Modified Paths: -------------- code/gazebo/trunk/examples/libgazebo/factory/factory.cc code/gazebo/trunk/server/rendering/OgreVisual.cc Modified: code/gazebo/trunk/examples/libgazebo/factory/factory.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/factory/factory.cc 2008-11-20 00:52:29 UTC (rev 7161) +++ code/gazebo/trunk/examples/libgazebo/factory/factory.cc 2008-11-21 01:34:37 UTC (rev 7162) @@ -1,6 +1,7 @@ #include <sstream> #include <gazebo/gazebo.h> #include <gazebo/GazeboError.hh> +#include <string.h> int main() { @@ -57,10 +58,13 @@ stream << "<xyz>" << i*0.5 << " 0 1</xyz>"; stream << "<body:sphere name='sphere_body_" << i << "'>"; stream << "<geom:sphere name='sphere_geom_" << i << "'>"; - stream << "<mesh>default</mesh>"; stream << "<size>0.1</size>"; stream << "<density>1.0</density>"; - stream << "<material>Gazebo/Rocky</material>"; + stream << "<visual>"; + stream << "<size>0.1 0.1 0.1</size>"; + stream << "<material>Gazebo/Rocky</material>"; + stream << "<mesh>unit_sphere</mesh>"; + stream << "</visual>"; stream << "</geom:sphere>"; stream << "</body:sphere>"; stream << "</model:physical>"; Modified: code/gazebo/trunk/server/rendering/OgreVisual.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreVisual.cc 2008-11-20 00:52:29 UTC (rev 7161) +++ code/gazebo/trunk/server/rendering/OgreVisual.cc 2008-11-21 01:34:37 UTC (rev 7162) @@ -95,6 +95,16 @@ delete this->meshTileP; delete this->materialNameP; delete this->castShadowsP; + + if (this->parentNode) + this->parentNode->removeChild( this->sceneNode ); + + this->sceneNode->removeAndDestroyAllChildren(); + + if (this->sceneNode) + OgreAdaptor::Instance()->sceneMgr->destroySceneNode(this->sceneNode); + if (this->boundingBoxNode) + OgreAdaptor::Instance()->sceneMgr->destroySceneNode(this->boundingBoxNode); } //////////////////////////////////////////////////////////////////////////////// This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-11-21 02:31:11
|
Revision: 7163 http://playerstage.svn.sourceforge.net/playerstage/?rev=7163&view=rev Author: natepak Date: 2008-11-21 02:31:05 +0000 (Fri, 21 Nov 2008) Log Message: ----------- Updated the bandit & generic_actarray controllers to stop oscillations Modified Paths: -------------- code/gazebo/trunk/examples/libgazebo/bandit/bandit.cc code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc code/gazebo/trunk/server/controllers/actarray/generic/Generic_Actarray.cc code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc code/gazebo/trunk/worlds/models/sonyvid30.model Modified: code/gazebo/trunk/examples/libgazebo/bandit/bandit.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/bandit/bandit.cc 2008-11-21 01:34:37 UTC (rev 7162) +++ code/gazebo/trunk/examples/libgazebo/bandit/bandit.cc 2008-11-21 02:31:05 UTC (rev 7163) @@ -56,7 +56,7 @@ printf("Shoulder2[%f]\n", RTOD(actarrayIface->data->actuators[R_SHOULDER2].position)); - actarrayIface->data->cmd_pos[R_SHOULDER2] = DTOR(80); + actarrayIface->data->cmd_pos[R_SHOULDER2] = DTOR(79); actarrayIface->data->cmd_pos[R_ELBOW] = DTOR(90); usleep(1000000); actarrayIface->data->cmd_pos[R_ELBOW2] = DTOR(100); @@ -84,6 +84,7 @@ actarrayIface->Unlock(); }*/ + return 0; } Modified: code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc =================================================================== --- code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc 2008-11-21 01:34:37 UTC (rev 7162) +++ code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc 2008-11-21 02:31:05 UTC (rev 7163) @@ -151,11 +151,12 @@ angle = cmdAngle - joint->GetAngle(); if (fabs(angle) > 0.01) - { joint->SetParam( dParamVel, **(this->gainsP[i]) * angle); - joint->SetParam( dParamFMax, **(this->forcesP[i]) ); - } + else + joint->SetParam(dParamVel, 0); + joint->SetParam( dParamFMax, **(this->forcesP[i]) ); + } else if (this->myIface->data->joint_mode[i] == GAZEBO_ACTARRAY_JOINT_SPEED_MODE) { Modified: code/gazebo/trunk/server/controllers/actarray/generic/Generic_Actarray.cc =================================================================== --- code/gazebo/trunk/server/controllers/actarray/generic/Generic_Actarray.cc 2008-11-21 01:34:37 UTC (rev 7162) +++ code/gazebo/trunk/server/controllers/actarray/generic/Generic_Actarray.cc 2008-11-21 02:31:05 UTC (rev 7163) @@ -153,11 +153,12 @@ delta_position = target_position - actual_position; if (fabs(delta_position) > tolerances[i]) - { joint->SetParam( dParamVel, this->gains[i] * delta_position); - joint->SetParam( dParamFMax, this->forces[i] ); - } + else + joint->SetParam( dParamVel, 0); + joint->SetParam( dParamFMax, this->forces[i] ); + this->myIface->data->actuators[i].position = actual_position; this->myIface->data->actuators[i].speed = actual_speed; } Modified: code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc =================================================================== --- code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc 2008-11-21 01:34:37 UTC (rev 7162) +++ code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc 2008-11-21 02:31:05 UTC (rev 7163) @@ -166,10 +166,18 @@ float tilt = this->cmdTilt - this->tiltJoint->GetAngle(); float pan = this->cmdPan - this->panJoint->GetAngle(); - this->tiltJoint->SetParam( dParamVel, **(this->motionGainP) * tilt); + if (fabs(tilt) > 0.01) + this->tiltJoint->SetParam( dParamVel, **(this->motionGainP) * tilt); + else + this->tiltJoint->SetParam( dParamVel, 0); + this->tiltJoint->SetParam( dParamFMax, **(this->forceP) ); - this->panJoint->SetParam( dParamVel, **(this->motionGainP) * pan); + if (fabs(pan) > 0.01) + this->panJoint->SetParam( dParamVel, **(this->motionGainP) * pan); + else + this->panJoint->SetParam( dParamVel, 0); + this->panJoint->SetParam( dParamFMax, **(this->forceP) ); this->PutPTZData(); Modified: code/gazebo/trunk/worlds/models/sonyvid30.model =================================================================== --- code/gazebo/trunk/worlds/models/sonyvid30.model 2008-11-21 01:34:37 UTC (rev 7162) +++ code/gazebo/trunk/worlds/models/sonyvid30.model 2008-11-21 02:31:05 UTC (rev 7163) @@ -124,6 +124,9 @@ <controller:generic_ptz name="sonyvid30_controller"> <panJoint>pan_joint</panJoint> <tiltJoint>tilt_joint</tiltJoint> + + <motionGain>1</motionGain> + <force>0.01</force> <interface:ptz name="ptz_iface_1"/> </controller:generic_ptz> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-11-27 13:08:17
|
Revision: 7174 http://playerstage.svn.sourceforge.net/playerstage/?rev=7174&view=rev Author: natepak Date: 2008-11-27 13:08:12 +0000 (Thu, 27 Nov 2008) Log Message: ----------- Added epuck patches Modified Paths: -------------- code/gazebo/trunk/Media/materials/scripts/Gazebo.material code/gazebo/trunk/SConstruct code/gazebo/trunk/libgazebo/Iface.cc code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/player/GazeboDriver.cc code/gazebo/trunk/player/SConscript code/gazebo/trunk/server/controllers/Controller.cc code/gazebo/trunk/server/controllers/SConscript code/gazebo/trunk/server/sensors/SConscript code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc Added Paths: ----------- code/gazebo/trunk/Media/materials/textures/floor.png code/gazebo/trunk/Media/materials/textures/lightwood.png code/gazebo/trunk/Media/materials/textures/logo.png code/gazebo/trunk/Media/materials/textures/plate.png code/gazebo/trunk/Media/materials/textures/plate1.png code/gazebo/trunk/Media/materials/textures/stone-2.png code/gazebo/trunk/Media/materials/textures/turret.png code/gazebo/trunk/Media/materials/textures/turret_new.png code/gazebo/trunk/Media/models/epuck/ code/gazebo/trunk/Media/models/epuck/camera.material code/gazebo/trunk/Media/models/epuck/camera.mesh code/gazebo/trunk/Media/models/epuck/chassis.material code/gazebo/trunk/Media/models/epuck/chassis.mesh code/gazebo/trunk/Media/models/epuck/epuck_chassis.mesh code/gazebo/trunk/Media/models/epuck/new_epuck.mesh code/gazebo/trunk/Media/models/epuck/plate.material code/gazebo/trunk/Media/models/epuck/plate.mesh code/gazebo/trunk/Media/models/epuck/ring.material code/gazebo/trunk/Media/models/epuck/ring.mesh code/gazebo/trunk/Media/models/epuck/turret.material code/gazebo/trunk/Media/models/epuck/turret.mesh code/gazebo/trunk/examples/libgazebo/epuck/ code/gazebo/trunk/examples/libgazebo/epuck/EpuckModel.cc code/gazebo/trunk/examples/libgazebo/epuck/EpuckModel.hh code/gazebo/trunk/examples/libgazebo/epuck/SConstruct code/gazebo/trunk/examples/libgazebo/epuck/epuck.cfg code/gazebo/trunk/examples/libgazebo/epuck/epuck.world code/gazebo/trunk/examples/libgazebo/epuck/epuck_single.cfg code/gazebo/trunk/examples/libgazebo/epuck/main.cc code/gazebo/trunk/player/IRInterface.cc code/gazebo/trunk/player/IRInterface.hh code/gazebo/trunk/server/controllers/irarray/ code/gazebo/trunk/server/controllers/irarray/IR_Array.cc code/gazebo/trunk/server/controllers/irarray/IR_Array.hh code/gazebo/trunk/server/controllers/irarray/SConscript code/gazebo/trunk/server/sensors/ir/ code/gazebo/trunk/server/sensors/ir/IRSensor.cc code/gazebo/trunk/server/sensors/ir/IRSensor.hh code/gazebo/trunk/server/sensors/ir/SConscript code/gazebo/trunk/worlds/epuck.world code/gazebo/trunk/worlds/epuck_single.world code/gazebo/trunk/worlds/models/epuck_new.model code/gazebo/trunk/worlds/player_cfgs/ code/gazebo/trunk/worlds/player_cfgs/epuck.cfg code/gazebo/trunk/worlds/player_cfgs/epuck_single.cfg Modified: code/gazebo/trunk/Media/materials/scripts/Gazebo.material =================================================================== --- code/gazebo/trunk/Media/materials/scripts/Gazebo.material 2008-11-26 21:26:26 UTC (rev 7173) +++ code/gazebo/trunk/Media/materials/scripts/Gazebo.material 2008-11-27 13:08:12 UTC (rev 7174) @@ -603,6 +603,20 @@ } } +material Gazebo/LightWood +{ + technique + { + pass + { + texture_unit + { + texture lightwood.png + } + } + } +} + material Gazebo/WoodTile { technique @@ -664,3 +678,203 @@ } } } + +material Gazebo/PCBGreen +{ + technique + { + pass + { + scene_blend alpha_blend + depth_write off + + texture_unit + { + colour_op_ex source1 src_manual src_current 0.2 0.43 0.36 + alpha_op_ex source1 src_manual src_current 0.2 + } + } + } +} + +material Gazebo/Turret +{ + technique + { + pass + { + lighting off + + + ambient 1 1 1 1 + diffuse 1 1 1 1 + specular 0 0 0 0 + emissive 0 0 0 + + alpha_rejection greater 128 + depth_write on + + texture_unit + { + texture turret_new.png + tex_coord_set 0 + colour_op modulate + } + } + } +} + + +material Gazebo/EpuckBody +{ + technique + { + pass + { + lighting off + + ambient 1 1 1 1 + diffuse 1 1 1 1 + specular 0 0 0 0 + emissive 1 1 1 + + + scene_blend alpha_blend + depth_write off + + texture_unit + { + colour_op_ex source1 src_manual src_current 0.523613 0.500817 0.499183 + alpha_op_ex source1 src_manual src_current 0.8 + } + } + + } +} + +material Gazebo/EpuckRing +{ + technique + { + pass + { + lighting off + + ambient 1 1 1 1 + diffuse 1 1 1 1 + specular 0 0 0 0 + emissive 0 0 0 + + scene_blend alpha_blend + depth_write off + + texture_unit + { + colour_op_ex source1 src_manual src_current 0.8 0.8 0.8 + alpha_op_ex source1 src_manual src_current 0.5 + } + } + + } +} + +material Gazebo/EpuckPlate +{ + technique + { + pass + { + lighting off + + + ambient 1 1 1 1 + diffuse 1 1 1 1 + specular 0 0 0 0 + emissive 0 0 0 + + + alpha_rejection greater 128 + depth_write on + + texture_unit + { + texture plate.png + tex_coord_set 0 + colour_op modulate + } + } + } +} + +material Gazebo/EpuckLogo +{ + technique + { + pass + { + lighting off + + + ambient 1 1 1 1 + diffuse 1 1 1 1 + specular 0 0 0 0 + emissive 0 0 0 + + alpha_rejection greater 128 + depth_write on + + texture_unit + { + texture logo.png + tex_coord_set 0 + colour_op modulate + } + } + } +} + +material Gazebo/EpuckMagenta +{ + receive_shadows on + technique + { + pass + { + ambient 0.000000 0.000000 0.000000 + diffuse 0.523613 0.500817 0.499183 + specular 0.500000 0.500000 0.500000 50.000000 + emissive 0.000000 0.000000 0.000000 + } + } +} + +material Gazebo/EpuckGold +{ + + technique + { + pass + { + + + lighting off + + + ambient 1 1 1 1 + diffuse 1 1 1 1 + specular 0 0 0 0 + emissive 0 0 0 + + + texture_unit + { + colour_op_ex source1 src_manual src_current 1.000000 0.671000 0.000000 + alpha_op_ex source1 src_manual src_current 0.5 + } + } + } +} + + + +1 Added: code/gazebo/trunk/Media/materials/textures/floor.png =================================================================== (Binary files differ) Property changes on: code/gazebo/trunk/Media/materials/textures/floor.png ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/gazebo/trunk/Media/materials/textures/lightwood.png =================================================================== (Binary files differ) Property changes on: code/gazebo/trunk/Media/materials/textures/lightwood.png ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/gazebo/trunk/Media/materials/textures/logo.png =================================================================== (Binary files differ) Property changes on: code/gazebo/trunk/Media/materials/textures/logo.png ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/gazebo/trunk/Media/materials/textures/plate.png =================================================================== (Binary files differ) Property changes on: code/gazebo/trunk/Media/materials/textures/plate.png ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/gazebo/trunk/Media/materials/textures/plate1.png =================================================================== (Binary files differ) Property changes on: code/gazebo/trunk/Media/materials/textures/plate1.png ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/gazebo/trunk/Media/materials/textures/stone-2.png =================================================================== (Binary files differ) Property changes on: code/gazebo/trunk/Media/materials/textures/stone-2.png ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/gazebo/trunk/Media/materials/textures/turret.png =================================================================== (Binary files differ) Property changes on: code/gazebo/trunk/Media/materials/textures/turret.png ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/gazebo/trunk/Media/materials/textures/turret_new.png =================================================================== (Binary files differ) Property changes on: code/gazebo/trunk/Media/materials/textures/turret_new.png ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/gazebo/trunk/Media/models/epuck/camera.material =================================================================== --- code/gazebo/trunk/Media/models/epuck/camera.material (rev 0) +++ code/gazebo/trunk/Media/models/epuck/camera.material 2008-11-27 13:08:12 UTC (rev 7174) @@ -0,0 +1,28 @@ +material Black +{ + receive_shadows on + technique + { + pass + { + ambient 0.500000 0.500000 0.500000 1.000000 + diffuse 0.000000 0.000000 0.000000 1.000000 + specular 0.500000 0.500000 0.500000 1.000000 12.500000 + emissive 0.000000 0.000000 0.000000 1.000000 + } + } +} +material Black +{ + receive_shadows on + technique + { + pass + { + ambient 0.500000 0.500000 0.500000 1.000000 + diffuse 0.000000 0.000000 0.000000 1.000000 + specular 0.500000 0.500000 0.500000 1.000000 12.500000 + emissive 0.000000 0.000000 0.000000 1.000000 + } + } +} Added: code/gazebo/trunk/Media/models/epuck/camera.mesh =================================================================== (Binary files differ) Property changes on: code/gazebo/trunk/Media/models/epuck/camera.mesh ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/gazebo/trunk/Media/models/epuck/chassis.material =================================================================== --- code/gazebo/trunk/Media/models/epuck/chassis.material (rev 0) +++ code/gazebo/trunk/Media/models/epuck/chassis.material 2008-11-27 13:08:12 UTC (rev 7174) @@ -0,0 +1,28 @@ +material Grey +{ + receive_shadows on + technique + { + pass + { + ambient 0.500000 0.500000 0.500000 1.000000 + diffuse 0.800000 0.800000 0.800000 1.000000 + specular 0.500000 0.500000 0.500000 1.000000 12.500000 + emissive 0.000000 0.000000 0.000000 1.000000 + } + } +} +material Grey +{ + receive_shadows on + technique + { + pass + { + ambient 0.500000 0.500000 0.500000 1.000000 + diffuse 0.800000 0.800000 0.800000 1.000000 + specular 0.500000 0.500000 0.500000 1.000000 12.500000 + emissive 0.000000 0.000000 0.000000 1.000000 + } + } +} Added: code/gazebo/trunk/Media/models/epuck/chassis.mesh =================================================================== (Binary files differ) Property changes on: code/gazebo/trunk/Media/models/epuck/chassis.mesh ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/gazebo/trunk/Media/models/epuck/epuck_chassis.mesh =================================================================== (Binary files differ) Property changes on: code/gazebo/trunk/Media/models/epuck/epuck_chassis.mesh ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/gazebo/trunk/Media/models/epuck/new_epuck.mesh =================================================================== (Binary files differ) Property changes on: code/gazebo/trunk/Media/models/epuck/new_epuck.mesh ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/gazebo/trunk/Media/models/epuck/plate.material =================================================================== --- code/gazebo/trunk/Media/models/epuck/plate.material (rev 0) +++ code/gazebo/trunk/Media/models/epuck/plate.material 2008-11-27 13:08:12 UTC (rev 7174) @@ -0,0 +1,56 @@ +material Grey +{ + receive_shadows on + technique + { + pass + { + ambient 0.500000 0.500000 0.500000 1.000000 + diffuse 0.800000 0.800000 0.800000 1.000000 + specular 0.500000 0.500000 0.500000 1.000000 12.500000 + emissive 0.000000 0.000000 0.000000 1.000000 + } + } +} +material HalveGreen +{ + receive_shadows on + technique + { + pass + { + ambient 0.500000 0.500000 0.500000 1.000000 + diffuse 0.147451 0.508235 0.147451 1.000000 + specular 0.500000 0.500000 0.500000 1.000000 12.500000 + emissive 0.000000 0.000000 0.000000 1.000000 + } + } +} +material Grey +{ + receive_shadows on + technique + { + pass + { + ambient 0.500000 0.500000 0.500000 1.000000 + diffuse 0.800000 0.800000 0.800000 1.000000 + specular 0.500000 0.500000 0.500000 1.000000 12.500000 + emissive 0.000000 0.000000 0.000000 1.000000 + } + } +} +material HalveGreen +{ + receive_shadows on + technique + { + pass + { + ambient 0.500000 0.500000 0.500000 1.000000 + diffuse 0.147451 0.508235 0.147451 1.000000 + specular 0.500000 0.500000 0.500000 1.000000 12.500000 + emissive 0.000000 0.000000 0.000000 1.000000 + } + } +} Added: code/gazebo/trunk/Media/models/epuck/plate.mesh =================================================================== (Binary files differ) Property changes on: code/gazebo/trunk/Media/models/epuck/plate.mesh ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/gazebo/trunk/Media/models/epuck/ring.material =================================================================== --- code/gazebo/trunk/Media/models/epuck/ring.material (rev 0) +++ code/gazebo/trunk/Media/models/epuck/ring.material 2008-11-27 13:08:12 UTC (rev 7174) @@ -0,0 +1,28 @@ +material Grey +{ + receive_shadows on + technique + { + pass + { + ambient 0.500000 0.500000 0.500000 1.000000 + diffuse 0.800000 0.800000 0.800000 1.000000 + specular 0.500000 0.500000 0.500000 1.000000 12.500000 + emissive 0.000000 0.000000 0.000000 1.000000 + } + } +} +material Grey +{ + receive_shadows on + technique + { + pass + { + ambient 0.500000 0.500000 0.500000 1.000000 + diffuse 0.800000 0.800000 0.800000 1.000000 + specular 0.500000 0.500000 0.500000 1.000000 12.500000 + emissive 0.000000 0.000000 0.000000 1.000000 + } + } +} Added: code/gazebo/trunk/Media/models/epuck/ring.mesh =================================================================== (Binary files differ) Property changes on: code/gazebo/trunk/Media/models/epuck/ring.mesh ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/gazebo/trunk/Media/models/epuck/turret.material =================================================================== --- code/gazebo/trunk/Media/models/epuck/turret.material (rev 0) +++ code/gazebo/trunk/Media/models/epuck/turret.material 2008-11-27 13:08:12 UTC (rev 7174) @@ -0,0 +1,56 @@ +material Grey +{ + receive_shadows on + technique + { + pass + { + ambient 0.500000 0.500000 0.500000 1.000000 + diffuse 0.800000 0.800000 0.800000 1.000000 + specular 0.500000 0.500000 0.500000 1.000000 12.500000 + emissive 0.000000 0.000000 0.000000 1.000000 + } + } +} +material HalveGreen +{ + receive_shadows on + technique + { + pass + { + ambient 0.500000 0.500000 0.500000 1.000000 + diffuse 0.147451 0.508235 0.147451 1.000000 + specular 0.500000 0.500000 0.500000 1.000000 12.500000 + emissive 0.000000 0.000000 0.000000 1.000000 + } + } +} +material Grey +{ + receive_shadows on + technique + { + pass + { + ambient 0.500000 0.500000 0.500000 1.000000 + diffuse 0.800000 0.800000 0.800000 1.000000 + specular 0.500000 0.500000 0.500000 1.000000 12.500000 + emissive 0.000000 0.000000 0.000000 1.000000 + } + } +} +material HalveGreen +{ + receive_shadows on + technique + { + pass + { + ambient 0.500000 0.500000 0.500000 1.000000 + diffuse 0.147451 0.508235 0.147451 1.000000 + specular 0.500000 0.500000 0.500000 1.000000 12.500000 + emissive 0.000000 0.000000 0.000000 1.000000 + } + } +} Added: code/gazebo/trunk/Media/models/epuck/turret.mesh =================================================================== (Binary files differ) Property changes on: code/gazebo/trunk/Media/models/epuck/turret.mesh ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Modified: code/gazebo/trunk/SConstruct =================================================================== --- code/gazebo/trunk/SConstruct 2008-11-26 21:26:26 UTC (rev 7173) +++ code/gazebo/trunk/SConstruct 2008-11-27 13:08:12 UTC (rev 7174) @@ -51,11 +51,13 @@ '#server/sensors/ray', '#server/sensors/contact', '#server/sensors/imu', + '#server/sensors/ir', '#server/physics', '#server/physics/ode', '#server/controllers', '#server/controllers/position2d', '#server/controllers/position2d/pioneer2dx', + '#server/controllers/irarray', ], LIBPATH=Split('#libgazebo'), @@ -154,9 +156,9 @@ Exit(1) # Check for trimesh support in ODE -if not conf.CheckODELib(): - print ' Error: ODE not compiled with trimesh support.' - Exit(1) +#if not conf.CheckODELib(): +# print ' Error: ODE not compiled with trimesh support.' +# Exit(1) env = conf.Finish() Added: code/gazebo/trunk/examples/libgazebo/epuck/EpuckModel.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/epuck/EpuckModel.cc (rev 0) +++ code/gazebo/trunk/examples/libgazebo/epuck/EpuckModel.cc 2008-11-27 13:08:12 UTC (rev 7174) @@ -0,0 +1,590 @@ +#include <string.h> +#include <string> +#include <iostream> +#include "EpuckModel.hh" + +double minfrontdistance = 0.095; +double minsidedistance = 0.04; +double forwardspeed=0.08; +double maxirrange=0.1; + + +double weightleft[8]={-1.6, -0.65, -0.3, 0, 0, 0.3, .6, 1.65}; +double weightright[8]={1.65, 0.8, 0.3, 0, 0, -0.3, -0.8, -1.6}; + +Epuck::Epuck() +{ + this->posIface = new PositionIface(); + this->irIface = new IRIface(); + this->id = 9999; + strcpy(this->robotName,""); + this->lastupdateTime = 0; + this->currentTime=0; + this->irCount = NUMS_IR; +} + +Epuck::~Epuck() +{ + //if(this->posIface) + delete this->posIface; + //if(this->irIface) + delete this->irIface; + +} + +bool Epuck::SetSpeed(double lspeed, double rspeed) +{ + posIface->Lock(1); + posIface->data->cmdVelocity.pos.x = (lspeed + rspeed )/2.0; + posIface->data->cmdVelocity.pos.y = 0; + posIface->data->cmdVelocity.yaw = (lspeed-rspeed)/( WHEEL_SEP); + posIface->Unlock(); + return true; +} + +bool Epuck::GetIRReading() +{ + + bool ret=false; + irIface->Lock(1); + if(irIface->data->ir_count == NUMS_IR) + { + for(int i=0;i<NUMS_IR;i++) + { + ir[i] = irIface->data->ranges[i]; + } + + ret = true; + } + irIface->Unlock(); + return ret; +} + +void Epuck::DoCircle(double speed, double r, bool anticlock) +{ + double w = speed/r; + double lspeed = w * (r-WHEEL_SEP/2.0 ); + double rspeed = w * (r+WHEEL_SEP/2.0); + if(anticlock) + SetSpeed(lspeed, rspeed); + else + SetSpeed(rspeed, lspeed); +} + + +bool Epuck::Subscribe(Client * client) +{ + try + { + posIface->Open(client, posIfaceName); + } + catch (std::string e) + { + std::cout << "Gazebo error: Unable to connect to the position interface\n" + << e << "\n"; + return -1; + } + + try + { + irIface->Open(client, irIfaceName); + } + catch (std::string e) + { + std::cout << "Gazebo error: Unable to connect to the ir interface\n" + << e << "\n"; + return -1; + } + + + return true; + + +} + +void Epuck::EnableMotor() +{ + posIface->Lock(1); + posIface->data->cmdEnableMotors = 1; + posIface->Unlock(); +} + + +//random walk and avoidance behaviour +void Epuck::Update() +{ + //printf("robot%d update\n",this->id); + if(GetIRReading()==false) + { + SetSpeed(0,0); + return; + } + + double leftspeed=forwardspeed; + double rightspeed=forwardspeed; + + bool obs = ((ir[0] < minfrontdistance) || + (ir[1] < minfrontdistance) || + (ir[6] < minfrontdistance) || + (ir[7] < minfrontdistance) || + (ir[2] < minsidedistance)|| + (ir[5] < minsidedistance)); + if(obs ) + { + leftspeed = 0.01; + rightspeed = 0.01; + for(int i=0;i<8;i++) + { + leftspeed += weightleft[i] * (ir[i]-minfrontdistance); + rightspeed += weightright[i] * (ir[i]-minfrontdistance); + } + } + SetSpeed(leftspeed, rightspeed); + + +} + + +const char * Epuck::CreateEpuckModel( int id, Pose& pose, bool withCamera) +{ + std::ostringstream stream; + stream << "<model:physical name='robot"<<id<<"'>"; + stream << "<xyz>"<<pose.pos.x<<" "<<pose.pos.y<<" "<<pose.pos.z<<"</xyz>"; + stream << "<rpy>"<<pose.roll<<" "<<pose.pitch<<" "<<pose.yaw<<"</rpy>"; + stream << "<static>false</static>"; + stream << "<controller:differential_position2d name='controller"<<id<<"'>"; + stream << "<leftJoint>left_wheel_hinge</leftJoint>"; + stream << "<rightJoint>right_wheel_hinge</rightJoint>"; + stream << "<wheelSeparation>0.052</wheelSeparation>"; + stream << "<wheelDiameter>0.042</wheelDiameter>"; + stream << "<torque>10</torque>"; + stream << "<interface:position name='position_iface_"<<"0"<<"'/>"; + stream << "</controller:differential_position2d>"; + + stream << "<canonicalBody>epuck_chassis</canonicalBody>"; + + stream << "<body:box name='epuck_chassis'>"; + stream << "<xyz>0.0 0.0 0.02</xyz>"; + stream << "<geom:box name='chassis_geom'>"; + stream << "<size>0.07 0.04 0.038</size>"; + stream << "<mass>0.1</mass>"; + stream << "<mul>1</mul>"; + stream << "<visual>"; + stream << "<rpy>90.0 0.0 90.0</rpy>"; + stream << "<mesh>epuck/chassis.mesh</mesh>"; + stream << "<material>Gazebo/EpuckBody</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<xyz>0.0 0.0 0.019</xyz>"; + stream << "<rpy>0 0 -90</rpy>"; + stream << "<size>0.07 0.07 0</size>"; + stream << "<mesh>unit_box</mesh>"; + stream << "<material>Gazebo/EpuckPlate</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<xyz>-0.004 0.000 0.0266</xyz>"; + stream << "<rpy>90 0 -90</rpy>"; + stream << "<mesh>epuck/turret.mesh</mesh>"; + stream << "<scale>0.0128 0.0128 0.0128</scale>"; + stream << "<material>Gazebo/PCBGreen</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<xyz>-0.004 0.000 0.028</xyz>"; + stream << "<mesh>unit_box</mesh>"; + stream << "<rpy>0 0 90</rpy>"; + stream << "<size>0.0625 0.0625 0</size>"; + stream << "<material>Gazebo/Turret</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<xyz>0.0 0.0 0.017</xyz>"; + stream << "<rpy>90.0 0.0 90.0</rpy>"; + stream << "<size>0.0725 0.007 0.0725</size>"; + stream << "<mesh>epuck/ring.mesh</mesh>"; + stream << "<material>Gazebo/EpuckRing</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<xyz>0.0254 0.0193 0.0226</xyz>"; + stream << "<rpy> 0.0 0.0 0.0</rpy>"; + stream << "<mesh>unit_cylinder</mesh>"; + stream << "<size>0.00225 0.00225 0.01</size>"; + stream << "<material>Gazebo/White</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<xyz>0.0254 -0.0193 0.0226</xyz>"; + stream << "<rpy> 0.0 0.0 0.0</rpy>"; + stream << "<mesh>unit_cylinder</mesh>"; + stream << "<size>0.00225 0.00225 0.01</size>"; + stream << "<material>Gazebo/White</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<xyz>-0.032 0 0.0226</xyz>"; + stream << "<rpy> 0.0 0.0 0.0</rpy>"; + stream << "<mesh>unit_cylinder</mesh>"; + stream << "<size>0.00225 0.00225 0.01</size>"; + stream << "<material>Gazebo/White</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<xyz>0.03 0.01 0.013</xyz>"; + stream << "<rpy>0.0 0.0 17.20</rpy>"; + stream << "<mesh>unit_box</mesh>"; + stream << "<size>0.002 0.007 0.004</size>"; + stream << "<material>Gazebo/Black</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<xyz>0.022 0.025 0.013 </xyz>"; + stream << "<rpy>0.0 0.0 45.8</rpy>"; + stream << "<mesh>unit_box</mesh>"; + stream << "<size>0.002 0.007 0.004</size>"; + stream << "<material>Gazebo/Black</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<xyz>0 0.031 0.013 </xyz>"; + stream << "<rpy>0.0 0.0 90.0</rpy>"; + stream << "<mesh>unit_box</mesh>"; + stream << "<size>0.002 0.007 0.004</size>"; + stream << "<material>Gazebo/Black</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<xyz>-0.03 0.015 0.013 </xyz>"; + stream << "<rpy>0.0 0.0 151.5</rpy>"; + stream << "<mesh>unit_box</mesh>"; + stream << "<size>0.002 0.007 0.004</size>"; + stream << "<material>Gazebo/Black</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<xyz> -0.03 -0.015 0.013 </xyz>"; + stream << "<rpy>0.0 0.0 -151.5</rpy>"; + stream << "<mesh>unit_box</mesh>"; + stream << "<size>0.002 0.007 0.004</size>"; + stream << "<material>Gazebo/Black</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<xyz> 0 -0.031 0.013</xyz>"; + stream << "<rpy>0.0 0.0 -90.0</rpy>"; + stream << "<mesh>unit_box</mesh>"; + stream << "<size>0.002 0.007 0.004</size>"; + stream << "<material>Gazebo/Black</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<xyz> 0.022 -0.025 0.013 </xyz>"; + stream << "<rpy>0.0 0.0 -45.8</rpy>"; + stream << "<mesh>unit_box</mesh>"; + stream << "<size>0.002 0.007 0.004</size>"; + stream << "<material>Gazebo/Black</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<xyz> 0.03 -0.01 0.013 </xyz>"; + stream << "<rpy>0.0 0.0 -17.2</rpy>"; + stream << "<mesh>unit_box</mesh>"; + stream << "<size>0.002 0.007 0.004</size>"; + stream << "<material>Gazebo/Black</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<rpy>0.0 0.0 90.0</rpy>"; + stream << "<xyz>0.026 0.0 0.010</xyz>"; + stream << "<mesh>epuck/camera.mesh</mesh>"; + stream << "<scale>1 1 1</scale>"; + stream << "<material>Gazebo/Black</material>"; + stream << "</visual>"; + stream << "</geom:box>"; + + stream << "<sensor:ir name='ir_ring"<<id<<"'>"; + stream << "<irCount> 8 </irCount>"; + stream << "<displayRays>false</displayRays>"; + stream << "<ir name='IR_0'>"; + stream << "<rayCount>4</rayCount>"; + stream << "<rangeCount>10</rangeCount>"; + stream << "<origin>0.03 0.01 0.013</origin>"; + + stream << "<minAngle>10.2</minAngle>"; + stream << "<maxAngle>24.2</maxAngle>"; + + stream << "<minRange>0.01</minRange>"; + stream << "<maxRange>0.1</maxRange>"; + stream << "</ir>"; + stream << "<ir name='IR_1'>"; + stream << "<rayCount>4</rayCount>"; + stream << "<rangeCount>10</rangeCount>"; + stream << "<origin>0.022 0.025 0.013</origin>"; + + stream << "<minAngle>38.8</minAngle>"; + stream << "<maxAngle>52.8</maxAngle>"; + + stream << "<minRange>0.005</minRange>"; + stream << "<maxRange>0.1</maxRange>"; + stream << "</ir>"; + stream << "<ir name='IR_2'>"; + stream << "<rayCount>4</rayCount>"; + stream << "<rangeCount>10</rangeCount>"; + stream << "<origin>0 0.031 0.013 </origin>"; + + stream << "<minAngle>83.0</minAngle>"; + stream << "<maxAngle>97.0</maxAngle>"; + + stream << "<minRange>0.005</minRange>"; + stream << "<maxRange>0.1</maxRange>"; + stream << "</ir>"; + stream << "<ir name='IR_3'>"; + stream << "<rayCount>4</rayCount>"; + stream << "<rangeCount>10</rangeCount>"; + stream << "<origin>-0.03 0.015 0.013</origin>"; + + stream << "<minAngle>144.5</minAngle>"; + stream << "<maxAngle>158.5</maxAngle>"; + + stream << "<minRange>0.01</minRange>"; + stream << "<maxRange>0.1</maxRange>"; + stream << "</ir>"; + stream << "<ir name='IR_4'>"; + stream << "<rayCount>4</rayCount>"; + stream << "<rangeCount>10</rangeCount>"; + stream << "<origin>-0.03 -0.015 0.013</origin>"; + + stream << "<minAngle>-158.5</minAngle>"; + stream << "<maxAngle>-144.5</maxAngle>"; + + stream << "<minRange>0.01</minRange>"; + stream << "<maxRange>0.1</maxRange>"; + stream << "</ir>"; + stream << "<ir name='IR_5'>"; + stream << "<rayCount>4</rayCount>"; + stream << "<rangeCount>10</rangeCount>"; + stream << "<origin>0 -0.031 0.013</origin>"; + + stream << "<minAngle>-97.0</minAngle>"; + stream << "<maxAngle>-83.0</maxAngle>"; + + stream << "<minRange>0.005</minRange>"; + stream << "<maxRange>0.1</maxRange>"; + stream << "</ir>"; + stream << "<ir name='IR_6'>"; + stream << "<rayCount>4</rayCount>"; + stream << "<rangeCount>10</rangeCount>"; + stream << "<origin>0.022 -0.025 0.013</origin>"; + + stream << "<minAngle>-52.8</minAngle>"; + stream << "<maxAngle>-38.8</maxAngle>"; + + stream << "<minRange>0.005</minRange>"; + stream << "<maxRange>0.1</maxRange>"; + stream << "</ir>"; + stream << "<ir name='IR_7'>"; + stream << "<rayCount>4</rayCount>"; + stream << "<rangeCount>10</rangeCount>"; + stream << "<origin> 0.03 -0.01 0.013</origin>"; + + stream << "<minAngle>-24.2</minAngle>"; + stream << "<maxAngle>-10.2</maxAngle>"; + + stream << "<minRange>0.01</minRange>"; + stream << "<maxRange>0.1</maxRange>"; + stream << "</ir>"; + stream << "<controller:irarray name='irarray"<<id<<"'>"; + stream << "<interface:irarray name='irarray_iface_"<<"0"<<"'/>"; + stream << "</controller:irarray>"; + stream << "</sensor:ir>"; + stream << "</body:box>"; + + + + stream << "<body:cylinder name='left_wheel'>"; + stream << "<xyz>0 0.026 0.02</xyz>"; + stream << "<rpy>0 90 -90</rpy>"; + + stream << "<geom:cylinder name='left_wheel_geom'>"; + stream << "<size>0.03 0.003</size>"; + stream << "<mass>0.01</mass>"; + stream << "<mul>0.5</mul>"; + stream << "<visual>"; + stream << "<size>0.04 0.04 0.003</size>"; + stream << "<mesh>unit_cylinder</mesh>"; + stream << "<material>Gazebo/Grey</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<xyz>0. 0 -0.0016</xyz>"; + stream << "<size>0.03 0.03 0</size>"; + stream << "<mesh>unit_box</mesh>"; + stream << "<material>Gazebo/EpuckLogo</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<mesh>unit_cylinder</mesh>"; + stream << "<size>0.042 0.042 0.0015</size>"; + stream << "<material>Gazebo/Black</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<mesh>unit_cylinder</mesh>"; + stream << "<size>0.012 0.012 0.011</size>"; + stream << "<material>Gazebo/Grey</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<mesh>unit_cylinder</mesh>"; + stream << "<size>0.01 0.01 0.014</size>"; + stream << "<material>Gazebo/EpuckGold</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<mesh>unit_cylinder</mesh>"; + stream << "<size>0.004 0.004 0.0195</size>"; + stream << "<material>Gazebo/EpuckGold</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<mesh>unit_cylinder</mesh>"; + stream << "<size>0.008 0.008 0.018</size>"; + stream << "<material>Gazebo/Grey</material>"; + stream << "</visual>"; + stream << "</geom:cylinder>"; + stream << "</body:cylinder>"; + + if(withCamera) + { + stream << "<model:physical name='cam"<<id<<"'>"; + stream << "<body:empty name='epuck_camera'>"; + stream << "<rpy>0.0 0.0 0.0</rpy>"; + stream << "<xyz>0.026 0.0 0.030</xyz>"; + stream << "<sensor:camera name='epuck_cam_sensor"<<id<<"'>"; + stream << "<imageSize>200 150</imageSize>"; + stream << "<hfov>60</hfov>"; + stream << "<nearClip>0.02</nearClip>"; + stream << "<farClip>20</farClip>"; + stream << "<saveFrames>false</saveFrames>"; + stream << "<saveFramePath>frames</saveFramePath>"; + stream << "<controller:generic_camera name='epuck_camera_controller'>"; + stream << "<interface:camera name='camera_iface_0'/>"; + stream << "</controller:generic_camera>"; + stream << "</sensor:camera>"; + stream << "</body:empty>"; + stream << "<attach>"; + stream << "<parentBody>epuck_chassis</parentBody>"; + stream << "<myBody>epuck_camera</myBody>"; + stream << "</attach>"; + stream << "</model:physical>"; + } + + + stream << "<body:cylinder name='right_wheel'>"; + stream << "<xyz>0 -0.026 0.02</xyz>"; + stream << "<rpy>0 90 90</rpy>"; + + stream << "<geom:cylinder name='right_wheel_geom'>"; + stream << "<size>0.03 0.003</size>"; + stream << "<mass>0.01</mass>"; + stream << "<mul>0.5</mul>"; + + stream << "<visual>"; + stream << "<size>0.04 0.04 0.003</size>"; + stream << "<mesh>unit_cylinder</mesh>"; + stream << "<material>Gazebo/Grey</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<xyz>0. 0 -0.0016</xyz>"; + stream << "<size>0.03 0.03 0</size>"; + stream << "<mesh>unit_box</mesh>"; + stream << "<material>Gazebo/EpuckLogo</material>"; + stream << "</visual>"; + + stream << "<visual>"; + stream << "<mesh>unit_cylinder</mesh>"; + stream << "<size>0.042 0.042 0.0015</size>"; + stream << "<material>Gazebo/Black</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<mesh>unit_cylinder</mesh>"; + stream << "<size>0.012 0.012 0.011</size>"; + stream << "<material>Gazebo/Grey</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<mesh>unit_cylinder</mesh>"; + stream << "<size>0.01 0.01 0.014</size>"; + stream << "<material>Gazebo/Gold</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<mesh>unit_cylinder</mesh>"; + stream << "<size>0.004 0.004 0.0195</size>"; + stream << "<material>Gazebo/Gold</material>"; + stream << "</visual>"; + stream << "<visual>"; + stream << "<mesh>unit_cylinder</mesh>"; + stream << "<size>0.008 0.008 0.018</size>"; + stream << "<material>Gazebo/Grey</material>"; + stream << "</visual>"; + stream << "</geom:cylinder>"; + stream << "</body:cylinder>"; + + stream << "<body:sphere name='castor_body_rear'>"; + stream << "<xyz>-0.026 0 -0.000</xyz>"; + stream << "<rpy>0 0 0</rpy>"; + stream << "<geom:sphere name='castor_geom_rear'>"; + stream << "<size>0.01</size>"; + stream << "<mass>0.1</mass>"; + stream << "<mu1>0.5</mu1>"; + + stream << "<visual>"; + stream << "<scale>0.01 0.01 0.01</scale>"; + stream << "<mesh>unit_sphere</mesh>"; + stream << "<material>Gazebo/Black</material>"; + stream << "</visual>"; + stream << "</geom:sphere>"; + stream << "</body:sphere>"; + + stream << "<body:sphere name='castor_body_front'>"; + stream << "<xyz>0.026 0 -0.000</xyz>"; + stream << "<rpy>0 0 0</rpy>"; + stream << "<geom:sphere name='castor_geom_front'>"; + stream << "<size>0.01</size>"; + stream << "<mass>0.1</mass>"; + stream << "<mu1>0.5</mu1>"; + + stream << "<visual>"; + stream << "<scale>0.01 0.01 0.01</scale>"; + stream << "<mesh>unit_sphere</mesh>"; + stream << "<material>Gazebo/Black</material>"; + stream << "</visual>"; + stream << "</geom:sphere>"; + stream << "</body:sphere>"; + + stream << "<joint:hinge name='left_wheel_hinge'>"; + stream << "<body1>left_wheel</body1>"; + stream << "<body2>epuck_chassis</body2>"; + stream << "<anchor>left_wheel</anchor>"; + stream << "<axis>0 1 0</axis>"; + stream << "<erp>0.8</erp>"; + stream << "<cfm>10e-5</cfm>"; + stream << "</joint:hinge>"; + stream << "<joint:hinge name='right_wheel_hinge'>"; + stream << "<body1>right_wheel</body1>"; + stream << "<body2>epuck_chassis</body2>"; + stream << "<anchor>right_wheel</anchor>"; + stream << "<axis>0 1 0</axis>"; + stream << "<erp>0.8</erp>"; + stream << "<cfm>10e-5</cfm>"; + stream << "</joint:hinge>"; + + stream << "<joint:ball name='ball_joint_front'>"; + stream << "<body1>castor_body_front</body1>"; + stream << "<body2>epuck_chassis</body2>"; + stream << "<anchor>castor_body_front</anchor>"; + stream << "<erp>0.8</erp>"; + stream << "<cfm>10e-5</cfm>"; + stream << "</joint:ball>"; + + stream << "<joint:ball name='ball_joint_rear'>"; + stream << "<body1>castor_body_rear</body1>"; + stream << "<body2>epuck_chassis</body2>"; + stream << "<anchor>castor_body_rear</anchor>"; + stream << "<erp>0.8</erp>"; + stream << "<cfm>10e-5</cfm>"; + stream << "</joint:ball>"; + + stream << "</model:physical>"; + + this->id = id; + sprintf(this->robotName,"robot%d",id); + + sprintf(this->posIfaceName,"robot%d-position_iface_0", id); + sprintf(this->irIfaceName,"robot%d-irarray_iface_0",id); + + + + return stream.str().c_str(); + + +} Added: code/gazebo/trunk/examples/libgazebo/epuck/EpuckModel.hh =================================================================== --- code/gazebo/trunk/examples/libgazebo/epuck/EpuckModel.hh (rev 0) +++ code/gazebo/trunk/examples/libgazebo/epuck/EpuckModel.hh 2008-11-27 13:08:12 UTC (rev 7174) @@ -0,0 +1,44 @@ +#ifndef EPUCKMODEL_HH_ +#define EPUCKMODEL_HH_ + +#include <sstream> +#include <gazebo/gazebo.h> +using namespace gazebo; + +#define NUMS_IR 8 +#define WHEEL_DIAMETER 0.042 +#define WHEEL_SEP 0.052 + + +class Epuck +{ + public: + Epuck(); + ~Epuck(); + public:const char * CreateEpuckModel(int id, Pose& pose, bool withCamera=false); + public:bool Subscribe(Client *client); + public:void EnableMotor(); + + public: void DoCircle(double speed, double r, bool anticlock=true); + + public:bool SetSpeed(const double left, const double right); + public:bool GetIRReading( ); + public:void Update(); + + public:double ir[NUMS_IR]; + + private: int id; + private: char robotName[64]; + private: char posIfaceName[64]; + private: char irIfaceName[64]; + private: double lastupdateTime; + private: double currentTime; + + private: PositionIface *posIface; + private: IRIface *irIface; + private: int irCount; + +}; + + +#endif /*EPUCKMODEL_HH_*/ Added: code/gazebo/trunk/examples/libgazebo/epuck/SConstruct =================================================================== --- code/gazebo/trunk/examples/libgazebo/epuck/SConstruct (rev 0) +++ code/gazebo/trunk/examples/libgazebo/epuck/SConstruct 2008-11-27 13:08:12 UTC (rev 7174) @@ -0,0 +1,23 @@ + +# 3rd party packages +parseConfigs=['pkg-config --cflags --libs libgazebo', + 'pkg-config --cflags --libs libgazeboServer'] + +env = Environment ( + CC = 'g++', + + CCFLAGS = Split ('-pthread -pipe -W -Wall -O2'), + + #LIBPATH=Split('#libgazebo'), + + #LIBS=Split('gazebo boost_python') +) + + +# Parse all the pacakge configurations +for cfg in parseConfigs: + env.ParseConfig(cfg) + +sources = Split('main.cc EpuckModel.cc') + +env.Program('swarm',sources) Added: code/gazebo/trunk/examples/libgazebo/epuck/epuck.cfg =================================================================== --- code/gazebo/trunk/examples/libgazebo/epuck/epuck.cfg (rev 0) +++ code/gazebo/trunk/examples/libgazebo/epuck/epuck.cfg 2008-11-27 13:08:12 UTC (rev 7174) @@ -0,0 +1,236 @@ + +# Desc: Player sample configuration file for controlling Gazebo devices +# Date: 18 Apr 2003 +# CVS: $Id: gazebo.cfg,v 1.4.2.1 2006/12/16 22:43:23 natepak Exp $ + +driver +( + name "gazebo" + provides ["simulation:0"] + plugin "libgazeboplugin" + server_id "default" +) + +driver +( + name "gazebo" + provides ["7001:position2d:0"] + gz_id "robot1-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7001:ir:0"] + gz_id "robot1-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7002:position2d:0"] + gz_id "robot2-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7002:ir:0"] + gz_id "robot2-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7003:position2d:0"] + gz_id "robot3-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7003:ir:0"] + gz_id "robot3-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7004:position2d:0"] + gz_id "robot4-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7004:ir:0"] + gz_id "robot4-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7005:position2d:0"] + gz_id "robot5-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7005:ir:0"] + gz_id "robot5-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7006:position2d:0"] + gz_id "robot6-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7006:ir:0"] + gz_id "robot6-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7007:position2d:0"] + gz_id "robot7-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7007:ir:0"] + gz_id "robot7-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7008:position2d:0"] + gz_id "robot8-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7008:ir:0"] + gz_id "robot8-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7009:position2d:0"] + gz_id "robot9-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7009:ir:0"] + gz_id "robot9-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7010:position2d:0"] + gz_id "robot10-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7010:ir:0"] + gz_id "robot10-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7011:position2d:0"] + gz_id "robot11-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7011:ir:0"] + gz_id "robot11-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7012:position2d:0"] + gz_id "robot12-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7012:ir:0"] + gz_id "robot12-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7013:position2d:0"] + gz_id "robot13-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7013:ir:0"] + gz_id "robot13-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7014:position2d:0"] + gz_id "robot14-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7014:ir:0"] + gz_id "robot14-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7015:position2d:0"] + gz_id "robot15-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7015:ir:0"] + gz_id "robot15-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7016:position2d:0"] + gz_id "robot16-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7016:ir:0"] + gz_id "robot16-irarray_iface_0" +) Added: code/gazebo/trunk/examples/libgazebo/epuck/epuck.world =================================================================== --- code/gazebo/trunk/examples/libgazebo/epuck/epuck.world (rev 0) +++ code/gazebo/trunk/examples/libgazebo/epuck/epuck.world 2008-11-27 13:08:12 UTC (rev 7174) @@ -0,0 +1,173 @@ +<?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: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:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" + xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable" + 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.05</stepTime> + <gravity>0 0 -9.8</gravity> + <cfm>10e-5</cfm> + <erp>0.8</erp> + </physics:ode> + + <rendering:gui> + <type>fltk</type> + <size>800 600</size> + <pos>0 0</pos> + </rendering:gui> + + <rendering:ogre> + <ambient>1.0 1.0 1.0 1.0</ambient> + <sky> + <material>Gazebo/CloudySky</material> + </sky> + <grid>false</grid> + </rendering:ogre> + + <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>3 2</size> + <segments>10 10</segments> + <uvTile>100 100</uvTile> + <material>Gazebo/LightWood</material> + </geom:plane> + </body:plane> + </model:physical> + + <model:physical name="Observercam_model"> + <xyz>0 0 2.65</xyz> + <rpy>-90 90 0</rpy> + <static>true</static> + <body:empty name="Observercam_body"> + <sensor:camera name="Observercam_sensor"> + <imageSize>800 600</imageSize> + <hfov>60</hfov> + <nearClip>0.1</nearClip> + <farClip>100</farClip> + <saveFrames>false</saveFrames> + <saveFramePath>frames</saveFramePath> + </sensor:camera> + </body:empty> + </model:physical> + + <model:physical name="wall_model"> + <xyz>0 0 0</xyz> + <static>true</static> + <body:empty name="wall_body"> + <geom:box name="wall_geom1"> + <xyz>1.5 0 0</xyz> + <size>0.01 2 0.1</size> + <visual> + <scale>0.01 2 0.1</scale> + <mesh>unit_box</mesh> + <material>Gazebo/BumpyMetal</material> + </visual> + </geom:box> + <geom:box name="wall_geom2"> + <xyz>-1.5 0 0</xyz> + <size>0.01 2 0.1</size> + <visual> + <scale>0.01 2 0.1</scale> + <mesh>unit_box</mesh> + <material>Gazebo/BumpyMetal</material> + </visual> + </geom:box> + <geom:box name="wall_geom3"> + <xyz>0 1 0</xyz> + <size>3 0.01 0.1</size> + <visual> + <scale>3 0.01 0.1</scale> + <mesh>unit_box</mesh> + <material>Gazebo/BumpyMetal</material> + </visual> + </geom:box> + <geom:box name="wall_geom4"> + <xyz>0 -1 0</xyz> + <size>3 0.01 0.1</size> + <visual> + <scale>3 0.01 0.1</scale> + <mesh>unit_box</mesh> + <material>Gazebo/BumpyMetal</material> + </visual> + </geom:box> + </body:empty> + <body:empty name="corner1"> + <rpy>0 0 45</rpy> + <xyz>1.4 -0.9 0</xyz> + <geom:box name="wall_corner1"> + + <size>0.29 0.01 0.1</size> + <visual> + <scale>0.29 0.01 0.1</scale> + <mesh>unit_box</mesh> + <material>Gazebo/BumpyMetal</material> + </visual> + </geom:box> + </body:empty> + <body:empty name="corner2"> + <rpy>0 0 -45</rpy> + <xyz>-1.4 -0.9 0</xyz> + <geom:box name="wall_corner2"> + + <size>0.29 0.01 0.1</size> + <visual> + <scale>0.29 0.01 0.1</scale> + <mesh>unit_box</mesh> + <material>Gazebo/BumpyMetal</material> + </visual> + </geom:box> + </body:empty> + <body:empty name="corner3"> + <rpy>0 0 -45</rpy> + <xyz>1.4 0.9 0</xyz> + <geom:box name="wall_corner3"> + + <size>0.29 0.01 0.1</size> + <visual> + <scale>0.29 0.01 0.1</scale> + <mesh>unit_box</mesh> + <material>Gazebo/BumpyMetal</material> + </visual> + </geom:box> + </body:empty> + <body:empty name="corner4"> + <rpy>0 0 45</rpy> + <xyz>-1.4 0.9 0</xyz> + <geom:box name="wall_corner4"> + + <size>0.29 0.01 0.1</size> + <visual> + <scale>0.29 0.01 0.1</scale> + <mesh>unit_box</mesh> + <material>Gazebo/BumpyMetal</material> + </visual> + </geom:box> + </body:empty> + </model:physical> + + <model:empty name="factory_model"> + <controller:factory name="factory_controller"> + <interface:factory name="factory_iface"/> + </controller:factory> + </model:empty> + + + +</gazebo:world> Added: code/gazebo/trunk/examples/libgazebo/epuck/epuck_single.cfg =================================================================== --- code/gazebo/trunk/examples/libgazebo/epuck/epuck_single.cfg (rev 0) +++ code/gazebo/trunk/examples/libgazebo/epuck/epuck_single.cfg 2008-11-27 13:08:12 UTC (rev 7174) @@ -0,0 +1,33 @@ + +# Desc: Player sample configuration file for controlling Gazebo devices +# Date: 18 Apr 2003 +# CVS: $Id: gazebo.cfg,v 1.4.2.1 2006/12/16 22:43:23 natepak Exp $ + +driver +( + name "gazebo" + provides ["simulation:0"] + plugin "libgazeboplugin" + server_id "default" +) + +driver +( + name "gazebo" + provides ["7000:position2d:0"] + gz_id "robot1-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7000:ir:0"] + gz_id "robot1-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7000:camera:0"] + gz_id "robot1-camera_iface_0" +) Added: code/gazebo/trunk/examples/libgazebo/epuck/main.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/epuck/main.cc (rev 0) +++ code/gazebo/trunk/examples/libgazebo/epuck/main.cc 2008-11-27 13:08:12 UTC (rev 7174) @@ -0,0 +1,177 @@ +#include <sstream> +#include <string.h> +#include <gazebo/gazebo.h> +#include <gazebo/GazeboError.hh> +#include <math.h> + +#include "EpuckModel.hh" + +#define DTOR(x) x*3.1415926/180.0 +const int swarm_size = 60; + +int main() +{ + gazebo::Client *client = new gazebo::Client(); + gazebo::SimulationIface *simIface = new gazebo::SimulationIface(); + gazebo::FactoryIface *factoryIface = new gazebo::FactoryIface(); + + + int serverId = 0; + + /// Connect to the libgazebo server + try + { + client->ConnectWait(serverId, GZ_CLIENT_ID_USER_FIRST); + } + catch (gazebo::GazeboError e) + { + std::cout << "Gazebo error: Unable to connect\n" << e << "\n"; + return -1; + } + + + /// Open the Simulation Interface + try + { + simIface->Open(client, "default"); + } + catch (gazebo::GazeboError e) + { + std::cout << "Gazebo error: Unable to connect to the sim interface\n" << e << "\n"; + return -1; + } + + + + /// Open the Factory interface + try + { + factoryIface->Open(client, "factory_model-factory_iface"); + } + catch (gazebo::GazeboError e) + { + std::cout << "Gazebo error: Unable to connect to the factory interface\n" + << e << "\n"; + return -1; + } + + //Deploy 60 robots in the simulator + Pose pose; + pose.pos.y = 0.6; + pose.pos.x = 0; + pose.pos.z = 0.01; + pose.roll = 0; + pose.pitch = 0; + pose.yaw = 0; + Epuck *robots = new Epuck[swarm_size]; + double angle=15; + double radius=0.8; + + + for (int i=0; i<swarm_size; i++) + { + factoryIface->Lock(1); + if (!strcmp((const char*)factoryIface->data->newModel,"")) + { + // std::ostringstream stream; + /* + if(i==8) + pose.pos.y = 0.4; + else if(i==16) + pose.pos.y = 0.2; + else if(i==24) + pose.pos.y = -0.0; + else if(i==32) + pose.pos.y = -0.2; + else if(i==40) + pose.pos.y = -0.4; + else if(i==48) + pose.pos.y = -0.6; + else if(i==56) + pose.pos.y = -0.8; + + pose.pos.x = -0.7 + (i%8) * 0.2;*/ + + if(i<24) + { + radius=0.8; + angle = 15; + pose.pos.x = radius * cos(DTOR(i*angle)); + pose.pos.y = radius * sin(DTOR(i*angle)); + pose.yaw = i * angle + 90; + + } + else if(i<48) + { + radius=0.6; + angle = 15; + pose.pos.x = radius * cos(DTOR(i*angle)); + pose.pos.y = radius * sin(DTOR(i*angle)); + pose.yaw = i * angle -90; + } + else + { + radius=0.4; + angle = 30; + pose.pos.x = radius * cos(DTOR(i*angle)); + pose.pos.y = radius * sin(DTOR(i*angle)); + pose.yaw = i * angle +90; + } + + //CreateEpuckModel( i, pose); + + //std::cout<<stream; + + printf("Creating[%d]\n",i); + //if(i==10 || i==8) + // strcpy((char*)factoryIface->data->newModel, robots[i].CreateEpuckModel( i+1, pose,true));//stream.str().c_str()); + // else + strcpy((char*)factoryIface->data->newModel, robots[i].CreateEpuckModel( i+1, pose, true));//stream.str().c_str()); + + + } + factoryIface->Unlock(); + usleep(200000); + } + + for(int i=0;i<swarm_size;i++) + { + robots[i].Subscribe(client); + robots[i].EnableMotor(); + } + + double lastupdateTime=0; + double currentTime=0; + + while(true) + { + simIface->Lock(1); + currentTime = simIface->data->simTime; + simIface->Unlock(); + + if(currentTime - lastupdateTime >=0.01) + { + lastupdateTime = currentTime; + if(currentTime < 30) + for(int i=0;i<swarm_size;i++) + { //robots[i].Update(); + if(i<24) + robots[i].DoCircle(0.15, 0.8); + else if(i<48) + robots[i].DoCircle(0.15, 0.6, false); + else + robots[i].DoCircle(0.15,0.4); + + } + else + for(int i=0;i<swarm_size;i++) + { + robots[i].Update(); + } + } + } + + + return 0; +} + Modified: code/gazebo/trunk/libgazebo/Iface.cc =================================================================== --- code/gazebo/trunk/libgazebo/Iface.cc 2008-11-26 21:26:26 UTC (rev 7173) +++ code/gazebo/trunk/libgazebo/Iface.cc 2008-11-27 13:08:12 UTC (rev 7174) @@ -60,6 +60,7 @@ GZ_REGISTER_IFACE("opaque", OpaqueIface); GZ_REGISTER_IFACE("bumper", BumperIface); GZ_REGISTER_IFACE("imu", ImuIface); +GZ_REGISTER_IFACE("irarray", IRIface); ////////////////////////////////////////////////////////////////////////////// // Create an interface Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-11-26 21:26:26 UTC (rev 7173) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-11-27 13:08:12 UTC (rev 7174) @@ -979,7 +979,7 @@ public: GazeboData head; /// String describing the model to be initiated - public: uint8_t newModel[4096]; + public: uint8_t newModel[4096*4]; /// Delete a model by name public: uint8_t deleteModel[512]; @@ -1547,7 +1547,72 @@ /// \} +/***************************************************************************/ +/// \addtogroup libgazebo_iface +/// \{ +/** \defgroup ir_iface opaque + \brief Interface for IR + +\{ + */ + +/// Max number of IR ranges +#define GZ_IR_MAX_RANGES 32 + +/// \brief IR interface data +class IRData +{ + public: GazeboData head; + + //number of ir + public: int ir_count; + + /// Number of range readings + public: int range_count; + + /// Range readings + public: double ranges[GZ_IR_MAX_RANGES]; + + public: Pose poses[GZ_IR_MAX_RANGES]; + +}; + +/// \brief IR interface +class IRIface : public Iface +{ + /// \brief Constructor + public: IRIface():Iface("irarray", sizeof(IRIface)+sizeof(IRData)) {} + + /// \brief Destructor + public: virtual ~IRIface() {this->data = NULL;} + + /// \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 = (IRData*)this->mMap; + } + + /// \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 = (IRData*)this->mMap; + } + + /// Pointer to the laser data + public: IRData *data; +}; + +/** \} */ +/// \} + + } Modified: code/gazebo/trunk/player/GazeboDriver.cc =================================================================== --- code/gazebo/trunk/player/GazeboDriver.cc 2008-11-26 21:26:26 UTC (rev 7173) +++ code/gazebo/trunk/player/GazeboDriver.cc 2008-11-27 13:08:12 UTC (rev 7174) @@ -41,6 +41,7 @@ #include "PTZInterface.hh" #include "BumperInterface.hh" #include "GripperInterface.hh" +#include "IRInterface.hh" /* #include "PowerInterface.hh" @@ -316,6 +317,11 @@ if (!player_quiet_startup) printf(" a bumper interface.\n"); ifsrc = new BumperInterface( playerAddr, this, cf, section ); break; + + case PLAYER_IR_CODE: + if (!player_quiet_startup) printf(" an ir interface.\n"); + ifsrc = new IRInterface( playerAddr, this, cf, section ); + break; /* case PLAYER_POWER_CODE: Added: code/gazebo/trunk/player/IRInterface.cc =================================================================== --- code/gazebo/trunk/player/IRInterface.cc (rev 0) +++ code/gazebo/trunk/player/IRInterface.cc 2008-11-27 13:08:12 UTC (rev 7174) @@ -0,0 +1,185 @@ +/* + * 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 Licens... [truncated message content] |
From: <na...@us...> - 2008-11-27 13:09:13
|
Revision: 7175 http://playerstage.svn.sourceforge.net/playerstage/?rev=7175&view=rev Author: natepak Date: 2008-11-27 13:09:07 +0000 (Thu, 27 Nov 2008) Log Message: ----------- Moved player cfgs from worlds to player_cfgs dir Added Paths: ----------- code/gazebo/trunk/player_cfgs/epuck.cfg code/gazebo/trunk/player_cfgs/epuck_single.cfg Removed Paths: ------------- code/gazebo/trunk/worlds/player_cfgs/epuck.cfg code/gazebo/trunk/worlds/player_cfgs/epuck_single.cfg Copied: code/gazebo/trunk/player_cfgs/epuck.cfg (from rev 7174, code/gazebo/trunk/worlds/player_cfgs/epuck.cfg) =================================================================== --- code/gazebo/trunk/player_cfgs/epuck.cfg (rev 0) +++ code/gazebo/trunk/player_cfgs/epuck.cfg 2008-11-27 13:09:07 UTC (rev 7175) @@ -0,0 +1,236 @@ + +# Desc: Player sample configuration file for controlling Gazebo devices +# Date: 18 Apr 2003 +# CVS: $Id: gazebo.cfg,v 1.4.2.1 2006/12/16 22:43:23 natepak Exp $ + +driver +( + name "gazebo" + provides ["simulation:0"] + plugin "libgazeboplugin" + server_id "default" +) + +driver +( + name "gazebo" + provides ["7001:position2d:0"] + gz_id "robot1-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7001:ir:0"] + gz_id "robot1-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7002:position2d:0"] + gz_id "robot2-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7002:ir:0"] + gz_id "robot2-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7003:position2d:0"] + gz_id "robot3-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7003:ir:0"] + gz_id "robot3-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7004:position2d:0"] + gz_id "robot4-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7004:ir:0"] + gz_id "robot4-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7005:position2d:0"] + gz_id "robot5-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7005:ir:0"] + gz_id "robot5-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7006:position2d:0"] + gz_id "robot6-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7006:ir:0"] + gz_id "robot6-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7007:position2d:0"] + gz_id "robot7-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7007:ir:0"] + gz_id "robot7-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7008:position2d:0"] + gz_id "robot8-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7008:ir:0"] + gz_id "robot8-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7009:position2d:0"] + gz_id "robot9-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7009:ir:0"] + gz_id "robot9-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7010:position2d:0"] + gz_id "robot10-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7010:ir:0"] + gz_id "robot10-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7011:position2d:0"] + gz_id "robot11-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7011:ir:0"] + gz_id "robot11-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7012:position2d:0"] + gz_id "robot12-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7012:ir:0"] + gz_id "robot12-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7013:position2d:0"] + gz_id "robot13-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7013:ir:0"] + gz_id "robot13-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7014:position2d:0"] + gz_id "robot14-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7014:ir:0"] + gz_id "robot14-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7015:position2d:0"] + gz_id "robot15-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7015:ir:0"] + gz_id "robot15-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["7016:position2d:0"] + gz_id "robot16-position_iface_0" +) + +driver +( + name "gazebo" + provides ["7016:ir:0"] + gz_id "robot16-irarray_iface_0" +) Property changes on: code/gazebo/trunk/player_cfgs/epuck.cfg ___________________________________________________________________ Added: svn:mergeinfo + Copied: code/gazebo/trunk/player_cfgs/epuck_single.cfg (from rev 7174, code/gazebo/trunk/worlds/player_cfgs/epuck_single.cfg) =================================================================== --- code/gazebo/trunk/player_cfgs/epuck_single.cfg (rev 0) +++ code/gazebo/trunk/player_cfgs/epuck_single.cfg 2008-11-27 13:09:07 UTC (rev 7175) @@ -0,0 +1,33 @@ + +# Desc: Player sample configuration file for controlling Gazebo devices +# Date: 18 Apr 2003 +# CVS: $Id: gazebo.cfg,v 1.4.2.1 2006/12/16 22:43:23 natepak Exp $ + +driver +( + name "gazebo" + provides ["simulation:0"] + plugin "libgazeboplugin" + server_id "default" +) + +driver +( + name "gazebo" + provides ["6665:position2d:0"] + gz_id "robot1-position_iface_0" +) + +driver +( + name "gazebo" + provides ["6665:ir:0"] + gz_id "robot1-irarray_iface_0" +) + +driver +( + name "gazebo" + provides ["6665:camera:0"] + gz_id "robot1-camera_iface_0" +) Property changes on: code/gazebo/trunk/player_cfgs/epuck_single.cfg ___________________________________________________________________ Added: svn:mergeinfo + Deleted: code/gazebo/trunk/worlds/player_cfgs/epuck.cfg =================================================================== --- code/gazebo/trunk/worlds/player_cfgs/epuck.cfg 2008-11-27 13:08:12 UTC (rev 7174) +++ code/gazebo/trunk/worlds/player_cfgs/epuck.cfg 2008-11-27 13:09:07 UTC (rev 7175) @@ -1,236 +0,0 @@ - -# Desc: Player sample configuration file for controlling Gazebo devices -# Date: 18 Apr 2003 -# CVS: $Id: gazebo.cfg,v 1.4.2.1 2006/12/16 22:43:23 natepak Exp $ - -driver -( - name "gazebo" - provides ["simulation:0"] - plugin "libgazeboplugin" - server_id "default" -) - -driver -( - name "gazebo" - provides ["7001:position2d:0"] - gz_id "robot1-position_iface_0" -) - -driver -( - name "gazebo" - provides ["7001:ir:0"] - gz_id "robot1-irarray_iface_0" -) - -driver -( - name "gazebo" - provides ["7002:position2d:0"] - gz_id "robot2-position_iface_0" -) - -driver -( - name "gazebo" - provides ["7002:ir:0"] - gz_id "robot2-irarray_iface_0" -) - -driver -( - name "gazebo" - provides ["7003:position2d:0"] - gz_id "robot3-position_iface_0" -) - -driver -( - name "gazebo" - provides ["7003:ir:0"] - gz_id "robot3-irarray_iface_0" -) - -driver -( - name "gazebo" - provides ["7004:position2d:0"] - gz_id "robot4-position_iface_0" -) - -driver -( - name "gazebo" - provides ["7004:ir:0"] - gz_id "robot4-irarray_iface_0" -) - -driver -( - name "gazebo" - provides ["7005:position2d:0"] - gz_id "robot5-position_iface_0" -) - -driver -( - name "gazebo" - provides ["7005:ir:0"] - gz_id "robot5-irarray_iface_0" -) - -driver -( - name "gazebo" - provides ["7006:position2d:0"] - gz_id "robot6-position_iface_0" -) - -driver -( - name "gazebo" - provides ["7006:ir:0"] - gz_id "robot6-irarray_iface_0" -) - -driver -( - name "gazebo" - provides ["7007:position2d:0"] - gz_id "robot7-position_iface_0" -) - -driver -( - name "gazebo" - provides ["7007:ir:0"] - gz_id "robot7-irarray_iface_0" -) - -driver -( - name "gazebo" - provides ["7008:position2d:0"] - gz_id "robot8-position_iface_0" -) - -driver -( - name "gazebo" - provides ["7008:ir:0"] - gz_id "robot8-irarray_iface_0" -) - -driver -( - name "gazebo" - provides ["7009:position2d:0"] - gz_id "robot9-position_iface_0" -) - -driver -( - name "gazebo" - provides ["7009:ir:0"] - gz_id "robot9-irarray_iface_0" -) - -driver -( - name "gazebo" - provides ["7010:position2d:0"] - gz_id "robot10-position_iface_0" -) - -driver -( - name "gazebo" - provides ["7010:ir:0"] - gz_id "robot10-irarray_iface_0" -) - -driver -( - name "gazebo" - provides ["7011:position2d:0"] - gz_id "robot11-position_iface_0" -) - -driver -( - name "gazebo" - provides ["7011:ir:0"] - gz_id "robot11-irarray_iface_0" -) - -driver -( - name "gazebo" - provides ["7012:position2d:0"] - gz_id "robot12-position_iface_0" -) - -driver -( - name "gazebo" - provides ["7012:ir:0"] - gz_id "robot12-irarray_iface_0" -) - -driver -( - name "gazebo" - provides ["7013:position2d:0"] - gz_id "robot13-position_iface_0" -) - -driver -( - name "gazebo" - provides ["7013:ir:0"] - gz_id "robot13-irarray_iface_0" -) - -driver -( - name "gazebo" - provides ["7014:position2d:0"] - gz_id "robot14-position_iface_0" -) - -driver -( - name "gazebo" - provides ["7014:ir:0"] - gz_id "robot14-irarray_iface_0" -) - -driver -( - name "gazebo" - provides ["7015:position2d:0"] - gz_id "robot15-position_iface_0" -) - -driver -( - name "gazebo" - provides ["7015:ir:0"] - gz_id "robot15-irarray_iface_0" -) - -driver -( - name "gazebo" - provides ["7016:position2d:0"] - gz_id "robot16-position_iface_0" -) - -driver -( - name "gazebo" - provides ["7016:ir:0"] - gz_id "robot16-irarray_iface_0" -) Deleted: code/gazebo/trunk/worlds/player_cfgs/epuck_single.cfg =================================================================== --- code/gazebo/trunk/worlds/player_cfgs/epuck_single.cfg 2008-11-27 13:08:12 UTC (rev 7174) +++ code/gazebo/trunk/worlds/player_cfgs/epuck_single.cfg 2008-11-27 13:09:07 UTC (rev 7175) @@ -1,33 +0,0 @@ - -# Desc: Player sample configuration file for controlling Gazebo devices -# Date: 18 Apr 2003 -# CVS: $Id: gazebo.cfg,v 1.4.2.1 2006/12/16 22:43:23 natepak Exp $ - -driver -( - name "gazebo" - provides ["simulation:0"] - plugin "libgazeboplugin" - server_id "default" -) - -driver -( - name "gazebo" - provides ["6665:position2d:0"] - gz_id "robot1-position_iface_0" -) - -driver -( - name "gazebo" - provides ["6665:ir:0"] - gz_id "robot1-irarray_iface_0" -) - -driver -( - name "gazebo" - provides ["6665:camera:0"] - gz_id "robot1-camera_iface_0" -) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-11-27 13:59:06
|
Revision: 7177 http://playerstage.svn.sourceforge.net/playerstage/?rev=7177&view=rev Author: natepak Date: 2008-11-27 13:58:59 +0000 (Thu, 27 Nov 2008) Log Message: ----------- Added patch 2280988 Modified Paths: -------------- code/gazebo/trunk/SConstruct code/gazebo/trunk/build.py Modified: code/gazebo/trunk/SConstruct =================================================================== --- code/gazebo/trunk/SConstruct 2008-11-27 13:09:50 UTC (rev 7176) +++ code/gazebo/trunk/SConstruct 2008-11-27 13:58:59 UTC (rev 7177) @@ -5,7 +5,7 @@ exec(open('build.py')) -PKG_CONFIG_VERSION = '0.23' +PKG_CONFIG_VERSION = '0.22' OGRE_VERSION = '= 1.6.0' ODE_VERSION = '>= 0.10.1' @@ -110,7 +110,7 @@ 'CheckPkg' : CheckPkg}) # Check for pkg-config if not conf.CheckPkgConfig(PKG_CONFIG_VERSION): - print ' Error: pkg-config version >= 0.23 not found.' + print ' Error: pkg-config version >= ' + PKG_CONFIG_VERSION + ' not found.' Exit(1) # Check for the correct OGRE version Modified: code/gazebo/trunk/build.py =================================================================== --- code/gazebo/trunk/build.py 2008-11-27 13:09:50 UTC (rev 7176) +++ code/gazebo/trunk/build.py 2008-11-27 13:58:59 UTC (rev 7177) @@ -43,6 +43,11 @@ def CheckPkg(context, name): context.Message( 'Checking for %s...' % name) action = 'PKG_CONFIG_PATH=%s pkg-config --exists \'%s\'' % (os.environ['PKG_CONFIG_PATH'], name); + pkg_config_cmd = "pkg-config" + if os.environ.has_key("PKG_CONFIG_PATH"): + pkg_config_cmd = "PKG_CONFIG_PATH="+os.environ["PKG_CONFIG_PATH"]+" pkg-config" + action = '%s --exists \'%s\'' % (pkg_config_cmd, name); + ret = context.TryAction(action)[0] context.Result(ret) return ret This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-12-22 18:10:05
|
Revision: 7212 http://playerstage.svn.sourceforge.net/playerstage/?rev=7212&view=rev Author: natepak Date: 2008-12-22 18:10:00 +0000 (Mon, 22 Dec 2008) Log Message: ----------- Implemented RequestGeom for the laser interface Modified Paths: -------------- code/gazebo/trunk/examples/player/laser/laser.cc code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/player/LaserInterface.cc code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc code/gazebo/trunk/server/sensors/Sensor.cc code/gazebo/trunk/server/sensors/Sensor.hh Modified: code/gazebo/trunk/examples/player/laser/laser.cc =================================================================== --- code/gazebo/trunk/examples/player/laser/laser.cc 2008-12-19 08:11:32 UTC (rev 7211) +++ code/gazebo/trunk/examples/player/laser/laser.cc 2008-12-22 18:10:00 UTC (rev 7212) @@ -16,10 +16,20 @@ // Subscribe to the laser proxy LaserProxy lp(&robot, 0); + robot.Read(); + // Print out some stuff std::cout << robot << std::endl; - for (;;) + lp.RequestGeom(); + + player_pose3d_t laserPose = lp.GetPose(); + + // Print out laser stuff + std::cout << "Laser Pose[" << laserPose.px << " " << + laserPose.py << " " << laserPose.pyaw << "]\n"; + + /*for (;;) { // This blocks until new data comes robot.Read(); @@ -29,7 +39,7 @@ printf("[%f %d]", lp.GetRange(i), lp.GetIntensity(i) ); } printf("\n"); - } + }*/ } catch (PlayerCc::PlayerError e) { Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-12-19 08:11:32 UTC (rev 7211) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-12-22 18:10:00 UTC (rev 7212) @@ -852,6 +852,12 @@ /// Commaned range count public: int cmd_range_count; + + /// Pose of the laser + public: Pose pose; + + /// Size of the laser + public: Vec3 size; }; /// \brief Laser interface Modified: code/gazebo/trunk/player/LaserInterface.cc =================================================================== --- code/gazebo/trunk/player/LaserInterface.cc 2008-12-19 08:11:32 UTC (rev 7211) +++ code/gazebo/trunk/player/LaserInterface.cc 2008-12-22 18:10:00 UTC (rev 7212) @@ -25,7 +25,6 @@ */ /* TODO -PLAYER_LASER_REQ_GET_GEOM */ #include <math.h> @@ -79,7 +78,7 @@ PLAYER_LASER_REQ_SET_CONFIG, this->device_addr)) { - player_laser_config_t* plc = (player_laser_config_t*)data; + //player_laser_config_t* plc = (player_laser_config_t*)data; if ( hdr->size == sizeof(player_laser_config_t) ) { @@ -137,15 +136,18 @@ { player_laser_geom_t rep; + // TODO: get geometry from somewhere memset(&rep.pose, 0, sizeof(rep.pose)); memset(&rep.size, 0, sizeof(rep.size)); -// rep.pose.px = 0.0; -// rep.pose.py = 0.0; -// rep.pose.pyaw = 0.0; -// rep.size.sw = 0.0; -// rep.size.sl = 0.0; + rep.pose.px = this->iface->data->pose.pos.x; + rep.pose.py = this->iface->data->pose.pos.y; + rep.pose.pyaw = this->iface->data->pose.yaw; + + rep.size.sw = this->iface->data->size.x; + rep.size.sl = this->iface->data->size.y; + this->driver->Publish(this->device_addr, respQueue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_LASER_REQ_GET_GEOM, Modified: code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc =================================================================== --- code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc 2008-12-19 08:11:32 UTC (rev 7211) +++ code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc 2008-12-22 18:10:00 UTC (rev 7212) @@ -123,6 +123,23 @@ { this->myParent->SetActive(false); } + + Quatern rot = this->myParent->GetPose().rot; + Vector3 pos = this->myParent->GetPose().pos; + + // Update the pose + this->laserIface->data->pose.pos.x = pos.x; + this->laserIface->data->pose.pos.y = pos.y; + this->laserIface->data->pose.pos.z = pos.z; + + this->laserIface->data->pose.roll = rot.GetRoll(); + this->laserIface->data->pose.pitch = rot.GetPitch(); + this->laserIface->data->pose.yaw = rot.GetYaw(); + + this->laserIface->data->size.x = 0.1; + this->laserIface->data->size.y = 0.1; + this->laserIface->data->size.z = 0.1; + } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/sensors/Sensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/Sensor.cc 2008-12-19 08:11:32 UTC (rev 7211) +++ code/gazebo/trunk/server/sensors/Sensor.cc 2008-12-22 18:10:00 UTC (rev 7212) @@ -185,4 +185,9 @@ this->active = value; } - +//////////////////////////////////////////////////////////////////////////////// +/// Get the current pose +const Pose3d &Sensor::GetPose() const +{ + return this->body->GetPose(); +} Modified: code/gazebo/trunk/server/sensors/Sensor.hh =================================================================== --- code/gazebo/trunk/server/sensors/Sensor.hh 2008-12-19 08:11:32 UTC (rev 7211) +++ code/gazebo/trunk/server/sensors/Sensor.hh 2008-12-22 18:10:00 UTC (rev 7212) @@ -28,6 +28,7 @@ #define SENSOR_HH #include "Param.hh" +#include "Pose3d.hh" #include "Entity.hh" namespace gazebo @@ -68,6 +69,9 @@ /// \brief Finalize the sensor public: void Fini(); + /// \brief Get the current pose + public: const Pose3d &GetPose() const; + /// \brief Set whether the sensor is active or not public: void SetActive(bool value); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-01-20 05:39:36
|
Revision: 7281 http://playerstage.svn.sourceforge.net/playerstage/?rev=7281&view=rev Author: natepak Date: 2009-01-20 05:39:31 +0000 (Tue, 20 Jan 2009) Log Message: ----------- Update to the openal implementation Modified Paths: -------------- code/gazebo/trunk/SConstruct code/gazebo/trunk/server/World.cc code/gazebo/trunk/server/audio_video/OpenAL.cc code/gazebo/trunk/server/audio_video/OpenAL.hh code/gazebo/trunk/server/audio_video/SConscript Modified: code/gazebo/trunk/SConstruct =================================================================== --- code/gazebo/trunk/SConstruct 2009-01-18 01:56:16 UTC (rev 7280) +++ code/gazebo/trunk/SConstruct 2009-01-20 05:39:31 UTC (rev 7281) @@ -58,12 +58,13 @@ '#server/controllers/position2d', '#server/controllers/position2d/pioneer2dx', '#server/controllers/irarray', + '#server/audio_video', ], - LIBPATH=Split('#libgazebo'), + LIBPATH=Split('#libgazebo #server/audio_video'), #LIBS=Split('gazebo boost_python') - LIBS=Split('gazebo boost_signals'), + LIBS=Split('gazebo gazeboav boost_signals'), LINKFLAGS=Split('-export-dynamic'), TARFLAGS = '-c -z', Modified: code/gazebo/trunk/server/World.cc =================================================================== --- code/gazebo/trunk/server/World.cc 2009-01-18 01:56:16 UTC (rev 7280) +++ code/gazebo/trunk/server/World.cc 2009-01-20 05:39:31 UTC (rev 7281) @@ -38,6 +38,7 @@ #include "Simulator.hh" #include "gazebo.h" #include "World.hh" +#include "OpenAL.hh" #include "Geom.hh" using namespace gazebo; @@ -170,8 +171,12 @@ (*miter)->Init(); } + // Initialize the physics engine this->physicsEngine->Init(); + // Initialize openal + OpenAL::Instance()->Init(); + this->toAddModels.clear(); this->toDeleteModels.clear(); @@ -219,6 +224,9 @@ this->toDeleteModels.clear(); + // Update the audio + OpenAL::Instance()->Update(); + return 0; } Modified: code/gazebo/trunk/server/audio_video/OpenAL.cc =================================================================== --- code/gazebo/trunk/server/audio_video/OpenAL.cc 2009-01-18 01:56:16 UTC (rev 7280) +++ code/gazebo/trunk/server/audio_video/OpenAL.cc 2009-01-20 05:39:31 UTC (rev 7281) @@ -2,32 +2,37 @@ #include <unistd.h> #include <iostream> -#include <AL/alut.h> +#ifdef ENABLE_OPENAL #include <AL/alc.h> +#endif +#include "GazeboError.hh" +#include "GazeboMessage.hh" #include "AudioDecoder.hh" #include "OpenAL.hh" -#ifndef NULL -#define NULL 0 -#endif +using namespace gazebo; - //////////////////////////////////////////////////////////////////////////////// ///// Constructor OpenAL::OpenAL() { +#ifdef ENABLE_OPENAL + printf("CONSTRUCTOR!!!!|\n\n\n"); this->context = NULL; this->audioDevice = NULL; this->pos[0] = this->pos[1] = this->pos[2] = 0.0; this->pos[0] = -10.0; +#endif } //////////////////////////////////////////////////////////////////////////////// /// Destructor OpenAL::~OpenAL() { + +#ifdef ENABLE_OPENAL if (this->context && this->audioDevice) { this->context = alcGetCurrentContext(); @@ -36,6 +41,7 @@ alcDestroyContext(this->context); alcCloseDevice(this->audioDevice); } +#endif this->sources.clear(); this->buffers.clear(); @@ -45,8 +51,11 @@ /// Initialize int OpenAL::Init() { +#ifdef ENABLE_OPENAL + // Open the default audio device this->audioDevice = alcOpenDevice("ALSA Software on HDA Intel"); + if (this->audioDevice == NULL) { printf("Unable to open audio device\n"); @@ -59,6 +68,7 @@ //Clear error code alGetError(); +#endif // TODO: put in function to set distance model //alDistanceModel(AL_EXPONENT_DISTANCE); @@ -69,6 +79,15 @@ /// Create a sound source unsigned int OpenAL::CreateSource() { +#ifdef ENABLE_OPENAL + + // Make sure we have an audio device + if (!this->audioDevice) + { + gzerr(0) << "Audio device not open\n"; + return -1; + } + unsigned int source; //Create 1 source @@ -76,17 +95,30 @@ this->sources.push_back(source); +#endif + return this->sources.size()-1; + } //////////////////////////////////////////////////////////////////////////////// /// Create an audio data buffer unsigned int OpenAL::CreateBuffer(const std::string &audioFile) { + +#ifdef ENABLE_OPENAL + unsigned int buffer; uint8_t *dataBuffer = NULL; unsigned int dataBufferSize; + // Make sure we have an audio device + if (!this->audioDevice) + { + gzerr(0) << "Audio device not open\n"; + return -1; + } + // Create an audio decoder AudioDecoder audioDecoder; @@ -106,6 +138,7 @@ if (dataBuffer) delete [] dataBuffer; +#endif return this->buffers.size()-1; } @@ -115,9 +148,18 @@ /// Update all the sources and the listener void OpenAL::Update() { +#ifdef ENABLE_OPENAL + //ALfloat pos[3]; ALfloat vel[3]; + // Make sure we have an audio device + if (!this->audioDevice) + { + gzerr(10) << "Audio device not open\n"; + return; + } + //for (int i=0; i< this->sourceCount; i++) { @@ -135,12 +177,22 @@ //printf("p[%f %f %f] v[%f %f %f]\n", pos[0], pos[1], pos[2], vel[0], vel[1], vel[2]); this->SetSourcePos(0, this->pos[0], this->pos[1], this->pos[2] ); } +#endif } //////////////////////////////////////////////////////////////////////////////// /// Set the data int OpenAL::SetData(unsigned int index, uint8_t *data, unsigned int dataSize, unsigned int freq) { +#ifdef ENABLE_OPENAL + + // Make sure we have an audio device + if (!this->audioDevice) + { + gzerr(0) << "Audio device not open\n"; + return -1; + } + // Clear the error buffer; alGetError(); @@ -158,8 +210,7 @@ alDeleteBuffers(1, &this->buffers[index]); return -1; } - - +#endif return 0; } @@ -167,6 +218,15 @@ /// \brief Attach a buffer to a source int OpenAL::SetSourceBuffer(unsigned int sourceIndex, unsigned int bufferIndex) { +#ifdef ENABLE_OPENAL + + // Make sure we have an audio device + if (!this->audioDevice) + { + gzerr(0) << "Audio device not open\n"; + return -1; + } + if (sourceIndex >= this->sources.size()) { std::cerr << "Invalid source index\n"; @@ -190,6 +250,7 @@ return -1; } +#endif return 0; } @@ -198,6 +259,15 @@ // Play the source void OpenAL::Play( unsigned int index ) { +#ifdef ENABLE_OPENAL + + // Make sure we have an audio device + if (!this->audioDevice) + { + gzerr(0) << "Audio device not open\n"; + return; + } + // Play the source alSourcePlay( this->sources[index] ); @@ -210,13 +280,22 @@ alGetSourcei(this->sources[index], AL_SOURCE_STATE, &state); } while ( state == AL_PLAYING); */ - +#endif } //////////////////////////////////////////////////////////////////////////////// /// Set the listener position int OpenAL::SetListenerPos( float x, float y, float z ) { +#ifdef ENABLE_OPENAL + + // Make sure we have an audio device + if (!this->audioDevice) + { + gzerr(0) << "Audio device not open\n"; + return -1; + } + // Clear error state alGetError(); @@ -227,7 +306,7 @@ std::cerr << "OpenAL SetListenerPos Error: [%d]\n" << this->error << "\n"; return -1; } - +#endif return 0; } @@ -235,6 +314,15 @@ /// Set the listener velocity int OpenAL::SetListenerVel( float x, float y, float z ) { +#ifdef ENABLE_OPENAL + + // Make sure we have an audio device + if (!this->audioDevice) + { + gzerr(0) << "Audio device not open\n"; + return -1; + } + // Clear error state alGetError(); @@ -244,7 +332,7 @@ std::cerr << "OpenAL SetListenerVel Error: [%d]" << this->error << "\n"; return -1; } - +#endif return 0; } @@ -253,8 +341,16 @@ int OpenAL::SetListenerOrient( float cx, float cy, float cz, float ux, float uy, float uz ) { +#ifdef ENABLE_OPENAL ALfloat orient[]={cx, cy, cz, ux, uy, uz}; + // Make sure we have an audio device + if (!this->audioDevice) + { + gzerr(0) << "Audio device not open\n"; + return -1; + } + // Clear error state alGetError(); @@ -265,7 +361,7 @@ std::cerr << "OpenAL SetListenerOrientation Error: [%d]" << this->error << "\n"; return -1; } - +#endif return 0; } @@ -273,8 +369,16 @@ // Set the position of the source int OpenAL::SetSourcePos(unsigned int index, float x, float y, float z) { +#ifdef ENABLE_OPENAL ALfloat p[3] = {x, y, z}; + // Make sure we have an audio device + if (!this->audioDevice) + { + gzerr(0) << "Audio device not open\n"; + return -1; + } + if (index >= this->sources.size()) { std::cerr << "Invalid source index[" << index <<" ]\n"; @@ -291,7 +395,7 @@ std::cerr << "OpenAL::SetSourcePos Error: [%d]" << this->error << "\n"; return -1; } - +#endif return 0; } @@ -299,8 +403,17 @@ // Set the position of the source int OpenAL::SetSourceVel(unsigned int index, float x, float y, float z) { +#ifdef ENABLE_OPENAL ALfloat v[3] = {x, y, z}; + // Make sure we have an audio device + if (!this->audioDevice) + { + gzerr(0) << "Audio device not open\n"; + return -1; + } + + if (index >= this->sources.size()) { std::cerr << "Invalid source index[" << index <<" ]\n"; @@ -317,7 +430,7 @@ std::cerr << "OpenAL::SetSourceVel Error: [%d]" << this->error << "\n"; return -1; } - +#endif return 0; } @@ -325,6 +438,16 @@ // Set the pitch of the source int OpenAL::SetSourcePitch(unsigned int index, float p) { + +#ifdef ENABLE_OPENAL + + // Make sure we have an audio device + if (!this->audioDevice) + { + gzerr(0) << "Audio device not open\n"; + return -1; + } + if (index >= this->sources.size()) { std::cerr << "Invalid source index[" << index <<" ]\n"; @@ -341,7 +464,7 @@ std::cerr << "OpenAL::SetSourcePitch Error: [%d]\n" << this->error; return -1; } - +#endif return 0; } @@ -349,6 +472,16 @@ // Set the pitch of the source int OpenAL::SetSourceGain(unsigned int index, float g) { +#ifdef ENABLE_OPENAL + + // Make sure we have an audio device + if (!this->audioDevice) + { + gzerr(0) << "Audio device not open\n"; + return -1; + } + + if (index >= this->sources.size()) { std::cerr << "Invalid source index[" << index <<" ]\n"; @@ -365,7 +498,7 @@ std::cerr << "OpenAL::SetSourceGain Error: [%d]\n" << this->error; return -1; } - +#endif return 0; } @@ -373,6 +506,16 @@ // Set whether the source loops the audio int OpenAL::SetSourceLoop(unsigned int index, bool state) { +#ifdef ENABLE_OPENAL + + // Make sure we have an audio device + if (!this->audioDevice) + { + gzerr(0) << "Audio device not open\n"; + return -1; + } + + if (index >= this->sources.size()) { std::cerr << "Invalid source index[" << index <<" ]\n"; @@ -390,7 +533,7 @@ std::cerr << "OpenAL::SetSourceLoop Error: [%d]\n" << this->error << "\n"; return -1; } +#endif - return 0; } Modified: code/gazebo/trunk/server/audio_video/OpenAL.hh =================================================================== --- code/gazebo/trunk/server/audio_video/OpenAL.hh 2009-01-18 01:56:16 UTC (rev 7280) +++ code/gazebo/trunk/server/audio_video/OpenAL.hh 2009-01-20 05:39:31 UTC (rev 7281) @@ -1,81 +1,95 @@ #ifndef OPENAL_HH #define OPENAL_HH +#ifndef DISABLE_OPENAL #include <AL/al.h> #include <AL/alc.h> #include <AL/alext.h> +#endif + #include <stdint.h> #include <deque> +#include "SingletonT.hh" -class OpenAL +namespace gazebo { - /// \brief Constructor - public: OpenAL(); + + class OpenAL : public SingletonT<OpenAL> + { + /// \brief Constructor + private: OpenAL(); + + /// \brief Destructor + private: virtual ~OpenAL(); + + /// \brief Initialize + public: int Init(); + + /// \brief Update all the sources and the listener + public: void Update(); + + /// \brief Create a sound source + public: unsigned int CreateSource(); + + /// \brief Create an audio data buffer + public: unsigned int CreateBuffer(const std::string &audioFile); + + /// \brief Set the data + public: int SetData(unsigned int index, uint8_t *data, unsigned int dataSize, unsigned int freq); + + /// \brief Attach a buffer to a source + public: int SetSourceBuffer(unsigned int sourceIndex, unsigned int bufferIndex); + + /// \brief Play a sound + public: void Play(unsigned int source); + + /// \brief Set the listener position + public: int SetListenerPos( float x, float y, float z ); + + /// \brief Set the listener velocity + public: int SetListenerVel( float x, float y, float z ); + + /// \brief Set the listener orientation + public: int SetListenerOrient(float cx, float cy, float cz, + float ux, float uy, float uz); + + /// \brief Set the position of the source + public: int SetSourcePos(unsigned int index, float x, float y, float z); + + /// \brief Set the position of the source + public: int SetSourceVel(unsigned int index, float x, float y, float z); + + /// \brief Set the pitch of the source + public: int SetSourcePitch(unsigned int index, float p); + + /// \brief Set the pitch of the source + public: int SetSourceGain(unsigned int index, float g); + + /// \brief Set whether the source loops the audio + public: int SetSourceLoop(unsigned int index, bool state); - /// \brief Destructor - public: virtual ~OpenAL(); +#ifndef DISABLE_OPENAL + private: ALCcontext *context; + private: ALCdevice *audioDevice; + + private: ALfloat pos[3]; - /// \brief Initialize - public: int Init(); + // OpenAL error code + private: ALenum error; +#endif - /// \brief Update all the sources and the listener - public: void Update(); + // Audio sources. + private: std::deque<unsigned int> sources; + + // Audio data buffers + private: std::deque<unsigned int> buffers; + + private: friend class DestroyerT<OpenAL>; + private: friend class SingletonT<OpenAL>; + }; + +} - /// \brief Create a sound source - public: unsigned int CreateSource(); - - /// \brief Create an audio data buffer - public: unsigned int CreateBuffer(const std::string &audioFile); - - /// \brief Set the data - public: int SetData(unsigned int index, uint8_t *data, unsigned int dataSize, unsigned int freq); - - /// \brief Attach a buffer to a source - public: int SetSourceBuffer(unsigned int sourceIndex, unsigned int bufferIndex); - - /// \brief Play a sound - public: void Play(unsigned int source); - - /// \brief Set the listener position - public: int SetListenerPos( float x, float y, float z ); - - /// \brief Set the listener velocity - public: int SetListenerVel( float x, float y, float z ); - - /// \brief Set the listener orientation - public: int SetListenerOrient(float cx, float cy, float cz, - float ux, float uy, float uz); - - /// \brief Set the position of the source - public: int SetSourcePos(unsigned int index, float x, float y, float z); - - /// \brief Set the position of the source - public: int SetSourceVel(unsigned int index, float x, float y, float z); - - /// \brief Set the pitch of the source - public: int SetSourcePitch(unsigned int index, float p); - - /// \brief Set the pitch of the source - public: int SetSourceGain(unsigned int index, float g); - - /// \brief Set whether the source loops the audio - public: int SetSourceLoop(unsigned int index, bool state); - - private: ALCcontext *context; - private: ALCdevice *audioDevice; - - // OpenAL error code - private: ALenum error; - - // Audio sources. - private: std::deque<unsigned int> sources; - - // Audio data buffers - private: std::deque<unsigned int> buffers; - - private: ALfloat pos[3]; -}; - #endif Modified: code/gazebo/trunk/server/audio_video/SConscript =================================================================== --- code/gazebo/trunk/server/audio_video/SConscript 2009-01-18 01:56:16 UTC (rev 7280) +++ code/gazebo/trunk/server/audio_video/SConscript 2009-01-20 05:39:31 UTC (rev 7281) @@ -1,11 +1,10 @@ #Import variable -Import('env sharedObjs headers') +Import('env sharedObjs headers install_prefix') parseConfigs = [ 'pkg-config --cflags --libs libavformat', 'pkg-config --cflags --libs libavcodec', - 'pkg-config --cflags --libs openal', - 'pkg-config --cflags --libs freealut' + 'pkg-config --cflags --libs openal' ] myEnv = env.Clone() @@ -18,6 +17,10 @@ ['server/audio_video/AudioDecoder.hh', 'server/audio_video/OpenAL.hh', ] ) + +enable_openal = 1 + + # # Parse all the pacakge configurations # @@ -32,10 +35,17 @@ if cfg.find("OpenAL") >= 0: print "OpenAL not found. 3D audio is disabled." print " http://connect.creativelabs.com/" - Exit(1) + enable_openal = 0 if cfg.find("avcodec") >= 0 or cfg.find("avformat") >= 0: print "FFMpeg not found. Audio decoding disabled." print " http://ffmpeg.mplayerhq.hu/" - Exit(1) + enable_openal = 0 -myEnv.SharedLibrary('gazeboav', sources) +conf = Configure(myEnv) +if conf.CheckLib('openal'): + conf.env.Append(CCFLAGS = " -DENABLE_OPENAL") +myEnv = conf.Finish() + +sharedLib = myEnv.SharedLibrary('gazeboav', sources) +myEnv.Install(install_prefix+'/lib', sharedLib) + This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-01-23 19:23:14
|
Revision: 7288 http://playerstage.svn.sourceforge.net/playerstage/?rev=7288&view=rev Author: natepak Date: 2009-01-23 19:23:09 +0000 (Fri, 23 Jan 2009) Log Message: ----------- Added in OpenAL interface and controller Modified Paths: -------------- code/gazebo/trunk/SConstruct code/gazebo/trunk/build.py code/gazebo/trunk/examples/player/bandit/bandit.cc code/gazebo/trunk/examples/player/position2d/position2d.cc code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/player/ActarrayInterface.cc code/gazebo/trunk/player/IRInterface.cc code/gazebo/trunk/player/IRInterface.hh code/gazebo/trunk/player/Position2dInterface.cc code/gazebo/trunk/server/Common.hh code/gazebo/trunk/server/SConscript code/gazebo/trunk/server/Simulator.cc code/gazebo/trunk/server/World.cc code/gazebo/trunk/server/XMLConfig.cc code/gazebo/trunk/server/audio_video/AudioDecoder.cc code/gazebo/trunk/server/audio_video/OpenAL.cc code/gazebo/trunk/server/audio_video/OpenAL.hh code/gazebo/trunk/server/audio_video/SConscript code/gazebo/trunk/server/controllers/Controller.cc code/gazebo/trunk/server/controllers/SConscript code/gazebo/trunk/server/controllers/audio/Audio.cc code/gazebo/trunk/server/controllers/audio/Audio.hh code/gazebo/trunk/server/physics/Geom.cc code/gazebo/trunk/server/physics/Geom.hh code/gazebo/trunk/server/rendering/OgreCreator.cc Added Paths: ----------- code/gazebo/trunk/Media/audio/ code/gazebo/trunk/Media/audio/test.mp3 code/gazebo/trunk/examples/libgazebo/openal/ code/gazebo/trunk/examples/libgazebo/openal/SConstruct code/gazebo/trunk/examples/libgazebo/openal/openal.cc code/gazebo/trunk/worlds/openal.world Added: code/gazebo/trunk/Media/audio/test.mp3 =================================================================== (Binary files differ) Property changes on: code/gazebo/trunk/Media/audio/test.mp3 ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Modified: code/gazebo/trunk/SConstruct =================================================================== --- code/gazebo/trunk/SConstruct 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/SConstruct 2009-01-23 19:23:09 UTC (rev 7288) @@ -20,7 +20,6 @@ opts.Add('prefix', 'The install path "prefix"', '/usr/local') opts.Add('destdir', 'The root directory to install into. Useful mainly for binary package building', '/') opts.Add('mode','Defines how Gazebo will be built, options available: optimized, profile, debug','debug') -opts.Add('with_audio','Builds support for 3d sound. Options available: no, yes ', 'no') # # 3rd party packages @@ -29,7 +28,11 @@ 'xml2-config --cflags --libs', 'ode-config --cflags --libs', 'fltk-config --cflags --libs --ldflags --use-gl --use-images', - 'pkg-config --cflags --libs xft' + 'pkg-config --cflags --libs xft', + + 'pkg-config --cflags --libs libavformat', + 'pkg-config --cflags --libs libavcodec', + 'pkg-config --cflags --libs openal' ] # @@ -64,7 +67,7 @@ LIBPATH=Split('#libgazebo #server/audio_video'), #LIBS=Split('gazebo boost_python') - LIBS=Split('gazebo gazeboav boost_signals'), + LIBS=Split('gazebo boost_signals'), LINKFLAGS=Split('-export-dynamic'), TARFLAGS = '-c -z', @@ -73,6 +76,7 @@ options=opts ) + Help(opts.GenerateHelpText(env)) if env['destdir'] != '/': @@ -100,8 +104,6 @@ elif env['mode'] == 'optimized': env['CCFLAGS'] += Split('-O3 -fPIC') -if env['with_audio'] == 'yes': - parseConfigs+=['pkg-config --cflags --libs OgreAL'] optimize_for_cpu(env); @@ -151,10 +153,14 @@ print "libfltk & development files are required, but not found." print " http://www.fltk.org" Exit(1) - elif cfg.find("OgreAL") >= 0: - print "You requested to activate the support for 3D audio. But OgreAL is not found." + elif cfg.find("OpenAL") >= 0: + print "OpenAL not found. 3D audio is disabled" print "http://www.ogre3d.org/phpBB2addons/viewtopic.php?t=3293" Exit(1) + elif cfg.find("avcodec") >= 0 or cfg.find("avformat") >= 0: + print "FFMpeg not found. MP3 decoding is disabled" + print " http://ffmpeg.mplayerhq.hu/" + Exit(1) # Check for trimesh support in ODE #if not conf.CheckODELib(): @@ -165,18 +171,22 @@ -# FIXME: if this check fails, it makes it fail the check for ODE -# This test should be done outside of the configure context below, because -# otherwise it tries to link against the not-yet-built libgazebo, causing -# the test to always fail. -simpleenv = Environment() -simpleconf = Configure(env) +simpleenv = Environment(CPPPATH="/usr/include/AL") +simpleconf = Configure(simpleenv) + +# Test for libtool if not simpleconf.CheckLibWithHeader('ltdl','ltdl.h','CXX'): print " Warning: Failed to find ltdl, no plugin support will be included" env["HAVE_LTDL"]=False else: env["HAVE_LTDL"]=True - env["CCFLAGS"].append("-DHAVE_LTDL") + env.Append(CCFLAGS = " -DHAVE_LTDL") + env.Append(LIBS = "ltdl") + +# Test for openal +if simpleconf.CheckLibWithHeader('openal', 'al.h', 'CXX'): + env["HAVE_OPENAL"] = True + simpleconf.Finish() Modified: code/gazebo/trunk/build.py =================================================================== --- code/gazebo/trunk/build.py 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/build.py 2009-01-23 19:23:09 UTC (rev 7288) @@ -42,7 +42,6 @@ def CheckPkg(context, name): context.Message( 'Checking for %s...' % name) - action = 'PKG_CONFIG_PATH=%s pkg-config --exists \'%s\'' % (os.environ['PKG_CONFIG_PATH'], name); pkg_config_cmd = "pkg-config" if os.environ.has_key("PKG_CONFIG_PATH"): pkg_config_cmd = "PKG_CONFIG_PATH="+os.environ["PKG_CONFIG_PATH"]+" pkg-config" Added: code/gazebo/trunk/examples/libgazebo/openal/SConstruct =================================================================== --- code/gazebo/trunk/examples/libgazebo/openal/SConstruct (rev 0) +++ code/gazebo/trunk/examples/libgazebo/openal/SConstruct 2009-01-23 19:23:09 UTC (rev 7288) @@ -0,0 +1,16 @@ + +# 3rd party packages +parseConfigs=['pkg-config --cflags --libs libgazebo', + 'pkg-config --cflags --libs libgazeboServer'] + + +env = Environment ( + CC = 'g++', + CCFLAGS = Split ('-pthread -pipe -W -Wall -O2'), +) + +# Parse all the package configurations +for cfg in parseConfigs: + env.ParseConfig(cfg) + +env.Program('openal','openal.cc') Added: code/gazebo/trunk/examples/libgazebo/openal/openal.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/openal/openal.cc (rev 0) +++ code/gazebo/trunk/examples/libgazebo/openal/openal.cc 2009-01-23 19:23:09 UTC (rev 7288) @@ -0,0 +1,57 @@ +#include <gazebo/gazebo.h> +#include <gazebo/GazeboError.hh> + +gazebo::Client *client = NULL; +gazebo::SimulationIface *simIface = NULL; +gazebo::AudioIface *audioIface = NULL; + +int main() +{ + client = new gazebo::Client(); + simIface = new gazebo::SimulationIface(); + audioIface = new gazebo::AudioIface(); + + int serverId = 0; + + /// Connect to the libgazebo server + try + { + client->ConnectWait(serverId, GZ_CLIENT_ID_USER_FIRST); + } + catch (gazebo::GazeboError e) + { + std::cout << "Gazebo error: Unable to connect\n" << e << "\n"; + return -1; + } + + /// Open the Simulation Interface + try + { + simIface->Open(client, "default"); + } + catch (gazebo::GazeboError e) + { + std::cout << "Gazebo error: Unable to connect to the sim interface\n" << e << "\n"; + return -1; + } + + /// Open the OpenAL interface + try + { + audioIface->Open(client, "audio_iface_1"); + } + catch (std::string e) + { + std::cout << "Gazebo error: Unable to connect to the audio interface\n" + << e << "\n"; + return -1; + } + + audioIface->Lock(1); + audioIface->data->cmd_play = 1; + audioIface->Unlock(); + + return 0; +} + + Modified: code/gazebo/trunk/examples/player/bandit/bandit.cc =================================================================== --- code/gazebo/trunk/examples/player/bandit/bandit.cc 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/examples/player/bandit/bandit.cc 2009-01-23 19:23:09 UTC (rev 7288) @@ -24,7 +24,7 @@ // This blocks until new data comes robot.Read(); - //ap.MoveTo(3, -M_PI/2.0); + ap.MoveTo(3, M_PI/2.0); // Left eyebrow ap.MoveTo(16, 0.5); @@ -38,7 +38,7 @@ // upper lip ap.MoveTo(19, 0.4); - player_actarray_actuator_t data = ap.GetActuatorData(17); + player_actarray_actuator_t data = ap.GetActuatorData(3); printf("Pos[%f] Speed[%f]\n",data.position, data.speed); } } Modified: code/gazebo/trunk/examples/player/position2d/position2d.cc =================================================================== --- code/gazebo/trunk/examples/player/position2d/position2d.cc 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/examples/player/position2d/position2d.cc 2009-01-23 19:23:09 UTC (rev 7288) @@ -14,7 +14,8 @@ PlayerClient robot(PlayerCc::PLAYER_HOSTNAME, PlayerCc::PLAYER_PORTNUM); // Subscribe to the simulation proxy - Position2dProxy pp(&robot, 1); + Position2dProxy pp(&robot, 0); + // Print out some stuff std::cout << robot << std::endl; Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/libgazebo/gazebo.h 2009-01-23 19:23:09 UTC (rev 7288) @@ -444,6 +444,7 @@ /// \} + /***************************************************************************/ /// \addtogroup libgazebo_iface /// \{ @@ -451,54 +452,38 @@ \brief Audio interface -The audio interface allows clients send and receive audio from the -simulated world. -It implements the microphones and also the sounds that can be -sent to the world. - + The audio interface controls playback of sound. \{ */ -#define GAZEBO_MAX_URL_SIZE 256 - /// \brief Audio interface data class AudioData { public: GazeboData head; - /// Data timestamp - public: float time; + /// Set the gain. Value >= 0 + public: float cmd_gain; - /// Play in a loop? - public: bool loop; + // Set the pitch. Value >= 0 + public: float cmd_pitch; - /// Gain - public: float gain; - - ///streaming of the file? - public: bool stream; - - ///Play + /// Play. 1 = play public: int cmd_play; - ///Pause + /// Pause. 1 = pause public: int cmd_pause; - ///Stop + /// Stop. 1 = stop public: int cmd_stop; + + /// Play in a loop? 1 = loop + public: int cmd_loop; -///Reset to the beginning - public: int cmd_reset; - - ///state given by the server, 1 for playing. + /// Rewind to the beginning. 1 = rewind + public: int cmd_rewind; + + /// Play state given by the server. 1 = playing public: int state; - - /// location of the file - public: char url[GAZEBO_MAX_URL_SIZE]; - - /// 3D Pose of the audio, given by the server - public: Pose audio_pose; - }; /// \brief The audio interface @@ -1611,7 +1596,7 @@ this->data = (IRData*)this->mMap; } - /// Pointer to the laser data + /// Pointer to the IR data public: IRData *data; }; @@ -1619,7 +1604,69 @@ /// \} +/***************************************************************************/ +/// \addtogroup libgazebo_iface +/// \{ +/** \defgroup openal_iface openal + + \brief OpenAL interface + +\{ + */ +/* +#define GAZEBO_OPENAL_MAX_REQUESTS 20 + +class OpenALRequest +{ + public: enum Type {PLAY, PAUSE, STOP}; + public: Type type; + public: char sourceName[512]; +}; + +/// \brief OpenAL interface data +class OpenALData +{ + public: GazeboData head; + + public: OpenALRequest requests[GAZEBO_OPENAL_MAX_REQUESTS]; + public: unsigned int requestCount; +}; + +/// \brief OpenAL interface +class OpenALIface : public Iface +{ + /// \brief Constructor + public: OpenALIface():Iface("openal", sizeof(OpenALIface)+sizeof(OpenALData)) {} + + /// \brief Destructor + public: virtual ~OpenALIface() {this->data = NULL;} + + /// \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 = (OpenALData*)this->mMap; + } + + /// \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 = (OpenALData*)this->mMap; + } + + /// Pointer to the laser data + public: OpenALData *data; +}; +*/ + +/** \} */ +/// \} + } - #endif Modified: code/gazebo/trunk/player/ActarrayInterface.cc =================================================================== --- code/gazebo/trunk/player/ActarrayInterface.cc 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/player/ActarrayInterface.cc 2009-01-23 19:23:09 UTC (rev 7288) @@ -139,6 +139,29 @@ } else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_ACTARRAY_REQ_SPEED, this->device_addr)) + { + assert(hdr->size >= sizeof(player_actarray_speed_config_t)); + player_actarray_speed_config_t *req; + player_actarray_speed_config_t response; + + req = (player_actarray_speed_config_t*)(data); + + if (req->joint < GAZEBO_ACTARRAY_MAX_NUM_ACTUATORS) + { + this->iface->Lock(1); + + response.joint = req->joint; + response.speed = this->iface->data->actuators[req->joint].speed; + + this->iface->Unlock(); + + driver->Publish(this->device_addr, respQueue, + PLAYER_MSGTYPE_RESP_ACK, PLAYER_ACTARRAY_REQ_SPEED, &response); + + } + } + else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_ACTARRAY_REQ_BRAKES, this->device_addr)) { assert(hdr->size >= sizeof(player_actarray_brakes_config_t)); @@ -233,9 +256,6 @@ // called from GazeboDriver::Update void ActarrayInterface::Update() { - - struct timeval ts; - this->iface->Lock(1); // Only Update when new data is present Modified: code/gazebo/trunk/player/IRInterface.cc =================================================================== --- code/gazebo/trunk/player/IRInterface.cc 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/player/IRInterface.cc 2009-01-23 19:23:09 UTC (rev 7288) @@ -82,7 +82,7 @@ rep.poses_count = this->iface->data->ir_count; rep.poses = new player_pose3d_t[rep.poses_count]; - for(int i=0;i<rep.poses_count;i++) + for(unsigned int i=0;i<rep.poses_count;i++) { rep.poses[i].px = this->iface->data->poses[i].pos.x; rep.poses[i].py = this->iface->data->poses[i].pos.y; @@ -115,10 +115,6 @@ // Only Update when new data is present if (this->iface->data->head.time > this->datatime) { - int i; - float rangeRes; - float angleRes; - this->datatime = this->iface->data->head.time; ts.tv_sec = (int) (this->iface->data->head.time); @@ -129,7 +125,7 @@ this->data.ranges_count = this->iface->data->ir_count; this->data.voltages_count = this->iface->data->ir_count; - if(oldCount != this->data.ranges_count) + if(oldCount != (int)this->data.ranges_count) { delete []this->data.ranges; delete []this->data.voltages; @@ -139,7 +135,7 @@ } - for (int i = 0; i < this->data.ranges_count; i++) + for (unsigned int i = 0; i < this->data.ranges_count; i++) { this->data.ranges[i] = (float)this->iface->data->ranges[i]; this->data.voltages[i]=0; Modified: code/gazebo/trunk/player/IRInterface.hh =================================================================== --- code/gazebo/trunk/player/IRInterface.hh 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/player/IRInterface.hh 2009-01-23 19:23:09 UTC (rev 7288) @@ -65,11 +65,11 @@ /// \brief Update this interface, publish new info. public: virtual void Update(); - /// \brief Open a SHM interface when a subscription is received. \ + /// \brief Open a SHM interface when a subscription is received. /// This is called fromGazeboDriver::Subscribe public: virtual void Subscribe(); - /// \brief Close a SHM interface. This is called from \ + /// \brief Close a SHM interface. This is called from /// GazeboDriver::Unsubscribe public: virtual void Unsubscribe(); Modified: code/gazebo/trunk/player/Position2dInterface.cc =================================================================== --- code/gazebo/trunk/player/Position2dInterface.cc 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/player/Position2dInterface.cc 2009-01-23 19:23:09 UTC (rev 7288) @@ -76,6 +76,7 @@ int Position2dInterface::ProcessMessage(QueuePointer &respQueue, player_msghdr_t *hdr, void *data) { + printf("Recived Message\n"); if (this->iface->Lock(1)) { @@ -195,11 +196,18 @@ return 0; } + else + { + printf("Unhandled\n"); + } this->iface->Unlock(); } else + { + printf("unable to get lock\n"); this->Unsubscribe(); + } return -1; Modified: code/gazebo/trunk/server/Common.hh =================================================================== --- code/gazebo/trunk/server/Common.hh 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/server/Common.hh 2009-01-23 19:23:09 UTC (rev 7288) @@ -35,7 +35,6 @@ namespace gazebo { - class Common { /// \brief Constructor Modified: code/gazebo/trunk/server/SConscript =================================================================== --- code/gazebo/trunk/server/SConscript 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/server/SConscript 2009-01-23 19:23:09 UTC (rev 7288) @@ -1,8 +1,11 @@ #Import variables Import('env install_prefix sharedObjs headers') -dirs = Split('physics rendering sensors controllers gui audio_video')# bindings') +dirs = Split('physics rendering sensors controllers gui')# bindings') +if env["HAVE_OPENAL"] == True: + dirs.append("audio_video") + for subdir in dirs: SConscript('%s/SConscript' % subdir) Modified: code/gazebo/trunk/server/Simulator.cc =================================================================== --- code/gazebo/trunk/server/Simulator.cc 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/server/Simulator.cc 2009-01-23 19:23:09 UTC (rev 7288) @@ -533,4 +533,6 @@ else gzerr(0) << "Unknown type\n"; } + + return model; } Modified: code/gazebo/trunk/server/World.cc =================================================================== --- code/gazebo/trunk/server/World.cc 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/server/World.cc 2009-01-23 19:23:09 UTC (rev 7288) @@ -56,8 +56,6 @@ this->showPhysics = false; this->physicsEngine = NULL; this->server = NULL; - this->simIface = NULL; - } //////////////////////////////////////////////////////////////////////////////// @@ -126,6 +124,8 @@ gzthrow(err); } + // Load OpenAL audio + OpenAL::Instance()->Load( rootNode->GetChild("openal", "audio") ); this->physicsEngine = new ODEPhysics(); //TODO: use exceptions here @@ -224,9 +224,6 @@ this->toDeleteModels.clear(); - // Update the audio - OpenAL::Instance()->Update(); - return 0; } @@ -263,6 +260,9 @@ gzthrow(e); } + // Close the openal server + OpenAL::Instance()->Fini(); + return 0; } @@ -545,7 +545,7 @@ unsigned int requestCount = this->simIface->data->requestCount; - // Max sure the request count is valid + // Make sure the request count is valid if (this->simIface->data->requestCount > GAZEBO_SIMULATION_MAX_REQUESTS) { gzerr(0) << "Request count[" << this->simIface->data->requestCount << "] greater than max allowable[" << GAZEBO_SIMULATION_MAX_REQUESTS << "]\n"; Modified: code/gazebo/trunk/server/XMLConfig.cc =================================================================== --- code/gazebo/trunk/server/XMLConfig.cc 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/server/XMLConfig.cc 2009-01-23 19:23:09 UTC (rev 7288) @@ -377,6 +377,8 @@ XMLConfigNode *XMLConfigNode::GetChild( const std::string &name, const std::string &prefix ) const { XMLConfigNode *tmp; + + for (tmp = this->childFirst; tmp != NULL; tmp = tmp->GetNext() ) { if (tmp->xmlNode->name && name == (const char*)tmp->xmlNode->name) Modified: code/gazebo/trunk/server/audio_video/AudioDecoder.cc =================================================================== --- code/gazebo/trunk/server/audio_video/AudioDecoder.cc 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/server/audio_video/AudioDecoder.cc 2009-01-23 19:23:09 UTC (rev 7288) @@ -44,7 +44,7 @@ if (this->codec == NULL) { - printf("Set and audio file before decoding.\n"); + printf("Set an audio file before decoding.\n"); return -1; } Modified: code/gazebo/trunk/server/audio_video/OpenAL.cc =================================================================== --- code/gazebo/trunk/server/audio_video/OpenAL.cc 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/server/audio_video/OpenAL.cc 2009-01-23 19:23:09 UTC (rev 7288) @@ -1,3 +1,29 @@ +/* + * 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: OpenAL Server + * Author: Nathan Koenig + * Date: 20 Jan 2008 + * SVN: $Id:$ + */ + #include <stdio.h> #include <unistd.h> #include <iostream> @@ -2,25 +28,22 @@ -#ifdef ENABLE_OPENAL #include <AL/alc.h> -#endif +#include "XMLConfig.hh" #include "GazeboError.hh" #include "GazeboMessage.hh" #include "AudioDecoder.hh" +#include "GazeboConfig.hh" +#include "Simulator.hh" #include "OpenAL.hh" using namespace gazebo; +OpenAL *OpenAL::myself = NULL; + //////////////////////////////////////////////////////////////////////////////// ///// Constructor OpenAL::OpenAL() { -#ifdef ENABLE_OPENAL - printf("CONSTRUCTOR!!!!|\n\n\n"); this->context = NULL; this->audioDevice = NULL; - - this->pos[0] = this->pos[1] = this->pos[2] = 0.0; - this->pos[0] = -10.0; -#endif } @@ -31,509 +54,463 @@ /// Destructor OpenAL::~OpenAL() { + this->Fini(); +} -#ifdef ENABLE_OPENAL - if (this->context && this->audioDevice) - { - this->context = alcGetCurrentContext(); - this->audioDevice = alcGetContextsDevice(this->context); - alcMakeContextCurrent(NULL); - alcDestroyContext(this->context); - alcCloseDevice(this->audioDevice); - } -#endif +//////////////////////////////////////////////////////////////////////////////// +// Instance +OpenAL *OpenAL::Instance() +{ + if (!myself) + myself = new OpenAL(); - this->sources.clear(); - this->buffers.clear(); + return myself; } //////////////////////////////////////////////////////////////////////////////// -/// Initialize -int OpenAL::Init() +/// Destructor +void OpenAL::Load(XMLConfigNode *node) { -#ifdef ENABLE_OPENAL + std::string deviceName = "default"; + // Get the audio device name + if (node) + { + deviceName = node->GetString("device", "default", 0); + } + // Open the default audio device - this->audioDevice = alcOpenDevice("ALSA Software on HDA Intel"); + if (deviceName == "default") + this->audioDevice = alcOpenDevice(NULL); + else + this->audioDevice = alcOpenDevice(deviceName.c_str()); + // Make sure that we could open the audio device if (this->audioDevice == NULL) { - printf("Unable to open audio device\n"); - return -1; + gzerr(0) << "Unable to open audio device[" << deviceName << "]\n Audio will be disabled.\n"; + return; } + // Create the audio context this->context = alcCreateContext(this->audioDevice, NULL); + if (this->context == NULL) + { + gzerr(0) << "Unable to create OpenAL Context.\nAudio will be disabled.\n"; + return; + } + + // Make the context current alcMakeContextCurrent(this->context); //Clear error code alGetError(); -#endif // TODO: put in function to set distance model //alDistanceModel(AL_EXPONENT_DISTANCE); - return 0; } //////////////////////////////////////////////////////////////////////////////// -/// Create a sound source -unsigned int OpenAL::CreateSource() +/// Initialize +void OpenAL::Init() { -#ifdef ENABLE_OPENAL +} - // Make sure we have an audio device - if (!this->audioDevice) +//////////////////////////////////////////////////////////////////////////////// +/// Finalize +void OpenAL::Fini() +{ + if (this->context) { - gzerr(0) << "Audio device not open\n"; - return -1; + this->context = alcGetCurrentContext(); + alcMakeContextCurrent(NULL); + alcDestroyContext(this->context); } - - unsigned int source; - - //Create 1 source - alGenSources(1, &source); - - this->sources.push_back(source); - -#endif - - return this->sources.size()-1; - + + if (this->audioDevice) + { + alcCloseDevice(this->audioDevice); + } } //////////////////////////////////////////////////////////////////////////////// -/// Create an audio data buffer -unsigned int OpenAL::CreateBuffer(const std::string &audioFile) +// Create an openal source from XML config node +OpenALSource *OpenAL::CreateSource( XMLConfigNode *node ) { + // Make sure the xml node is valid + if (!node) + { + gzerr(0) << "Invalid xmlconfig node\n"; + return NULL; + } -#ifdef ENABLE_OPENAL - - unsigned int buffer; - uint8_t *dataBuffer = NULL; - unsigned int dataBufferSize; - - // Make sure we have an audio device + // Make sure the audio device has been opened if (!this->audioDevice) { gzerr(0) << "Audio device not open\n"; - return -1; + return NULL; } - // Create an audio decoder - AudioDecoder audioDecoder; + // Create a source + OpenALSource *source = new OpenALSource(); - // Create and openAL audio buffer - alGenBuffers(1, &buffer); + // Load the source + source->Load(node); - // Store the openAL buffer - this->buffers.push_back(buffer); - - // Set the audio file to decode - audioDecoder.SetFile(audioFile); - audioDecoder.Decode(&dataBuffer, &dataBufferSize); - - // Fill the openAL data buffer - this->SetData(buffer, dataBuffer, dataBufferSize, - audioDecoder.GetSampleRate() ); - - if (dataBuffer) - delete [] dataBuffer; -#endif - - return this->buffers.size()-1; + // Return a pointer to the source + return source; } //////////////////////////////////////////////////////////////////////////////// -/// Update all the sources and the listener -void OpenAL::Update() +/// Set the listener position +void OpenAL::SetListenerPos( const Vector3 pos ) { -#ifdef ENABLE_OPENAL + ALenum error; - //ALfloat pos[3]; - ALfloat vel[3]; - // Make sure we have an audio device if (!this->audioDevice) { - gzerr(10) << "Audio device not open\n"; + gzerr(0) << "Audio disabled\n"; return; } - //for (int i=0; i< this->sourceCount; i++) - { + // Clear error state + alGetError(); - //alGetSourcefv(0, AL_VELOCITY, vel); - //alGetSourcefv(0, AL_POSITION, pos); + alListener3f(AL_POSITION, pos.x, pos.y, pos.z); - vel[0] = 0.1; - vel[1] = 0.0; - vel[2] = 0.0; - - this->pos[0] += 0.01; - this->pos[1] = 0; - this->pos[2] = 0; - - //printf("p[%f %f %f] v[%f %f %f]\n", pos[0], pos[1], pos[2], vel[0], vel[1], vel[2]); - this->SetSourcePos(0, this->pos[0], this->pos[1], this->pos[2] ); + if ((error = alGetError()) != AL_NO_ERROR) + { + gzerr(0) << " Error: [" << error << "]\n"; } -#endif } //////////////////////////////////////////////////////////////////////////////// -/// Set the data -int OpenAL::SetData(unsigned int index, uint8_t *data, unsigned int dataSize, unsigned int freq) +/// Set the listener velocity +void OpenAL::SetListenerVel( const Vector3 vel ) { -#ifdef ENABLE_OPENAL + ALenum error; // Make sure we have an audio device if (!this->audioDevice) { - gzerr(0) << "Audio device not open\n"; - return -1; + gzerr(0) << "Audio disabled\n"; } - // Clear the error buffer; + // Clear error state alGetError(); - printf("Set Data Freq[%d] DataSize[%d]\n",freq, dataSize); - - // Copy raw buffer into AL buffer 0 - // AL_FORMAT_MONO8, AL_FORMAT_MONO16, AL_FORMAT_STEREO8, - // AL_FORMAT_STEREO16 - alBufferData( this->buffers[0], AL_FORMAT_MONO16, data, dataSize, freq); - - if ( alGetError() != AL_NO_ERROR) + alListener3f(AL_VELOCITY, vel.x, vel.y, vel.z); + if ((error = alGetError()) != AL_NO_ERROR) { - printf("Unable to copy data into openAL buffer\n"); - - alDeleteBuffers(1, &this->buffers[index]); - return -1; + gzerr(0) << " Error: [" << error << "]\n"; } -#endif - return 0; + } //////////////////////////////////////////////////////////////////////////////// -/// \brief Attach a buffer to a source -int OpenAL::SetSourceBuffer(unsigned int sourceIndex, unsigned int bufferIndex) +/// Set the listener orientation +void OpenAL::SetListenerOrient( float cx, float cy, float cz, + float ux, float uy, float uz ) { -#ifdef ENABLE_OPENAL + ALenum error; + ALfloat orient[]={cx, cy, cz, ux, uy, uz}; // Make sure we have an audio device if (!this->audioDevice) { - gzerr(0) << "Audio device not open\n"; - return -1; + gzerr(0) << "Audio disabled\n"; } - if (sourceIndex >= this->sources.size()) - { - std::cerr << "Invalid source index\n"; - return -1; - } + // Clear error state + alGetError(); - if (bufferIndex >= this->buffers.size()) + alListenerfv( AL_ORIENTATION, orient ); + + if ((error = alGetError()) != AL_NO_ERROR) { - std::cerr << "Invalid buffer index\n"; - return -1; + gzerr(0) << " Error: [" << error << "]\n"; } - // Buffer data must be set before calling this function - - // Attach buffer to source - alSourcei(this->sources[sourceIndex], AL_BUFFER, this->buffers[bufferIndex] ); +} - if ((this->error = alGetError()) != AL_NO_ERROR) - { - std::cerr << "OpenAL SetSourceBuffer Error: [%d]\n" << this->error << "\n"; - return -1; - } -#endif - return 0; + +//////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// +// OPENAL SOURCE +//////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// + + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +OpenALSource::OpenALSource() +{ + //Create 1 source + alGenSources(1, &this->alSource); + + // Create 1 buffer + alGenBuffers(1, &this->alBuffer); } //////////////////////////////////////////////////////////////////////////////// -// Play the source -void OpenAL::Play( unsigned int index ) +// Destructor +OpenALSource::~OpenALSource() { -#ifdef ENABLE_OPENAL + alDeleteSources(1, &this->alSource); + alDeleteBuffers(1, &this->alBuffer); +} - // Make sure we have an audio device - if (!this->audioDevice) - { - gzerr(0) << "Audio device not open\n"; - return; - } +//////////////////////////////////////////////////////////////////////////////// +// Load from xml node +void OpenALSource::Load(XMLConfigNode *node) +{ + // Set the pitch of the source + this->SetPitch( node->GetDouble("pitch",1.0,0) ); - // Play the source - alSourcePlay( this->sources[index] ); + // Set the gain of the source + this->SetGain( node->GetDouble("gain",1.0,0) ); + // Set whether the source should loop when played + this->SetLoop( node->GetBool("loop",false,0) ); - // Wait until finished - /*ALint state; - do - { - usleep(100000); - alGetSourcei(this->sources[index], AL_SOURCE_STATE, &state); - } while ( state == AL_PLAYING); - */ -#endif + if (node->GetChild("mp3") != NULL) + this->FillBufferFromFile(node->GetString("mp3","",1)); } //////////////////////////////////////////////////////////////////////////////// -/// Set the listener position -int OpenAL::SetListenerPos( float x, float y, float z ) +// Set the position of the source +int OpenALSource::SetPos(const Vector3 &pos) { -#ifdef ENABLE_OPENAL + ALfloat p[3] = {pos.x, pos.y, pos.z}; + ALenum error; - // Make sure we have an audio device - if (!this->audioDevice) - { - gzerr(0) << "Audio device not open\n"; - return -1; - } - // Clear error state alGetError(); - alListener3f(AL_POSITION, x, y, z); + alSourcefv( this->alSource, AL_POSITION, p); - if ((this->error = alGetError()) != AL_NO_ERROR) + if ((error = alGetError()) != AL_NO_ERROR) { - std::cerr << "OpenAL SetListenerPos Error: [%d]\n" << this->error << "\n"; + gzerr(0) << "Error: [" << error << "]\n"; return -1; } -#endif + return 0; } //////////////////////////////////////////////////////////////////////////////// -/// Set the listener velocity -int OpenAL::SetListenerVel( float x, float y, float z ) +// Set the position of the source +int OpenALSource::SetVel(const Vector3 &vel) { -#ifdef ENABLE_OPENAL + ALenum error; + ALfloat v[3] = {vel.x, vel.y, vel.z}; - // Make sure we have an audio device - if (!this->audioDevice) - { - gzerr(0) << "Audio device not open\n"; - return -1; - } - // Clear error state alGetError(); - alListener3f(AL_VELOCITY, x, y, z); - if ((this->error = alGetError()) != AL_NO_ERROR) + alSourcefv( this->alSource, AL_VELOCITY, v); + + if ((error = alGetError()) != AL_NO_ERROR) { - std::cerr << "OpenAL SetListenerVel Error: [%d]" << this->error << "\n"; + gzerr(0) << "Error: [" << error << "]\n"; return -1; } -#endif + return 0; } //////////////////////////////////////////////////////////////////////////////// -/// Set the listener orientation -int OpenAL::SetListenerOrient( float cx, float cy, float cz, - float ux, float uy, float uz ) +// Set the pitch of the source +int OpenALSource::SetPitch(float p) { -#ifdef ENABLE_OPENAL - ALfloat orient[]={cx, cy, cz, ux, uy, uz}; + ALenum error; - // Make sure we have an audio device - if (!this->audioDevice) - { - gzerr(0) << "Audio device not open\n"; - return -1; - } - - // Clear error state + // clear error state alGetError(); - alListenerfv( AL_ORIENTATION, orient ); + alSourcef(this->alSource, AL_PITCH, p); - if ((this->error = alGetError()) != AL_NO_ERROR) + if ((error = alGetError()) != AL_NO_ERROR) { - std::cerr << "OpenAL SetListenerOrientation Error: [%d]" << this->error << "\n"; + gzerr(0) << " Error: [" << error << "]\n"; return -1; } -#endif + return 0; } //////////////////////////////////////////////////////////////////////////////// -// Set the position of the source -int OpenAL::SetSourcePos(unsigned int index, float x, float y, float z) +// Set the pitch of the source +int OpenALSource::SetGain(float g) { -#ifdef ENABLE_OPENAL - ALfloat p[3] = {x, y, z}; - // Make sure we have an audio device - if (!this->audioDevice) - { - gzerr(0) << "Audio device not open\n"; - return -1; - } + ALenum error; - if (index >= this->sources.size()) - { - std::cerr << "Invalid source index[" << index <<" ]\n"; - return -1; - } - - // Clear error state + // clear error state alGetError(); - alSourcefv( this->sources[index], AL_POSITION, p); + alSourcef(this->alSource, AL_GAIN, g); - if ((this->error = alGetError()) != AL_NO_ERROR) + if ((error = alGetError()) != AL_NO_ERROR) { - std::cerr << "OpenAL::SetSourcePos Error: [%d]" << this->error << "\n"; + gzerr(0) << "Error: [" << error << "]\n"; return -1; } -#endif + return 0; } //////////////////////////////////////////////////////////////////////////////// -// Set the position of the source -int OpenAL::SetSourceVel(unsigned int index, float x, float y, float z) +// Set whether the source loops the audio +int OpenALSource::SetLoop(bool state) { -#ifdef ENABLE_OPENAL - ALfloat v[3] = {x, y, z}; + ALenum error; - // Make sure we have an audio device - if (!this->audioDevice) - { - gzerr(0) << "Audio device not open\n"; - return -1; - } - - - if (index >= this->sources.size()) - { - std::cerr << "Invalid source index[" << index <<" ]\n"; - return -1; - } - - // Clear error state + // clear error state alGetError(); - alSourcefv( this->sources[index], AL_VELOCITY, v); + // Set looping state + alSourcei(this->alSource, AL_LOOPING, state); - if ((this->error = alGetError()) != AL_NO_ERROR) + if ((error = alGetError()) != AL_NO_ERROR) { - std::cerr << "OpenAL::SetSourceVel Error: [%d]" << this->error << "\n"; + gzerr(0) << " Error: [" << error << "]\n"; return -1; } -#endif + return 0; } //////////////////////////////////////////////////////////////////////////////// -// Set the pitch of the source -int OpenAL::SetSourcePitch(unsigned int index, float p) +// Play the source +void OpenALSource::Play() { + int sourceState; + alGetSourcei(this->alSource, AL_SOURCE_STATE, &sourceState); -#ifdef ENABLE_OPENAL - - // Make sure we have an audio device - if (!this->audioDevice) + // Play the source, if it's not already playing + if (sourceState != AL_PLAYING) { - gzerr(0) << "Audio device not open\n"; - return -1; + alSourcePlay( this->alSource ); } +} - if (index >= this->sources.size()) - { - std::cerr << "Invalid source index[" << index <<" ]\n"; - return -1; - } +//////////////////////////////////////////////////////////////////////////////// +// Pause the source +void OpenALSource::Pause() +{ + int sourceState; + alGetSourcei(this->alSource, AL_SOURCE_STATE, &sourceState); - // clear error state - alGetError(); + // Pause the source if it playing + if (sourceState == AL_PLAYING) + alSourcePause( this->alSource ); +} - alSourcef(this->sources[index], AL_PITCH, p); +//////////////////////////////////////////////////////////////////////////////// +// Stop the source +void OpenALSource::Stop() +{ + int sourceState; + alGetSourcei(this->alSource, AL_SOURCE_STATE, &sourceState); - if ((this->error = alGetError()) != AL_NO_ERROR) - { - std::cerr << "OpenAL::SetSourcePitch Error: [%d]\n" << this->error; - return -1; - } -#endif - return 0; + // Stop the source if it is not already stopped + if (sourceState != AL_STOPPED) + alSourcePause( this->alSource); } //////////////////////////////////////////////////////////////////////////////// -// Set the pitch of the source -int OpenAL::SetSourceGain(unsigned int index, float g) +/// Rewind the sound to the beginning +void OpenALSource::Rewind() { -#ifdef ENABLE_OPENAL + alSourceRewind(this->alSource); +} - // Make sure we have an audio device - if (!this->audioDevice) - { - gzerr(0) << "Audio device not open\n"; - return -1; - } +//////////////////////////////////////////////////////////////////////////////// +/// Is the audio playing +bool OpenALSource::IsPlaying() +{ + int sourceState; + alGetSourcei(this->alSource, AL_SOURCE_STATE, &sourceState); + return sourceState == AL_PLAYING; +} - if (index >= this->sources.size()) - { - std::cerr << "Invalid source index[" << index <<" ]\n"; - return -1; - } - // clear error state - alGetError(); +//////////////////////////////////////////////////////////////////////////////// +// Fill the audio buffer from PCM data +void OpenALSource::FillBufferFromPCM(uint8_t *pcmData, unsigned int dataCount, + int sampleRate ) +{ + // First detach the buffer + alSourcei(this->alSource, AL_BUFFER, 0 ); - alSourcef(this->sources[index], AL_GAIN, g); + // Copy raw buffer into AL buffer + // AL_FORMAT_MONO8, AL_FORMAT_MONO16, AL_FORMAT_STEREO8, + // AL_FORMAT_STEREO16 + alBufferData( this->alBuffer, AL_FORMAT_MONO16, pcmData, dataCount, + sampleRate); - if ((this->error = alGetError()) != AL_NO_ERROR) + // Attach buffer to source + alSourcei(this->alSource, AL_BUFFER, this->alBuffer ); + + if ( alGetError() != AL_NO_ERROR) { - std::cerr << "OpenAL::SetSourceGain Error: [%d]\n" << this->error; - return -1; + gzthrow("Unable to copy data into openAL buffer\n"); } -#endif - return 0; } //////////////////////////////////////////////////////////////////////////////// -// Set whether the source loops the audio -int OpenAL::SetSourceLoop(unsigned int index, bool state) +/// Fill the OpenAL audio buffer with data from a sound file +void OpenALSource::FillBufferFromFile( const std::string &audioFile ) { -#ifdef ENABLE_OPENAL - // Make sure we have an audio device - if (!this->audioDevice) - { - gzerr(0) << "Audio device not open\n"; - return -1; - } + std::string fullPathAudioFile = audioFile; + // Try to open the audio file in the current directory + FILE *testFile = fopen(fullPathAudioFile.c_str(), "r"); - if (index >= this->sources.size()) + // If the audio file couldn't be opened, try the gazebo paths + if (testFile == NULL) { - std::cerr << "Invalid source index[" << index <<" ]\n"; - return -1; + std::list<std::string>::iterator iter; + GazeboConfig *gzconfig = Simulator::Instance()->GetGazeboConfig(); + + for (iter = gzconfig->GetGazeboPaths().begin(); + iter != gzconfig->GetGazeboPaths().end(); + iter++) + { + fullPathAudioFile = *iter + "/Media/audio/" + audioFile; + testFile = fopen(fullPathAudioFile.c_str(), "r"); + + if (testFile) + { + break; + } + } } - // clear error state - alGetError(); + uint8_t *dataBuffer = NULL; + unsigned int dataBufferSize; - // Set looping state - alSourcei(this->sources[index], AL_LOOPING, state); + // Create an audio decoder + AudioDecoder audioDecoder; - if ((this->error = alGetError()) != AL_NO_ERROR) - { - std::cerr << "OpenAL::SetSourceLoop Error: [%d]\n" << this->error << "\n"; - return -1; - } -#endif + // Set the audio file to decode + audioDecoder.SetFile(fullPathAudioFile); + audioDecoder.Decode(&dataBuffer, &dataBufferSize); - return 0; + // Copy raw buffer into AL buffer + // AL_FORMAT_MONO8, AL_FORMAT_MONO16, AL_FORMAT_STEREO8, + // AL_FORMAT_STEREO16 + this->FillBufferFromPCM( dataBuffer, dataBufferSize, + audioDecoder.GetSampleRate()); + + if (dataBuffer) + delete [] dataBuffer; } Modified: code/gazebo/trunk/server/audio_video/OpenAL.hh =================================================================== --- code/gazebo/trunk/server/audio_video/OpenAL.hh 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/server/audio_video/OpenAL.hh 2009-01-23 19:23:09 UTC (rev 7288) @@ -1,95 +1,145 @@ +/* + * 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: OpenAL Server + * Author: Nathan Koenig + * Date: 20 Jan 2008 + * SVN: $Id:$ + */ + #ifndef OPENAL_HH #define OPENAL_HH -#ifndef DISABLE_OPENAL #include <AL/al.h> #include <AL/alc.h> #include <AL/alext.h> -#endif #include <stdint.h> +#include <string> -#include <deque> +#include "Vector3.hh" -#include "SingletonT.hh" - namespace gazebo { + + class OpenALIface; + class XMLConfigNode; + class OpenALSource; - class OpenAL : public SingletonT<OpenAL> + class OpenAL { /// \brief Constructor private: OpenAL(); /// \brief Destructor private: virtual ~OpenAL(); + + /// \brief Load the OpenAL server + public: void Load(XMLConfigNode *node); + + // \brief Get an instance of this class + public: static OpenAL *Instance(); /// \brief Initialize - public: int Init(); - - /// \brief Update all the sources and the listener - public: void Update(); - - /// \brief Create a sound source - public: unsigned int CreateSource(); - - /// \brief Create an audio data buffer - public: unsigned int CreateBuffer(const std::string &audioFile); - - /// \brief Set the data - public: int SetData(unsigned int index, uint8_t *data, unsigned int dataSize, unsigned int freq); - - /// \brief Attach a buffer to a source - public: int SetSourceBuffer(unsigned int sourceIndex, unsigned int bufferIndex); - - /// \brief Play a sound - public: void Play(unsigned int source); - + public: void Init(); + + /// \brief Finalize + public: void Fini(); + + /// \brief Create an OpenALSource object from XML node + /// \param node The XML config node containing the auiod information + /// \return A pointer to an OpenALSource object + public: OpenALSource *CreateSource( XMLConfigNode *node ); + /// \brief Set the listener position - public: int SetListenerPos( float x, float y, float z ); + public: void SetListenerPos( const Vector3 pos ); /// \brief Set the listener velocity - public: int SetListenerVel( float x, float y, float z ); + public: void SetListenerVel( const Vector3 vel ); /// \brief Set the listener orientation - public: int SetListenerOrient(float cx, float cy, float cz, + public: void SetListenerOrient(float cx, float cy, float cz, float ux, float uy, float uz); + + private: ALCcontext *context; + private: ALCdevice *audioDevice; + private: static OpenAL *myself; + }; + + + /// \brief OpenAL Source. This can be thought of as a speaker. + class OpenALSource + { + /// \brief Constructor + public: OpenALSource(); + + /// \brief Destructor + public: virtual ~OpenALSource(); + + /// \brief Load the source from xml + public: void Load(XMLConfigNode *node); + /// \brief Set the position of the source - public: int SetSourcePos(unsigned int index, float x, float y, float z); + public: int SetPos(const Vector3 &pos); - /// \brief Set the position of the source - public: int SetSourceVel(unsigned int index, float x, float y, float z); + /// \brief Set the velocity of the source + public: int SetVel(const Vector3 &vel); /// \brief Set the pitch of the source - public: int SetSourcePitch(unsigned int index, float p); + public: int SetPitch(float p); /// \brief Set the pitch of the source - public: int SetSourceGain(unsigned int index, float g); + public: int SetGain(float g); /// \brief Set whether the source loops the audio - public: int SetSourceLoop(unsigned int index, bool state); + public: int SetLoop(bool state); + + /// \brief Play a sound + public: void Play(); -#ifndef DISABLE_OPENAL - private: ALCcontext *context; - private: ALCdevice *audioDevice; - - private: ALfloat pos[3]; + /// \brief Pause a sound + public: void Pause(); - // OpenAL error code - private: ALenum error; -#endif + /// \brief Stop a sound + public: void Stop(); - // Audio sources. - private: std::deque<unsigned int> sources; - - // Audio data buffers - private: std::deque<unsigned int> buffers; - - private: friend class DestroyerT<OpenAL>; - private: friend class SingletonT<OpenAL>; + /// \brief Rewind the sound to the beginning + public: void Rewind(); + + /// \brief Is the audio playing + public: bool IsPlaying(); + + /// \brief Fill the OpenAL audio buffer from PCM data + public: void FillBufferFromPCM(uint8_t *pcmData, unsigned int dataCount, + int sampleRate ); + + /// \brief Fill the OpenAL audio buffer with data from a sound file + public: void FillBufferFromFile( const std::string &audioFile ); + + // OpenAL source index + private: unsigned int alSource; + + // OpenAL buffer index + private: unsigned int alBuffer; }; - } #endif Modified: code/gazebo/trunk/server/audio_video/SConscript =================================================================== --- code/gazebo/trunk/server/audio_video/SConscript 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/server/audio_video/SConscript 2009-01-23 19:23:09 UTC (rev 7288) @@ -1,14 +1,6 @@ #Import variable Import('env sharedObjs headers install_prefix') -parseConfigs = [ - 'pkg-config --cflags --libs libavformat', - 'pkg-config --cflags --libs libavcodec', - 'pkg-config --cflags --libs openal' -] - -myEnv = env.Clone() - sources = ['AudioDecoder.cc', 'OpenAL.cc' ] @@ -18,34 +10,4 @@ 'server/audio_video/OpenAL.hh', ] ) -enable_openal = 1 - - -# -# Parse all the pacakge configurations -# -if not myEnv.GetOption('clean'): - for cfg in parseConfigs: - print "Checking for ["+cfg+"]" - try: - myEnv.ParseConfig(cfg) - print " Success" - except OSError,e: - print "Unable to parse config ["+cfg+"]" - if cfg.find("OpenAL") >= 0: - print "OpenAL not found. 3D audio is disabled." - print " http://connect.creativelabs.com/" - enable_openal = 0 - if cfg.find("avcodec") >= 0 or cfg.find("avformat") >= 0: - print "FFMpeg not found. Audio decoding disabled." - print " http://ffmpeg.mplayerhq.hu/" - enable_openal = 0 - -conf = Configure(myEnv) -if conf.CheckLib('openal'): - conf.env.Append(CCFLAGS = " -DENABLE_OPENAL") -myEnv = conf.Finish() - -sharedLib = myEnv.SharedLibrary('gazeboav', sources) -myEnv.Install(install_prefix+'/lib', sharedLib) - +sharedObjs.append( env.SharedObject(sources) ) Modified: code/gazebo/trunk/server/controllers/Controller.cc =================================================================== --- code/gazebo/trunk/server/controllers/Controller.cc 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/server/controllers/Controller.cc 2009-01-23 19:23:09 UTC (rev 7288) @@ -118,10 +118,6 @@ gzthrow(e); } - - - std::cout<<ifaceName<<std::endl; - this->ifaces.push_back(iface); childNode = childNode->GetNextByNSPrefix("interface"); Modified: code/gazebo/trunk/server/controllers/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/SConscript 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/server/controllers/SConscript 2009-01-23 19:23:09 UTC (rev 7288) @@ -3,8 +3,8 @@ dirs = Split('position2d laser camera factory gripper actarray ptz opaque bumper imu irarray') -if env['with_audio'] == 'yes': - dirs+=Split('audio') +if env["HAVE_OPENAL"] == True: + dirs.append("audio") for subdir in dirs : SConscript('%s/SConscript' % subdir) Modified: code/gazebo/trunk/server/controllers/audio/Audio.cc =================================================================== --- code/gazebo/trunk/server/controllers/audio/Audio.cc 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/server/controllers/audio/Audio.cc 2009-01-23 19:23:09 UTC (rev 7288) @@ -19,9 +19,9 @@ * */ /* - * Desc: Audio controller - * Author: Jordi Polo - * Date: 23 Feb 2008 + * Desc: Audio Output controller + * Author: Nate Koenig + * Date: 23 Jan 2009 */ #include "Global.hh" @@ -32,48 +32,45 @@ #include "GazeboError.hh" #include "ControllerFactory.hh" #include "Audio.hh" -#include "OgreVisual.hh" + #include <string.h> -#include <OgreAL.h> using namespace gazebo; GZ_REGISTER_STATIC_CONTROLLER("audio", AudioController); -enum {DRIVE, STEER, FULL}; - //////////////////////////////////////////////////////////////////////////////// // Constructor AudioController::AudioController(Entity *parent ) : Controller(parent) { - this->soundManager =0; -// this->myParent = dynamic_cast<Model*>(this->parent); + this->myParent = dynamic_cast<Model*>(this->parent); -// if (!this->myParent) -// gzthrow("AudioController controller requires a Model as its parent"); + if (!this->myParent) + gzthrow("AudioController controller requires a geom as its parent"); + this->audioIface = NULL; + this->openALSource = NULL; } //////////////////////////////////////////////////////////////////////////////// // Destructor AudioController::~AudioController() { - GZ_DELETE(this->soundManager) + if (this->openALSource) + delete this->openALSource; } //////////////////////////////////////////////////////////////////////////////// // Load the controller void AudioController::LoadChild(XMLConfigNode *node) { + this->audioIface = dynamic_cast<AudioIface*>(this->ifaces[0]); - this->myIface = dynamic_cast<AudioIface*>(this->ifaces[0]); - - if (!this->myIface) + if (!this->audioIface) gzthrow("Audio controller requires an audio interface"); - this->loopSound= node->GetBool("loop",false,0); - this->stream=node->GetBool("stream",false,0); + this->openALSource = OpenAL::Instance()->CreateSource( node ); } @@ -81,123 +78,88 @@ // Initialize the controller void AudioController::InitChild() { - try - { - this->soundManager = new OgreAL::SoundManager(); - } - catch (Ogre::Exception e) - { - gzthrow("The 3d Sound manager can not be initialized, check your OpenAL and OgreAL installation\n"); - } } //////////////////////////////////////////////////////////////////////////////// // Reset the controller void AudioController::ResetChild() { - //this->cmdReset=1; + this->openALSource->Rewind(); } //////////////////////////////////////////////////////////////////////////////// // Update the controller void AudioController::UpdateChild() { - - OgreAL::Sound *sound; - std::string uniqueSoundName = this->GetUniqueName() + this->url; - this->GetAudioCmd(); - - if (cmdPlay) + // Update the position of the audio source, if it exists + if (this->openALSource) { + this->GetAudioCmd(); - if (!soundManager->hasSound(uniqueSoundName) - sound = soundManager->createSound(uniqueSoundName, this->url, this->loopSound, this->stream); - else - sound = soundManager->getSound(uniqueSoundName); - //TODO: check what happens if I call this twice - this->parent->GetVisualNode()->AttachObject(sound); - sound->addSoundFinishedHandler(this, &AudioController::SoundFinished); - sound->play(); - this->state=1; - } - if (cmdStop) - { - sound = soundManager->getSound(uniqueSoundName); - if (sound) - { - sound->stop(); - this->state=1; - } - } - if (cmdPause) - { - sound = soundManager->getSound(uniqueSoundName); - if (sound) - { - sound->pause(); - this->state=2; - } + this->PutAudioData(); + this->openALSource->SetPos(this->myParent->GetPose().pos); } - if (cmdReset) - { - - } - - //more stuff here - - this->PutAudioData(); } //////////////////////////////////////////////////////////////////////////////// // Finalize the controller void AudioController::FiniChild() { + delete this->openALSource; + this->openALSource = NULL; } - -void AudioController::soundFinished(OgreAL::Sound *sound) -{ - this->state=0; -} - ////////////////////////////////////////////////////////////////////////////// // Get commands from the external interface void AudioController::GetAudioCmd() { - if (this->myIface->Lock(1)) + if (this->audioIface->Lock(1)) { + // Control output of the audio + if (this->audioIface->data->cmd_play) + this->openALSource->Play(); + else if (this->audioIface->data->cmd_pause) + this->openALSource->Pause(); + else if (this->audioIface->data->cmd_stop) + this->openALSource->Stop(); + else if (this->audioIface->data->cmd_rewind) + this->openALSource->Rewind(); - this->cmdPlay = this->myIface->data->cmd_play; - this->cmdPause = this->myIface->data->cmd_pause; - this->cmdStop = this->myIface->data->cmd_stop; - this->cmdReset = this->myIface->data->cmd_reset; - this->gain = this->myIface->data->gain; + // Loop the audio + if (this->audioIface->data->cmd_loop) + this->openALSource->SetLoop(1); - if (this->cmdPlay) - this->url = ((const char *)this->myIface->data->url); + // Set the gain and pitch + if (this->audioIface->data->cmd_gain > 0) + this->openALSource->SetGain( this->audioIface->data->cmd_gain ); + if (this->audioIface->data->cmd_pitch > 0) + this->openALSource->SetPitch(this->audioIface->data->cmd_pitch ); + + // Reset the cmd variables + this->audioIface->data->cmd_play=0; + this->audioIface->data->cmd_pause=0; + this->audioIface->data->cmd_stop=0; + this->audioIface->data->cmd_rewind=0; + this->audioIface->data->cmd_loop=0; - //we got the command, not sure if this is correct - this->myIface->data->cmd_reset=0; - this->myIface->data->cmd_play=0; - this->myIface->data->cmd_pause=0; - this->myIface->data->cmd_stop=0; - - this->myIface->Unlock(); + this->audioIface->data->cmd_pitch = 0; + this->audioIface->data->cmd_gain = 0; + + this->audioIface->Unlock(); } - std::cout << this->url << std::endl; + } ////////////////////////////////////////////////////////////////////////////// // Update the data in the interface void AudioController::PutAudioData() { - if (this->myIface->Lock(1)) + if (this->audioIface->Lock(1)) { - this->myIface->data->head.time = Simulator::Instance()->GetSimTime(); - this->myIface->data->state = this->state; - - this->myIface->Unlock(); + this->audioIface->data->head.time = Simulator::Instance()->GetSimTime(); + this->audioIface->data->state = this->openALSource->IsPlaying(); + this->audioIface->Unlock(); } } Modified: code/gazebo/trunk/server/controllers/audio/Audio.hh =================================================================== --- code/gazebo/trunk/server/controllers/audio/Audio.hh 2009-01-22 23:58:58 UTC (rev 7287) +++ code/gazebo/trunk/server/controllers/audio/Audio.hh 2009-01-23 19:23:09 UTC (rev 7288) @@ -29,26 +29,28 @@ #include "Controller.hh" #include "Entity.hh" -#include "OgreAL.h" +#include "OpenAL.hh" namespace gazebo { + class Model; + class OpenALSource; class AudioIface; - class OgreAL::SoundManager; /// \addtogroup gazebo_controller /// \{ /** \defgroup audio audio - \brief Audio controller + \brief Audio output controller - Controller for the sounds + Controller for output of sounds \verbatim - <controller:audio name="talking_box"> + <controller:audio name="audio_1"> + <pitch>1</pitch> + <gain>1</gain> <loop>f... [truncated message content] |
From: <na...@us...> - 2009-03-07 08:08:37
|
Revision: 7363 http://playerstage.svn.sourceforge.net/playerstage/?rev=7363&view=rev Author: natepak Date: 2009-03-07 08:08:24 +0000 (Sat, 07 Mar 2009) Log Message: ----------- Fixed some problems with the scons build Modified Paths: -------------- code/gazebo/trunk/SConstruct code/gazebo/trunk/build.py code/gazebo/trunk/libgazebo/SConscript code/gazebo/trunk/libgazebo/Server.cc code/gazebo/trunk/player/SConscript code/gazebo/trunk/server/Entity.cc code/gazebo/trunk/server/SConscript code/gazebo/trunk/server/World.cc code/gazebo/trunk/server/audio_video/SConscript code/gazebo/trunk/server/controllers/SConscript code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc code/gazebo/trunk/server/gui/SConscript code/gazebo/trunk/server/physics/HeightmapGeom.cc code/gazebo/trunk/server/physics/SConscript code/gazebo/trunk/server/physics/ode/SConscript code/gazebo/trunk/server/rendering/SConscript code/gazebo/trunk/server/sensors/SConscript code/gazebo/trunk/server/sensors/Sensor.cc code/gazebo/trunk/server/sensors/Sensor.hh code/gazebo/trunk/server/sensors/camera/SConscript code/gazebo/trunk/server/sensors/contact/SConscript code/gazebo/trunk/server/sensors/imu/SConscript code/gazebo/trunk/server/sensors/ir/SConscript code/gazebo/trunk/server/sensors/ray/SConscript Modified: code/gazebo/trunk/SConstruct =================================================================== --- code/gazebo/trunk/SConstruct 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/SConstruct 2009-03-07 08:08:24 UTC (rev 7363) @@ -5,65 +5,117 @@ exec(open('build.py')) + PKG_CONFIG_VERSION = '0.22' OGRE_VERSION = '>= 1.6.0' ODE_VERSION = '>= 0.10.1' +####### +# Create a release +####### if 'release' in COMMAND_LINE_TARGETS: CreateRelease() exit() -# +####### # Setup the Options -# +####### opts = Options() opts.Add('prefix', 'The install path "prefix"', '/usr/local') opts.Add('destdir', 'The root directory to install into. Useful mainly for binary package building', '/') opts.Add('mode','Defines how Gazebo will be built, options available: optimized, profile, debug','debug') -# -# 3rd party packages -# -parseConfigs=['pkg-config --cflags --libs OGRE', - 'xml2-config --cflags --libs', - 'ode-config --cflags --libs', - 'fltk-config --cflags --libs --ldflags --use-gl --use-images', - 'pkg-config --cflags --libs xft', - 'pkg-config --cflags --libs libavformat', - 'pkg-config --cflags --libs libavcodec', - 'pkg-config --cflags --libs openal' - ] +####### +# List of all the required packages +####### +packages = { + 'OGRE' : { + 'pkgcfg': 'pkg-config --cflags --libs OGRE', + 'check' : 'OGRE ' + OGRE_VERSION, + 'msg' : 'Error: Ogre3d and development files are required, but not\ + found. ', + 'flags' : '', + 'web' : 'http://www.ogre3d.org' + }, -# -# setup the build environment -# -env = Environment ( - CC = 'g++', + 'ODE' : { + 'pkgcfg' : 'pkg-config --cflags --libs ode', + 'check' : 'ode ' + ODE_VERSION, + 'msg' : 'Error: ODE and development files are required, but not\ + found.', + 'flags' : '', + 'web' : 'http://www.ode.org' + }, - CPPPATH = [ - '#.', - '#server', - '#server/models', - '#server/gui', - '#server/gui/fltk', - '#libgazebo', - '#server/rendering', - '#server/sensors', - '#server/sensors/camera', - '#server/sensors/ray', - '#server/sensors/contact', - '#server/sensors/imu', - '#server/sensors/ir', - '#server/physics', - '#server/physics/ode', - '#server/controllers', - '#server/controllers/position2d', - '#server/controllers/position2d/pioneer2dx', - '#server/controllers/irarray', - '#server/audio_video', - ], + 'OpenAL' : { + 'pkgcfg' : 'pkg-config --cflags --libs openal', + 'check' : 'openal', + 'msg' : 'Warning: OpenAL not found. 3D audio will be disbled', + 'flags' : ['-DHAVE_OPENAL'], + 'web' : 'http://www.openal.org' + }, + 'XFT' : { + 'pkgcfg' : 'pkg-config --cflags --libs xft', + 'check' : 'xft', + 'msg' : 'Error: ', + 'flags' : '', + 'web' : 'http://www.xft.org' + }, + + 'avformat' : { + 'pkgcfg' : 'pkg-config --cflags --libs libavformat', + 'check' : 'libavformat', + 'msg' : 'Warning: FFMpeg pkg-config not found. MP3 decoding\ + is disabled', + 'flags' : '', + 'web' : 'http://ffmpeg.mplayerhq.hu/' + }, + + 'avcodec' : { + 'pkgcfg' : 'pkg-config --cflags --libs libavcodec', + 'check' : 'libavcodec', + 'msg' : 'Warning: FFMpeg pkg-config not found. MP3 decoding is\ + disabled', + 'flags' : '', + 'web' : 'http://ffmpeg.mplayerhq.hu/' + }, + + 'XML2' : { + 'pkgcfg' : 'xml2-config --cflags --libs', + 'check' : '', + 'msg' : 'Error: libxml2 and development files are required, but\ + not found.', + 'flags' : '', + 'web' : 'http://www.xmlsoft.org' + }, + + 'FLTK' : { + 'pkgcfg' : 'fltk-config --cflags --libs --ldflags --use-gl\ + --use-images', + 'check' : '', + 'msg' : 'Error: libfltk & development files are required, but not\ + found.', + 'flags' : '', + 'web' : 'http://www.fltk.org' + }, + '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' + }, +} + +####### +# setup the build environment +####### +env = Environment ( + CC = 'gcc', + CXX = 'g++', + CPPPATH = [ '#.' ], LIBPATH=Split('#libgazebo #server/audio_video'), #LIBS=Split('gazebo boost_python') @@ -73,18 +125,44 @@ TARFLAGS = '-c -z', TARSUFFIX = '.tar.gz', - options=opts + options = opts, + + # Static object compile string + CXXCOMSTR = 'Compiling $TARGET', + CCCOMSTR = 'Compiling $TARGET', + LINKCOMSTR = 'Linking $TARGET', + + # Shared object compile strings + SHCXXCOMSTR = 'Compiling $TARGET', + SHCCCOMSTR = 'Compiling $TARGET', + SHLINKCOMSTR = 'Linking $TARGET', ) +####### +# DEFAULT list of subdirectories to build +####### +subdirs = ['libgazebo','server', 'player'] + + +####### +# Generate help text +####### Help(opts.GenerateHelpText(env)) + +####### +# Setup default install destination +####### if env['destdir'] != '/': install_prefix = env['destdir'] + '/' + env['prefix'] else: install_prefix = env['prefix'] +####### +# Create the custom builder +####### env['BUILDERS']['PkgConfig'] = Builder(action = createPkgConfig) pkgconfig = env.PkgConfig(target='libgazeboServer.pc', source=Value(install_prefix)) env.Install(dir=install_prefix+'/lib/pkgconfig', source=pkgconfig) @@ -92,10 +170,9 @@ env['BUILDERS']['RCConfig'] = Builder(action = createGazeborc) rcconfig = env.RCConfig(target='gazeborc', source=Value(install_prefix)) -# DEFAULT list of subdirectories to build -subdirs = ['libgazebo','server', 'player'] - +####### # Set the compile mode +####### if env['mode'] == 'debug': env['CCFLAGS'] += Split('-ggdb -g2 -Wall -Wno-deprecated -fPIC') elif env['mode'] == 'profile': @@ -104,137 +181,54 @@ elif env['mode'] == 'optimized': env['CCFLAGS'] += Split('-O3 -fPIC') +optimize_for_cpu(env) -optimize_for_cpu(env); +####### +# Conifgure the system +####### +Config(env, packages) -# Setup the configuration environment. -conf = Configure(env, custom_tests = {'CheckODELib' : CheckODELib, - 'CheckPkgConfig' : CheckPkgConfig, - 'CheckPkg' : CheckPkg}) -# Check for pkg-config -if not conf.CheckPkgConfig(PKG_CONFIG_VERSION): - print ' Error: pkg-config version >= ' + PKG_CONFIG_VERSION + ' not found.' - Exit(1) - -# Check for the correct OGRE version -if not conf.CheckPkg('OGRE ' + OGRE_VERSION): - print ' Error: OGRE version = ' + OGRE_VERSION + ' not found.' - Exit(1) - -# Check for the correct ODE version -if not conf.CheckPkg('ode ' + ODE_VERSION): - print ' Error: ODE version = ' + ODE_VERSION + ' not found.' - Exit(1) - -# -# Parse all the pacakge configurations -# -if not env.GetOption('clean'): - for cfg in parseConfigs: - print "Checking for ["+cfg+"]" - try: - env.ParseConfig(cfg) - print " Success" - except OSError,e: - print "Unable to parse config ["+cfg+"]" - if cfg.find("OGRE") >= 0: - print "Ogre3d and development files are required, but not found." - print " http://www.ogre3d.org/" - Exit(1) - elif cfg.find("ode") >= 0: - print "ODE and development files are required, but not found." - print " http://www.ode.org" - Exit(1) - elif cfg.find("xml2") >= 0: - print "libxml2 and development files are required, but not found." - print " http://www.xmlsoft.org" - Exit(1) - elif cfg.find("fltk") >= 0: - print "libfltk & development files are required, but not found." - print " http://www.fltk.org" - Exit(1) - elif cfg.find("OpenAL") >= 0: - print "OpenAL not found. 3D audio is disabled" - print "http://www.ogre3d.org/phpBB2addons/viewtopic.php?t=3293" - Exit(1) - elif cfg.find("avcodec") >= 0 or cfg.find("avformat") >= 0: - print "FFMpeg not found. MP3 decoding is disabled" - print " http://ffmpeg.mplayerhq.hu/" - Exit(1) - -# Check for trimesh support in ODE -#if not conf.CheckODELib(): -# print ' Error: ODE not compiled with trimesh support.' -# Exit(1) - -env = conf.Finish() - - - -simpleenv = Environment(CPPPATH="/usr/include/AL") -simpleconf = Configure(simpleenv) - -# Test for libtool -if not simpleconf.CheckLibWithHeader('ltdl','ltdl.h','CXX'): - print " Warning: Failed to find ltdl, no plugin support will be included" - env["HAVE_LTDL"]=False -else: - env["HAVE_LTDL"]=True - env.Append(CCFLAGS = " -DHAVE_LTDL") - env.Append(LIBS = "ltdl") - -# Test for openal -if simpleconf.CheckLibWithHeader('openal', 'al.h', 'CXX'): - env["HAVE_OPENAL"] = True -else: - env["HAVE_OPENAL"] = False - -simpleconf.Finish() - - -# # Check for boost_python -#if not conf.CheckLibWithHeader('boost_python', 'boost/python.hpp', 'C'): -# print 'Did not find libboost_python exiting' -# Exit(1) -#else: -# conf.env.Append(LIBS="boost_python") - - +####### +# Export the environment +####### #staticObjs = [] sharedObjs = [] headers = [] - -# -# Export the environment -# Export('env install_prefix version sharedObjs headers subdirs') -# -# Process subdirectories -# + +####### +# Parse subdirectories +####### for subdir in subdirs: - print subdir + print 'Parsing sub directory ' + subdir SConscript('%s/SConscript' % subdir) -# -# Create the gazebo executable -# -gazebo = env.Program('gazebo',sharedObjs) +print '' -Depends(gazebo, 'libgazebo/libgazebo.so') -# -# Create static and shared libraries for the server -# -#libgazeboServerStatic = env.StaticLibrary('gazeboServer', staticObjs) -libgazeboServerShared = env.SharedLibrary('gazeboServer', sharedObjs) -# -# Install gazebo -# -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') +####### +# Create the gazebo executable and libraries +####### +if not 'configure' in COMMAND_LINE_TARGETS: + #Progress('Evaluating $TARGET\n') + + gazebo = env.Program('gazebo',sharedObjs) + Depends(gazebo, 'libgazebo/libgazebo.so') + + #libgazeboServerStatic = env.StaticLibrary('gazeboServer', staticObjs) + libgazeboServerShared = env.SharedLibrary('gazeboServer', sharedObjs) + + # + # Install gazebo + # + 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') +else: + print 'Configure done' + Exit() Modified: code/gazebo/trunk/build.py =================================================================== --- code/gazebo/trunk/build.py 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/build.py 2009-03-07 08:08:24 UTC (rev 7363) @@ -114,3 +114,96 @@ env['CCFLAGS'] += Split (line_atr) print "detected CPU atributes: " +line_atr + +def Config(env, packages): + # Setup the configuration environment. + conf = Configure(env, custom_tests = {'CheckODELib' : CheckODELib, + 'CheckPkgConfig' : CheckPkgConfig, + 'CheckPkg' : CheckPkg}) + + print '\n=== 3rd Party Dependency Checks === ' + + # Check for pkg-config + if not conf.CheckPkgConfig(PKG_CONFIG_VERSION): + print ' Error: pkg-config version >= ' + PKG_CONFIG_VERSION + ' not found.' + Exit(1) + + # + # Parse all the pacakge configurations + # + for key in packages: + pkgcfg = packages[key]['pkgcfg'] + check = packages[key]['check'] + msg = packages[key]['msg'] + web = packages[key]['web'] + flags = packages[key]['flags'] + docfg = True + valid = True + + # Check for the package + if check and not conf.CheckPkg(check): + docfg = False + valid = False + print ' !!' + msg + key + ' not found.' + print ' See: ' + web + if msg.find('Error') > 0: + Exit(1) + + # Try parsing the pkg-config + if docfg: + try: + if not check: + print "Checking for "+key+"...", + env.ParseConfig(pkgcfg) + if not check: + print 'yes' + except OSError,e: + valid = False + if not check: + print 'no' + print "Unable to parse config ["+pkgcfg+"]" + print ' !!' + msg + key + ' not found.' + print ' See: ' + web + if msg.find('Error') > 0: + Exit(1) + + # If valid so far, apply any flags to the environment + if valid: + for flag in flags: + env.Append(CCFLAGS = " " + flag) + + # Check for trimesh support in ODE + #if not conf.CheckODELib(): + # print ' Error: ODE not compiled with trimesh support.' + # Exit(1) + + env = conf.Finish() + + #simpleenv = Environment(CPPPATH="/usr/include/AL") + #simpleconf = Configure(simpleenv) + # + ## Test for libtool + #if not simpleconf.CheckLibWithHeader('ltdl','ltdl.h','CXX'): + # print " Warning: Failed to find ltdl, no plugin support will be included" + # env["HAVE_LTDL"]=False + #else: + # env["HAVE_LTDL"]=True + # env.Append(CCFLAGS = " -DHAVE_LTDL") + # env.Append(LIBS = "ltdl") + + #if not simpleconf.CheckHeader('boost/signal.hpp',language='C++'): + # print "Error: Boost signals not found. Please install." + # Exit(0) + # + #simpleconf.Finish() + + # # Check for boost_python + #if not conf.CheckLibWithHeader('boost_python', 'boost/python.hpp', 'C'): + # print 'Did not find libboost_python exiting' + # Exit(1) + #else: + # conf.env.Append(LIBS="boost_python") + + print '=== Done ===\n' + + Modified: code/gazebo/trunk/libgazebo/SConscript =================================================================== --- code/gazebo/trunk/libgazebo/SConscript 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/libgazebo/SConscript 2009-03-07 08:08:24 UTC (rev 7363) @@ -1,6 +1,8 @@ #Import variable -Import('install_prefix version') +Import('*') +env.Append(CPPPATH = '#libgazebo') + # # Create the pkg-config file # @@ -23,6 +25,16 @@ env = Environment ( CC = 'g++', CCFLAGS = Split ('-pthread -pipe -W -Wall -O2'), + + CXXCOMSTR = 'Compiling $TARGET', + CCCOMSTR = 'Compiling $TARGET', + + SHCXXCOMSTR = 'Compiling $TARGET', + SHCCCOMSTR = 'Compiling $TARGET', + + SHLINKCOMSTR = 'Linking $TARGET', + LINKCOMSTR = 'Linking $TARGET', + ) env['BUILDERS']['PkgConfig'] = Builder(action = createPkgConfig) Modified: code/gazebo/trunk/libgazebo/Server.cc =================================================================== --- code/gazebo/trunk/libgazebo/Server.cc 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/libgazebo/Server.cc 2009-03-07 08:08:24 UTC (rev 7363) @@ -106,7 +106,9 @@ if(fp) { int pid; - fscanf(fp, "%d", &pid); + if (fscanf(fp, "%d", &pid) != 0) + std::cerr << "Error\n"; + fclose(fp); std::cout << "found a pid file: pid=" << pid << "\n"; @@ -188,7 +190,8 @@ << "] : [" << strerror(errno) << "]\n"; snprintf(cmd, sizeof(cmd), "rm -rf %s", this->filename.c_str()); - system(cmd); + if (system(cmd)) + std::cerr << "System cmd invalid\n"; } // Finalize semaphores Modified: code/gazebo/trunk/player/SConscript =================================================================== --- code/gazebo/trunk/player/SConscript 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/player/SConscript 2009-03-07 08:08:24 UTC (rev 7363) @@ -1,8 +1,6 @@ import os -Import('env install_prefix sharedObjs subdirs') +Import('*') -parseConfigs = ['pkg-config --cflags --libs playerc++'] - sources = ['GazeboDriver.cc', 'GazeboClient.cc', 'GazeboInterface.cc', @@ -23,34 +21,19 @@ # Position3dInterface.cc PowerInterface.cc SonarInterface.ccGpsInterface.cc ') -playerEnv = env.Clone() - -for cfg in parseConfigs: - print "Checking for ["+cfg+"]" - try: - playerEnv.ParseConfig(cfg) - print " Success" - except OSError,e: - print "Unable to parse config ["+cfg+"]" - if cfg.find("player") >=0: - print "\n================================================================" - print "Player not found, bindings will not be built." - print " To install player visit(http://playerstage.sourceforge.net)" - print "================================================================" - subdirs.remove('player') - Return - #time.sleep(3) - -if ('player' in subdirs): +try: + env["CCFLAGS"].index('-DHAVE_PLAYER') -#staticPlayerObjs = playerEnv.StaticObject(sources) - sharedPlayerObjs = playerEnv.SharedObject(sources) - -#staticPlayerObjs.append(staticObjs); + #staticPlayerObjs = env.StaticObject(sources) + sharedPlayerObjs = env.SharedObject(sources) + + #staticPlayerObjs.append(staticObjs); sharedPlayerObjs.append(sharedObjs); - - sharedLib = playerEnv.SharedLibrary('gazeboplugin', sharedPlayerObjs) -# staticLib = playerEnv.StaticLibrary('gazeboplugin', staticPlayerObjs) - + + sharedLib = env.SharedLibrary('gazeboplugin', sharedPlayerObjs) + # staticLib = env.StaticLibrary('gazeboplugin', staticPlayerObjs) + env.Install(install_prefix+'/lib', sharedLib) -#env.Install(install_prefix+'/lib', staticLib) + #env.Install(install_prefix+'/lib', staticLib) +except ValueError,e: + garbage = 1 Modified: code/gazebo/trunk/server/Entity.cc =================================================================== --- code/gazebo/trunk/server/Entity.cc 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/server/Entity.cc 2009-03-07 08:08:24 UTC (rev 7363) @@ -168,6 +168,8 @@ if (body) body->SetEnabled(!s); } + + return true; } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/SConscript =================================================================== --- code/gazebo/trunk/server/SConscript 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/server/SConscript 2009-03-07 08:08:24 UTC (rev 7363) @@ -1,12 +1,18 @@ #Import variables -Import('env install_prefix sharedObjs headers') +Import('*') +env.Append(CPPPATH = '#server') + dirs = Split('physics rendering sensors controllers gui')# bindings') -if env["HAVE_OPENAL"] == True: +try: + env["CCFLAGS"].index('-DHAVE_OPENAL') dirs.append("audio_video") +except ValueError,e: + garbage = 1 for subdir in dirs: + print 'Parsing sub directory ' + subdir SConscript('%s/SConscript' % subdir) sources = ['main.cc', Modified: code/gazebo/trunk/server/World.cc =================================================================== --- code/gazebo/trunk/server/World.cc 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/server/World.cc 2009-03-07 08:08:24 UTC (rev 7363) @@ -38,7 +38,11 @@ #include "Simulator.hh" #include "gazebo.h" #include "World.hh" + +#ifdef HAVE_OPENAL #include "OpenAL.hh" +#endif + #include "Geom.hh" using namespace gazebo; @@ -124,9 +128,11 @@ gzthrow(err); } +#ifdef HAVE_OPENAL // Load OpenAL audio if (rootNode->GetChild("openal","audio")) OpenAL::Instance()->Load( rootNode->GetChild("openal", "audio") ); +#endif this->physicsEngine = new ODEPhysics(); //TODO: use exceptions here @@ -176,7 +182,9 @@ this->physicsEngine->Init(); // Initialize openal +#ifdef HAVE_OPENAL OpenAL::Instance()->Init(); +#endif this->toAddModels.clear(); this->toDeleteModels.clear(); @@ -262,7 +270,9 @@ } // Close the openal server +#ifdef HAVE_OPENAL OpenAL::Instance()->Fini(); +#endif return 0; } Modified: code/gazebo/trunk/server/audio_video/SConscript =================================================================== --- code/gazebo/trunk/server/audio_video/SConscript 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/server/audio_video/SConscript 2009-03-07 08:08:24 UTC (rev 7363) @@ -1,6 +1,8 @@ #Import variable -Import('env sharedObjs headers install_prefix') +Import('*') +env.Append(CPPPATH = '#server/audio_video') + sources = ['AudioDecoder.cc', 'OpenAL.cc' ] Modified: code/gazebo/trunk/server/controllers/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/SConscript 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/server/controllers/SConscript 2009-03-07 08:08:24 UTC (rev 7363) @@ -1,10 +1,15 @@ #Import variable -Import('env sharedObjs headers') +Import('*') +env.Append(CPPPATH = '#server/controllers') + dirs = Split('position2d laser camera factory gripper actarray ptz opaque bumper imu irarray') -if env["HAVE_OPENAL"] == True: +try: + env["CCFLAGS"].index('-DHAVE_OPENAL') dirs.append("audio") +except ValueError,e: + garbage = 1 for subdir in dirs : SConscript('%s/SConscript' % subdir) Modified: code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc =================================================================== --- code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc 2009-03-07 08:08:24 UTC (rev 7363) @@ -69,7 +69,6 @@ // Load the controller void Pioneer2_Gripper::LoadChild(XMLConfigNode *node) { - int i; XMLConfigNode *jNode; this->myIface = dynamic_cast<GripperIface*>(this->ifaces[0]); Modified: code/gazebo/trunk/server/gui/SConscript =================================================================== --- code/gazebo/trunk/server/gui/SConscript 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/server/gui/SConscript 2009-03-07 08:08:24 UTC (rev 7363) @@ -1,6 +1,8 @@ #Import variable -Import('env sharedObjs headers') +Import('*') +env.Append(CPPPATH = '#server/gui') + sources = ['Gui.cc', 'GLWindow.cc', 'MainMenu.cc', Modified: code/gazebo/trunk/server/physics/HeightmapGeom.cc =================================================================== --- code/gazebo/trunk/server/physics/HeightmapGeom.cc 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/server/physics/HeightmapGeom.cc 2009-03-07 08:08:24 UTC (rev 7363) @@ -229,7 +229,7 @@ // Create a data stream for loading the terrain into Ogre char *mstr = new char[1024];//stream.str().size()]; bzero (mstr, 1024); - sprintf(mstr, stream.str().c_str()); + sprintf(mstr, (char*)(stream.str().c_str())); Ogre::DataStreamPtr dataStream( new Ogre::MemoryDataStream(mstr,strlen(mstr)) ); Modified: code/gazebo/trunk/server/physics/SConscript =================================================================== --- code/gazebo/trunk/server/physics/SConscript 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/server/physics/SConscript 2009-03-07 08:08:24 UTC (rev 7363) @@ -1,6 +1,8 @@ #Import variable -Import('env sharedObjs headers') +Import('*') +env.Append(CPPPATH = '#server/physics') + dirs = Split('ode') for subdir in dirs : Modified: code/gazebo/trunk/server/physics/ode/SConscript =================================================================== --- code/gazebo/trunk/server/physics/ode/SConscript 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/server/physics/ode/SConscript 2009-03-07 08:08:24 UTC (rev 7363) @@ -1,7 +1,9 @@ #Import variable -Import('env sharedObjs') +Import('*') -sources = Split('ODEPhysics.cc') +env.Append(CPPPATH = '#server/physics/ode') +sources = ['ODEPhysics.cc'] + #staticObjs.append(env.StaticObject('ODEPhysics.cc')) -sharedObjs.append(env.SharedObject('ODEPhysics.cc')) +sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/rendering/SConscript =================================================================== --- code/gazebo/trunk/server/rendering/SConscript 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/server/rendering/SConscript 2009-03-07 08:08:24 UTC (rev 7363) @@ -1,6 +1,8 @@ #Import variable -Import('env sharedObjs headers') +Import('*') +env.Append(CPPPATH = '#server/rendering') + sources = [ 'MovableText.cc', 'OgreCreator.cc', Modified: code/gazebo/trunk/server/sensors/SConscript =================================================================== --- code/gazebo/trunk/server/sensors/SConscript 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/server/sensors/SConscript 2009-03-07 08:08:24 UTC (rev 7363) @@ -1,6 +1,8 @@ #Import variable -Import('env sharedObjs headers') +Import('*') +env.Append(CPPPATH = '#server/sensors') + dirs = Split('camera ray contact imu ir') for subdir in dirs : Modified: code/gazebo/trunk/server/sensors/Sensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/Sensor.cc 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/server/sensors/Sensor.cc 2009-03-07 08:08:24 UTC (rev 7363) @@ -190,7 +190,7 @@ //////////////////////////////////////////////////////////////////////////////// /// Get the current pose -const Pose3d &Sensor::GetPose() const +const Pose3d Sensor::GetPose() const { return this->body->GetPose(); } Modified: code/gazebo/trunk/server/sensors/Sensor.hh =================================================================== --- code/gazebo/trunk/server/sensors/Sensor.hh 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/server/sensors/Sensor.hh 2009-03-07 08:08:24 UTC (rev 7363) @@ -70,7 +70,7 @@ public: void Fini(); /// \brief Get the current pose - public: const Pose3d &GetPose() const; + public: const Pose3d GetPose() const; /// \brief Set whether the sensor is active or not public: void SetActive(bool value); Modified: code/gazebo/trunk/server/sensors/camera/SConscript =================================================================== --- code/gazebo/trunk/server/sensors/camera/SConscript 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/server/sensors/camera/SConscript 2009-03-07 08:08:24 UTC (rev 7363) @@ -1,6 +1,8 @@ #Import variable -Import('env sharedObjs') +Import('*') +env.Append(CPPPATH = '#server/sensors/camera') + sources = [ 'MonoCameraSensor.cc', 'StereoCameraSensor.cc' Modified: code/gazebo/trunk/server/sensors/contact/SConscript =================================================================== --- code/gazebo/trunk/server/sensors/contact/SConscript 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/server/sensors/contact/SConscript 2009-03-07 08:08:24 UTC (rev 7363) @@ -1,6 +1,8 @@ #Import variable -Import('env sharedObjs') +Import('*') +env.Append(CPPPATH = '#server/sensors/contact') + sources = Split('ContactSensor.cc') sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/sensors/imu/SConscript =================================================================== --- code/gazebo/trunk/server/sensors/imu/SConscript 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/server/sensors/imu/SConscript 2009-03-07 08:08:24 UTC (rev 7363) @@ -1,6 +1,8 @@ #Import variable Import('env sharedObjs') +env.Append(CPPPATH = '#server/sensors/imu') + sources = Split('ImuSensor.cc') sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/sensors/ir/SConscript =================================================================== --- code/gazebo/trunk/server/sensors/ir/SConscript 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/server/sensors/ir/SConscript 2009-03-07 08:08:24 UTC (rev 7363) @@ -1,6 +1,8 @@ #Import variable -Import('env sharedObjs') +Import('*') +env.Append(CPPPATH = '#server/sensors/ir') + sources = Split('IRSensor.cc') #staticObjs.append(env.StaticObject(sources)) Modified: code/gazebo/trunk/server/sensors/ray/SConscript =================================================================== --- code/gazebo/trunk/server/sensors/ray/SConscript 2009-03-04 22:19:10 UTC (rev 7362) +++ code/gazebo/trunk/server/sensors/ray/SConscript 2009-03-07 08:08:24 UTC (rev 7363) @@ -1,6 +1,7 @@ -#Import variable -Import('env sharedObjs') +Import('*') +env.Append(CPPPATH = '#server/sensors/ray') + sources = Split('RaySensor.cc') #staticObjs.append(env.StaticObject(sources)) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-03-07 23:37:29
|
Revision: 7365 http://playerstage.svn.sourceforge.net/playerstage/?rev=7365&view=rev Author: natepak Date: 2009-03-07 23:36:57 +0000 (Sat, 07 Mar 2009) Log Message: ----------- Set state of model through sim iface Modified Paths: -------------- code/gazebo/trunk/examples/libgazebo/simiface/SConstruct code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/server/Model.cc code/gazebo/trunk/server/Model.hh code/gazebo/trunk/server/World.cc code/gazebo/trunk/server/sensors/ray/RaySensor.cc Modified: code/gazebo/trunk/examples/libgazebo/simiface/SConstruct =================================================================== --- code/gazebo/trunk/examples/libgazebo/simiface/SConstruct 2009-03-07 08:19:41 UTC (rev 7364) +++ code/gazebo/trunk/examples/libgazebo/simiface/SConstruct 2009-03-07 23:36:57 UTC (rev 7365) @@ -1,7 +1,8 @@ # 3rd party packages parseConfigs=['pkg-config --cflags --libs libgazebo', - 'pkg-config --cflags --libs libgazeboServer'] + 'pkg-config --cflags --libs libgazeboServer', + 'pkg-config --cflags --libs playerc++'] env = Environment ( Modified: code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc 2009-03-07 08:19:41 UTC (rev 7364) +++ code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc 2009-03-07 23:36:57 UTC (rev 7365) @@ -41,11 +41,30 @@ gazebo::SimulationRequestData *request = &(simIface->data->requests[simIface->data->requestCount++]); - request->type = gazebo::SimulationRequestData::SET_POSE2D; + 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(); Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2009-03-07 08:19:41 UTC (rev 7364) +++ code/gazebo/trunk/libgazebo/gazebo.h 2009-03-07 23:36:57 UTC (rev 7365) @@ -375,12 +375,17 @@ GET_POSE2D, SET_POSE3D, SET_POSE2D, + SET_STATE }; public: Type type; public: char modelName[512]; public: Pose modelPose; + public: Vec3 modelLinearVel; + public: Vec3 modelAngularVel; + public: Vec3 modelLinearAccel; + public: Vec3 modelAngularAccel; }; /// \brief Simulation interface data Modified: code/gazebo/trunk/server/Model.cc =================================================================== --- code/gazebo/trunk/server/Model.cc 2009-03-07 08:19:41 UTC (rev 7364) +++ code/gazebo/trunk/server/Model.cc 2009-03-07 23:36:57 UTC (rev 7365) @@ -490,6 +490,36 @@ } //////////////////////////////////////////////////////////////////////////////// +/// Set the linear velocity of the model +void Model::SetLinearVel( const Vector3 &vel ) +{ + Body *body; + std::map<std::string, Body* >::iterator iter; + + for (iter=this->bodies.begin(); iter!=this->bodies.end(); iter++) + { + body = iter->second; + + body->SetLinearVel( vel ); + } +} + +//////////////////////////////////////////////////////////////////////////////// +/// Set the angular velocity of the model +void Model::SetAngularVel( const Vector3 &vel ) +{ + Body *body; + std::map<std::string, Body* >::iterator iter; + + for (iter=this->bodies.begin(); iter!=this->bodies.end(); iter++) + { + body = iter->second; + + body->SetAngularVel( vel ); + } +} + +//////////////////////////////////////////////////////////////////////////////// // Get the current pose const Pose3d &Model::GetPose() const { Modified: code/gazebo/trunk/server/Model.hh =================================================================== --- code/gazebo/trunk/server/Model.hh 2009-03-07 08:19:41 UTC (rev 7364) +++ code/gazebo/trunk/server/Model.hh 2009-03-07 23:36:57 UTC (rev 7365) @@ -107,6 +107,12 @@ /// \brief Set the rotation of the model public: void SetRotation( const Quatern &rot ); + + /// \brief Set the linear velocity of the model + public: void SetLinearVel( const Vector3 &vel ); + + /// \brief Set the angular velocity of the model + public: void SetAngularVel( const Vector3 &vel ); /// \brief Get the current pose public: const Pose3d &GetPose() const; Modified: code/gazebo/trunk/server/World.cc =================================================================== --- code/gazebo/trunk/server/World.cc 2009-03-07 08:19:41 UTC (rev 7364) +++ code/gazebo/trunk/server/World.cc 2009-03-07 23:36:57 UTC (rev 7365) @@ -584,6 +584,40 @@ Simulator::Instance()->Save(); break; + case SimulationRequestData::SET_STATE: + { + Model *model = this->GetModelByName((char*)req->modelName); + + if (model) + { + Pose3d pose; + Vector3 linearVel( req->modelLinearVel.x, + req->modelLinearVel.y, + req->modelLinearVel.z); + Vector3 angularVel( req->modelAngularVel.x, + req->modelAngularVel.y, + req->modelAngularVel.z); + + pose.pos.x = req->modelPose.pos.x; + pose.pos.y = req->modelPose.pos.y; + pose.pos.z = req->modelPose.pos.z; + + pose.rot.SetFromEuler( + Vector3(req->modelPose.roll, + req->modelPose.pitch, + req->modelPose.yaw)); + model->SetPose(pose); + + model->SetLinearVel(linearVel); + model->SetAngularVel(angularVel); + } + else + { + gzerr(0) << "Invalid model name[" << req->modelName + << "] in simulation interface Set State Request.\n"; + } + break; + } case SimulationRequestData::SET_POSE3D: { Pose3d pose; Modified: code/gazebo/trunk/server/sensors/ray/RaySensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/ray/RaySensor.cc 2009-03-07 08:19:41 UTC (rev 7364) +++ code/gazebo/trunk/server/sensors/ray/RaySensor.cc 2009-03-07 23:36:57 UTC (rev 7365) @@ -306,6 +306,7 @@ dSpaceCollide2( ( dGeomID ) ( this->superSpaceId ), ( dGeomID ) ( ode->GetSpaceId() ), this, &UpdateCallback ); + } } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-03-08 01:44:41
|
Revision: 7374 http://playerstage.svn.sourceforge.net/playerstage/?rev=7374&view=rev Author: natepak Date: 2009-03-08 01:42:39 +0000 (Sun, 08 Mar 2009) Log Message: ----------- Fixed a few bugs with setting a model's state Modified Paths: -------------- code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc code/gazebo/trunk/server/World.cc Modified: code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc 2009-03-08 01:32:18 UTC (rev 7373) +++ code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc 2009-03-08 01:42:39 UTC (rev 7374) @@ -40,9 +40,9 @@ { gazebo::Pose pose; - gazebo::Vec3 linearVel(0, 0, 0); + gazebo::Vec3 linearVel(0.1, 0, 0); gazebo::Vec3 angularVel(0, 0, 0); - gazebo::Vec3 linearAccel(50, 0, 0); + gazebo::Vec3 linearAccel(0, 0, 0); gazebo::Vec3 angularAccel(0, 0, 0); simIface->SetState(name, pose, linearVel, angularVel, Modified: code/gazebo/trunk/server/World.cc =================================================================== --- code/gazebo/trunk/server/World.cc 2009-03-08 01:32:18 UTC (rev 7373) +++ code/gazebo/trunk/server/World.cc 2009-03-08 01:42:39 UTC (rev 7374) @@ -614,15 +614,15 @@ Vector3(req->modelPose.roll, req->modelPose.pitch, req->modelPose.yaw)); - //model->SetPose(pose); + model->SetPose(pose); // Set the model's linear and angular velocity - //model->SetLinearVel(linearVel); - //model->SetAngularVel(angularVel); + model->SetLinearVel(linearVel); + model->SetAngularVel(angularVel); // Set the model's linear and angular acceleration model->SetLinearAccel(linearAccel); - //model->SetAngularAccel(angularAccel); + model->SetAngularAccel(angularAccel); } else { 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:22:25
|
Revision: 7373 http://playerstage.svn.sourceforge.net/playerstage/?rev=7373&view=rev Author: natepak Date: 2009-03-08 01:32:18 +0000 (Sun, 08 Mar 2009) Log Message: ----------- Added convenience functions to libgazebo Modified Paths: -------------- code/gazebo/trunk/SConstruct code/gazebo/trunk/examples/libgazebo/simiface/SConstruct code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc code/gazebo/trunk/libgazebo/SConscript code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/server/Model.cc code/gazebo/trunk/server/Model.hh code/gazebo/trunk/server/World.cc code/gazebo/trunk/server/physics/Body.cc code/gazebo/trunk/server/physics/Body.hh Modified: code/gazebo/trunk/SConstruct =================================================================== --- code/gazebo/trunk/SConstruct 2009-03-08 00:07:03 UTC (rev 7372) +++ code/gazebo/trunk/SConstruct 2009-03-08 01:32:18 UTC (rev 7373) @@ -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/gazebo/trunk/examples/libgazebo/simiface/SConstruct =================================================================== --- code/gazebo/trunk/examples/libgazebo/simiface/SConstruct 2009-03-08 00:07:03 UTC (rev 7372) +++ code/gazebo/trunk/examples/libgazebo/simiface/SConstruct 2009-03-08 01:32:18 UTC (rev 7373) @@ -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/gazebo/trunk/examples/libgazebo/simiface/simiface.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc 2009-03-08 00:07:03 UTC (rev 7372) +++ code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc 2009-03-08 01:32:18 UTC (rev 7373) @@ -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, 0, 0); + gazebo::Vec3 angularVel(0, 0, 0); + gazebo::Vec3 linearAccel(50, 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/gazebo/trunk/libgazebo/SConscript =================================================================== --- code/gazebo/trunk/libgazebo/SConscript 2009-03-08 00:07:03 UTC (rev 7372) +++ code/gazebo/trunk/libgazebo/SConscript 2009-03-08 01:32:18 UTC (rev 7373) @@ -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) Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2009-03-08 00:07:03 UTC (rev 7372) +++ code/gazebo/trunk/libgazebo/gazebo.h 2009-03-08 01:32:18 UTC (rev 7373) @@ -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; }; Modified: code/gazebo/trunk/server/Model.cc =================================================================== --- code/gazebo/trunk/server/Model.cc 2009-03-08 00:07:03 UTC (rev 7372) +++ code/gazebo/trunk/server/Model.cc 2009-03-08 01:32:18 UTC (rev 7373) @@ -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/gazebo/trunk/server/Model.hh =================================================================== --- code/gazebo/trunk/server/Model.hh 2009-03-08 00:07:03 UTC (rev 7372) +++ code/gazebo/trunk/server/Model.hh 2009-03-08 01:32:18 UTC (rev 7373) @@ -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/gazebo/trunk/server/World.cc =================================================================== --- code/gazebo/trunk/server/World.cc 2009-03-08 00:07:03 UTC (rev 7372) +++ code/gazebo/trunk/server/World.cc 2009-03-08 01:32:18 UTC (rev 7373) @@ -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); + //model->SetPose(pose); - model->SetLinearVel(linearVel); - model->SetAngularVel(angularVel); + // 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/gazebo/trunk/server/physics/Body.cc =================================================================== --- code/gazebo/trunk/server/physics/Body.cc 2009-03-08 00:07:03 UTC (rev 7372) +++ code/gazebo/trunk/server/physics/Body.cc 2009-03-08 01:32:18 UTC (rev 7373) @@ -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/gazebo/trunk/server/physics/Body.hh =================================================================== --- code/gazebo/trunk/server/physics/Body.hh 2009-03-08 00:07:03 UTC (rev 7372) +++ code/gazebo/trunk/server/physics/Body.hh 2009-03-08 01:32:18 UTC (rev 7373) @@ -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 This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ro...@us...> - 2009-03-09 00:06:57
|
Revision: 7392 http://playerstage.svn.sourceforge.net/playerstage/?rev=7392&view=rev Author: robotos Date: 2009-03-09 00:06:43 +0000 (Mon, 09 Mar 2009) Log Message: ----------- Save (filename), load(filename), close and exit implemented for the Simulation interface This should allow for batch mode processing of different simulations on the same gazebo server Modified Paths: -------------- code/gazebo/trunk/libgazebo/SimIface.cc code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/server/Simulator.cc code/gazebo/trunk/server/World.cc Modified: code/gazebo/trunk/libgazebo/SimIface.cc =================================================================== --- code/gazebo/trunk/libgazebo/SimIface.cc 2009-03-08 08:46:29 UTC (rev 7391) +++ code/gazebo/trunk/libgazebo/SimIface.cc 2009-03-09 00:06:43 UTC (rev 7392) @@ -34,6 +34,48 @@ } //////////////////////////////////////////////////////////////////////////////// +/// Save the simulation +void SimulationIface::Save(const char* fileName) +{ + this->Lock(1); + SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); + request->type = SimulationRequestData::SAVEFILENAME; + memcpy(request->fileName, fileName, 512); + this->Unlock(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Load the simulation +void SimulationIface::Load(const char* fileName) +{ + this->Lock(1); + SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); + request->type = SimulationRequestData::LOAD; + memcpy(request->fileName, fileName, 512); + this->Unlock(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Save the simulation +void SimulationIface::Close() +{ + this->Lock(1); + SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); + request->type = SimulationRequestData::CLOSE; + this->Unlock(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Save the simulation +void SimulationIface::Exit() +{ + this->Lock(1); + SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); + request->type = SimulationRequestData::EXIT; + this->Unlock(); +} + +//////////////////////////////////////////////////////////////////////////////// /// Get the 3d pose of a model void SimulationIface::GetPose3d(const char *modelName) { Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2009-03-08 08:46:29 UTC (rev 7391) +++ code/gazebo/trunk/libgazebo/gazebo.h 2009-03-09 00:06:43 UTC (rev 7392) @@ -391,6 +391,10 @@ public: enum Type { PAUSE, RESET, SAVE, + SAVEFILENAME, + LOAD, + CLOSE, + EXIT, GET_POSE3D, GET_POSE2D, SET_POSE3D, @@ -399,6 +403,7 @@ }; public: Type type; + public: char fileName[512]; public: char modelName[512]; public: Pose modelPose; public: Vec3 modelLinearVel; @@ -467,9 +472,21 @@ /// \brief Reset the simulation public: void Reset(); - /// \brief Save the simulation + /// \brief Save the simulation (automatic file) public: void Save(); + /// \brief Save the simulation (named file) + public: void Save(const char* fileName); + + /// \brief Load the simulation + public: void Load(const char* fileName); + + /// \brief Close current simulation (not exit) + public: void Close(); + + /// \brief Exit current simulation + public: void Exit(); + /// \brief Get the 3d pose of a model public: void GetPose3d(const char *modelName); Modified: code/gazebo/trunk/server/Simulator.cc =================================================================== --- code/gazebo/trunk/server/Simulator.cc 2009-03-08 08:46:29 UTC (rev 7391) +++ code/gazebo/trunk/server/Simulator.cc 2009-03-09 00:06:43 UTC (rev 7392) @@ -158,8 +158,6 @@ // Create the GUI this->gui = new Gui(x, y, width, height, "Gazebo"); - Fl::check(); - Fl::wait(0.3); this->gui->Load(childNode); } } @@ -219,9 +217,28 @@ void Simulator::Save(const std::string& filename) { std::fstream output; + std::string real_filename; - output.open(filename.c_str(), std::ios::out); + if (filename.empty()) + { + int index = 0; + std::string name = "worldfile"; + std::string random_name = "worldfile.world"; + + while (!std::ifstream(random_name.c_str()).is_open()) + { + std::ostringstream os; + ++index; + os << index; + random_name = name + os.str() + ".world"; + } + real_filename = random_name; + } + else + real_filename = filename; + output.open(real_filename.c_str(), std::ios::out); + // Write out the xml header output << "<?xml version=\"1.0\"?>\n"; output << "<gazebo:world\n\ @@ -253,8 +270,11 @@ this->GetRenderEngine()->Save(prefix, output); output << "\n"; - this->gui->Save(prefix, output); - output << "\n"; + if (this->gui) //TODO: running with -g and saving the world deletes the gui part of the file? + { + this->gui->Save(prefix, output); + output << "\n"; + } World::Instance()->Save(prefix, output); output << "\n"; Modified: code/gazebo/trunk/server/World.cc =================================================================== --- code/gazebo/trunk/server/World.cc 2009-03-08 08:46:29 UTC (rev 7391) +++ code/gazebo/trunk/server/World.cc 2009-03-09 00:06:43 UTC (rev 7392) @@ -584,6 +584,18 @@ Simulator::Instance()->Save(); break; + case SimulationRequestData::SAVEFILENAME: + Simulator::Instance()->Save(req->fileName); + break; + + case SimulationRequestData::CLOSE: + Simulator::Instance()->Close(); + break; + + case SimulationRequestData::EXIT: + Simulator::Instance()->SetUserQuit(); + break; + case SimulationRequestData::SET_STATE: { Model *model = this->GetModelByName((char*)req->modelName); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-03-13 14:30:54
|
Revision: 7476 http://playerstage.svn.sourceforge.net/playerstage/?rev=7476&view=rev Author: natepak Date: 2009-03-13 14:30:44 +0000 (Fri, 13 Mar 2009) Log Message: ----------- Updates Modified Paths: -------------- code/gazebo/trunk/examples/libgazebo/openal/openal.cc code/gazebo/trunk/examples/player/bandit/bandit.cc code/gazebo/trunk/examples/player/laser/laser.cc code/gazebo/trunk/player/CameraInterface.cc code/gazebo/trunk/server/audio_video/AudioDecoder.hh code/gazebo/trunk/worlds/bandit.world code/gazebo/trunk/worlds/openal.world code/gazebo/trunk/worlds/terrain.world Modified: code/gazebo/trunk/examples/libgazebo/openal/openal.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/openal/openal.cc 2009-03-13 14:21:49 UTC (rev 7475) +++ code/gazebo/trunk/examples/libgazebo/openal/openal.cc 2009-03-13 14:30:44 UTC (rev 7476) @@ -58,7 +58,7 @@ printf("Pause\n"); /// Pause the sound audioIface->Lock(1); - audioIface->data->cmd_pause = 1; + audioIface->data->cmd_stop = 1; audioIface->Unlock(); usleep(1000000); @@ -66,9 +66,10 @@ printf("Continue\n"); /// Play the sound in the buffer - audioIface->Lock(1); + /*audioIface->Lock(1); audioIface->data->cmd_play = 1; audioIface->Unlock(); + */ return 0; } Modified: code/gazebo/trunk/examples/player/bandit/bandit.cc =================================================================== --- code/gazebo/trunk/examples/player/bandit/bandit.cc 2009-03-13 14:21:49 UTC (rev 7475) +++ code/gazebo/trunk/examples/player/bandit/bandit.cc 2009-03-13 14:30:44 UTC (rev 7476) @@ -24,7 +24,7 @@ // This blocks until new data comes robot.Read(); - ap.MoveTo(3, M_PI/2.0); + ap.MoveTo(10, -M_PI/2.0); // Left eyebrow ap.MoveTo(16, 0.5); @@ -38,7 +38,7 @@ // upper lip ap.MoveTo(19, 0.4); - player_actarray_actuator_t data = ap.GetActuatorData(3); + player_actarray_actuator_t data = ap.GetActuatorData(9); printf("Pos[%f] Speed[%f]\n",data.position, data.speed); } } Modified: code/gazebo/trunk/examples/player/laser/laser.cc =================================================================== --- code/gazebo/trunk/examples/player/laser/laser.cc 2009-03-13 14:21:49 UTC (rev 7475) +++ code/gazebo/trunk/examples/player/laser/laser.cc 2009-03-13 14:30:44 UTC (rev 7476) @@ -11,23 +11,27 @@ // Create a player client object, using the variables assigned by the // call to parse_args() - PlayerClient robot(PlayerCc::PLAYER_HOSTNAME, PlayerCc::PLAYER_PORTNUM); + //PlayerClient robot(PlayerCc::PLAYER_HOSTNAME, PlayerCc::PLAYER_PORTNUM); + PlayerClient robot("localhost"); // Subscribe to the laser proxy - LaserProxy lp(&robot, 0); + //LaserProxy lp(&robot, 0); + printf("Read\n"); robot.Read(); + printf("Done\n"); // Print out some stuff std::cout << robot << std::endl; - lp.RequestGeom(); +/* lp.RequestGeom(); player_pose3d_t laserPose = lp.GetPose(); // Print out laser stuff std::cout << "Laser Pose[" << laserPose.px << " " << laserPose.py << " " << laserPose.pyaw << "]\n"; + */ /*for (;;) { Modified: code/gazebo/trunk/player/CameraInterface.cc =================================================================== --- code/gazebo/trunk/player/CameraInterface.cc 2009-03-13 14:21:49 UTC (rev 7475) +++ code/gazebo/trunk/player/CameraInterface.cc 2009-03-13 14:30:44 UTC (rev 7476) @@ -118,7 +118,7 @@ this->driver->Publish(this->device_addr, PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE, - (void*)&this->data, size, &this->datatime); + &this->data);//, 0, NULL, false);//, size, &this->datatime); // Save frames if (this->save) Modified: code/gazebo/trunk/server/audio_video/AudioDecoder.hh =================================================================== --- code/gazebo/trunk/server/audio_video/AudioDecoder.hh 2009-03-13 14:21:49 UTC (rev 7475) +++ code/gazebo/trunk/server/audio_video/AudioDecoder.hh 2009-03-13 14:30:44 UTC (rev 7476) @@ -2,8 +2,8 @@ #define AUDIODECODER_HH extern "C" { -#include <avformat.h> -#include <avcodec.h> +#include <libavformat/avformat.h> +#include <libavcodec/avcodec.h> } #include <string> Modified: code/gazebo/trunk/worlds/bandit.world =================================================================== --- code/gazebo/trunk/worlds/bandit.world 2009-03-13 14:21:49 UTC (rev 7475) +++ code/gazebo/trunk/worlds/bandit.world 2009-03-13 14:30:44 UTC (rev 7476) @@ -18,7 +18,7 @@ <physics:ode> <stepTime>0.03</stepTime> <gravity>0 0 -9.8</gravity> - <cfm>10e-5</cfm> + <cfm>10e-1</cfm> <erp>0.3</erp> </physics:ode> Modified: code/gazebo/trunk/worlds/openal.world =================================================================== --- code/gazebo/trunk/worlds/openal.world 2009-03-13 14:21:49 UTC (rev 7475) +++ code/gazebo/trunk/worlds/openal.world 2009-03-13 14:30:44 UTC (rev 7476) @@ -64,7 +64,7 @@ <controller:audio name="audio_controller_1"> <pitch>1.0</pitch> <gain>1.0</gain> - <loop>false</loop> + <loop>true</loop> <mp3>test.mp3</mp3> <interface:audio name="audio_iface_1"/> </controller:audio> Modified: code/gazebo/trunk/worlds/terrain.world =================================================================== --- code/gazebo/trunk/worlds/terrain.world 2009-03-13 14:21:49 UTC (rev 7475) +++ code/gazebo/trunk/worlds/terrain.world 2009-03-13 14:30:44 UTC (rev 7476) @@ -17,8 +17,8 @@ <verbosity>5</verbosity> <physics:ode> - <stepTime>0.02</stepTime> - <gravity>0 0 -9.80665</gravity> + <stepTime>0.002</stepTime> + <gravity>0 0 -1.80665</gravity> <cfm>10e-5</cfm> <erp>0.3</erp> </physics:ode> @@ -27,6 +27,14 @@ <type>fltk</type> <size>800 600</size> <pos>0 0</pos> + <frames> + <row height="100%"> + <camera width="100%"> + <xyz>214 -74.74 1.13</xyz> + <rpy>0 3.50 -79.5</rpy> + </camera> + </row> + </frames> </rendering:gui> <rendering:ogre> @@ -38,7 +46,9 @@ </rendering:ogre> <model:physical name="sphere1_model"> - <xyz>197 130 4.0</xyz> + <!--<xyz>197 130 4.0</xyz>--> + + <xyz>214.76 -79.34 4.29</xyz> <static>false</static> <body:sphere name="sphere1_body"> <geom:sphere name="sphere1_geom"> @@ -80,8 +90,8 @@ </body:plane> </model:physical> - <model:physical name="pioneer2dx_model2"> - <xyz>214.76 -79.34 1.29</xyz> + <!--<model:physical name="pioneer2dx_model2"> + <xyz>214.76 -79.34 2.29</xyz> <rpy>0 0 20</rpy> <controller:differential_position2d name="controller1"> <leftJoint>left_wheel_hinge</leftJoint> @@ -103,5 +113,6 @@ <xi:include href="models/pioneer2dx.model" /> </include> </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-27 16:15:38
|
Revision: 7551 http://playerstage.svn.sourceforge.net/playerstage/?rev=7551&view=rev Author: natepak Date: 2009-03-27 16:15:13 +0000 (Fri, 27 Mar 2009) Log Message: ----------- Added threads and a new graphics interface Modified Paths: -------------- code/gazebo/trunk/Media/materials/scripts/Gazebo.material code/gazebo/trunk/SConstruct code/gazebo/trunk/build.py code/gazebo/trunk/examples/libgazebo/bandit/SConstruct code/gazebo/trunk/examples/libgazebo/bandit/bandit.cc code/gazebo/trunk/examples/libgazebo/factory/SConstruct code/gazebo/trunk/examples/libgazebo/openal/openal.cc code/gazebo/trunk/examples/libgazebo/position/SConstruct code/gazebo/trunk/examples/libgazebo/position/position.cc code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc code/gazebo/trunk/examples/player/bandit/bandit.cc code/gazebo/trunk/examples/player/laser/laser.cc code/gazebo/trunk/libgazebo/SConscript code/gazebo/trunk/libgazebo/Server.cc code/gazebo/trunk/libgazebo/SimIface.cc code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/player/CameraInterface.cc code/gazebo/trunk/player/GazeboTime.cc code/gazebo/trunk/player/SimulationInterface.cc code/gazebo/trunk/server/Angle.hh code/gazebo/trunk/server/Entity.cc code/gazebo/trunk/server/Entity.hh code/gazebo/trunk/server/Global.hh code/gazebo/trunk/server/Model.cc code/gazebo/trunk/server/Model.hh code/gazebo/trunk/server/Quatern.cc code/gazebo/trunk/server/Quatern.hh code/gazebo/trunk/server/SConscript code/gazebo/trunk/server/Simulator.cc code/gazebo/trunk/server/Simulator.hh code/gazebo/trunk/server/World.cc code/gazebo/trunk/server/World.hh code/gazebo/trunk/server/audio_video/AudioDecoder.hh code/gazebo/trunk/server/controllers/Controller.cc code/gazebo/trunk/server/controllers/SConscript code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc code/gazebo/trunk/server/controllers/factory/Factory.cc code/gazebo/trunk/server/controllers/factory/Factory.hh code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc code/gazebo/trunk/server/gui/GLWindow.cc code/gazebo/trunk/server/gui/Gui.cc code/gazebo/trunk/server/gui/Gui.hh code/gazebo/trunk/server/gui/StatusBar.cc code/gazebo/trunk/server/gui/StatusBar.hh code/gazebo/trunk/server/main.cc code/gazebo/trunk/server/physics/Body.cc code/gazebo/trunk/server/physics/Body.hh code/gazebo/trunk/server/physics/ContactParams.hh code/gazebo/trunk/server/physics/Geom.cc code/gazebo/trunk/server/physics/Geom.hh code/gazebo/trunk/server/physics/HeightmapGeom.cc code/gazebo/trunk/server/physics/Joint.cc code/gazebo/trunk/server/physics/RayGeom.cc code/gazebo/trunk/server/physics/RayGeom.hh code/gazebo/trunk/server/physics/ode/ODEPhysics.cc code/gazebo/trunk/server/rendering/OgreAdaptor.cc code/gazebo/trunk/server/rendering/OgreAdaptor.hh code/gazebo/trunk/server/rendering/OgreCamera.cc code/gazebo/trunk/server/rendering/OgreCamera.hh code/gazebo/trunk/server/rendering/OgreCreator.cc code/gazebo/trunk/server/rendering/OgreCreator.hh code/gazebo/trunk/server/rendering/OgreDynamicLines.cc code/gazebo/trunk/server/rendering/OgreVisual.cc code/gazebo/trunk/server/rendering/OgreVisual.hh code/gazebo/trunk/server/rendering/SConscript code/gazebo/trunk/server/rendering/UserCamera.cc code/gazebo/trunk/server/rendering/UserCamera.hh code/gazebo/trunk/server/sensors/Sensor.cc code/gazebo/trunk/server/sensors/Sensor.hh code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.hh code/gazebo/trunk/server/sensors/ir/IRSensor.cc code/gazebo/trunk/server/sensors/ray/RaySensor.cc code/gazebo/trunk/server/sensors/ray/RaySensor.hh code/gazebo/trunk/worlds/audio.world code/gazebo/trunk/worlds/bandit.world code/gazebo/trunk/worlds/bumper.world code/gazebo/trunk/worlds/factory.world code/gazebo/trunk/worlds/laser.world code/gazebo/trunk/worlds/lights.world code/gazebo/trunk/worlds/map.world code/gazebo/trunk/worlds/models/pioneer2gripper.model code/gazebo/trunk/worlds/models/sicklms200.model code/gazebo/trunk/worlds/openal.world code/gazebo/trunk/worlds/pioneer2at.world code/gazebo/trunk/worlds/pioneer2dx.world code/gazebo/trunk/worlds/pioneer2dx_camera.world code/gazebo/trunk/worlds/pioneer2dx_gripper.world code/gazebo/trunk/worlds/simplecar.world code/gazebo/trunk/worlds/simpleshapes.world code/gazebo/trunk/worlds/terrain.world code/gazebo/trunk/worlds/test.world code/gazebo/trunk/worlds/trimesh.world code/gazebo/trunk/worlds/wizbot.world Added Paths: ----------- code/gazebo/trunk/Media/materials/textures/cave.png code/gazebo/trunk/Media/materials/textures/heartpsg.png code/gazebo/trunk/examples/libgazebo/graphics3d/ code/gazebo/trunk/examples/libgazebo/graphics3d/SConstruct code/gazebo/trunk/examples/libgazebo/graphics3d/graphics3d.cc code/gazebo/trunk/libgazebo/Graphics3dIface.cc code/gazebo/trunk/server/GraphicsIfaceHandler.cc code/gazebo/trunk/server/GraphicsIfaceHandler.hh code/gazebo/trunk/server/rendering/OgreMovableText.cc code/gazebo/trunk/server/rendering/OgreMovableText.hh code/gazebo/trunk/webgazebo/ code/gazebo/trunk/webgazebo/README code/gazebo/trunk/webgazebo/SConscript code/gazebo/trunk/webgazebo/WebGazebo.cc code/gazebo/trunk/webgazebo/WebGazebo.hh code/gazebo/trunk/webgazebo/client.cc code/gazebo/trunk/webgazebo/http.cc code/gazebo/trunk/webgazebo/httpd.cc code/gazebo/trunk/webgazebo/main.cc code/gazebo/trunk/worlds/federation.world Removed Paths: ------------- code/gazebo/trunk/examples/libgazebo/graphics3d/SConstruct code/gazebo/trunk/examples/libgazebo/graphics3d/graphics3d.cc code/gazebo/trunk/server/rendering/MovableText.cc code/gazebo/trunk/server/rendering/MovableText.hh code/gazebo/trunk/webgazebo/README code/gazebo/trunk/webgazebo/SConscript code/gazebo/trunk/webgazebo/WebGazebo.cc code/gazebo/trunk/webgazebo/WebGazebo.hh code/gazebo/trunk/webgazebo/client.cc code/gazebo/trunk/webgazebo/http.cc code/gazebo/trunk/webgazebo/httpd.cc code/gazebo/trunk/webgazebo/main.cc Property Changed: ---------------- code/gazebo/trunk/ code/gazebo/trunk/player_cfgs/epuck.cfg code/gazebo/trunk/player_cfgs/epuck_single.cfg Property changes on: code/gazebo/trunk ___________________________________________________________________ Added: svn:externals + Added: svn:mergeinfo + /code/branches/federation/gazebo:7371-7550 Modified: code/gazebo/trunk/Media/materials/scripts/Gazebo.material =================================================================== --- code/gazebo/trunk/Media/materials/scripts/Gazebo.material 2009-03-27 15:31:55 UTC (rev 7550) +++ code/gazebo/trunk/Media/materials/scripts/Gazebo.material 2009-03-27 16:15:13 UTC (rev 7551) @@ -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 Copied: code/gazebo/trunk/Media/materials/textures/cave.png (from rev 7550, code/branches/federation/gazebo/Media/materials/textures/cave.png) =================================================================== (Binary files differ) Copied: code/gazebo/trunk/Media/materials/textures/heartpsg.png (from rev 7550, code/branches/federation/gazebo/Media/materials/textures/heartpsg.png) =================================================================== (Binary files differ) Modified: code/gazebo/trunk/SConstruct =================================================================== --- code/gazebo/trunk/SConstruct 2009-03-27 15:31:55 UTC (rev 7550) +++ code/gazebo/trunk/SConstruct 2009-03-27 16:15:13 UTC (rev 7551) @@ -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 ####### @@ -36,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' : { @@ -45,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' : { @@ -53,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' : { @@ -61,7 +65,8 @@ 'check' : 'xft', 'msg' : 'Error: ', 'flags' : '', - 'web' : 'http://www.xft.org' + 'web' : 'http://www.xft.org', + 'required' : True }, 'avformat' : { @@ -70,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' : { @@ -79,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' : { @@ -88,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' : { @@ -98,15 +106,49 @@ '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/', + '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', + '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', + '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', + 'required' : False + }, } ####### @@ -131,11 +173,15 @@ 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, ) @@ -143,8 +189,15 @@ # DEFAULT list of subdirectories to build ####### subdirs = ['libgazebo','server', 'player'] +# Need test for <event.h> +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 ####### @@ -188,6 +241,11 @@ ####### Config(env, packages) + +if env['HAS_ERROR'] == True: + print "Build failed due to missing packages. See above errors" + Exit() + ####### # Export the environment ####### @@ -232,3 +290,4 @@ else: print 'Configure done' Exit() + Modified: code/gazebo/trunk/build.py =================================================================== --- code/gazebo/trunk/build.py 2009-03-27 15:31:55 UTC (rev 7550) +++ code/gazebo/trunk/build.py 2009-03-27 16:15:13 UTC (rev 7551) @@ -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,32 @@ # Parse all the pacakge configurations # for key in packages: - pkgcfg = packages[key]['pkgcfg'] - check = packages[key]['check'] + pkgcfg ='' + header='' + lib='' + check='' + requred = True + + 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'] + + if packages[key].has_key('required'): + required = packages[key]['required'] + + msg = packages[key]['msg'] web = packages[key]['web'] flags = packages[key]['flags'] + docfg = True valid = True @@ -146,11 +169,11 @@ 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: + if docfg and pkgcfg: try: if not check: print "Checking for "+key+"...", @@ -164,21 +187,41 @@ 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 + print msg + else: + env.Append(LIBS = lib) + valid = True # 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(): # 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/gazebo/trunk/examples/libgazebo/bandit/SConstruct =================================================================== --- code/gazebo/trunk/examples/libgazebo/bandit/SConstruct 2009-03-27 15:31:55 UTC (rev 7550) +++ code/gazebo/trunk/examples/libgazebo/bandit/SConstruct 2009-03-27 16:15:13 UTC (rev 7551) @@ -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/gazebo/trunk/examples/libgazebo/bandit/bandit.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/bandit/bandit.cc 2009-03-27 15:31:55 UTC (rev 7550) +++ code/gazebo/trunk/examples/libgazebo/bandit/bandit.cc 2009-03-27 16:15:13 UTC (rev 7551) @@ -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/gazebo/trunk/examples/libgazebo/factory/SConstruct =================================================================== --- code/gazebo/trunk/examples/libgazebo/factory/SConstruct 2009-03-27 15:31:55 UTC (rev 7550) +++ code/gazebo/trunk/examples/libgazebo/factory/SConstruct 2009-03-27 16:15:13 UTC (rev 7551) @@ -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 ( CC = 'g++', Deleted: code/gazebo/trunk/examples/libgazebo/graphics3d/SConstruct =================================================================== --- code/branches/federation/gazebo/examples/libgazebo/graphics3d/SConstruct 2009-03-27 15:31:55 UTC (rev 7550) +++ code/gazebo/trunk/examples/libgazebo/graphics3d/SConstruct 2009-03-27 16:15:13 UTC (rev 7551) @@ -1,14 +0,0 @@ - -# 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') Copied: code/gazebo/trunk/examples/libgazebo/graphics3d/SConstruct (from rev 7550, code/branches/federation/gazebo/examples/libgazebo/graphics3d/SConstruct) =================================================================== --- code/gazebo/trunk/examples/libgazebo/graphics3d/SConstruct (rev 0) +++ code/gazebo/trunk/examples/libgazebo/graphics3d/SConstruct 2009-03-27 16:15:13 UTC (rev 7551) @@ -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') Deleted: code/gazebo/trunk/examples/libgazebo/graphics3d/graphics3d.cc =================================================================== --- code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc 2009-03-27 15:31:55 UTC (rev 7550) +++ code/gazebo/trunk/examples/libgazebo/graphics3d/graphics3d.cc 2009-03-27 16:15:13 UTC (rev 7551) @@ -1,249 +0,0 @@ -#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() -{ - client = new gazebo::Client(); - simIface = new gazebo::SimulationIface(); - posIface = new gazebo::PositionIface(); - g3dIface = new 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 - { - 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 an interface\n" - << e << "\n"; - return -1; - } - - // Set the size of the square - squareSize.x = 0.25; - squareSize.y = 0.25; - squareSize.z = 0.0; - - // Set the size of the sphere - sphereSize.x = 0.1; - sphereSize.y = 0.1; - sphereSize.z = 0.1; - - // 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) - { - UpdateSquare(); - UpdateSphere(); - UpdatePath(); - UpdateText(); - - usleep(20000); - } - - simIface->Close(); - posIface->Close(); - pioneerG3DIface->Close(); - g3dIface->Close(); - - delete simIface; - delete posIface; - delete pioneerG3DIface; - delete g3dIface; - delete client; - return 0; -} - Copied: code/gazebo/trunk/examples/libgazebo/graphics3d/graphics3d.cc (from rev 7550, code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc) =================================================================== --- code/gazebo/trunk/examples/libgazebo/graphics3d/graphics3d.cc (rev 0) +++ code/gazebo/trunk/examples/libgazebo/graphics3d/graphics3d.cc 2009-03-27 16:15:13 UTC (rev 7551) @@ -0,0 +1,249 @@ +#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() +{ + client = new gazebo::Client(); + simIface = new gazebo::SimulationIface(); + posIface = new gazebo::PositionIface(); + g3dIface = new 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 + { + 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 an interface\n" + << e << "\n"; + return -1; + } + + // Set the size of the square + squareSize.x = 0.25; + squareSize.y = 0.25; + squareSize.z = 0.0; + + // Set the size of the sphere + sphereSize.x = 0.1; + sphereSize.y = 0.1; + sphereSize.z = 0.1; + + // 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) + { + UpdateSquare(); + UpdateSphere(); + UpdatePath(); + UpdateText(); + + usleep(20000); + } + + simIface->Close(); + posIface->Close(); + pioneerG3DIface->Close(); + g3dIface->Close(); + + delete simIface; + delete posIface; + delete pioneerG3DIface; + delete g3dIface; + delete client; + return 0; +} + Modified: code/gazebo/trunk/examples/libgazebo/openal/openal.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/openal/openal.cc 2009-03-27 15:31:55 UTC (rev 7550) +++ code/gazebo/trunk/examples/libgazebo/openal/openal.cc 2009-03-27 16:15:13 UTC (rev 7551) @@ -58,7 +58,7 @@ printf("Pause\n"); /// Pause the sound audioIface->Lock(1); - audioIface->data->cmd_stop = 1; + audioIface->data->cmd_pause = 1; audioIface->Unlock(); usleep(1000000); @@ -66,10 +66,9 @@ printf("Continue\n"); /// Play the sound in the buffer - /*audioIface->Lock(1); + audioIface->Lock(1); audioIface->data->cmd_play = 1; audioIface->Unlock(); - */ return 0; } Modified: code/gazebo/trunk/examples/libgazebo/position/SConstruct =================================================================== --- code/gazebo/trunk/examples/libgazebo/position/SConstruct 2009-03-27 15:31:55 UTC (rev 7550) +++ code/gazebo/trunk/examples/libgazebo/position/SConstruct 2009-03-27 16:15:13 UTC (rev 7551) @@ -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/gazebo/trunk/examples/libgazebo/position/position.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/position/position.cc 2009-03-27 15:31:55 UTC (rev 7550) +++ code/gazebo/trunk/examples/libgazebo/position/position.cc 2009-03-27 16:15:13 UTC (rev 7551) @@ -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; @@ -48,6 +48,7 @@ posIface->data->cmdEnableMotors = 1; posIface->Unlock(); + posIface->data->cmdVelocity.yaw = -0.1; while (true) { posIface->Lock(1); Modified: code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc 2009-03-27 15:31:55 UTC (rev 7550) +++ code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc 2009-03-27 16:15:13 UTC (rev 7551) @@ -1,7 +1,18 @@ #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(); @@ -36,26 +47,68 @@ // Example of how to move a model (box1_model) char name[512] = "pioneer2dx_model1"; - for (int i=0; i< 10; i++) + gazebo::Pose pose; + bool got = false; + + for (int i=0; i< 600; i++) { - - gazebo::Pose pose; - gazebo::Vec3 linearVel(0.1, 0, 0); + printf("Iter [%d]\n",i); + + simIface->GetPose3d(name); + + got = false; + while (!got) + { + //simIface->Lock(1); + got = simIface->data->responseCount > 0; + //simIface->Unlock(); + usleep(1000); + } + + simIface->SetPose3d(name, pose); + } + + /*gazebo::Pose pose; + //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); gazebo::Vec3 angularAccel(0, 0, 0); simIface->SetState(name, pose, linearVel, angularVel, linearAccel, angularAccel ); - usleep(9000000); - } + */ + /*usleep(1000000); + printf("GO!\n"); + // First GO + simIface->Go(5*10e4, &Callback); + + // Wait until the callback is called + while (gotCallback == 0) + usleep(10000); + + gotCallback = 0; + + // 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); + // Example of resetting the simulator // simIface->Reset(); + //usleep(1000000); - usleep(1000000); - + */ simIface->Close(); delete simIface; Modified: code/gazebo/trunk/examples/player/bandit/bandit.cc =================================================================== --- code/gazebo/trunk/examples/player/bandit/bandit.cc 2009-03-27 15:31:55 UTC (rev 7550) +++ code/gazebo/trunk/examples/player/bandit/bandit.cc 2009-03-27 16:15:13 UTC (rev 7551) @@ -24,7 +24,7 @@ // This blocks until new data comes robot.Read(); - ap.MoveTo(10, -M_PI/2.0); + ap.MoveTo(3, M_PI/2.0); // Left eyebrow ap.MoveTo(16, 0.5); @@ -38,7 +38,7 @@ // upper lip ap.MoveTo(19, 0.4); - player_actarray_actuator_t data = ap.GetActuatorData(9); + player_actarray_actuator_t data = ap.GetActuatorData(3); printf("Pos[%f] Speed[%f]\n",data.position, data.speed); } } Modified: code/gazebo/trunk/examples/player/laser/laser.cc =================================================================== --- code/gazebo/trunk/examples/player/laser/laser.cc 2009-03-27 15:31:55 UTC (rev 7550) +++ code/gazebo/trunk/examples/player/laser/laser.cc 2009-03-27 16:15:13 UTC (rev 7551) @@ -11,27 +11,23 @@ // Create a player client object, using the variables assigned by the // call to parse_args() - //PlayerClient robot(PlayerCc::PLAYER_HOSTNAME, PlayerCc::PLAYER_PORTNUM); - PlayerClient robot("localhost"); + PlayerClient robot(PlayerCc::PLAYER_HOSTNAME, PlayerCc::PLAYER_PORTNUM); // Subscribe to the laser proxy - //LaserProxy lp(&robot, 0); + LaserProxy lp(&robot, 0); - printf("Read\n"); robot.Read(); - printf("Done\n"); // Print out some stuff std::cout << robot << std::endl; -/* lp.RequestGeom(); + lp.RequestGeom(); player_pose3d_t laserPose = lp.GetPose(); // Print out laser stuff std::cout << "Laser Pose[" << laserPose.px << " " << laserPose.py << " " << laserPose.pyaw << "]\n"; - */ /*for (;;) { Copied: code/gazebo/trunk/libgazebo/Graphics3dIface.cc (from rev 7550, code/branches/federation/gazebo/libgazebo/Graphics3dIface.cc) =================================================================== --- code/gazebo/trunk/libgazebo/Graphics3dIface.cc (rev 0) +++ code/gazebo/trunk/libgazebo/Graphics3dIface.cc 2009-03-27 16:15:13 UTC (rev 7551) @@ -0,0 +1,159 @@ +#include "gazebo.h" + +#include <iostream> + +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(); +} + +//////////////////////////////////////////////////////////////////////////////// +/// 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/gazebo/trunk/libgazebo/SConscript =================================================================== --- code/gazebo/trunk/libgazebo/SConscript 2009-03-27 15:31:55 UTC (rev 7550) +++ code/gazebo/trunk/libgazebo/SConscript 2009-03-27 16:15:13 UTC (rev 7551) @@ -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', @@ -41,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/gazebo/trunk/libgazebo/Server.cc =================================================================== --- code/gazebo/trunk/libgazebo/Server.cc 2009-03-27 15:31:55 UTC (rev 7550) +++ code/gazebo/trunk/libgazebo/Server.cc 2009-03-27 16:15:13 UTC (rev 7551) @@ -107,7 +107,7 @@ { int pid; if (fscanf(fp, "%d", &pid) != 0) - std::cerr << "Error\n"; + std::cerr << "Error in libgazebo Server.\n"; fclose(fp); std::cout << "found a pid file: pid=" << pid << "\n"; Modified: code/gazebo/trunk/libgazebo/SimIface.cc =================================================================== --- code/gazebo/trunk/libgazebo/SimIface.cc 2009-03-27 15:31:55 UTC (rev 7550) +++ code/gazebo/trunk/libgazebo/SimIface.cc 2009-03-27 16:15:13 UTC (rev 7551) @@ -1,77 +1,129 @@ #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; //////////////////////////////////////////////////////////////////////////////// -/// Pause the simulation -void SimulationIface::Pause() +/// Create an interface +SimulationIface::SimulationIface() + : Iface("simulation",sizeof(SimulationIface)+sizeof(SimulationData)) + { - this->Lock(1); - SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); - request->type = SimulationRequestData::PAUSE; - this->Unlock(); + this->goAckThread = NULL; } + //////////////////////////////////////////////////////////////////////////////// -/// Reset the simulation -void SimulationIface::Reset() +/// Destroy an interface +SimulationIface::~SimulationIface() { - this->Lock(1); - SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); - request->type = SimulationRequestData::RESET; - this->Unlock(); + if (this->goAckThread) + delete this->goAckThread; + this->goAckThread = NULL; + + this->data = NULL; } + //////////////////////////////////////////////////////////////////////////////// -/// Save the simulation -void SimulationIface::Save() +/// Create a simulation interface +void SimulationIface::Create(Server *server, std::string id) { - this->Lock(1); - SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); - request->type = SimulationRequestData::SAVE; - this->Unlock(); + 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)); + + // 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"); } //////////////////////////////////////////////////////////////////////////////// -/// Save the simulation -void SimulationIface::Save(const char* fileName) +/// Open a simulation interface +void SimulationIface::Open(Client *client, std::string id) { - this->Lock(1); - SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); - request->type = SimulationRequestData::SAVEFILENAME; - memcpy(request->fileName, fileName, 512); - this->Unlock(); + 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->goAckThread = new boost::thread( + boost::bind(&SimulationIface::BlockThread, this)); + usleep(100); + } } //////////////////////////////////////////////////////////////////////////////// -/// Load the simulation -void SimulationIface::Load(const char* fileName) +void SimulationIface::BlockThread() { + + while (true) + { + // Wait for Gazebo to send a Post + this->GoAckWait(); + + // Signal the callback function + this->goAckSignal(); + } + + printf("Done with the thread\n"); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Pause the simulation +void SimulationIface::Pause() +{ this->Lock(1); SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); - request->type = SimulationRequestData::LOAD; - memcpy(request->fileName, fileName, 512); + request->type = SimulationRequestData::PAUSE; this->Unlock(); } //////////////////////////////////////////////////////////////////////////////// -/// Save the simulation -void SimulationIface::Close() +/// Reset the simulation +void SimulationIface::Reset() { this->Lock(1); SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); - request->type = SimulationRequestData::CLOSE; + request->type = SimulationRequestData::RESET; this->Unlock(); } //////////////////////////////////////////////////////////////////////////////// /// Save the simulation -void SimulationIface::Exit() +void SimulationIface::Save() { this->Lock(1); SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); - request->type = SimulationRequestData::EXIT; + request->type = SimulationRequestData::SAVE; this->Unlock(); } @@ -82,7 +134,9 @@ this->Lock(1); SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); request->type = SimulationRequestData::GET_POSE3D; - memcpy(request->modelName, modelName, 512); + memset(request->modelName, 0, 512); + strncpy(request->modelName, modelName, 512); + request->modelName[511] = '\0'; this->Unlock(); } @@ -94,7 +148,9 @@ this->Lock(1); SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); request->type = SimulationRequestData::GET_POSE2D; - memcpy(request->modelName, modelName, 512); + memset(request->modelName, 0, 512); + strncpy(request->modelName, modelName, 512); + request->modelName[511] = '\0'; this->Unlock(); } @@ -109,7 +165,9 @@ request->type = SimulationRequestData::SET_POSE3D; request->modelPose = modelPose; - memcpy(request->modelName, modelName, 512); + memset(request->modelName, 0, 512); + strncpy(request->modelName, modelName, 512); + request->modelName[511] = '\0'; this->Unlock(); } @@ -123,7 +181,9 @@ request->type = gazebo::SimulationRequestData::SET_POSE2D; - memcpy(request->modelName, modelName, 512); + memset(request->modelName, 0, 512); + strncpy(request->modelName, modelName, 512); + request->modelName[511] = '\0'; request->modelPose.pos.x = x; request->modelPose.pos.y = y; @@ -142,7 +202,9 @@ SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]); request->type = gazebo::SimulationRequestData::SET_STATE; - memcpy(request->modelName, modelName, 512); + memset(request->modelName, 0, 512); + strncpy(request->modelName, modelName, 512); + request->modelName[511] = '\0'; request->modelPose = modelPose; request->modelLinearVel = linearVel; @@ -154,3 +216,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/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2009-03-27 15:31:55 UTC (rev 7550) +++ code/gazebo/trunk/libgazebo/gazebo.h 2009-03-27 16:15:13 UTC (rev 7551) @@ -32,6 +32,11 @@ #include <sys/types.h> #include <stdlib.h> #include <stdint.h> +#include <boost/signal.hpp> +#include <boost/thread/thread.hpp> +#include <boost/thread/mutex.hpp> +#include <boost/thread/condition.hpp> +#include <boost/bind.hpp> #include "IfaceFactory.hh" @@ -54,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 { @@ -352,7 +379,7 @@ /// Number of times the interface has been opened public: int openCount; - public: float time; + public: double time; public: int version; @@ -391,25 +418,22 @@ public: enum Type { PAUSE, RESET, SAVE, - SAVEFILENAME, - LOAD, - CLOSE, - EXIT, GET_POSE3D, GET_POSE2D, SET_POSE3D, SET_POSE2D, - SET_STATE + SET_STATE, + GO }; public: Type type; - public: char fileName[512]; public: char modelName[512]; public: Pose modelPose; public: Vec3 modelLinearVel; public: Vec3 modelAngularVel; public: Vec3 modelLinearAccel; public: Vec3 modelAngularAccel; + public: unsigned int runTime; }; /// \brief Simulation interface data @@ -437,33 +461,50 @@ public: SimulationRequestData responses[GAZEBO_SIMULATION_MAX_REQUESTS]; public: unsigned int responseCount; + public: int semId; + public: int semKey; + }; /// \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) + 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: template<typename T> + void Go(unsigned int us,T subscriber) { - Iface::Open(client,id); - this->data = (SimulationData*)((char*)this->mMap+sizeof(SimulationIface)); + // 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(); + + { + if (this->currentConnection.connected()) + this->currentConnection.disconnect(); + + // Connect the callback. This is signaled when the thread + // (below) finishes waiting + this->currentConnection = this->goAckSignal.connect( subscriber ); + } } /// \brief Pause the simulation @@ -472,21 +513,9 @@ /// \brief Reset the simulation public: void Reset(); - /// \brief Save the simulation (automatic file) + /// \brief Save the simulation public: void Save(); - /// \brief Save the simulation (named file) - public: void Save(const char* fileName); - - /// \brief Load the simulation - public: void Load(const char* fileName); - - /// \brief Close current simulation (not exit) - public: void Close(); - - /// \brief Exit current simulation - public: void Exit(); - /// \brief Get the 3d pose of a model public: void GetPose3d(const char *modelName); @@ -504,6 +533,14 @@ const Vec3 &linearVel, const Vec3 &angularVel, const Vec3 &linearAccel, const Vec3 &angularAccel ); + public: void GoAckWait(); + public: void GoAckPost(); + + private: void BlockThread(); + private: boost::signal<void (void)> goAckSignal; + private: boost::signals::connection currentConnection; + private: boost::thread *goAckThread; + /// Pointer to the simulation data public: SimulationData *data; }; @@ -762,55 +799,90 @@ /// 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, TEXT }; /// Drawing mode - public: DrawMode drawmode; + 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 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... [truncated message content] |
From: <na...@us...> - 2009-04-10 14:58:28
|
Revision: 7593 http://playerstage.svn.sourceforge.net/playerstage/?rev=7593&view=rev Author: natepak Date: 2009-04-10 14:58:25 +0000 (Fri, 10 Apr 2009) Log Message: ----------- Added range resolution to ray sensor Modified Paths: -------------- code/gazebo/trunk/player/LaserInterface.cc code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc code/gazebo/trunk/server/physics/ContactParams.cc code/gazebo/trunk/server/sensors/ray/RaySensor.cc code/gazebo/trunk/server/sensors/ray/RaySensor.hh code/gazebo/trunk/worlds/models/sicklms200.model Modified: code/gazebo/trunk/player/LaserInterface.cc =================================================================== --- code/gazebo/trunk/player/LaserInterface.cc 2009-04-10 14:55:29 UTC (rev 7592) +++ code/gazebo/trunk/player/LaserInterface.cc 2009-04-10 14:58:25 UTC (rev 7593) @@ -182,19 +182,9 @@ ts.tv_sec = (int) (this->iface->data->head.time); ts.tv_usec = (int) (fmod(this->iface->data->head.time, 1) * 1e6); - - // Pick the rage resolution to use (1, 10, 100) - if (this->iface->data->max_range <= 8.192) - rangeRes = 1.0; - else if (this->iface->data->max_range <= 81.92) - rangeRes = 10.0; - else - rangeRes = 100.0; - + rangeRes = this->iface->data->res_range; angleRes = this->iface->data->res_angle; - //printf("range res = %f %f\n", rangeRes, this->iface->data->max_range); - double oldCount = this->data.ranges_count; this->data.min_angle = this->iface->data->min_angle; Modified: code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc =================================================================== --- code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc 2009-04-10 14:55:29 UTC (rev 7592) +++ code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc 2009-04-10 14:58:25 UTC (rev 7593) @@ -184,7 +184,9 @@ double minRange = this->myParent->GetMinRange(); int rayCount = this->myParent->GetRayCount(); int rangeCount = this->myParent->GetRangeCount(); + float resRange = this->myParent->GetResRange(); + if (this->laserIface->Lock(1)) { // Data timestamp @@ -194,7 +196,7 @@ this->laserIface->data->min_angle = minAngle.GetAsRadian(); this->laserIface->data->max_angle = maxAngle.GetAsRadian(); this->laserIface->data->res_angle = (maxAngle.GetAsRadian() - minAngle.GetAsRadian()) / (rangeCount - 1); - this->laserIface->data->res_range = 0.1; + this->laserIface->data->res_range = resRange; this->laserIface->data->max_range = maxRange; this->laserIface->data->range_count = rangeCount; Modified: code/gazebo/trunk/server/physics/ContactParams.cc =================================================================== --- code/gazebo/trunk/server/physics/ContactParams.cc 2009-04-10 14:55:29 UTC (rev 7592) +++ code/gazebo/trunk/server/physics/ContactParams.cc 2009-04-10 14:58:25 UTC (rev 7593) @@ -43,8 +43,8 @@ this->mu1 = dInfinity; this->mu2 = dInfinity; - this->slip1 = 0.1; - this->slip2 = 0.1; + this->slip1 = 0.01; + this->slip2 = 0.01; } ////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/sensors/ray/RaySensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/ray/RaySensor.cc 2009-04-10 14:55:29 UTC (rev 7592) +++ code/gazebo/trunk/server/sensors/ray/RaySensor.cc 2009-04-10 14:58:25 UTC (rev 7593) @@ -72,6 +72,7 @@ this->maxAngleP = new ParamT<Angle>("maxAngle",DTOR(-90),1); this->minRangeP = new ParamT<double>("minRange",0,1); this->maxRangeP = new ParamT<double>("maxRange",0,1); + this->resRangeP = new ParamT<double>("resRange",0.1,1); this->originP = new ParamT<Vector3>("origin", Vector3(0,0,0), 0); this->displayRaysP = new ParamT<std::string>("displayRays", "off", 0); Param::End(); @@ -94,6 +95,7 @@ delete this->maxAngleP; delete this->minRangeP; delete this->maxRangeP; + delete this->resRangeP; delete this->originP; delete this->displayRaysP; } @@ -113,6 +115,7 @@ this->maxAngleP->Load(node); this->minRangeP->Load(node); this->maxRangeP->Load(node); + this->resRangeP->Load(node); this->originP->Load(node); this->displayRaysP->Load(node); @@ -138,6 +141,7 @@ stream << prefix << " " << *(this->maxAngleP) << "\n"; stream << prefix << " " << *(this->minRangeP) << "\n"; stream << prefix << " " << *(this->maxRangeP) << "\n"; + stream << prefix << " " << *(this->resRangeP) << "\n"; stream << prefix << " " << *(this->originP) << "\n"; stream << prefix << " " << *(this->rayCountP) << "\n"; stream << prefix << " " << *(this->rangeCountP) << "\n"; @@ -242,6 +246,13 @@ } ////////////////////////////////////////////////////////////////////////////// +/// Get the range resolution +double RaySensor::GetResRange() const +{ + return this->resRangeP->GetValue(); +} + +////////////////////////////////////////////////////////////////////////////// /// Get the ray count int RaySensor::GetRayCount() const { Modified: code/gazebo/trunk/server/sensors/ray/RaySensor.hh =================================================================== --- code/gazebo/trunk/server/sensors/ray/RaySensor.hh 2009-04-10 14:55:29 UTC (rev 7592) +++ code/gazebo/trunk/server/sensors/ray/RaySensor.hh 2009-04-10 14:58:25 UTC (rev 7593) @@ -94,6 +94,9 @@ /// \return The maximum range public: double GetMaxRange() const; + /// \brief Get the range resolution + public: double GetResRange() const; + /// \brief Get the ray count /// \return The number of rays public: int GetRayCount() const; @@ -134,7 +137,7 @@ private: std::vector<RayGeom*> rays; private: ParamT<Angle> *minAngleP, *maxAngleP; - private: ParamT<double> *minRangeP, *maxRangeP; + private: ParamT<double> *minRangeP, *maxRangeP, *resRangeP; private: ParamT<Vector3> *originP; private: Pose3d prevPose; Modified: code/gazebo/trunk/worlds/models/sicklms200.model =================================================================== --- code/gazebo/trunk/worlds/models/sicklms200.model 2009-04-10 14:55:29 UTC (rev 7592) +++ code/gazebo/trunk/worlds/models/sicklms200.model 2009-04-10 14:58:25 UTC (rev 7593) @@ -44,13 +44,14 @@ <rangeCount>361</rangeCount> <origin>0.05 0.0 0</origin> - <displayRays>false</displayRays> + <displayRays>fan</displayRays> <minAngle>-90</minAngle> <maxAngle>90</maxAngle> <minRange>0.1</minRange> <maxRange>8</maxRange> + <resRange>.1</resRange> <controller:sicklms200_laser name="laser_controller_1"> <interface:laser name="laser_iface_0"/> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-05-03 17:51:57
|
Revision: 7614 http://playerstage.svn.sourceforge.net/playerstage/?rev=7614&view=rev Author: natepak Date: 2009-05-03 17:51:49 +0000 (Sun, 03 May 2009) Log Message: ----------- Made the player drivers thread safe Modified Paths: -------------- code/gazebo/trunk/SConstruct code/gazebo/trunk/build.py code/gazebo/trunk/libgazebo/Iface.cc code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/player/ActarrayInterface.cc code/gazebo/trunk/player/ActarrayInterface.hh code/gazebo/trunk/player/BumperInterface.cc code/gazebo/trunk/player/BumperInterface.hh code/gazebo/trunk/player/CameraInterface.cc code/gazebo/trunk/player/CameraInterface.hh code/gazebo/trunk/player/FiducialInterface.cc code/gazebo/trunk/player/FiducialInterface.hh code/gazebo/trunk/player/GpsInterface.cc code/gazebo/trunk/player/GpsInterface.hh code/gazebo/trunk/player/Graphics3dInterface.cc code/gazebo/trunk/player/Graphics3dInterface.hh code/gazebo/trunk/player/GripperInterface.cc code/gazebo/trunk/player/GripperInterface.hh code/gazebo/trunk/player/IRInterface.cc code/gazebo/trunk/player/IRInterface.hh code/gazebo/trunk/player/LaserInterface.cc code/gazebo/trunk/player/LaserInterface.hh code/gazebo/trunk/player/OpaqueInterface.cc code/gazebo/trunk/player/OpaqueInterface.hh code/gazebo/trunk/player/PTZInterface.cc code/gazebo/trunk/player/PTZInterface.hh code/gazebo/trunk/player/Position2dInterface.cc code/gazebo/trunk/player/Position2dInterface.hh code/gazebo/trunk/player/Position3dInterface.cc code/gazebo/trunk/player/Position3dInterface.hh code/gazebo/trunk/player/SimulationInterface.cc code/gazebo/trunk/player/SimulationInterface.hh code/gazebo/trunk/server/Model.cc code/gazebo/trunk/server/Simulator.cc code/gazebo/trunk/server/World.cc code/gazebo/trunk/server/physics/Body.cc code/gazebo/trunk/server/physics/Geom.cc code/gazebo/trunk/server/rendering/OgreCreator.cc code/gazebo/trunk/server/sensors/Sensor.cc Modified: code/gazebo/trunk/SConstruct =================================================================== --- code/gazebo/trunk/SConstruct 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/SConstruct 2009-05-03 17:51:49 UTC (rev 7614) @@ -21,7 +21,7 @@ ####### # Setup the Options ####### -opts = Options() +opts = Variables() opts.Add('prefix', 'The install path "prefix"', '/usr/local') opts.Add('destdir', 'The root directory to install into. Useful mainly for binary package building', '/') opts.Add('mode','Defines how Gazebo will be built, options available: optimized, profile, debug','debug') @@ -55,7 +55,7 @@ 'pkgcfg' : 'pkg-config --cflags --libs openal', 'check' : 'openal', 'msg' : 'Warning: OpenAL not found. 3D audio will be disbled', - 'flags' : ['-DHAVE_OPENAL'], + 'flags' : '-DHAVE_OPENAL', 'web' : 'http://www.openal.org', 'required' : False }, @@ -113,7 +113,7 @@ 'pkgcfg' : 'pkg-config --cflags --libs playerc++', 'check' : 'playerc++', 'msg' : 'Warning: Player not found, bindings will not be built.', - 'flags' : ['-DHAVE_PLAYER'], + 'flags' : '-DHAVE_PLAYER', 'web' : 'http://playerstage.sourceforge.net', 'required' : False }, @@ -121,7 +121,7 @@ 'header' : 'event.h', 'lib' : 'event', 'msg' : 'Warning: libevent not found. Webgazebo will no be built.', - 'flags' : ['-DHAVE_EVENT'], + 'flags' : '-DHAVE_EVENT', 'web' : 'http://www.monkey.org/~provos/libevent/', 'required' : False }, @@ -129,7 +129,7 @@ 'header' : 'yaml.h', 'lib' : 'yaml', 'msg' : 'Warning: yaml not found. Webgazebo will not be built.', - 'flags' : ['-DHAVE_YAML'], + 'flags' : '-DHAVE_YAML', 'web' : 'http://www.yaml.org', 'required' : False }, @@ -137,7 +137,7 @@ 'pkgcfg' : 'pkg-config --cflags --libs websim', 'check' : 'websim', 'msg' : 'Warning: websim not found. Webgazebo will not be built.', - 'flags' : ['-DHAVE_WEBSIM'], + 'flags' : '-DHAVE_WEBSIM', 'web' : 'http://www.playerstage.sourceforge.net', 'required' : False }, Modified: code/gazebo/trunk/build.py =================================================================== --- code/gazebo/trunk/build.py 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/build.py 2009-05-03 17:51:49 UTC (rev 7614) @@ -177,6 +177,7 @@ try: if not check: print "Checking for "+key+"...", + pkgcfg = "PKG_CONFIG_PATH="+os.environ["PKG_CONFIG_PATH"]+" "+pkgcfg; env.ParseConfig(pkgcfg) if not check: print 'yes' @@ -199,8 +200,7 @@ # If valid so far, apply any flags to the environment if valid: - for flag in flags: - env.Append(CCFLAGS = flag) + env.Append(CCFLAGS = flags) # Check for trimesh support in ODE #if not conf.CheckODELib(): Modified: code/gazebo/trunk/libgazebo/Iface.cc =================================================================== --- code/gazebo/trunk/libgazebo/Iface.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/libgazebo/Iface.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -331,23 +331,28 @@ ////////////////////////////////////////////////////////////////////////////// // Lock the interface. -int Iface::Lock(int /*blocking*/) +bool Iface::Lock(int blocking) { // Some 2.4 kernels seem to screw up the lock count somehow; keep an eye out //printf(" lock %p %s\n", this, this->filename); + int op = LOCK_EX; + + if (!blocking) + op |= LOCK_NB; + // Lock the file - if (flock(this->mmapFd, LOCK_EX) != 0) + if (flock(this->mmapFd, op) != 0) { std::ostringstream stream; stream << "flock " << this->filename.c_str() << "error: " << strerror(errno); std::cout << "ERROR[" << stream.str() << "]\n"; - return 0; + return false; //throw(stream.str().c_str()); } - return 1; + return true; } @@ -400,9 +405,11 @@ if (this->mMap) { - this->Lock(1); - result = ((GazeboData*)this->mMap)->openCount; - this->Unlock(); + if (this->Lock(1)) + { + result = ((GazeboData*)this->mMap)->openCount; + this->Unlock(); + } } return result; Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/libgazebo/gazebo.h 2009-05-03 17:51:49 UTC (rev 7614) @@ -40,6 +40,7 @@ #include "IfaceFactory.hh" + namespace gazebo { @@ -328,8 +329,8 @@ /// \brief Lock the interface. /// \param blocking 1=caller should block, 0=no-block - /// \return 0 if the lock is acquired - public: int Lock(int blocking); + /// \return True if the lock is acquired + public: bool Lock(int blocking); /// \brief Unlock the interface public: int Unlock(); Modified: code/gazebo/trunk/player/ActarrayInterface.cc =================================================================== --- code/gazebo/trunk/player/ActarrayInterface.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/ActarrayInterface.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -26,6 +26,7 @@ #include <math.h> +#include <boost/thread/recursive_mutex.hpp> #include "GazeboError.hh" #include "gazebo.h" @@ -34,6 +35,8 @@ using namespace gazebo; +boost::recursive_mutex *ActarrayInterface::mutex = NULL; + // // The data for the interface (gz_actarray_data_t in gazebo.h) contains information about each joint // plus variables to store the commands. When a new command is sent, it modifies the cmd_pos, cmd_speed, ... @@ -63,6 +66,9 @@ memset(&this->actData, 0, sizeof(this->actData)); this->datatime = -1; + + if (this->mutex == NULL) + this->mutex = new boost::recursive_mutex(); } /////////////////////////////////////////////////////////////////////////////// @@ -82,6 +88,8 @@ int ActarrayInterface::ProcessMessage(QueuePointer &respQueue, player_msghdr_t *hdr, void *data) { + + boost::recursive_mutex::scoped_lock lock(*this->mutex); if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_ACTARRAY_CMD_POS, this->device_addr)) { @@ -256,6 +264,7 @@ // called from GazeboDriver::Update void ActarrayInterface::Update() { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Lock(1); // Only Update when new data is present @@ -309,6 +318,7 @@ { try { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Open(GazeboClient::client, this->gz_id); } catch (std::string e) @@ -324,5 +334,6 @@ // Close a SHM interface. This is called from GazeboDriver::Unsubscribe void ActarrayInterface::Unsubscribe() { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Close(); } Modified: code/gazebo/trunk/player/ActarrayInterface.hh =================================================================== --- code/gazebo/trunk/player/ActarrayInterface.hh 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/ActarrayInterface.hh 2009-05-03 17:51:49 UTC (rev 7614) @@ -28,6 +28,11 @@ #include "GazeboInterface.hh" +namespace boost +{ + class recursive_mutex; +} + namespace gazebo { /// \addtogroup player_iface @@ -72,6 +77,8 @@ private: double datatime; private: player_actarray_data_t actData; + + private: static boost::recursive_mutex *mutex; }; /// \} Modified: code/gazebo/trunk/player/BumperInterface.cc =================================================================== --- code/gazebo/trunk/player/BumperInterface.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/BumperInterface.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -36,12 +36,14 @@ #include <iostream> #include <math.h> +#include <boost/thread/recursive_mutex.hpp> #include "gazebo.h" #include "GazeboDriver.hh" #include "BumperInterface.hh" using namespace gazebo; +boost::recursive_mutex *BumperInterface::mutex = NULL; /////////////////////////////////////////////////////////////////////////////// // Constructor @@ -62,6 +64,9 @@ this->data.bumpers_count = 1; this->data.bumpers = new uint8_t[1]; + + if (this->mutex == NULL) + this->mutex = new boost::recursive_mutex(); } /////////////////////////////////////////////////////////////////////////////// @@ -87,6 +92,7 @@ // called from GazeboDriver::Update void BumperInterface::Update() { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Lock(1); // Only Update when new data is present @@ -123,6 +129,7 @@ { try { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Open( GazeboClient::client, this->gz_id); } catch (std::string e) @@ -137,5 +144,6 @@ // Close a SHM interface. This is called from GazeboDriver::Unsubscribe void BumperInterface::Unsubscribe() { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Close(); } Modified: code/gazebo/trunk/player/BumperInterface.hh =================================================================== --- code/gazebo/trunk/player/BumperInterface.hh 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/BumperInterface.hh 2009-05-03 17:51:49 UTC (rev 7614) @@ -29,6 +29,11 @@ #include "GazeboInterface.hh" +namespace boost +{ + class recursive_mutex; +} + namespace gazebo { /// \addtogroup player_iface @@ -73,6 +78,7 @@ private: double datatime; private: player_bumper_data_t data; + private: static boost::recursive_mutex *mutex; }; /// \} Modified: code/gazebo/trunk/player/CameraInterface.cc =================================================================== --- code/gazebo/trunk/player/CameraInterface.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/CameraInterface.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -25,6 +25,7 @@ */ #include <math.h> +#include <boost/thread/recursive_mutex.hpp> #include "gazebo.h" #include "GazeboError.hh" @@ -33,6 +34,8 @@ using namespace gazebo; +boost::recursive_mutex *CameraInterface::mutex = NULL; + /////////////////////////////////////////////////////////////////////////////// // Constructor CameraInterface::CameraInterface(player_devaddr_t addr, @@ -52,6 +55,9 @@ this->frameno = 0; this->datatime = -1; + + if (this->mutex == NULL) + this->mutex = new boost::recursive_mutex(); } /////////////////////////////////////////////////////////////////////////////// @@ -80,10 +86,12 @@ struct timeval ts; + // Thread safe locking + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Lock(1); // Only Update when new data is present - if (this->iface->data->head.time > this->datatime) + if (this->iface->data->head.time > this->datatime ) { this->datatime = this->iface->data->head.time; @@ -126,10 +134,11 @@ snprintf(filename, sizeof(filename), "click-%04d.ppm",this->frameno++); this->SaveFrame(filename); } - } + // Done with the interface this->iface->Unlock(); + } @@ -138,9 +147,11 @@ // GazeboDriver::Subscribe void CameraInterface::Subscribe() { + // Open the interface try { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Open(GazeboClient::client, this->gz_id); } catch (std::string e) @@ -151,12 +162,15 @@ //gzthrow(stream.str()); exit(0); } + } /////////////////////////////////////////////////////////////////////////////// // Close a SHM interface. This is called from GazeboDriver::Unsubscribe void CameraInterface::Unsubscribe() { + boost::recursive_mutex::scoped_lock lock(*this->mutex); + this->iface->Close(); } Modified: code/gazebo/trunk/player/CameraInterface.hh =================================================================== --- code/gazebo/trunk/player/CameraInterface.hh 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/CameraInterface.hh 2009-05-03 17:51:49 UTC (rev 7614) @@ -29,6 +29,10 @@ #include "GazeboInterface.hh" +namespace boost +{ + class recursive_mutex; +} namespace gazebo { @@ -83,6 +87,7 @@ private: int save; private: int frameno; + private: static boost::recursive_mutex *mutex; }; /// \} Modified: code/gazebo/trunk/player/FiducialInterface.cc =================================================================== --- code/gazebo/trunk/player/FiducialInterface.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/FiducialInterface.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -33,6 +33,7 @@ #include <math.h> #include <libplayerxdr/playerxdr.h> +#include <boost/thread/recursive_mutex.hpp> #include "GazeboError.hh" #include "gazebo.h" @@ -41,6 +42,8 @@ using namespace gazebo; +boost::recursive_mutex *FiducialInterface::mutex = NULL; + /////////////////////////////////////////////////////////////////////////////// // Constructor FiducialInterface::FiducialInterface(player_devaddr_t addr, @@ -60,6 +63,8 @@ memset(&this->data, 0, sizeof(this->data)); + if (this->mutex == NULL) + this->mutex = new boost::recursive_mutex(); } /////////////////////////////////////////////////////////////////////////////// @@ -77,6 +82,7 @@ int FiducialInterface::ProcessMessage(QueuePointer &respQueue, player_msghdr_t *hdr, void *data) { + boost::recursive_mutex::scoped_lock lock(*this->mutex); // Request the pose and size of the fiducial device if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_FIDUCIAL_REQ_GET_GEOM, this->device_addr)) @@ -161,6 +167,7 @@ { struct timeval ts; + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Lock(1); // Only Update when new data is present @@ -219,6 +226,7 @@ // Open the interface try { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Open(GazeboClient::client, this->gz_id); } catch (std::string e) @@ -236,5 +244,6 @@ // Close a SHM interface. This is called from GazeboDriver::Unsubscribe void FiducialInterface::Unsubscribe() { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Close(); } Modified: code/gazebo/trunk/player/FiducialInterface.hh =================================================================== --- code/gazebo/trunk/player/FiducialInterface.hh 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/FiducialInterface.hh 2009-05-03 17:51:49 UTC (rev 7614) @@ -29,6 +29,11 @@ #include "GazeboInterface.hh" +namespace boost +{ + class recursive_mutex; +} + namespace gazebo { /// \addtogroup player_iface @@ -82,6 +87,7 @@ private: double datatime; private: player_fiducial_data_t data; + private: static boost::recursive_mutex *mutex; }; /// \} /// \} Modified: code/gazebo/trunk/player/GpsInterface.cc =================================================================== --- code/gazebo/trunk/player/GpsInterface.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/GpsInterface.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -30,13 +30,15 @@ - PLAYER_GPS_DATA_STATE */ - #include <math.h> +#include <boost/thread/recursive_mutex.hpp> #include "gazebo.h" #include "GazeboDriver.hh" #include "GpsInterface.hh" +boost::recursive_mutex *GpsInterface::mutex = NULL; + /////////////////////////////////////////////////////////////////////////////// // Constructor GpsInterface::GpsInterface(player_devaddr_t addr, GazeboDriver *driver, @@ -52,6 +54,9 @@ this->iface = gz_gps_alloc(); this->datatime = -1; + + if (this->mutex == NULL) + this->mutex = new boost::recursive_mutex(); } /////////////////////////////////////////////////////////////////////////////// @@ -79,6 +84,7 @@ double e[3]; struct timeval ts; + boost::recursive_mutex::scoped_lock lock(*this->mutex); gz_gps_lock(this->iface, 1); // Only Update when new data is present @@ -117,6 +123,7 @@ // GazeboDriver::Subscribe void GpsInterface::Subscribe() { + boost::recursive_mutex::scoped_lock lock(*this->mutex); // Open the interface if (gz_gps_open(this->iface, GazeboClient::client, this->gz_id) != 0) { @@ -128,5 +135,6 @@ // Close a GPS interface. This is called from GazeboDriver::Unsubscribe void GpsInterface::Unsubscribe() { + boost::recursive_mutex::scoped_lock lock(*this->mutex); gz_gps_close(this->iface); } Modified: code/gazebo/trunk/player/GpsInterface.hh =================================================================== --- code/gazebo/trunk/player/GpsInterface.hh 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/GpsInterface.hh 2009-05-03 17:51:49 UTC (rev 7614) @@ -29,6 +29,12 @@ #include "GazeboInterface.hh" +namespace boost +{ + class recursive_mutex; +} + + // Forward declarations typedef struct gz_gps gz_gps_t; @@ -65,6 +71,7 @@ /// @brief Timestamp on last data update private: double datatime; + private: static boost::recursive_mutex *mutex; }; #endif Modified: code/gazebo/trunk/player/Graphics3dInterface.cc =================================================================== --- code/gazebo/trunk/player/Graphics3dInterface.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/Graphics3dInterface.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -31,6 +31,8 @@ - PLAYER_GRAPHICS3D_CMD_DRAW */ +#include <boost/thread/recursive_mutex.hpp> + #include "gazebo.h" #include "GazeboError.hh" #include "GazeboDriver.hh" @@ -38,6 +40,8 @@ using namespace gazebo; +boost::recursive_mutex *Graphics3dInterface::mutex = NULL; + /////////////////////////////////////////////////////////////////////////////// // Constructor Graphics3dInterface::Graphics3dInterface(player_devaddr_t addr, @@ -51,6 +55,10 @@ // Allocate a Graphics3d Interface this->iface = new Graphics3dIface(); + + if (this->mutex == NULL) + this->mutex = new boost::recursive_mutex(); + } /////////////////////////////////////////////////////////////////////////////// @@ -68,6 +76,7 @@ { int result = -1; + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Lock(1); // COMMAND CLEAR: @@ -129,6 +138,7 @@ // Open the interface try { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Open(GazeboClient::client, this->gz_id); } catch (std::string e) @@ -145,5 +155,6 @@ // Close a SHM interface. This is called from GazeboDriver::Unsubscribe void Graphics3dInterface::Unsubscribe() { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Close(); } Modified: code/gazebo/trunk/player/Graphics3dInterface.hh =================================================================== --- code/gazebo/trunk/player/Graphics3dInterface.hh 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/Graphics3dInterface.hh 2009-05-03 17:51:49 UTC (rev 7614) @@ -29,6 +29,11 @@ #include "GazeboInterface.hh" +namespace boost +{ + class recursive_mutex; +} + namespace gazebo { @@ -65,6 +70,7 @@ /// \brief Gazebo id. This needs to match and ID in a Gazebo WorldFile private: char *gz_id; + private: static boost::recursive_mutex *mutex; }; } Modified: code/gazebo/trunk/player/GripperInterface.cc =================================================================== --- code/gazebo/trunk/player/GripperInterface.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/GripperInterface.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -36,6 +36,7 @@ #include <iostream> #include <math.h> +#include <boost/thread/recursive_mutex.hpp> #include "gazebo.h" #include "GazeboDriver.hh" @@ -43,6 +44,8 @@ using namespace gazebo; +boost::recursive_mutex *GripperInterface::mutex = NULL; + /////////////////////////////////////////////////////////////////////////////// // Constructor GripperInterface::GripperInterface(player_devaddr_t addr, @@ -59,6 +62,9 @@ this->iface = new GripperIface(); this->datatime = -1; + + if (this->mutex == NULL) + this->mutex = new boost::recursive_mutex(); } /////////////////////////////////////////////////////////////////////////////// @@ -75,6 +81,7 @@ player_msghdr_t *hdr, void *data) { + boost::recursive_mutex::scoped_lock lock(*this->mutex); if (this->iface->Lock(1)) { if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, @@ -157,6 +164,7 @@ player_gripper_data_t data; struct timeval ts; + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Lock(1); // Only Update when new data is present @@ -209,6 +217,7 @@ { try { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Open( GazeboClient::client, this->gz_id); } catch (std::string e) @@ -223,5 +232,6 @@ // Close a SHM interface. This is called from GazeboDriver::Unsubscribe void GripperInterface::Unsubscribe() { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Close(); } Modified: code/gazebo/trunk/player/GripperInterface.hh =================================================================== --- code/gazebo/trunk/player/GripperInterface.hh 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/GripperInterface.hh 2009-05-03 17:51:49 UTC (rev 7614) @@ -29,6 +29,11 @@ #include "GazeboInterface.hh" +namespace boost +{ + class recursive_mutex; +} + namespace gazebo { /// \addtogroup player_iface @@ -72,6 +77,7 @@ /// \brief Timestamp on last data update private: double datatime; + private: static boost::recursive_mutex *mutex; }; /// \} Modified: code/gazebo/trunk/player/IRInterface.cc =================================================================== --- code/gazebo/trunk/player/IRInterface.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/IRInterface.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -30,6 +30,7 @@ #include <math.h> #include <libplayerxdr/playerxdr.h> +#include <boost/thread/recursive_mutex.hpp> #include "GazeboError.hh" #include "gazebo.h" @@ -38,6 +39,8 @@ using namespace gazebo; +boost::recursive_mutex *IRInterface::mutex = NULL; + /////////////////////////////////////////////////////////////////////////////// // Constructor IRInterface::IRInterface(player_devaddr_t addr, @@ -54,7 +57,8 @@ this->datatime = -1; - + if (this->mutex == NULL) + this->mutex = new boost::recursive_mutex(); } /////////////////////////////////////////////////////////////////////////////// @@ -73,6 +77,7 @@ player_msghdr_t *hdr, void *data) { + boost::recursive_mutex::scoped_lock lock(*this->mutex); if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_IR_REQ_POSE, this->device_addr)) { @@ -110,6 +115,7 @@ { struct timeval ts; + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Lock(1); // Only Update when new data is present @@ -160,6 +166,7 @@ // Open the interface try { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Open(GazeboClient::client, this->gz_id); } catch (std::string e) @@ -177,5 +184,6 @@ // Close a SHM interface. This is called from GazeboDriver::Unsubscribe void IRInterface::Unsubscribe() { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Close(); } Modified: code/gazebo/trunk/player/IRInterface.hh =================================================================== --- code/gazebo/trunk/player/IRInterface.hh 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/IRInterface.hh 2009-05-03 17:51:49 UTC (rev 7614) @@ -29,6 +29,11 @@ #include "GazeboInterface.hh" +namespace boost +{ + class recursive_mutex; +} + namespace gazebo { @@ -84,6 +89,7 @@ private: player_ir_data_t data; + private: static boost::recursive_mutex *mutex; }; /// \} Modified: code/gazebo/trunk/player/LaserInterface.cc =================================================================== --- code/gazebo/trunk/player/LaserInterface.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/LaserInterface.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -29,6 +29,7 @@ #include <math.h> #include <libplayerxdr/playerxdr.h> +#include <boost/thread/recursive_mutex.hpp> #include "GazeboError.hh" #include "gazebo.h" @@ -36,6 +37,7 @@ #include "LaserInterface.hh" using namespace gazebo; +boost::recursive_mutex *LaserInterface::mutex = NULL; /////////////////////////////////////////////////////////////////////////////// // Constructor @@ -56,6 +58,10 @@ this->datatime = -1; memset(&this->data, 0, sizeof(this->data)); + + if (this->mutex == NULL) + this->mutex = new boost::recursive_mutex(); + } /////////////////////////////////////////////////////////////////////////////// @@ -73,6 +79,7 @@ int LaserInterface::ProcessMessage(QueuePointer &respQueue, player_msghdr_t *hdr, void *data) { + boost::recursive_mutex::scoped_lock lock(*this->mutex); // Is it a request to set the laser's config? if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_LASER_REQ_SET_CONFIG, @@ -167,6 +174,7 @@ { struct timeval ts; + boost::recursive_mutex::scoped_lock lock(*this->mutex); if (this->iface->Lock(1)) { @@ -233,6 +241,7 @@ // Open the interface try { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Open(GazeboClient::client, this->gz_id); } catch (std::string e) @@ -249,5 +258,6 @@ // Close a SHM interface. This is called from GazeboDriver::Unsubscribe void LaserInterface::Unsubscribe() { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Close(); } Modified: code/gazebo/trunk/player/LaserInterface.hh =================================================================== --- code/gazebo/trunk/player/LaserInterface.hh 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/LaserInterface.hh 2009-05-03 17:51:49 UTC (rev 7614) @@ -29,6 +29,11 @@ #include "GazeboInterface.hh" +namespace boost +{ + class recursive_mutex; +} + namespace gazebo { @@ -85,6 +90,8 @@ private: int scanId; private: player_laser_data_t data; + + private: static boost::recursive_mutex *mutex; }; /// \} Modified: code/gazebo/trunk/player/OpaqueInterface.cc =================================================================== --- code/gazebo/trunk/player/OpaqueInterface.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/OpaqueInterface.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -34,6 +34,7 @@ - PLAYER_OPAQUE_REQ_DATA */ #include <math.h> +#include <boost/thread/recursive_mutex.hpp> #include "GazeboError.hh" #include "gazebo.h" @@ -42,6 +43,8 @@ using namespace gazebo; +boost::recursive_mutex *OpaqueInterface::mutex = NULL; + /////////////////////////////////////////////////////////////////////////////// // Constructor OpaqueInterface::OpaqueInterface(player_devaddr_t addr, @@ -57,6 +60,9 @@ this->iface = new OpaqueIface(); this->datatime = -1; + + if (this->mutex == NULL) + this->mutex = new boost::recursive_mutex(); } /////////////////////////////////////////////////////////////////////////////// @@ -72,6 +78,8 @@ int OpaqueInterface::ProcessMessage(QueuePointer &respQueue, player_msghdr_t *hdr, void *data) { + + boost::recursive_mutex::scoped_lock lock(*this->mutex); if (this->iface->Lock(1)) { // nothing yet @@ -88,6 +96,7 @@ // called from GazeboDriver::Update void OpaqueInterface::Update() { + boost::recursive_mutex::scoped_lock lock(*this->mutex); player_opaque_data_t data; struct timeval ts; @@ -126,6 +135,7 @@ // Open the interface try { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Open(GazeboClient::client, this->gz_id); } catch (std::string e) @@ -142,5 +152,6 @@ // Close a SHM interface. This is called from GazeboDriver::Unsubscribe void OpaqueInterface::Unsubscribe() { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Close(); } Modified: code/gazebo/trunk/player/OpaqueInterface.hh =================================================================== --- code/gazebo/trunk/player/OpaqueInterface.hh 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/OpaqueInterface.hh 2009-05-03 17:51:49 UTC (rev 7614) @@ -28,6 +28,11 @@ #include "GazeboInterface.hh" +namespace boost +{ + class recursive_mutex; +} + namespace gazebo { @@ -72,6 +77,8 @@ /// \brief Timestamp on last data update private: double datatime; + + private: static boost::recursive_mutex *mutex; }; /// \} Modified: code/gazebo/trunk/player/PTZInterface.cc =================================================================== --- code/gazebo/trunk/player/PTZInterface.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/PTZInterface.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -35,6 +35,7 @@ */ #include <math.h> +#include <boost/thread/recursive_mutex.hpp> #include "GazeboError.hh" #include "gazebo.h" @@ -43,6 +44,8 @@ using namespace gazebo; +boost::recursive_mutex *PTZInterface::mutex = NULL; + /////////////////////////////////////////////////////////////////////////////// // Constructor PTZInterface::PTZInterface(player_devaddr_t addr, @@ -58,6 +61,9 @@ this->iface = new PTZIface(); this->datatime = -1; + + if (this->mutex == NULL) + this->mutex = new boost::recursive_mutex(); } /////////////////////////////////////////////////////////////////////////////// @@ -73,6 +79,7 @@ int PTZInterface::ProcessMessage(QueuePointer &respQueue, player_msghdr_t *hdr, void *data) { + boost::recursive_mutex::scoped_lock lock(*this->mutex); if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_PTZ_CMD_STATE, this->device_addr)) { @@ -155,6 +162,7 @@ player_ptz_data_t data; struct timeval ts; + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Lock(1); // Only Update when new data is present @@ -188,6 +196,7 @@ { try { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Open(GazeboClient::client, this->gz_id); } catch (std::string e) @@ -203,5 +212,6 @@ // Close a SHM interface. This is called from GazeboDriver::Unsubscribe void PTZInterface::Unsubscribe() { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Close(); } Modified: code/gazebo/trunk/player/PTZInterface.hh =================================================================== --- code/gazebo/trunk/player/PTZInterface.hh 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/PTZInterface.hh 2009-05-03 17:51:49 UTC (rev 7614) @@ -29,6 +29,11 @@ #include "GazeboInterface.hh" +namespace boost +{ + class recursive_mutex; +} + namespace gazebo { @@ -72,6 +77,8 @@ /// \brief Timestamp on last data update private: double datatime; + + private: static boost::recursive_mutex *mutex; }; } Modified: code/gazebo/trunk/player/Position2dInterface.cc =================================================================== --- code/gazebo/trunk/player/Position2dInterface.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/Position2dInterface.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -37,6 +37,7 @@ - PLAYER_POSITION2D_REQ_RESET_ODOM */ #include <math.h> +#include <boost/thread/recursive_mutex.hpp> #include "GazeboError.hh" #include "gazebo.h" @@ -45,6 +46,8 @@ using namespace gazebo; +boost::recursive_mutex *Position2dInterface::mutex = NULL; + /////////////////////////////////////////////////////////////////////////////// // Constructor Position2dInterface::Position2dInterface(player_devaddr_t addr, @@ -61,6 +64,9 @@ this->datatime = -1; + if (this->mutex == NULL) + this->mutex = new boost::recursive_mutex(); + } /////////////////////////////////////////////////////////////////////////////// @@ -76,6 +82,9 @@ int Position2dInterface::ProcessMessage(QueuePointer &respQueue, player_msghdr_t *hdr, void *data) { + int result = 0; + + boost::recursive_mutex::scoped_lock lock(*this->mutex); if (this->iface->Lock(1)) { @@ -91,7 +100,7 @@ this->iface->data->cmdVelocity.pos.y = cmd->vel.py; this->iface->data->cmdVelocity.yaw = cmd->vel.pa; - return 0; + result = 0; } // REQUEST SET ODOMETRY @@ -101,19 +110,22 @@ if (hdr->size != sizeof(player_position2d_set_odom_req_t)) { PLAYER_WARN("Arg to odometry set requestes wrong size; ignoring"); - return -1; + result = -1; } + else + { + player_position2d_set_odom_req_t *odom = + (player_position2d_set_odom_req_t*)data; - player_position2d_set_odom_req_t *odom = (player_position2d_set_odom_req_t*)data; + this->iface->data->pose.pos.x = odom->pose.px; + this->iface->data->pose.pos.y = odom->pose.py; + this->iface->data->pose.yaw = odom->pose.pa; - this->iface->data->pose.pos.x = odom->pose.px; - this->iface->data->pose.pos.y = odom->pose.py; - this->iface->data->pose.yaw = odom->pose.pa; + this->driver->Publish(this->device_addr, respQueue, + PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_SET_ODOM); - this->driver->Publish(this->device_addr, respQueue, - PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_SET_ODOM); - - return 0; + result = 0; + } } // CMD Set Motor Power @@ -121,94 +133,103 @@ PLAYER_POSITION2D_REQ_MOTOR_POWER, this->device_addr)) { // TODO - return 0; + result = 0; } // REQUEST SET MOTOR POWER else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_POSITION2D_REQ_MOTOR_POWER, this->device_addr)) + PLAYER_POSITION2D_REQ_MOTOR_POWER, this->device_addr)) { if (hdr->size != sizeof(player_position2d_power_config_t)) { PLAYER_WARN("Arg to motor set requestes wrong size; ignoring"); - return -1; + result = -1; } + else + { - player_position2d_power_config_t *power; + player_position2d_power_config_t *power; - power = (player_position2d_power_config_t*) data; + power = (player_position2d_power_config_t*) data; - this->iface->data->cmdEnableMotors = power->state; + this->iface->data->cmdEnableMotors = power->state; - this->driver->Publish(this->device_addr, respQueue, - PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_MOTOR_POWER); + this->driver->Publish(this->device_addr, respQueue, + PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_MOTOR_POWER); - return 0; + result = 0; + } } // REQUEST GET GEOMETRY else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_POSITION2D_REQ_GET_GEOM, this->device_addr)) + PLAYER_POSITION2D_REQ_GET_GEOM, this->device_addr)) { if (hdr->size != 0) { PLAYER_WARN("Arg get robot geom is wrong size; ignoring"); - return -1; + result = -1; } + else + { - player_position2d_geom_t geom; + player_position2d_geom_t geom; - // TODO: get correct dimensions; there are for the P2AT + // TODO: get correct dimensions; there are for the P2AT - geom.pose.px = 0; - geom.pose.py = 0; - geom.pose.pz = 0; - geom.pose.pyaw = 0; - geom.pose.ppitch = 0; - geom.pose.proll = 0; - geom.size.sw= 0.53; - geom.size.sl = 0.38; - geom.size.sh = 0.31; + geom.pose.px = 0; + geom.pose.py = 0; + geom.pose.pz = 0; + geom.pose.pyaw = 0; + geom.pose.ppitch = 0; + geom.pose.proll = 0; + geom.size.sw= 0.53; + geom.size.sl = 0.38; + geom.size.sh = 0.31; - this->driver->Publish(this->device_addr, respQueue, - PLAYER_MSGTYPE_RESP_ACK, - PLAYER_POSITION2D_REQ_GET_GEOM, - &geom, sizeof(geom), NULL); + this->driver->Publish(this->device_addr, respQueue, + PLAYER_MSGTYPE_RESP_ACK, + PLAYER_POSITION2D_REQ_GET_GEOM, + &geom, sizeof(geom), NULL); - return 0; + result = 0; + } } // REQUEST RESET ODOMETRY else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_POSITION2D_REQ_RESET_ODOM, this->device_addr)) + PLAYER_POSITION2D_REQ_RESET_ODOM, this->device_addr)) { if (hdr->size != 0) { PLAYER_WARN("Arg reset position request is wrong size; ignoring"); - return -1; + result = -1; } + else + { + // TODO: Make this work!! + // + this->driver->Publish(this->device_addr, respQueue, + PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_RESET_ODOM); - // TODO: Make this work!! - // - this->driver->Publish(this->device_addr, respQueue, - PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_RESET_ODOM); + result = 0; + } + } - return 0; - } else { - printf("Unhandled\n"); + result = -1; } this->iface->Unlock(); } else { - printf("unable to get lock\n"); this->Unsubscribe(); + result = -1; } - return -1; + return result; } @@ -222,6 +243,7 @@ memset(&data, 0, sizeof(data)); + boost::recursive_mutex::scoped_lock lock(*this->mutex); if (this->iface->Lock(1)) { // Only Update when new data is present @@ -264,6 +286,7 @@ // Open the interface try { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Open(GazeboClient::client, this->gz_id); } catch (std::string e) @@ -280,5 +303,6 @@ // Close a SHM interface. This is called from GazeboDriver::Unsubscribe void Position2dInterface::Unsubscribe() { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Close(); } Modified: code/gazebo/trunk/player/Position2dInterface.hh =================================================================== --- code/gazebo/trunk/player/Position2dInterface.hh 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/Position2dInterface.hh 2009-05-03 17:51:49 UTC (rev 7614) @@ -29,6 +29,12 @@ #include "GazeboInterface.hh" +namespace boost +{ + class recursive_mutex; +} + + namespace gazebo { @@ -73,6 +79,8 @@ /// \brief Timestamp on last data update private: double datatime; + + private: static boost::recursive_mutex *mutex; }; /// \} Modified: code/gazebo/trunk/player/Position3dInterface.cc =================================================================== --- code/gazebo/trunk/player/Position3dInterface.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/Position3dInterface.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -30,6 +30,7 @@ */ #include <math.h> +#include <boost/thread/recursive_mutex.hpp> #include "GazeboError.hh" #include "gazebo.h" @@ -38,6 +39,8 @@ using namespace gazebo; +boost::recursive_mutex *Position3dInterface::mutex = NULL; + /////////////////////////////////////////////////////////////////////////////// // Constructor Position3dInterface::Position3dInterface(player_devaddr_t addr, @@ -53,6 +56,10 @@ this->iface = new PositionIface(); this->datatime = -1; + + if (this->mutex == NULL) + this->mutex = new boost::recursive_mutex(); + } /////////////////////////////////////////////////////////////////////////////// @@ -68,6 +75,7 @@ int Position3dInterface::ProcessMessage(QueuePointer &respQueue, player_msghdr_t *hdr, void *data) { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Lock(1); // COMMAND VELOCITY: @@ -198,6 +206,7 @@ player_position3d_data_t data; struct timeval ts; + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Lock(1); // Only Update when new data is present @@ -244,6 +253,7 @@ // Open the interface try { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Open(GazeboClient::client, this->gz_id); } catch (std::string e) @@ -260,5 +270,6 @@ // Close a SHM interface. This is called from GazeboDriver::Unsubscribe void Position3dInterface::Unsubscribe() { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Close(); } Modified: code/gazebo/trunk/player/Position3dInterface.hh =================================================================== --- code/gazebo/trunk/player/Position3dInterface.hh 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/Position3dInterface.hh 2009-05-03 17:51:49 UTC (rev 7614) @@ -29,6 +29,11 @@ #include "GazeboInterface.hh" +namespace boost +{ + class recursive_mutex; +} + // Forward declarations namespace gazebo { @@ -74,6 +79,7 @@ /// \brief Timestamp on last data update private: double datatime; + private: static boost::recursive_mutex *mutex; }; } Modified: code/gazebo/trunk/player/SimulationInterface.cc =================================================================== --- code/gazebo/trunk/player/SimulationInterface.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/SimulationInterface.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -25,6 +25,7 @@ */ #include <time.h> +#include <boost/thread/recursive_mutex.hpp> #include "gazebo.h" #include "GazeboError.hh" @@ -33,6 +34,8 @@ using namespace gazebo; +boost::recursive_mutex *SimulationInterface::mutex = NULL; + /////////////////////////////////////////////////////////////////////////////// // Constructor SimulationInterface::SimulationInterface(player_devaddr_t addr, GazeboDriver *driver, ConfigFile *cf, int section) @@ -56,6 +59,9 @@ memset( &this->pose3dReq, 0, sizeof(this->pose3dReq) ); memset( &this->pose2dReq, 0, sizeof(this->pose2dReq) ); + + if (this->mutex == NULL) + this->mutex = new boost::recursive_mutex(); } /////////////////////////////////////////////////////////////////////////////// @@ -76,6 +82,7 @@ int SimulationInterface::ProcessMessage(QueuePointer &respQueue, player_msghdr_t *hdr, void *data) { + boost::recursive_mutex::scoped_lock lock(*this->mutex); if (this->responseQueue) delete this->responseQueue; @@ -236,6 +243,7 @@ // called from GazeboDriver::Update void SimulationInterface::Update() { + boost::recursive_mutex::scoped_lock lock(*this->mutex); gazebo::SimulationRequestData *response = NULL; this->iface->Lock(1); @@ -322,6 +330,7 @@ // Open the interface try { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Open(GazeboClient::client, this->gz_id); } catch (std::string e) @@ -338,5 +347,6 @@ // Close a SHM interface. This is called from GazeboDriver::Unsubscribe void SimulationInterface::Unsubscribe() { + boost::recursive_mutex::scoped_lock lock(*this->mutex); this->iface->Close(); } Modified: code/gazebo/trunk/player/SimulationInterface.hh =================================================================== --- code/gazebo/trunk/player/SimulationInterface.hh 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/player/SimulationInterface.hh 2009-05-03 17:51:49 UTC (rev 7614) @@ -29,6 +29,11 @@ #include "GazeboInterface.hh" +namespace boost +{ + class recursive_mutex; +} + namespace gazebo { /// \addtogroup player_iface @@ -87,6 +92,7 @@ private: player_simulation_pose3d_req_t pose3dReq; private: player_simulation_pose2d_req_t pose2dReq; + private: static boost::recursive_mutex *mutex; }; /// \} Modified: code/gazebo/trunk/server/Model.cc =================================================================== --- code/gazebo/trunk/server/Model.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/server/Model.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -429,7 +429,9 @@ } if (this->graphicsHandler) + { this->graphicsHandler->Update(); + } return this->UpdateChild(); } Modified: code/gazebo/trunk/server/Simulator.cc =================================================================== --- code/gazebo/trunk/server/Simulator.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/server/Simulator.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -333,7 +333,9 @@ OgreAdaptor::Instance()->UpdateCameras(); if (this->gui) + { this->gui->Update(); + } currTime = this->GetWallTime(); Modified: code/gazebo/trunk/server/World.cc =================================================================== --- code/gazebo/trunk/server/World.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/server/World.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -786,7 +786,6 @@ for (miter=this->toDeleteModels.begin(); miter!=this->toDeleteModels.end(); miter++) { - std::cout << "Erasing a model[" << (*miter)->GetName() << "]\n"; // (*miter)->Fini(); this->models.erase( std::remove(this->models.begin(), this->models.end(), *miter) ); Modified: code/gazebo/trunk/server/physics/Body.cc =================================================================== --- code/gazebo/trunk/server/physics/Body.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/server/physics/Body.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -297,7 +297,6 @@ avel = this->GetAngularVel(); dBodyAddTorque(this->GetId(), -avel.x * force, -avel.y * force, -avel.z * force); } - } //////////////////////////////////////////////////////////////////////////////// @@ -328,7 +327,6 @@ // Set the pose of the body void Body::SetPose(const Pose3d &_pose) { - //boost::recursive_mutex::scoped_lock lock(*this->mutex); if (this->IsStatic()) { @@ -365,7 +363,6 @@ // Return the pose of the body Pose3d Body::GetPose() const { - //boost::recursive_mutex::scoped_lock lock(*this->mutex); /*if (this->IsStatic()) return this->staticPose; @@ -378,7 +375,6 @@ // Update the pose of the body void Body::UpdatePose() { - //boost::recursive_mutex::scoped_lock lock(*this->mutex); this->pose.pos = this->GetPosition(); this->pose.rot = this->GetRotation(); Modified: code/gazebo/trunk/server/physics/Geom.cc =================================================================== --- code/gazebo/trunk/server/physics/Geom.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/server/physics/Geom.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -313,7 +313,6 @@ // Set the pose relative to the body void Geom::SetPose(const Pose3d &newPose, bool updateCoM) { - //boost::recursive_mutex::scoped_lock lock(*this->mutex); if (this->placeable && this->geomId) { @@ -346,7 +345,6 @@ { Pose3d pose; - //boost::recursive_mutex::scoped_lock lock(*this->mutex); if (this->placeable && this->geomId) { @@ -380,7 +378,6 @@ void Geom::SetPosition(const Vector3 &pos) { Pose3d pose; - //boost::recursive_mutex::scoped_lock lock(*this->mutex); pose = this->GetPose(); pose.pos = pos; Modified: code/gazebo/trunk/server/rendering/OgreCreator.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreCreator.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/server/rendering/OgreCreator.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -704,7 +704,10 @@ } { + //printf("OgreCreator scoped lock\n"); boost::recursive_mutex::scoped_lock lock(*Simulator::Instance()->GetMutex()); + //printf("OgreCreator got lock\n"); + // Update the visuals for (viter = this->visuals.begin(); viter != this->visuals.end(); viter++) { Modified: code/gazebo/trunk/server/sensors/Sensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/Sensor.cc 2009-05-01 12:22:15 UTC (rev 7613) +++ code/gazebo/trunk/server/sensors/Sensor.cc 2009-05-03 17:51:49 UTC (rev 7614) @@ -114,7 +114,9 @@ } if (this->controller) + { this->controller->Update(); + } } //////////////////////////////////////////////////////////////////////////////// This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2009-05-04 18:28:03
|
Revision: 7616 http://playerstage.svn.sourceforge.net/playerstage/?rev=7616&view=rev Author: natepak Date: 2009-05-04 18:27:58 +0000 (Mon, 04 May 2009) Log Message: ----------- Added start of CMake build system Modified Paths: -------------- code/gazebo/trunk/server/audio_video/AudioDecoder.hh Added Paths: ----------- code/gazebo/trunk/CMakeLists.txt code/gazebo/trunk/cmake/ code/gazebo/trunk/cmake/GazeboUtils.cmake code/gazebo/trunk/cmake/SearchForStuff.cmake code/gazebo/trunk/cmake/libgazeboshm_pkgconfig.cmake code/gazebo/trunk/libgazebo/CMakeLists.txt code/gazebo/trunk/server/CMakeLists.txt code/gazebo/trunk/server/audio_video/CMakeLists.txt code/gazebo/trunk/server/controllers/CMakeLists.txt code/gazebo/trunk/server/controllers/actarray/CMakeLists.txt code/gazebo/trunk/server/controllers/actarray/bandit/CMakeLists.txt code/gazebo/trunk/server/controllers/actarray/generic/CMakeLists.txt code/gazebo/trunk/server/controllers/audio/CMakeLists.txt code/gazebo/trunk/server/controllers/bumper/CMakeLists.txt code/gazebo/trunk/server/controllers/bumper/generic/CMakeLists.txt code/gazebo/trunk/server/controllers/camera/CMakeLists.txt code/gazebo/trunk/server/controllers/camera/generic/CMakeLists.txt code/gazebo/trunk/server/controllers/camera/stereo/CMakeLists.txt code/gazebo/trunk/server/controllers/factory/CMakeLists.txt code/gazebo/trunk/server/controllers/gripper/CMakeLists.txt code/gazebo/trunk/server/controllers/gripper/pioneer2/CMakeLists.txt code/gazebo/trunk/server/controllers/imu/CMakeLists.txt code/gazebo/trunk/server/controllers/irarray/CMakeLists.txt code/gazebo/trunk/server/controllers/laser/CMakeLists.txt code/gazebo/trunk/server/controllers/laser/sicklms200/CMakeLists.txt code/gazebo/trunk/server/controllers/opaque/CMakeLists.txt code/gazebo/trunk/server/controllers/opaque/jointforce/CMakeLists.txt code/gazebo/trunk/server/controllers/position2d/CMakeLists.txt code/gazebo/trunk/server/controllers/position2d/differential/CMakeLists.txt code/gazebo/trunk/server/controllers/position2d/holonome3sw/CMakeLists.txt code/gazebo/trunk/server/controllers/position2d/steering/CMakeLists.txt code/gazebo/trunk/server/controllers/ptz/CMakeLists.txt code/gazebo/trunk/server/controllers/ptz/generic/CMakeLists.txt code/gazebo/trunk/server/gui/CMakeLists.txt code/gazebo/trunk/server/physics/CMakeLists.txt code/gazebo/trunk/server/physics/ode/CMakeLists.txt code/gazebo/trunk/server/rendering/CMakeLists.txt code/gazebo/trunk/server/sensors/CMakeLists.txt code/gazebo/trunk/server/sensors/camera/CMakeLists.txt code/gazebo/trunk/server/sensors/contact/CMakeLists.txt code/gazebo/trunk/server/sensors/imu/CMakeLists.txt code/gazebo/trunk/server/sensors/ir/CMakeLists.txt code/gazebo/trunk/server/sensors/ray/CMakeLists.txt Added: code/gazebo/trunk/CMakeLists.txt =================================================================== --- code/gazebo/trunk/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,31 @@ + +CMAKE_MINIMUM_REQUIRED( VERSION 2.6 FATAL_ERROR ) + +IF(COMMAND CMAKE_POLICY) + CMAKE_POLICY(SET CMP0003 NEW) + CMAKE_POLICY(SET CMP0004 NEW) +ENDIF(COMMAND CMAKE_POLICY) + +PROJECT (Gazebo) +SET (GAZEBO_VERSION 0.9) +STRING (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER) + +SET (gazeboserver_sources_desc "List of server sources" + CACHE INTERNAL "Gazebo server sources list description" FORCE) +SET (gazeboserver_include_dirs_desc "List of server include directories" + CACHE INTERNAL "Include dirs description") +SET (gazeboserver_link_dirs_desc "List of server link directories" + CACHE INTERNAL "Link dirs description" FORCE) +SET (gazeboserver_link_libs_desc "List of server link libraries" + CACHE INTERNAL "Link libs" FORCE) + +SET (OGRE_VERSION 1.6.0 CACHE INTERNAL "Ogre Version Requirement") +SET (ODE_VERSION 0.10.1 CACHE INTERNAL "ODE Version Requirement") + +SET (gazebo_cmake_dir ${PROJECT_SOURCE_DIR}/cmake CACHE PATH "Location of CMake scripts") +set (gazebo_cflags "-ggdb -g2 -Wall -fPIC" CACHE INTERNAL "Gazebo CFlags") + +ADD_SUBDIRECTORY(libgazebo) +ADD_SUBDIRECTORY(server) +#ADD_SUBDIRECTORY(player) + Added: code/gazebo/trunk/cmake/GazeboUtils.cmake =================================================================== --- code/gazebo/trunk/cmake/GazeboUtils.cmake (rev 0) +++ code/gazebo/trunk/cmake/GazeboUtils.cmake 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,34 @@ + +################################################################################ +# APPEND_TO_CACHED_LIST (_list _cacheDesc [items...] +# Appends items to a cached list. +MACRO (APPEND_TO_CACHED_LIST _list _cacheDesc) + SET (tempList ${${_list}}) + FOREACH (newItem ${ARGN}) + LIST (APPEND tempList ${newItem}) + ENDFOREACH (newItem ${newItem}) + SET (${_list} ${tempList} CACHE INTERNAL ${_cacheDesc} FORCE) +ENDMACRO(APPEND_TO_CACHED_LIST) + +############################################################################### +# Append sources to the server sources +MACRO (APPEND_TO_SERVER_SOURCES) + FOREACH (src ${ARGN}) + APPEND_TO_CACHED_LIST(gazeboserver_sources + ${gazeboserver_sources_desc} + ${CMAKE_CURRENT_SOURCE_DIR}/${src}) + ENDFOREACH (src ${ARGN}) +ENDMACRO (APPEND_TO_SERVER_SOURCES) + +############################################################################### +# Reset lists +MACRO (GAZEBOSERVER_RESET_LISTS) + SET (gazeboserver_sources "" CACHE INTERNAL + ${gazeboserver_sources_desc} FORCE) + SET (gazeboserver_include_dirs "" CACHE INTERNAL + ${gazeboserver_include_dirs_desc} FORCE) + SET (gazeboserver_link_dirs "" CACHE INTERNAL + ${gazeboserver_link_dirs_desc} FORCE) + SET (gazeboserver_link_libs "" CACHE INTERNAL + ${gazeboserver_link_libs_desc} FORCE) +ENDMACRO (GAZEBOSERVER_RESET_LISTS) Added: code/gazebo/trunk/cmake/SearchForStuff.cmake =================================================================== --- code/gazebo/trunk/cmake/SearchForStuff.cmake (rev 0) +++ code/gazebo/trunk/cmake/SearchForStuff.cmake 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,126 @@ +INCLUDE (${gazebo_cmake_dir}/GazeboUtils.cmake) + +INCLUDE (FindFLTK) +INCLUDE (FindBoost) +INCLUDE (FindPkgConfig) + +SET (INCLUDE_AV ON CACHE BOOL "Include audio/video functionality" FORCE) + +######################################## +# Find packages +IF (PKG_CONFIG_FOUND) + pkg_check_modules(OGRE OGRE>=${OGRE_VERSION}) + IF (NOT OGRE_FOUND) + MESSAGE (SEND_ERROR "\nError: Ogre3d and development files not found. See the following website: http://www.orge3d.org") + ELSE (NOT OGRE_FOUND) + APPEND_TO_CACHED_LIST(gazeboserver_include_dirs "Include dirs" + ${OGRE_INCLUDE_DIRS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_dirs "Link dirs" + ${OGRE_LIBRARY_DIRS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_libs "Link libs" ${OGRE_LINK_LIBS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_libs "Link libs" ${OGRE_LIBRARIES}) + ENDIF (NOT OGRE_FOUND) + + pkg_check_modules(ODE ode>=${ODE_VERSION}) + IF (NOT ODE_FOUND) + MESSAGE (SEND_ERROR "\nError: ODE and development files not found. See the following website: http://www.ode.org") + ELSE (NOT ODE_FOUND) + APPEND_TO_CACHED_LIST(gazeboserver_include_dirs "Include dirs" + ${ODE_INCLUDE_DIRS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_dirs "Link dirs" + ${ODE_LIBRARY_DIRS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_libs "Link libs" ${ODE_LINK_LIBS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_libs "Link libs" ${ODE_LIBRARIES}) + ENDIF (NOT ODE_FOUND) + + pkg_check_modules(XML libxml-2.0) + IF (NOT XML_FOUND) + MESSAGE (SEND_ERROR "\nError: libxml2 and development files not found. See the following website: http://www.xmlsoft.org") + ELSE (NOT XML_FOUND) + APPEND_TO_CACHED_LIST(gazeboserver_include_dirs "Include dirs" + ${XML_INCLUDE_DIRS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_dirs "Link dirs" + ${XML_LIBRARY_DIRS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_libs "Link libs" ${XML_LINK_LIBS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_libs "Link libs" ${XML_LIBRARIES}) + ENDIF (NOT XML_FOUND) + + pkg_check_modules(XFT xft) + IF (NOT XFT_FOUND) + MESSAGE (SEND_ERROR "\nError: XFT and development files not found. See the following website: http://www.fontconfig.org") + ELSE (NOT XFT_FOUND) + APPEND_TO_CACHED_LIST(gazeboserver_include_dirs "Include dirs" + ${XFT_INCLUDE_DIRS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_dirs "Link dirs" + ${XFT_LIBRARY_DIRS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_libs "Link libs" ${XFT_LINK_LIBS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_libs "Link libs" ${XFT_LIBRARIES}) + ENDIF (NOT XFT_FOUND) + + pkg_check_modules(OAL openal) + IF (NOT OAL_FOUND) + SET (INCLUDE_AV OFF CACHE BOOL "Include audio/video functionality" FORCE) + MESSAGE (STATUS "Warning: Openal and development files not found. Audio capabilities will be disabled. See the following website: http://connect.creativelabs.com/openal/default.aspx") + ELSE (NOT OAL_FOUND) + APPEND_TO_CACHED_LIST(gazeboserver_include_dirs "Include dirs" + ${OAL_INCLUDE_DIRS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_dirs "Link dirs" + ${OAL_LIBRARY_DIRS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_libs "Link libs" ${OAL_LINK_LIBS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_libs "Link libs" ${OAL_LIBRARIES}) + ENDIF (NOT OAL_FOUND) + + pkg_check_modules(AVF libavformat) + IF (NOT AVF_FOUND) + SET (INCLUDE_AV OFF CACHE BOOL "Include audio/video functionality" FORCE) + MESSAGE (STATUS "Warning: libavformat and development files not found. Audio capabilities will be disabled.") + ELSE (NOT AVF_FOUND) + APPEND_TO_CACHED_LIST(gazeboserver_include_dirs "Include dirs" + ${AVF_INCLUDE_DIRS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_dirs "Link dirs" + ${AVF_LIBRARY_DIRS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_libs "Link libs" ${AVF_LINK_LIBS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_libs "Link libs" ${AVF_LIBRARIES}) + ENDIF (NOT AVF_FOUND) + + pkg_check_modules(AVC libavcodec) + IF (NOT AVC_FOUND) + SET (INCLUDE_AV OFF CACHE BOOL "Include audio/video functionality" FORCE) + MESSAGE (STATUS "Warning: libavcodec and development files not found. Audio capabilities will be disabled.") + ELSE (NOT AVC_FOUND) + APPEND_TO_CACHED_LIST(gazeboserver_include_dirs "Include dirs" + ${AVC_INCLUDE_DIRS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_dirs "Link dirs" + ${AVC_LIBRARY_DIRS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_libs "Link libs" ${AVC_LINK_LIBS}) + APPEND_TO_CACHED_LIST(gazeboserver_link_libs "Link libs" ${AVC_LIBRARIES}) + ENDIF (NOT AVC_FOUND) + +ELSE (PKG_CONFIG_FOUND) + MESSAGE (FATAL_ERROR "\nError: pkg-config not found") + SET (BUILD_GAZEBO OFF CACHE BOOL "Build Gazebo" FORCE) +ENDIF (PKG_CONFIG_FOUND) + + +######################################## +# Find Boost +FIND_PACKAGE( Boost 1.34.1 COMPONENTS thread signals) +IF (NOT Boost_FOUND) + MESSAGE (FATAL_ERROR "Boost thread and signals not found") + SET (BUILD_GAZEBO OFF CACHE BOOL "Build Gazebo" FORCE) +ENDIF (NOT Boost_FOUND) + +######################################## +# Find avformat and avcodec +SET (libavformat_search_path + /usr/include /usr/include/libavformat /usr/local/include + /usr/local/include/libavformat +) + +SET (libavcodec_search_path + /usr/include /usr/include/libavcodec /usr/local/include + /usr/local/include/libavcodec +) + +FIND_PATH(LIBAVFORMAT_PATH avformat.h ${libavformat_search_path}) +FIND_PATH(LIBAVCODEC_PATH avcodec.h ${libavcodec_search_path}) Added: code/gazebo/trunk/cmake/libgazeboshm_pkgconfig.cmake =================================================================== --- code/gazebo/trunk/cmake/libgazeboshm_pkgconfig.cmake (rev 0) +++ code/gazebo/trunk/cmake/libgazeboshm_pkgconfig.cmake 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,10 @@ +prefix=@CMAKE_INSTALL_PREFIX@ +libdir=${prefix}/lib +includedir=${prefix}/include + +Name: libgazeboshm +Description: Shared memory interface to Gazebo +Version: @GAZEBO_VERSION@ +Requires: +Libs: -L${libdir} -lgazeboshm +CFlags: -I${includedir} Added: code/gazebo/trunk/libgazebo/CMakeLists.txt =================================================================== --- code/gazebo/trunk/libgazebo/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/libgazebo/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,17 @@ +INCLUDE (FindBoost) + +FIND_PACKAGE( Boost 1.34.1 COMPONENTS thread) + +SET (sources Server.cc + Client.cc + Iface.cc + IfaceFactory.cc + SimIface.cc + Graphics3dIface.cc +) + +ADD_LIBRARY(gazeboshm SHARED ${sources}) +INSTALL (TARGETS gazeboshm DESTINATION ${CMAKE_INSTALL_PREFIX}/lib) + +CONFIGURE_FILE(${CMAKE_SOURCE_DIR}/cmake/libgazeboshm_pkgconfig.cmake ${CMAKE_CURRENT_BINARY_DIR}/libgazeboshm.pc @ONLY) +INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/libgazeboshm.pc DESTINATION lib/pkgconfig COMPONENT pkgconfig) Added: code/gazebo/trunk/server/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,99 @@ +INCLUDE (${gazebo_cmake_dir}/GazeboUtils.cmake) +INCLUDE (${gazebo_cmake_dir}/SearchForStuff.cmake) + +######################################## +# Include all the search paths for headers +INCLUDE_DIRECTORIES( . + ${CMAKE_SOURCE_DIR}/libgazebo + audio_video + gui + rendering + physics + physics/ode + sensors + sensors/camera + sensors/contact + sensors/imu + sensors/ir + sensors/ray + controllers + ${gazeboserver_include_dirs} + ${LIBAVCODEC_PATH} + ${LIBAVFORMAT_PATH} + ${FLTK_INCLUDE_DIR} + ${Boost_INCLUDE_DIRS} +) + +######################################## +# All the library search paths +LINK_DIRECTORIES( + ${CMAKE_SOURCE_DIR}/libgazebo + ${Boost_LIBRARY_DIRS} + ${gazeboserver_link_dirs} +) + +######################################## +# Process all the subdirectories +ADD_SUBDIRECTORY(rendering) +ADD_SUBDIRECTORY(sensors) +ADD_SUBDIRECTORY(physics) +ADD_SUBDIRECTORY(controllers) +ADD_SUBDIRECTORY(gui) +IF (INCLUDE_AV) + ADD_SUBDIRECTORY(audio_video) +ENDIF (INCLUDE_AV) + +SET (sources Common.cc + Vector3.cc + Vector4.cc + Quatern.cc + Pose3d.cc + World.cc + XMLConfig.cc + GazeboConfig.cc + Time.cc + Entity.cc + GazeboError.cc + GazeboMessage.cc + Model.cc + Simulator.cc + Angle.cc + Param.cc + GraphicsIfaceHandler.cc +) + +SET (headers Entity.hh + GazeboError.hh + GazeboMessage.hh + Global.hh + Model.hh + Pose3d.hh + Quatern.hh + Simulator.hh + SingletonT.hh + StaticPluginRegister.hh + Time.hh + Vector2.hh + Vector3.hh + Vector4.hh + World.hh + XMLConfig.hh + GazeboConfig.hh + Angle.hh + Param.hh + GraphicsIfaceHandler.hh +) + +APPEND_TO_SERVER_SOURCES(${sources}) + +INCLUDE_DIRECTORIES( ${gazeboserver_include_dirs} ) + +ADD_EXECUTABLE(gazebo ${gazeboserver_sources} main.cc) + +SET_SOURCE_FILES_PROPERTIES(${gazeboserver_sources} PROPERTIES COMPILE_FLAGS ${gazebo_cflags}) +TARGET_LINK_LIBRARIES( gazebo ${gazeboserver_link_libs} ${FLTK_LIBRARIES} ${Boost_LIBRARIES} gazeboshm) + +INSTALL (TARGETS gazebo DESTINATION ${CMAKE_INSTALL_PREFIX}/bin) +INSTALL (FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/gazebo COMPONENT headers) + +GAZEBOSERVER_RESET_LISTS() Modified: code/gazebo/trunk/server/audio_video/AudioDecoder.hh =================================================================== --- code/gazebo/trunk/server/audio_video/AudioDecoder.hh 2009-05-04 13:16:36 UTC (rev 7615) +++ code/gazebo/trunk/server/audio_video/AudioDecoder.hh 2009-05-04 18:27:58 UTC (rev 7616) @@ -3,17 +3,8 @@ 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> Added: code/gazebo/trunk/server/audio_video/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/audio_video/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/audio_video/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +SET (sources AudioDecoder.cc + OpenAL.cc +) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/controllers/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,23 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +ADD_SUBDIRECTORY(actarray) +ADD_SUBDIRECTORY(bumper) +ADD_SUBDIRECTORY(camera) +ADD_SUBDIRECTORY(factory) +ADD_SUBDIRECTORY(gripper) +ADD_SUBDIRECTORY(imu) +ADD_SUBDIRECTORY(irarray) +ADD_SUBDIRECTORY(laser) +ADD_SUBDIRECTORY(opaque) +ADD_SUBDIRECTORY(position2d) +ADD_SUBDIRECTORY(ptz) + +IF (INCLUDE_AV) + ADD_SUBDIRECTORY(audio) +ENDIF (INCLUDE_AV) + +SET (sources Controller.cc + ControllerFactory.cc +) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/controllers/actarray/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/actarray/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/actarray/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,2 @@ +ADD_SUBDIRECTORY(bandit) +ADD_SUbDIRECTORY(generic) Added: code/gazebo/trunk/server/controllers/actarray/bandit/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/actarray/bandit/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/actarray/bandit/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +SET (sources Bandit_Actarray.cc) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/controllers/actarray/generic/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/actarray/generic/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/actarray/generic/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +SET (sources Generic_Actarray.cc) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/controllers/audio/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/audio/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/audio/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +SET (sources Audio.cc) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/controllers/bumper/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/bumper/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/bumper/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1 @@ +ADD_SUBDIRECTORY(generic) Added: code/gazebo/trunk/server/controllers/bumper/generic/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/bumper/generic/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/bumper/generic/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +SET (sources Generic_Bumper.cc) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/controllers/camera/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/camera/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/camera/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,2 @@ +ADD_SUBDIRECTORY(generic) +ADD_SUBDIRECTORY(stereo) Added: code/gazebo/trunk/server/controllers/camera/generic/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/camera/generic/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/camera/generic/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +SET (sources Generic_Camera.cc) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/controllers/camera/stereo/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/camera/stereo/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +SET (sources Stereo_Camera.cc) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/controllers/factory/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/factory/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/factory/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +SET (sources Factory.cc) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/controllers/gripper/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/gripper/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/gripper/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1 @@ +ADD_SUBDIRECTORY(pioneer2) Added: code/gazebo/trunk/server/controllers/gripper/pioneer2/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/gripper/pioneer2/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/gripper/pioneer2/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +SET (sources Pioneer2_Gripper.cc) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/controllers/imu/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/imu/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/imu/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +SET (sources Generic_Imu.cc) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/controllers/irarray/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/irarray/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/irarray/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +SET (sources IR_Array.cc) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/controllers/laser/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/laser/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/laser/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1 @@ +ADD_SUBDIRECTORY(sicklms200) Added: code/gazebo/trunk/server/controllers/laser/sicklms200/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/laser/sicklms200/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/laser/sicklms200/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +SET (sources SickLMS200_Laser.cc) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/controllers/opaque/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/opaque/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/opaque/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1 @@ +ADD_SUBDIRECTORY(jointforce) Added: code/gazebo/trunk/server/controllers/opaque/jointforce/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/opaque/jointforce/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/opaque/jointforce/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +SET (sources JointForce.cc) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/controllers/position2d/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/position2d/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/position2d/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,3 @@ +ADD_SUBDIRECTORY(differential) +ADD_SUBDIRECTORY(holonome3sw) +ADD_SUBDIRECTORY(steering) Added: code/gazebo/trunk/server/controllers/position2d/differential/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/position2d/differential/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/position2d/differential/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +SET (sources Differential_Position2d.cc) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/controllers/position2d/holonome3sw/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/position2d/holonome3sw/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/position2d/holonome3sw/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +SET (sources Holonome3Sw_Position2d.cc) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/controllers/position2d/steering/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/position2d/steering/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/position2d/steering/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +SET (sources Steering_Position2d.cc Wheel.cc DriveWheel.cc FullWheel.cc) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/controllers/ptz/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/ptz/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/ptz/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1 @@ +ADD_SUBDIRECTORY(generic) Added: code/gazebo/trunk/server/controllers/ptz/generic/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/controllers/ptz/generic/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/controllers/ptz/generic/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +SET (sources Generic_PTZ.cc) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/gui/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/gui/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/gui/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,10 @@ +SET (sources Gui.cc + GLWindow.cc + MainMenu.cc + Toolbar.cc + StatusBar.cc + GLFrameManager.cc + GLFrame.cc +) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/physics/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/physics/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/physics/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,25 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +ADD_SUBDIRECTORY(ode) + +SET (sources BallJoint.cc + Body.cc + BoxGeom.cc + ContactParams.cc + CylinderGeom.cc + Geom.cc + Hinge2Joint.cc + HingeJoint.cc + Joint.cc + PhysicsEngine.cc + PlaneGeom.cc + SliderJoint.cc + SphereGeom.cc + UniversalJoint.cc + RayGeom.cc + TrimeshGeom.cc + HeightmapGeom.cc + MapGeom.cc +) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/physics/ode/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/physics/ode/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/physics/ode/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +SET (sources ODEPhysics.cc ) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/rendering/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/rendering/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/rendering/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,17 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +SET (sources OgreMovableText.cc + OgreCreator.cc + OgreAdaptor.cc + OgreFrameListener.cc + OgreDynamicRenderable.cc + OgreDynamicLines.cc + OgreSimpleShape.cc + OgreHUD.cc + OgreVisual.cc + OgreCamera.cc + CameraManager.cc + UserCamera.cc +) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/sensors/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/sensors/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/sensors/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,13 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +ADD_SUBDIRECTORY(camera) +ADD_SUBDIRECTORY(contact) +ADD_SUBDIRECTORY(imu) +ADD_SUBDIRECTORY(ir) +ADD_SUBDIRECTORY(ray) + +SET (sources Sensor.cc + SensorFactory.cc +) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/sensors/camera/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/sensors/camera/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/sensors/camera/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,7 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +set (sources MonoCameraSensor.cc + StereoCameraSensor.cc +) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/sensors/contact/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/sensors/contact/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/sensors/contact/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +set (sources ContactSensor.cc) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/sensors/imu/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/sensors/imu/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/sensors/imu/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +set (sources ImuSensor.cc) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/sensors/ir/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/sensors/ir/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/sensors/ir/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +set (sources IRSensor.cc) + +APPEND_TO_SERVER_SOURCES(${sources}) Added: code/gazebo/trunk/server/sensors/ray/CMakeLists.txt =================================================================== --- code/gazebo/trunk/server/sensors/ray/CMakeLists.txt (rev 0) +++ code/gazebo/trunk/server/sensors/ray/CMakeLists.txt 2009-05-04 18:27:58 UTC (rev 7616) @@ -0,0 +1,5 @@ +include (${gazebo_cmake_dir}/GazeboUtils.cmake) + +set (sources RaySensor.cc) + +APPEND_TO_SERVER_SOURCES(${sources}) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |