Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [1034] pkg/trunk
|
From: <hsu...@us...> - 2008-06-26 22:18:39
|
Revision: 1034
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1034&view=rev
Author: hsujohnhsu
Date: 2008-06-26 15:18:48 -0700 (Thu, 26 Jun 2008)
Log Message:
-----------
switching to new gazebo repository from SF.
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/world/Media/materials/scripts/Gazebo.material
pkg/trunk/demos/2dnav-gazebo/world/pr2.model
pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml
pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
pkg/trunk/simulators/gazebo_plugin/Makefile
pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/P3D.hh
pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh
pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Gripper.hh
pkg/trunk/simulators/gazebo_plugin/manifest.xml
pkg/trunk/simulators/gazebo_plugin/src/P3D.cc
pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc
pkg/trunk/simulators/gazebo_plugin/src/Pr2_Gripper.cc
pkg/trunk/simulators/rosgazebo/rosgazebo.cc
pkg/trunk/simulators/rosgazebo/setup.bash
pkg/trunk/simulators/rosgazebo/setup.tcsh
Added Paths:
-----------
pkg/trunk/3rdparty/gazebo/
pkg/trunk/3rdparty/gazebo/Makefile
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/3rdparty/gazebo/jointforce.diff
pkg/trunk/3rdparty/gazebo/manifest.xml
pkg/trunk/3rdparty/gazebo/patches/
pkg/trunk/3rdparty/gazebo/patches/GLWindow.cc.diff
pkg/trunk/3rdparty/gazebo/patches/GazeboConfig.cc.diff
pkg/trunk/3rdparty/gazebo/patches/Generic_Camera.cc.diff
pkg/trunk/3rdparty/gazebo/patches/HingeJoint.cc.diff
pkg/trunk/3rdparty/gazebo/patches/Iface.cc.diff
pkg/trunk/3rdparty/gazebo/patches/Server.cc.diff
pkg/trunk/3rdparty/gazebo/patches/SliderJoint.cc.diff
pkg/trunk/3rdparty/gazebo/patches/World.cc.diff
pkg/trunk/3rdparty/gazebo/patches/World.hh.diff
pkg/trunk/3rdparty/gazebo/patches/gazebo.h.diff
pkg/trunk/3rdparty/gazebo/setup.bash
pkg/trunk/3rdparty/gazebo/setup.tcsh
Removed Paths:
-------------
pkg/trunk/3rdparty/gazebo/Makefile
pkg/trunk/3rdparty/gazebo/manifest.xml
pkg/trunk/3rdparty/gazebo/setup.bash
pkg/trunk/3rdparty/gazebo/setup.tcsh
pkg/trunk/3rdparty/gazebo2/
Copied: pkg/trunk/3rdparty/gazebo (from rev 1021, pkg/trunk/3rdparty/gazebo2)
Deleted: pkg/trunk/3rdparty/gazebo/Makefile
===================================================================
--- pkg/trunk/3rdparty/gazebo2/Makefile 2008-06-26 16:54:16 UTC (rev 1021)
+++ pkg/trunk/3rdparty/gazebo/Makefile 2008-06-26 22:18:48 UTC (rev 1034)
@@ -1,25 +0,0 @@
-all: gazebo2
-
-#REVISION = -r 6641
-
-OGRE_PATH = $(shell rospack find ogre)/ogre
-ODE_PATH = $(shell rospack find opende)/opende
-ROOT = $(shell rospack find gazebo2)/gazebo
-
-gazebo-checkout:
- @if [ ! -d gazebo-svn ]; then svn co $(REVISION) https://playerstage.svn.sf.net/svnroot/playerstage/code/gazebo/trunk gazebo-svn; fi
- -cd gazebo-svn && svn up $(REVISION)
-
-gazebo2: gazebo-checkout
- if [ ! -d gazebo ]; then mkdir -p gazebo; fi
- cd gazebo-svn && export PKG_CONFIG_PATH=${OGRE_PATH}/lib/pkgconfig:${PKG_CONFIG_PATH} && export PATH=${ODE_PATH}/bin:${PATH} && scons prefix=$(ROOT) && scons prefix=$(ROOT) install
- touch gazebo
-
-clean:
- cd gazebo-svn && scons --clean
- rm -rf gazebo
-wipe:
- rm -rf gazebo-svn gazebo
-
-download: gazebo-checkout
-
Copied: pkg/trunk/3rdparty/gazebo/Makefile (from rev 1033, pkg/trunk/3rdparty/gazebo2/Makefile)
===================================================================
--- pkg/trunk/3rdparty/gazebo/Makefile (rev 0)
+++ pkg/trunk/3rdparty/gazebo/Makefile 2008-06-26 22:18:48 UTC (rev 1034)
@@ -0,0 +1,26 @@
+all: gazebo
+
+REVISION = -r 6696
+
+OGRE_PATH = $(shell rospack find ogre)/ogre
+ODE_PATH = $(shell rospack find opende)/opende
+ROOT = $(shell rospack find gazebo)/gazebo
+
+gazebo-checkout:
+ @if [ ! -d gazebo-svn ]; then svn co $(REVISION) https://playerstage.svn.sf.net/svnroot/playerstage/code/gazebo/trunk gazebo-svn; fi
+ -cd gazebo-svn && svn up $(REVISION)
+ -cd gazebo-svn/server && patch -N -p0 < gazebo_patch.diff
+
+gazebo: gazebo-checkout
+ if [ ! -d gazebo ]; then mkdir -p gazebo; fi
+ cd gazebo-svn && export PKG_CONFIG_PATH=${OGRE_PATH}/lib/pkgconfig:${PKG_CONFIG_PATH} && export PATH=${ODE_PATH}/bin:${PATH} && scons prefix=$(ROOT) && scons prefix=$(ROOT) install
+ touch gazebo
+
+clean:
+ cd gazebo-svn && scons --clean
+ rm -rf gazebo
+wipe:
+ rm -rf gazebo-svn gazebo
+
+download: gazebo-checkout
+
Added: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff (rev 0)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-06-26 22:18:48 UTC (rev 1034)
@@ -0,0 +1,721 @@
+Index: gazebo-svn/libgazebo/Server.cc
+===================================================================
+--- gazebo-svn/libgazebo/Server.cc (revision 6696)
++++ gazebo-svn/libgazebo/Server.cc (working copy)
+@@ -38,6 +38,7 @@
+ #include <sys/sem.h>
+ #include <sstream>
+ #include <iostream>
++#include <signal.h>
+
+ #include "gazebo.h"
+
+@@ -92,6 +93,42 @@
+
+ std::cout << "creating " << this->filename << "\n";
+
++ // check to see if there is already a directory created.
++ struct stat astat;
++ if (stat(this->filename.c_str(), &astat) == 0) {
++ // directory already exists, check gazebo.pid to see if
++ // another gazebo is already running.
++
++ std::string pidfn = this->filename + "/gazebo.pid";
++
++ FILE *fp = fopen(pidfn.c_str(), "r");
++ if(fp) {
++ int pid;
++ fscanf(fp, "%d", &pid);
++ fclose(fp);
++ std::cout << "found a pid file: pid=" << pid << "\n";
++
++ if(kill(pid, 0) == 0) {
++ // a gazebo process is still alive.
++ errStream << "directory [" << this->filename
++ << "] already exists (previous crash?)\n"
++ << "gazebo (pid=" << pid << ") is still running.";
++ throw(errStream.str());
++ } else {
++ // the gazebo process is not alive.
++ // remove directory.
++ std::cout << "The gazebo process is not alive.\n";
++
++ // remove the existing directory.
++ std::string cmd = "rm -rf '" + this->filename + "'";
++ if(system(cmd.c_str()) != 0) {
++ errStream << "couldn't remove directory [" << this->filename << "]";
++ throw(errStream.str());
++ }
++ }
++ }
++ }
++
+ // Create the directory
+ if (mkdir(this->filename.c_str(), S_IRUSR | S_IWUSR | S_IXUSR) != 0)
+ {
+@@ -108,7 +145,17 @@
+ << strerror(errno) << "]";
+ throw(errStream.str());
+ }
++
+ }
++
++ // write the PID to a file
++ std::string pidfn = this->filename + "/gazebo.pid";
++
++ FILE *fp = fopen(pidfn.c_str(), "w");
++ if(fp) {
++ fprintf(fp, "%d\n", getpid());
++ fclose(fp);
++ }
+ }
+
+
+@@ -120,6 +167,15 @@
+
+ std::cout << "deleting " << this->filename << "\n";
+
++ // unlink the pid file
++ std::string pidfn = this->filename + "/gazebo.pid";
++ if (unlink(pidfn.c_str()) < 0)
++ {
++ std::ostringstream stream;
++ stream << "error deleting pid file: " << strerror(errno);
++ throw(stream.str());
++ }
++
+ // Delete the server dir
+ if (rmdir(this->filename.c_str()) != 0)
+ {
+Index: gazebo-svn/libgazebo/Iface.cc
+===================================================================
+--- gazebo-svn/libgazebo/Iface.cc (revision 6696)
++++ gazebo-svn/libgazebo/Iface.cc (working copy)
+@@ -55,6 +55,8 @@
+ GZ_REGISTER_IFACE("factory", FactoryIface);
+ GZ_REGISTER_IFACE("gripper", GripperIface);
+ GZ_REGISTER_IFACE("actarray", ActarrayIface);
++GZ_REGISTER_IFACE("pr2array", PR2ArrayIface);
++GZ_REGISTER_IFACE("pr2gripper", PR2GripperIface);
+ GZ_REGISTER_IFACE("ptz", PTZIface);
+ GZ_REGISTER_IFACE("stereocamera", StereoCameraIface);
+
+Index: gazebo-svn/libgazebo/gazebo.h
+===================================================================
+--- gazebo-svn/libgazebo/gazebo.h (revision 6696)
++++ gazebo-svn/libgazebo/gazebo.h (working copy)
+@@ -550,7 +550,7 @@
+
+
+ /// Maximum image pixels (width x height)
+-#define GAZEBO_CAMERA_MAX_IMAGE_SIZE 640 * 480 * 3
++#define GAZEBO_CAMERA_MAX_IMAGE_SIZE 1024 * 1024 * 3
+
+ /// \brief Camera interface data
+ class CameraData
+@@ -1057,6 +1057,30 @@
+
+ /// Lift down flag
+ public: int lift_down;
++
++ /// Enable flag
++ public: int cmdEnableMotor;
++
++ /// Gripper Position Rate Command
++ public: float cmdPositionRate;
++ /// Gripper Force Command
++ public: float cmdForce;
++ /// Gripper Gap Command
++ public: float cmdGap;
++
++ /// Gripper force
++ public: double gripperForce;
++ /// Gripper controller gains
++ public: double pGain;
++ public: double dGain;
++ public: double iGain;
++
++ /// Position
++ public: float actualFingerPosition[2];
++ /// Position Rate
++ public: float actualFingerPositionRate[2];
++
++
+ };
+
+ /// \brief Gripper interface
+@@ -1254,9 +1278,327 @@
+ /// \} */
+
+
++
++
+ /***************************************************************************/
+ /// \addtogroup libgazebo_iface
+ /// \{
++/** \defgroup gripper_iface gripper
++
++ \brief PR2 Gripper interface
++
++The gripper interface allows control of a simple pseudo-1-DOF PR2 gripper, such as
++that found on the Pioneer series robots.
++
++
++\{
++*/
++
++/** Gripper state: open */
++#define GAZEBO_PR2GRIPPER_STATE_OPEN 1
++/** Gripper state: closed */
++#define GAZEBO_PR2GRIPPER_STATE_CLOSED 2
++/** Gripper state: moving */
++#define GAZEBO_PR2GRIPPER_STATE_MOVING 3
++/** Gripper state: error */
++#define GAZEBO_PR2GRIPPER_STATE_ERROR 4
++
++/** Gripper command: open */
++#define GAZEBO_PR2GRIPPER_CMD_OPEN 1
++/** Gripper command: close */
++#define GAZEBO_PR2GRIPPER_CMD_CLOSE 2
++/** Gripper command: stop */
++#define GAZEBO_PR2GRIPPER_CMD_STOP 3
++/** Gripper command: store */
++#define GAZEBO_PR2GRIPPER_CMD_STORE 4
++/** Gripper command: retrieve */
++#define GAZEBO_PR2GRIPPER_CMD_RETRIEVE 5
++
++
++/// \brief Fudicial interface data
++class PR2GripperData
++{
++ public: GazeboData head;
++
++ /// \brief Current command for the gripper
++ public: int cmd;
++
++ /// Current state of the gripper
++ public: int state;
++
++ /// Gripped limit reached flag
++ public: int grip_limit_reach;
++
++ /// Lift limit reached flag
++ public: int lift_limit_reach;
++
++ /// Outer beam obstruct flag
++ public: int outer_beam_obstruct;
++
++ /// control mode, TODO: yet to be defined
++ public: int controlMode;
++
++ /// Inner beam obstructed flag
++ public: int inner_beam_obstruct;
++
++ /// Left paddle open flag
++ public: int left_paddle_open;
++
++ /// Right paddle open flag
++ public: int right_paddle_open;
++
++ /// Lift up flag
++ public: int lift_up;
++
++ /// Lift down flag
++ public: int lift_down;
++
++ /// Enable flag
++ public: int cmdEnableMotor;
++
++ /// Gripper Position Rate Command
++ public: float cmdPositionRate;
++ /// Gripper Force Command
++ public: float cmdForce;
++ /// Gripper Gap Command
++ public: float cmdGap;
++
++ /// Gripper force
++ public: double gripperForce;
++ /// Gripper controller gains
++ public: double pGain;
++ public: double dGain;
++ public: double iGain;
++
++ /// Position
++ public: float actualFingerPosition[2];
++ /// Position Rate
++ public: float actualFingerPositionRate[2];
++
++
++};
++
++/// \brief Gripper interface
++class PR2GripperIface : public Iface
++{
++ /// \brief Constructor
++ public: PR2GripperIface():Iface("pr2gripper", sizeof(PR2GripperIface)+sizeof(PR2GripperData)) {}
++
++ /// \brief Destructor
++ public: virtual ~PR2GripperIface() {this->data = NULL;}
++
++ /// \brief Create the server
++ public: virtual void Create(Server *server, std::string id)
++ {
++ Iface::Create(server,id);
++ this->data = (PR2GripperData*)this->mMap;
++ }
++
++ /// \brief Open the iface
++ public: virtual void Open(Client *client, std::string id)
++ {
++ Iface::Open(client,id);
++ this->data = (PR2GripperData*)this->mMap;
++ }
++
++ /// Pointer to the gripper data
++ public: PR2GripperData *data;
++};
++
++/** \} */
++/// \}
++
++
++/***************************************************************************/
++/// \addtogroup libgazebo_iface
++/// \{
++/** \defgroup pr2array_iface actarray
++
++ \brief PR2 Array
++
++The PR2 array interface allows a user to control a set of actuators.
++
++\{
++*/
++
++/// maximum number of actuators
++#define GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS 64
++#define GAZEBO_PR2ARRAY_JOINT_POSITION_MODE 0
++#define GAZEBO_PR2ARRAY_JOINT_SPEED_MODE 1
++#define GAZEBO_PR2ARRAY_JOINT_CURRENT_MODE 2
++
++//Actuator states
++/// Idle state
++#define GAZEBO_PR2ARRAY_ACTSTATE_IDLE 1
++
++/// Moving state
++#define GAZEBO_PR2ARRAY_ACTSTATE_MOVING 2
++
++/// Braked state
++#define GAZEBO_PR2ARRAY_ACTSTATE_BRAKED 3
++
++/// Stalled state
++#define GAZEBO_PR2ARRAY_ACTSTATE_STALLED 4
++
++/// Linear type
++#define GAZEBO_PR2ARRAY_TYPE_LINEAR 1
++/// Rotary type
++#define GAZEBO_PR2ARRAY_TYPE_ROTARY 2
++
++/// Request subtype: power
++#define GAZEBO_PR2ARRAY_POWER_REQ 1
++/// Request subtype: brakes
++#define GAZEBO_PR2ARRAY_BRAKES_REQ 2
++/// Request subtype: get geometry
++#define GAZEBO_PR2ARRAY_GET_GEOM_REQ 3
++/// Request subtype: speed
++#define GAZEBO_PR2ARRAY_SPEED_REQ 4
++
++/// Command subtype: position
++#define GAZEBO_PR2ARRAY_POS_CMD 1
++/// Command subtype: speed
++#define GAZEBO_PR2ARRAY_SPEED_CMD 2
++/// Command subtype: home
++#define GAZEBO_PR2ARRAY_HOME_CMD 3
++
++
++/// \brief Actuator geometry
++class PR2ArrayActuatorGeom
++{
++
++/// Data subtype: state
++#define GAZEBO_PR2ARRAY_DATA_STATE 1
++
++
++ /// The type of the actuator - linear or rotary.
++ public: uint8_t type;
++
++ /// Min range of motion (m or rad depending on the type)
++ public: float min;
++
++ /// Center position (m or rad)
++ public: float center;
++
++ /// Max range of motion (m or rad depending on the type)
++ public: float max;
++
++ /// Home position (m or rad depending on the type)
++ public: float home;
++
++ /// The configured speed - different from current speed.
++ public: float config_speed;
++
++ /// The maximum achievable speed of the actuator.
++ public: float max_speed;
++
++ /// If the actuator has brakes or not.
++ public: uint8_t hasbrakes;
++};
++
++/// \brief Structure containing a single actuator's information
++class PR2ArrayActuator
++{
++ /// The position of the actuator in m or rad depending on the type.
++ public: float actualPosition;
++ /// The speed of the actuator in m/s or rad/s depending on the type.
++ public: float actualSpeed;
++ /// The current state of the actuator.
++ public: uint8_t state;
++};
++
++ typedef struct
++ {
++ double timestamp;
++ double actualPosition;
++ double actualSpeed;
++ double actualEffectorForce;
++
++ int controlMode;
++ int jointType;
++
++ double cmdPosition;
++ double cmdSpeed;
++ double cmdEffectorForce;
++
++ int cmdEnableMotor;
++
++ double pGain;
++ double iGain;
++ double dGain;
++ double iClamp;
++ double saturationTorque;
++ } JointData;
++
++/// \brief The actuator array data packet.
++class PR2ArrayData
++{
++ public: GazeboData head;
++
++ /// The number of actuators in the array.
++ public: unsigned int actuators_count;
++
++ /// The actuator data.
++// public: PR2ArrayActuator actuators[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
++ public: JointData actuators[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
++
++ /// The actuators geoms
++ public: PR2ArrayActuatorGeom actuator_geoms[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
++
++ /// position commands
++ public: float cmd_pos[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
++
++ /// speed commands
++ public: float cmd_speed[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
++
++ /// bad command flag - (speed to high set for the actuators or position not reachable)
++ public: int bad_cmd;
++
++ /// True if new command
++ public: bool new_cmd;
++
++ /// position / speed comand
++ public: unsigned int joint_mode[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
++
++};
++
++/// \brief The PR2Array interface
++class PR2ArrayIface : public Iface
++{
++ /// \brief Create an interface
++ public: PR2ArrayIface():Iface("pr2array", sizeof(PR2ArrayIface)+sizeof(PR2ArrayData)) {}
++
++ /// \brief Destroy and Interface
++ public: virtual ~PR2ArrayIface() {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 = (PR2ArrayData*)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 = (PR2ArrayData*)this->mMap;
++ }
++
++ /// Pointer to the act array data
++ public: PR2ArrayData *data;
++};
++
++/** \} */
++/// \} */
++
++
++/***************************************************************************/
++/// \addtogroup libgazebo_iface
++/// \{
+ /** \defgroup ptz_iface ptz
+
+ \brief PTZ interface
+Index: gazebo-svn/server/physics/HingeJoint.cc
+===================================================================
+--- gazebo-svn/server/physics/HingeJoint.cc (revision 6696)
++++ gazebo-svn/server/physics/HingeJoint.cc (working copy)
+@@ -38,6 +38,7 @@
+ : Joint()
+ {
+ this->jointId = dJointCreateHinge( worldId, NULL );
++ this->type = Joint::HINGE;
+ }
+
+
+Index: gazebo-svn/server/physics/SliderJoint.cc
+===================================================================
+--- gazebo-svn/server/physics/SliderJoint.cc (revision 6696)
++++ gazebo-svn/server/physics/SliderJoint.cc (working copy)
+@@ -35,6 +35,8 @@
+ : Joint()
+ {
+ this->jointId = dJointCreateSlider( worldId, NULL );
++ this->type = Joint::SLIDER;
++ fprintf(stderr," slider jointId %d\n",this->jointId);
+ }
+
+
+Index: gazebo-svn/server/GazeboConfig.cc
+===================================================================
+--- gazebo-svn/server/GazeboConfig.cc (revision 6696)
++++ gazebo-svn/server/GazeboConfig.cc (working copy)
+@@ -56,6 +56,17 @@
+
+ cfgFile.open(rcFilename.c_str(), std::ios::in);
+
++ char *ogre_resource_path = getenv("OGRE_RESOURCE_PATH");
++ if(ogre_resource_path) {
++ this->ogrePaths.push_back(ogre_resource_path);
++ }
++ char *gazebo_resource_path = getenv("GAZEBO_RESOURCE_PATH");
++ if(gazebo_resource_path) {
++ this->gazeboPaths.push_back(gazebo_resource_path);
++ }
++ // if both paths are set, don't check the config file or use the defaults.
++ if(ogre_resource_path && gazebo_resource_path) return;
++
+ if (cfgFile)
+ {
+ XMLConfig rc;
+Index: gazebo-svn/server/gui/GLWindow.cc
+===================================================================
+--- gazebo-svn/server/gui/GLWindow.cc (revision 6696)
++++ gazebo-svn/server/gui/GLWindow.cc (working copy)
+@@ -66,7 +66,11 @@
+ this->directionVec.x = 0;
+ this->directionVec.y = 0;
+ this->directionVec.z = 0;
++ this->leftMousePressed = false;
++ this->rightMousePressed = false;
++ this->middleMousePressed = false;
+
++
+ this->keys.clear();
+
+ if (activeWin == NULL)
+@@ -235,6 +239,21 @@
+ this->activeCamera->RotateYaw(DTOR(-d.x * this->rotateAmount));
+ this->activeCamera->RotatePitch(DTOR(d.y * this->rotateAmount));
+ }
++ if (this->rightMousePressed)
++ {
++ Vector2<int> d = this->mousePos - this->prevMousePos;
++ this->directionVec.x = 0;
++ this->directionVec.y = d.x * this->moveAmount;
++ this->directionVec.z = d.y * this->moveAmount;
++ }
++ if (this->middleMousePressed)
++ {
++ Vector2<int> d = this->mousePos - this->prevMousePos;
++ this->directionVec.x = d.y * this->moveAmount;
++ this->directionVec.y = 0;
++ this->directionVec.z = 0;
++ }
++
+ }
+
+ this->mouseDrag = true;
+@@ -247,12 +266,28 @@
+ std::map<int,int>::iterator iter;
+ this->keys[keyNum] = 1;
+
++ // loop through the keys to find the modifiers -- swh
++ float moveAmount = this->moveAmount;
+ for (iter = this->keys.begin(); iter!= this->keys.end(); iter++)
+ {
+ if (iter->second == 1)
+ {
+ switch (iter->first)
+ {
++ case FL_Control_L:
++ case FL_Control_R:
++ moveAmount = this->moveAmount * 10;
++ break;
++ }
++ }
++ }
++
++ for (iter = this->keys.begin(); iter!= this->keys.end(); iter++)
++ {
++ if (iter->second == 1)
++ {
++ switch (iter->first)
++ {
+ case '=':
+ case '+':
+ this->moveAmount *= 2;
+Index: gazebo-svn/server/World.hh
+===================================================================
+--- gazebo-svn/server/World.hh (revision 6696)
++++ gazebo-svn/server/World.hh (working copy)
+@@ -91,6 +91,26 @@
+ /// \return Pointer to the physics engine
+ public: PhysicsEngine *GetPhysicsEngine() const;
+
++ /// Get the simulation time
++ /// \return The simulation time
++ public: double GetSimTime() const;
++
++ /// Get the pause time
++ /// \return The pause time
++ public: double GetPauseTime() const;
++
++ /// Get the start time
++ /// \return The start time
++ public: double GetStartTime() const;
++
++ /// Get the real time (elapsed time)
++ /// \return The real time
++ public: double GetRealTime() const;
++
++ /// \brief Get the wall clock time
++ /// \return The wall clock time
++ public: double GetWallTime() const;
++
+ /// \brief Load all entities
+ /// \param node XMLConfg node pointer
+ /// \param parent Parent of the model to load
+@@ -185,6 +205,9 @@
+ /// Simulation interface
+ private: SimulationIface *simIface;
+
++ /// Current simulation time
++ private: double simTime, pauseTime, startTime;
++
+ private: friend class DestroyerT<World>;
+ private: friend class SingletonT<World>;
+ };
+Index: gazebo-svn/server/World.cc
+===================================================================
+--- gazebo-svn/server/World.cc (revision 6696)
++++ gazebo-svn/server/World.cc (working copy)
+@@ -27,6 +27,7 @@
+ #include <assert.h>
+ #include <sstream>
+ #include <fstream>
++#include <sys/time.h> //gettimeofday
+
+ #include "Global.hh"
+ #include "GazeboError.hh"
+@@ -57,6 +58,9 @@
+ this->server = NULL;
+ this->simIface = NULL;
+
++ this->simTime = 0.0;
++ this->pauseTime = 0.0;
++ this->startTime = 0.0;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+@@ -157,6 +161,7 @@
+
+ this->physicsEngine->Init();
+
++ this->startTime = this->GetWallTime();
+ this->toAddModels.clear();
+ this->toDeleteModels.clear();
+
+@@ -170,6 +175,8 @@
+ std::vector< Model* >::iterator miter;
+ std::vector< Model* >::iterator miter2;
+
++ this->simTime += this->physicsEngine->GetStepTime();
++
+ // Update all the models
+ for (miter=this->models.begin(); miter!=this->models.end(); miter++)
+ {
+@@ -183,6 +190,10 @@
+ {
+ this->physicsEngine->Update();
+ }
++ else
++ {
++ this->pauseTime += this->physicsEngine->GetStepTime();
++ }
+
+ this->UpdateSimulationIface();
+
+@@ -250,6 +261,41 @@
+ return this->physicsEngine;
+ }
+
++////////////////////////////////////////////////////////////////////////////////
++// Get the simulation time
++double World::GetSimTime() const
++{
++ return this->simTime;
++}
++////////////////////////////////////////////////////////////////////////////////
++// Get the pause time
++double World::GetPauseTime() const
++{
++ return this->pauseTime;
++}
++
++////////////////////////////////////////////////////////////////////////////////
++/// Get the start time
++double World::GetStartTime() const
++{
++ return this->startTime;
++}
++////////////////////////////////////////////////////////////////////////////////
++/// Get the real time (elapsed time)
++double World::GetRealTime() const
++{
++ return this->GetWallTime() - this->startTime;
++}
++
++////////////////////////////////////////////////////////////////////////////////
++/// Get the wall clock time
++double World::GetWallTime() const
++{
++ struct timeval tv;
++ gettimeofday(&tv, NULL);
++ return tv.tv_sec + tv.tv_usec * 1e-6;
++}
++
+ ///////////////////////////////////////////////////////////////////////////////
+ // Load a model
+ int World::LoadEntities(XMLConfigNode *node, Model *parent)
Added: pkg/trunk/3rdparty/gazebo/jointforce.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/jointforce.diff (rev 0)
+++ pkg/trunk/3rdparty/gazebo/jointforce.diff 2008-06-26 22:18:48 UTC (rev 1034)
@@ -0,0 +1,300 @@
+Index: server/physics/Joint.hh
+===================================================================
+--- server/physics/Joint.hh (Revision 6545)
++++ server/physics/Joint.hh (Arbeitskopie)
+@@ -124,6 +124,9 @@
+ /// \brief Get the CFM of this joint
+ public: double GetCFM();
+
++ /// \brief Get the feedback data structure for this joint, if set
++ public: dJointFeedback *GetFeedback();
++
+ /// \brief Get the high stop of an axis(index).
+ public: double GetHighStop(int index=0);
+
+@@ -145,6 +148,9 @@
+ /// \brief Name of this joint
+ private: std::string name;
+
++ /// \brief Feedback data for this joint
++ private: dJointFeedback* feedback;
++
+ private: OgreVisual *visual;
+
+ private: Model *model;
+Index: server/physics/Joint.cc
+===================================================================
+--- server/physics/Joint.cc (Revision 6545)
++++ server/physics/Joint.cc (Arbeitskopie)
+@@ -88,6 +88,10 @@
+ this->line1->AddPoint(Vector3(0,0,0));
+ this->line2->AddPoint(Vector3(0,0,0));
+ this->line2->AddPoint(Vector3(0,0,0));
++ if(node->GetBool("providefeedback", false, 0)) {
++ this->feedback = new dJointFeedback;
++ dJointSetFeedback(this->jointId, this->feedback);
++ }
+
+ }
+
+@@ -258,6 +262,13 @@
+ return this->GetParam(dParamSuspensionCFM);
+ }
+
++///////////////////////////////////////////////////////////////////////////////
++/// Get the feedback data structure of this joint
++dJointFeedback *Joint::GetFeedback()
++{
++ return dJointGetFeedback(this->jointId);
++}
++
+ ////////////////////////////////////////////////////////////////////////////////
+ /// Get the high stop of an axis(index).
+ double Joint::GetHighStop(int index)
+Index: server/controllers/opaque/jointforce/JointForce.hh
+===================================================================
+--- server/controllers/opaque/jointforce/JointForce.hh (Revision 0)
++++ server/controllers/opaque/jointforce/JointForce.hh (Revision 0)
+@@ -0,0 +1,92 @@
++/*
++ * 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: Joint Force Controller
++ * Author: Benjamin Kloster
++ * Date: 13 March 2008
++ */
++#ifndef JOINTFORCE_CONTROLLER_HH
++#define JOINTFORCE_CONTROLLER_HH
++
++/// Maximum number of joints that can be watched by one controller
++#define GAZEBO_JOINTFORCE_CONTROLLER_MAX_FEEDBACKS 16
++
++#include "Controller.hh"
++#include "Entity.hh"
++#include <ode/ode.h>
++#include <sys/time.h>
++
++
++namespace gazebo
++{
++/// \addtogroup gazebo_controller
++/// \{
++/** \defgroup jointforce_controller jointforce
++
++ \brief A controller that measures forces and torques exerted by joints
++
++ \{
++*/
++
++/// \brief A JointForce controller
++class JointForce : public Controller
++{
++ /// Constructor
++ public: JointForce(Entity *parent );
++
++ /// Destructor
++ public: virtual ~JointForce();
++
++ /// Load the controller
++ /// \param node XML config node
++ /// \return 0 on success
++ protected: virtual void LoadChild(XMLConfigNode *node);
++
++ /// Init the controller
++ /// \return 0 on success
++ protected: virtual void InitChild();
++
++ /// Update the controller
++ /// \return 0 on success
++ protected: virtual void UpdateChild(UpdateParams ¶ms);
++
++ /// Finalize the controller
++ /// \return 0 on success
++ protected: virtual void FiniChild();
++
++ /// The parent Model
++ private: Model *myParent;
++
++ /// The Iface. The dJointFeedback structs are rather arbitrary, so we use an Opaque Interface
++ private: OpaqueIface *myIface;
++ /// The joint feedbacks
++ private: dJointFeedback *jointfeedbacks[GAZEBO_JOINTFORCE_CONTROLLER_MAX_FEEDBACKS];
++ /// The number of joints we are watching
++ private: int n_joints;
++};
++
++/** \} */
++/// \}
++
++}
++
++#endif
++
+Index: server/controllers/opaque/jointforce/JointForce.cc
+===================================================================
+--- server/controllers/opaque/jointforce/JointForce.cc (Revision 0)
++++ server/controllers/opaque/jointforce/JointForce.cc (Revision 0)
+@@ -0,0 +1,121 @@
++/*
++ * 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: Joint Force controller
++ * Author: Benjamin Kloster
++ * Date: 13 March 2008
++ */
++
++#include "Global.hh"
++#include "XMLConfig.hh"
++#include "Model.hh"
++#include "World.hh"
++#include "gazebo.h"
++#include "GazeboError.hh"
++#include "ControllerFactory.hh"
++#include "Simulator.hh"
++#include "JointForce.hh"
++
++using namespace gazebo;
++
++GZ_REGISTER_STATIC_CONTROLLER("jointforce", JointForce);
++
++////////////////////////////////////////////////////////////////////////////////
++// Constructor
++JointForce::JointForce(Entity *parent )
++ : Controller(parent)
++{
++ this->myParent = dynamic_cast<Model*>(this->parent);
++
++ if (!this->myParent)
++ gzthrow("ControllerStub controller requires a Model as its parent");
++}
++
++////////////////////////////////////////////////////////////////////////////////
++// Destructor
++JointForce::~JointForce()
++{
++}
++
++////////////////////////////////////////////////////////////////////////////////
++// Load the controller
++void JointForce::LoadChild(XMLConfigNode *node)
++{
++ XMLConfigNode *jNode;
++ Joint *joint;
++ std::string jointName;
++ dJointFeedback *jFeedback = new dJointFeedback;
++ int i =0;
++ this->myIface = dynamic_cast<OpaqueIface*>(this->ifaces[0]);
++ if (!this->myIface) {
++ gzthrow("JointForce controller requires an OpaqueIface");
++ }
++ jNode = node->GetChild("joint");
++ while(jNode && i < GAZEBO_JOINTFORCE_CONTROLLER_MAX_FEEDBACKS)
++ {
++ jointName = jNode->GetString("name","",1);
++ joint = this->myParent->GetJoint(jointName);
++ jFeedback = joint->GetFeedback();
++ if(jFeedback) {
++ this->jointfeedbacks[i] = jFeedback;
++ i++;
++ }
++ jNode = jNode->GetNext("joint");
++ }
++ this->n_joints = i;
++}
++
++////////////////////////////////////////////////////////////////////////////////
++// Initialize the controller
++void JointForce::InitChild()
++{
++ //this->myIface->data->data = new uint8_t[sizeof(dJointFeedback)*n_joints];
++}
++
++////////////////////////////////////////////////////////////////////////////////
++// Update the controller
++void JointForce::UpdateChild(UpdateParams ¶ms)
++{
++ this->myIface->Lock(1);
++ this->myIface->data->head.time = Simulator::Instance()->GetSimTime();
++ // Let me explain this number: each joint reports 4 vectors: Force and torque
++ // on each jointed object, respectively. These vectors have 4 elements: x,y,z
++ // and a fourth one. So we transmit 4 dReals per vector.
++ this->myIface->data->data_count = n_joints*4*4*sizeof(dReal);
++
++ for(int i=0; i< n_joints; i++) {
++ // Copy vector for force on first object
++ memcpy(this->myIface->data->data + (i*4 + 0)*4*sizeof(dReal), this->jointfeedbacks[i]->f1, 4*sizeof(dReal));
++ // Copy vector for torque on first object
++ memcpy(this->myIface->data->data + (i*4 + 1)*4*sizeof(dReal), this->jointfeedbacks[i]->t1, 4*sizeof(dReal));
++ // Copy vector for force on second object
++ memcpy(this->myIface->data->data + (i*4 + 2)*4*sizeof(dReal), this->jointfeedbacks[i]->f2, 4*sizeof(dReal));
++ // Copy vector for torque on second object
++ memcpy(this->myIface->data->data + (i*4 + 3)*4*sizeof(dReal), this->jointfeedbacks[i]->t2, 4*sizeof(dReal));
++ }
++ this->myIface->Unlock();
++}
++
++////////////////////////////////////////////////////////////////////////////////
++// Finalize the controller
++void JointForce::FiniChild()
++{
++}
+Index: server/controllers/opaque/jointforce/SConscript
+===================================================================
+--- server/controllers/opaque/jointforce/SConscript (Revision 0)
++++ server/controllers/opaque/jointforce/SConscript (Revision 0)
+@@ -0,0 +1,7 @@
++#Import variable
++Import('env staticObjs sharedObjs')
++
++sources = Split('JointForce.cc')
++
++staticObjs.append(env.StaticObject(sources))
++sharedObjs.append(env.SharedObject(sources))
+Index: server/controllers/opaque/SConscript
+===================================================================
+--- server/controllers/opaque/SConscript (Revision 0)
++++ server/controllers/opaque/SConscript (Revision 0)
+@@ -0,0 +1,7 @@
++#Import variable
++Import('env staticObjs sharedObjs')
++
++dirs = Split('jointforce contact')
++
++for subdir in dirs :
++ SConscript('%s/SConscript' % subdir)
Deleted: pkg/trunk/3rdparty/gazebo/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/gazebo2/manifest.xml 2008-06-26 16:54:16 UTC (rev 1021)
+++ pkg/trunk/3rdparty/gazebo/manifest.xml 2008-06-26 22:18:48 UTC (rev 1034)
@@ -1,18 +0,0 @@
-<package>
-<description brief="gazebo2">
-
-This package contains Gazebo, from the Player Project
-(http://playerstage.sf.net).
-
-</description>
-<author>Nate Koenig, with contributions from many others. See web page for a full credits llist.</author>
-<license>LGPL</license>
-<url>http://playerstage.sf.net</url>
-<depend package="opende" />
-<depend package="ogre" />
-<export>
- <cpp lflags="-Xlinker -rpath ${prefix}/gazebo/lib -L${prefix}/gazebo/lib -lgazebo -lgazeboServer" cflags="-I${prefix}/gazebo/include"/>
-</export>
-</package>
-
-
Copied: pkg/trunk/3rdparty/gazebo/manifest.xml (from rev 1033, pkg/trunk/3rdparty/gazebo2/manifest.xml)
===================================================================
--- pkg/trunk/3rdparty/gazebo/manifest.xml (rev 0)
+++ pkg/trunk/3rdparty/gazebo/manifest.xml 2008-06-26 22:18:48 UTC (rev 1034)
@@ -0,0 +1,18 @@
+<package>
+<description brief="gazebo">
+
+This package contains Gazebo, from the Player Project
+(http://playerstage.sf.net).
+
+</description>
+<author>Nate Koenig, with contributions from many others. See web page for a full credits llist.</author>
+<license>LGPL</license>
+<url>http://playerstage.sf.net</url>
+<depend package="opende" />
+<depend package="ogre" />
+<export>
+ <cpp lflags="-Xlinker -rpath ${prefix}/gazebo/lib -L${prefix}/gazebo/lib -lgazebo -lgazeboServer" cflags="-I${prefix}/gazebo/include"/>
+</export>
+</package>
+
+
Added: pkg/trunk/3rdparty/gazebo/patches/GLWindow.cc.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/patches/GLWindow.cc.diff (rev 0)
+++ pkg/trunk/3rdparty/gazebo/patches/GLWindow.cc.diff 2008-06-26 22:18:48 UTC (rev 1034)
@@ -0,0 +1,67 @@
+Index: gazebo-svn/server/gui/GLWindow.cc
+===================================================================
+--- gazebo-svn/server/gui/GLWindow.cc (revision 6690)
++++ gazebo-svn/server/gui/GLWindow.cc (working copy)
+@@ -66,7 +66,11 @@
+ this->directionVec.x = 0;
+ this->directionVec.y = 0;
+ this->directionVec.z = 0;
++ this->leftMousePressed = false;
++ this->rightMousePressed = false;
++ this->middleMousePressed = false;
+
++
+ this->keys.clear();
+
+ if (activeWin == NULL)
+@@ -235,6 +239,21 @@
+ this->activeCamera->RotateYaw(DTOR(-d.x * this->rotateAmount));
+ this->activeCamera->RotatePitch(DTOR(d.y * this->rotateAmount));
+ }
++ if (this->rightMousePressed)
++ {
++ Vector2<int> d = this->mousePos - this->prevMousePos;
++ this->directionVec.x = 0;
++ this->directionVec.y = d.x * this->moveAmount;
++ this->directionVec.z = d.y * this->moveAmount;
++ }
++ if (this->middleMousePressed)
++ {
++ Vector2<int> d = this->mousePos - this->prevMousePos;
++ this->directionVec.x = d.y * this->moveAmount;
++ this->directionVec.y = 0;
++ this->directionVec.z = 0;
++ }
++
+ }
+
+ this->mouseDrag = true;
+@@ -247,12 +266,28 @@
+ std::map<int,int>::iterator iter;
+ this->keys[keyNum] = 1;
+
++ // loop through the keys to find the modifiers -- swh
++ float moveAmount = this->moveAmount;
+ for (iter = this->keys.begin(); iter!= this->keys.end(); iter++)
+ {
+ if (iter->second == 1)
+ {
+ switch (iter->first)
+ {
++ case FL_Control_L:
++ case FL_Control_R:
++ moveAmount = this->moveAmount * 10;
++ break;
++ }
++ }
++ }
++
++ for (iter = this->keys.begin(); iter!= this->keys.end(); iter++)
++ {
++ if (iter->second == 1)
++ {
++ switch (iter->first)
++ {
+ case '=':
+ case '+':
+ this->moveAmount *= 2;
Added: pkg/trunk/3rdparty/gazebo/patches/GazeboConfig.cc.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/patches/GazeboConfig.cc.diff (rev 0)
+++ pkg/trunk/3rdparty/gazebo/patches/GazeboConfig.cc.diff 2008-06-26 22:18:48 UTC (rev 1034)
@@ -0,0 +1,22 @@
+Index: gazebo-svn/server/GazeboConfig.cc
+===================================================================
+--- gazebo-svn/server/GazeboConfig.cc (revision 6690)
++++ gazebo-svn/server/GazeboConfig.cc (working copy)
+@@ -56,6 +56,17 @@
+
+ cfgFile.open(rcFilename.c_str(), std::ios::in);
+
++ char *ogre_resource_path = getenv("OGRE_RESOURCE_PATH");
++ if(ogre_resource_path) {
++ this->ogrePaths.push_back(ogre_resource_path);
++ }
++ char *gazebo_resource_path = getenv("GAZEBO_RESOURCE_PATH");
++ if(gazebo_resource_path) {
++ this->gazeboPaths.push_back(gazebo_resource_path);
++ }
++ // if both paths are set, don't check the config file or use the defaults.
++ if(ogre_resource_path && gazebo_resource_path) return;
++
+ if (cfgFile)
+ {
+ XMLConfig rc;
Added: pkg/trunk/3rdparty/gazebo/patches/Generic_Camera.cc.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/patches/Generic_Camera.cc.diff (rev 0)
+++ pkg/trunk/3rdparty/gazebo/patches/Generic_Camera.cc.diff 2008-06-26 22:18:48 UTC (rev 1034)
@@ -0,0 +1,22 @@
+Index: gazebo-svn/server/controllers/camera/generic/Generic_Camera.cc
+===================================================================
+--- gazebo-svn/server/controllers/camera/generic/Generic_Camera.cc (revision 6690)
++++ gazebo-svn/server/controllers/camera/generic/Generic_Camera.cc (working copy)
+@@ -93,7 +93,7 @@
+ void Generic_Camera::PutCameraData()
+ {
+ CameraData *data = this->cameraIface->data;
+- const unsigned char *src;
++ //const unsigned char *src;
+ unsigned char *dst;
+ Pose3d cameraPose;
+
+@@ -126,7 +126,7 @@
+ //src = this->myParent->GetImageData();
+ dst = data->image;
+
+- memcpy(dst, src, data->image_size);
++ //memcpy(dst, src, data->image_size);
+
+
+ //unsigned int i, j, k;
Added: pkg/trunk/3rdparty/gazebo/patches/HingeJoint.cc.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/patches/HingeJoint.cc.diff (rev 0)
+++ pkg/trunk/3rdparty/gazebo/patches/HingeJoint.cc.diff 2008-06-26 22:18:48 UTC (rev 1034)
@@ -0,0 +1,12 @@
+Index: gazebo-svn/server/physics/HingeJoint.cc
+===================================================================
+--- gazebo-svn/server/physics/HingeJoint.cc (revision 6690)
++++ gazebo-svn/server/physics/HingeJoint.cc (working copy)
+@@ -38,6 +38,7 @@
+ : Joint()
+ {
+ this->jointId = dJointCreateHinge( worldId, NULL );
++ this->type = Joint::HINGE;
+ }
+
+
Added: pkg/trunk/3rdparty/gazebo/patches/Iface.cc.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/patches/Iface.cc.diff (rev 0)
+++ pkg/trunk/3rdparty/gazebo/patches/Iface.cc.diff 2008-06-26 22:18:48 UTC (rev 1034)
@@ -0,0 +1,13 @@
+Index: gazebo-svn/libgazebo/Iface.cc
+===================================================================
+--- gazebo-svn/libgazebo/Iface.cc (revision 6690)
++++ gazebo-svn/libgazebo/Iface.cc (working copy)
+@@ -55,6 +55,8 @@
+ GZ_REGISTER_IFACE("factory", FactoryIface);
+ GZ_REGISTER_IFACE("gripper", GripperIface);
+ GZ_REGISTER_IFACE("actarray", ActarrayIface);
++GZ_REGISTER_IFACE("pr2array", PR2ArrayIface);
++GZ_REGISTER_IFACE("pr2gripper", PR2GripperIface);
+ GZ_REGISTER_IFACE("ptz", PTZIface);
+ GZ_REGISTER_IFACE("stereocamera", StereoCameraIface);
+
Added: pkg/trunk/3rdparty/gazebo/patches/Server.cc.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/patches/Server.cc.diff (rev 0)
+++ pkg/trunk/3rdparty/gazebo/patches/Server.cc.diff 2008-06-26 22:18:48 UTC (rev 1034)
@@ -0,0 +1,89 @@
+Index: gazebo-svn/libgazebo/Server.cc
+===================================================================
+--- gazebo-svn/libgazebo/Server.cc (revision 6690)
++++ gazebo-svn/libgazebo/Server.cc (working copy)
+@@ -38,6 +38,7 @@
+ #include <sys/sem.h>
+ #include <sstream>
+ #include <iostream>
++#include <signal.h>
+
+ #include "gazebo.h"
+
+@@ -92,6 +93,42 @@
+
+ std::cout << "creating " << this->filename << "\n";
+
++ // check to see if there is already a directory created.
++ struct stat astat;
++ if (stat(this->filename.c_str(), &astat) == 0) {
++ // directory already exists, check gazebo.pid to see if
++ // another gazebo is already running.
++
++ std::string pidfn = this->filename + "/gazebo.pid";
++
++ FILE *fp = fopen(pidfn.c_str(), "r");
++ if(fp) {
++ int pid;
++ fscanf(fp, "%d", &pid);
++ fclose(fp);
++ std::cout << "found a pid file: pid=" << pid << "\n";
++
++ if(kill(pid, 0) == 0) {
++ // a gazebo process is still alive.
++ errStream << "directory [" << this->filename
++ << "] already exists (previous crash?)\n"
++ << "gazebo (pid=" << pid << ") is still running.";
++ throw(errStream.str());
++ } else {
++ // the gazebo process is not alive.
++ // remove directory.
++ std::cout << "The gazebo process is not alive.\n";
++
++ // remove the existing directory.
++ std::string cmd = "rm -rf '" + this->filename + "'";
++ if(system(cmd.c_str()) != 0) {
++ errStream << "couldn't remove directory [" << this->filename << "]";
++ throw(errStream.str());
++ }
++ }
++ }
++ }
++
+ // Create the directory
+ if (mkdir(this->filename.c_str(), S_IRUSR | S_IWUSR | S_IXUSR) != 0)
+ {
+@@ -108,7 +145,17 @@
+ << strerror(errno) << "]";
+ throw(errStream.str());
+ }
++
+ }
++
++ // write the PID to a file
++ std::string pidfn = this->filename + "/gazebo.pid";
++
++ FILE *fp = fopen(pidfn.c_str(), "w");
++ if(fp) {
++ fprintf(fp, "%d\n", getpid());
++ fclose(fp);
++ }
+ }
+
+
+@@ -120,6 +167,15 @@
+
+ std::cout << "deleting " << this->filename << "\n";
+
++ // unlink the pid file
++ std::string pidfn = this->filename + "/gazebo.pid";
++ if (unlink(pidfn.c_str()) < 0)
++ {
++ std::ostringstream stream;
++ stream << "error deleting pid file: " << strerror(errno);
++ throw(stream.str());
++ }
++
+ // Delete the server dir
+ if (rmdir(this->filename.c_str()) != 0)
+ {
Added: pkg/trunk/3rdparty/gazebo/patches/SliderJoint.cc.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/patches/SliderJoint.cc.diff (rev 0)
+++ pkg/trunk/3rdparty/gazebo/patches/SliderJoint.cc.diff 2008-06-26 22:18:48 UTC (rev 1034)
@@ -0,0 +1,13 @@
+Index: gazebo-svn/server/physics/SliderJoint.cc
+===================================================================
+--- gazebo-svn/server/physics/SliderJoint.cc (revision 6690)
++++ gazebo-svn/server/physics/SliderJoint.cc (working copy)
+@@ -35,6 +35,8 @@
+ : Joint()
+ {
+ this->jointId = dJointCreateSlider( worldId, NULL );
++ this->type = Joint::SLIDER;
++ fprintf(stderr," slider jointId %d\n",this->jointId);
+ }
+
+
Added: pkg/trunk/3rdparty/gazebo/patches/World.cc.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/patches/World.cc.diff (rev 0)
+++ pkg/trunk/3rdparty/gazebo/patches/World.cc.diff 2008-06-26 22:18:48 UTC (rev 1034)
@@ -0,0 +1,92 @@
+Index: gazebo-svn/server/World.cc
+===================================================================
+--- gazebo-svn/server/World.cc (revision 6690)
++++ gazebo-svn/server/World.cc (working copy)
+@@ -27,6 +27,7 @@
+ #include <assert.h>
+ #include <sstream>
+ #include <fstream>
++#include <sys/time.h> //gettimeofday
+
+ #include "Global.hh"
+ #include "GazeboError.hh"
+@@ -57,6 +58,9 @@
+ this->server = NULL;
+ this->simIface = NULL;
+
++ this->simTime = 0.0;
++ this->pauseTime = 0.0;
++ this->startTime = 0.0;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+@@ -157,6 +161,7 @@
+
+ this->physicsEngine->Init();
+
++ this->startTime = this->GetWallTime();
+ this->toAddModels.clear();
+ this->toDeleteModels.clear();
+
+@@ -170,6 +175,8 @@
+ std::vector< Model* >::iterator miter;
+ std::vector< Model* >::iterator miter2;
+
++ this->simTime += this->physicsEngine->GetStepTime();
++
+ // Update all the models
+ for (miter=this->models.begin(); miter!=this->models.end(); miter++)
+ {
+@@ -183,6 +190,10 @@
+ {
+ this->physicsEngine->Update();
+ }
++ else
++ {
++ this->pauseTime += this->physicsEngine->GetStepTime();
++ }
+
+ this->UpdateSimulationIface();
+
+@@ -250,6 +261,41 @@
+ return this->physicsEngine;
+ }
+
++////////////////////////////////////////////////////////////////////////////////
++// Get the simulation time
++double World::GetSimTime() const
++{
++ return this->simTime;
++}
++////////////////////////////////////////////////////////////////////////////////
++// Get the pause time
++double World::GetPauseTime() const
++{
++ return this->pauseTime;
++}
++
++////////////////////////////////////////////////////////////////////////////////
++/// Get the start time
++double World::GetStartTime() const
++{
++ return this->startTime;
++}
++////////////////////////////////////////////////////////////////////////////////
++/// Get the real time (elapsed time)
++double World::GetRealTime() const
++{
++ return this->GetWallTime() - this->startTime;
++}
++
++////////////////////////////////////////////////////////////////////////////////
++/// Get the wall clock time
++double World::GetWallTime() const
++{
++ struct timeval tv;
++ gettimeofday(&tv, NULL);
++ return tv.tv_sec + tv.tv_usec * 1e-6;
++}
++
+ ///////////////////////////////////////////////////////////////////////////////
+ // Load a model
+ int World::LoadEntities(XMLConfigNode *node, Model *parent)
Added: pkg/trunk/3rdparty/gazebo/patches/World.hh.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/patches/World.hh.diff (rev 0)
+++ pkg/trunk/3rdparty/gazebo/patches/World.hh.diff 2008-06-26 22:18:48 UTC (rev 1034)
@@ -0,0 +1,41 @@
+Index: gazebo-svn/server/World.hh
+===================================================================
+--- gazebo-svn/server/World.hh (revision 6690)
++++ gazebo-svn/server/World.hh (working copy)
+@@ -91,6 +91,26 @@
+ /// \return Pointer to the physics engine
+ public: PhysicsEngine *GetPhysicsEngine() const;
+
++ /// Get the simulation time
++ /// \return The simulation time
++ public: double GetSimTime() const;
++
++ /// Get the pause time
++ /// \return The pause time
++ public: double GetPauseTime() const;
++
++ /// Get the start time
++ /// \return The start time
++ public: double GetStartTime() const;
++
++ /// Get the real time (elapsed time)
++ /// \return The real time
++ public: double GetRealTime() const;
++
++ /// \brief Get the wall clock time
++ /// \return The wall clock time
++ public: double GetWallTime() const;
++
+ /// \brief Load all entities
+ /// \param node XMLConfg node pointer
+ /// \param parent Parent of the model to load
+@@ -185,6 +205,9 @@
+ /// Simulation interface
+ private: SimulationIface *simIface;
+
++ /// Current simulation time
++ private: double simTime, pauseTime, startTime;
++
+ private: friend class DestroyerT<World>;
+ private: friend class SingletonT<World>;
+ };
Added: pkg/trunk/3rdparty/gazebo/patches/gazebo.h.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/patches/gazebo.h.diff (rev 0)
+++ pkg/trunk/3rdparty/gazebo/patches/gazebo.h.diff 2008-06-26 22:18:48 UTC (rev 1034)
@@ -0,0 +1,372 @@
+Index: gazebo-svn/libgazebo/gazebo.h
+===================================================================
+--- gazebo-svn/libgazebo/gazebo.h (revision 6690)
++++ gazebo-svn/libgazebo/gazebo.h (working copy)
+@@ -550,7 +550,7 @@
+
+
+ /// Maximum image pixels (width x height)
+-#define GAZEBO_CAMERA_MAX_IMAGE_SIZE 640 * 480 * 3
++#define GAZEBO_CAMERA_MAX_IMAGE_SIZE 1024 * 1024 * 3
+
+ /// \brief Camera interface data
+ class CameraData
+@@ -1057,6 +1057,30 @@
+
+ /// Lift down flag
+ public: int lift_down;
++
++ /// Enable flag
++ public: int cmdEnableMotor;
++
++ /// Gripper Position Rate Command
++ public: float cmdPositionRate;
++ /// Gripper Force Command
++ public: float cmdForce;
++ /// Gripper Gap Command
++ public: float cmdGap;
++
++ /// Gripper force
++ public: double gripperForce;
++ /// Gripper controller gains
++ public: double pGain;
++ public: double dGain;
++ public: double iGain;
++
++ /// Position
++ public: float actualFingerPosition[2];
++ /// Position Rate
++ public: float actualFingerPositionRate[2];
++
++
+ };
+
+ /// \brief Gripper interface
+@@ -1254,9 +1278,327 @@
+ /// \} */
+
+
++
++
+ /***************************************************************************/
+ /// \addtogroup libgazebo_iface
+ /// \{
++/** \defgroup gripper_iface gripper
++
++ \brief PR2 Gripper interface
++
++The gripper interface allows control of a simple pseudo-1-DOF PR2 gripper, such as
++that found on the Pioneer series robots.
++
++
++\{
++*/
++
++/** Gripper state: open */
++#define GAZEBO_PR2GRIPPER_STATE_OPEN 1
++/** Gripper state: closed */
++#define GAZEBO_PR2GRIPPER_STATE_CLOSED 2
++/** Gripper state: moving */
++#define GAZEBO_PR2GRIPPER_STATE_MOVING 3
++/** Gripper state: error */
++#define GAZEBO_PR2GRIPPER_STATE_ERROR 4
++
++/** Gripper command: open */
++#define GAZEBO_PR2GRIPPER_CMD_OPEN 1
++/** Gripper command: close */
++#define GAZEBO_PR2GRIPPER_CMD_CLOSE 2
++/** Gripper command: stop */
++#define GAZEBO_PR2GRIPPER_CMD_STOP 3
++/** Gripper command: store */
++#define GAZEBO_PR2GRIPPER_CMD_STORE 4
++/** Gripper command: retrieve */
++#define GAZEBO_PR2GRIPPER_CMD_RETRIEVE 5
++
++
++/// \brief Fudicial interface data
++class PR2GripperData
++{
++ public: GazeboData head;
++
++ /// \brief Current command for the gripper
++ public: int cmd;
++
++ /// Current state of the gripper
++ public: int state;
++
++ /// Gripped limit reached flag
++ public: int grip_limit_reach;
++
++ /// Lift limit reached flag
++ public: int lift_limit_reach;
++
++ /// Outer beam obstruct flag
++ public: int outer_beam_obstruct;
++
++ /// control mode, TODO: yet to be defined
++ public: int controlMode;
++
++ /// Inner beam obstructed flag
++ public: int inner_beam_obstruct;
++
++ /// Left paddle open flag
++ public: int left_paddle_open;
++
++ /// Right paddle open flag
++ public: int right_paddle_open;
++
++ /// Lift up flag
++ public: int lift_up;
++
++ /// Lift down flag
++ public: int lift_down;
++
++ /// Enable flag
++ public: int cmdEnableMotor;
++
++ /// Gripper Position Rate Command
++ public: float cmdPositionRate;
++ /// Gripper Force Command
++ public: float cmdForce;
++ /// Gripper Gap Command
++ public: float cmdGap;
++
++ /// Gripper force
++ public: double gripperForce;
++ /// Gripper controller gains
++ public: double pGain;
++ public: double dGain;
++ public: double iGain;
++
++ /// Position
++ public: float actualFingerPosition[2];
++ /// Position Rate
++ public: float actualFingerPositionRate[2];
++
++
++};
++
++/// \brief Gripper interface
++class PR2GripperIface : public Iface
++{
++ /// \brief Constructor
++ public: PR2GripperIface():Iface("pr2gripper", sizeof(PR2GripperIface)+sizeof(PR2GripperData)) {}
++
++ /// \brief Destructor
++ public: virtual ~PR2GripperIface() {this->data = NULL;}
++
++ /// \brief Create the server
++ public: virtual void Create(Server *server, std::string id)
++ {
++ Iface::Create(server,id);
++ this->data = (PR2GripperData*)this->mMap;
++ }
++
++ /// \brief Open the iface
++ public: virtual void Open(Client *client, std::string id)
++ {
++ Iface::Open(client,id);
++ this->data = (PR2GripperData*)this->mMap;
++ }
++
++ /// Pointer to the gripper data
++ public: PR2GripperData *data;
++};
++
++/** \} */
++/// \}
++
++
++/***************************************************************************/
++/// \addtogroup libgazebo_iface
++/// \{
++/** \defgroup pr2array_iface actarray
++
++ \brief PR2 Array
++
++The PR2 array interface allows a user to control a set of actuators.
++
++\{
++*/
++
++/// maximum number of actuators
++#define GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS 64
++#define GAZEBO_PR2ARRAY_JOINT_POSITION_MODE 0
++#define GAZEBO_PR2ARRAY_JOINT_SPEED_MODE 1
++#define GAZEBO_PR2ARRAY_JOINT_CURRENT_MODE 2
++
++//Actuator states
++/// Idle state
++#define GAZEBO_PR2ARRAY_ACTSTATE_IDLE 1
++
++/// Moving state
++#define GAZEBO_PR2ARRAY_ACTSTATE_MOVING 2
++
++/// Braked state
++#define GAZEBO_PR2ARRAY_ACTSTATE_BRAKED 3
++
++/// Stalled state
++#define GAZEBO_PR2ARRAY_ACTSTATE_STALLED 4
++
++/// Linear type
++#define GAZEBO_PR2ARRAY_TYPE_LINEAR 1
++/// Rotary type
++#define GAZEBO_PR2ARRAY_TYPE_ROTARY 2
++
++/// Request subtype: power
++#define GAZEBO_PR2ARRAY_POWER_REQ 1
++/// Request subtype: brakes
++#define GAZEBO_PR2ARRAY_BRAKES_REQ 2
++/// Request subtype: get geometry
++#define GAZEBO_PR2ARRAY_GET_GEOM_REQ 3
++/// Request subtype: speed
++#define GAZEBO_PR2ARRAY_SPEED_REQ 4
++
++/// Command subtype: position
++#define GAZEBO_PR2ARRAY_POS_CMD 1
++/// Command subtype: speed
++#define GAZEBO_PR2ARRAY_SPEED_CMD 2
++/// Command subtype: home
++#define GAZEBO_PR2ARRAY_HOME_CMD 3
++
++
++/// \brief Actuator geometry
++class PR2ArrayActuatorGeom
++{
++
++/// Data subtype: state
++#define GAZEBO_PR2ARRAY_DATA_STATE 1
++
++
++ /// The type of the actuator - linear or rotary.
++ public: uint8_t type;
++
++ /// Min range of motion (m or rad depending on the type)
++ public: float min;
++
++ /// Center position (m or rad)
++ public: float center;
++
++ /// Max range of motion (m or rad depending on the type)
++ public: float max;
++
++ /// Home position (m or rad depending on the type)
++ public: float home;
++
++ /// The configured speed - different from current speed.
++ public: float config_speed;
++
++ /// The maximum achievable speed of the actuator.
++ public: float max_speed;
++
++ /// If the actuator has brakes or not.
++ public: uint8_t hasbrakes;
++};
++
++/// \brief Structure containing a single actuator's information
++class PR2ArrayActuator
++{
++ /// The position of the actuator in m or rad depending on the type.
++ public: float actualPosition;
++ /// The speed of the actuator in m/s or rad/s depending on the type.
++ public: float actualSpeed;
++ /// The current state of the actuator.
++ public: uint8_t state;
++};
++
++ typedef struct
++ {
++ double timestamp;
++ double actualPosition;
++ double actualSpeed;
++ double actualEffectorForce;
++
++ int controlMode;
++ int jointType;
++
++ double cmdPosition;
++ double cmdSpeed;
++ double cmdEffectorForce;
++
++ int cmdEnableMotor;
++
++ double pGain;
++ double iGain;
++ double dGain;
++ double iClamp;
++ double saturationTorque;
++ } JointData;
++
++/// \brief The actuator array data packet.
++class PR2ArrayData
++{
++ public: GazeboData head;
++
++ /// The number of actuators in the array.
++ public: unsigned int actuators_count;
++
++ /// The actuator data.
++// public: PR2ArrayActuator actuators[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
++ public: JointData actuators[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
++
++ /// The actuators geoms
++ public: PR2ArrayActuatorGeom actuator_geoms[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
++
++ /// position commands
++ public: float cmd_pos[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
++
++ /// speed commands
++ public: float cmd_speed[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
++
++ /// bad command flag - (speed to high set for the actuators or position not reachable)
++ public: int bad_cmd;
++
++ /// True if new command
++ public: bool new_cmd;
++
++ /// position / speed comand
++ public: unsigned int joint_mode[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
++
++};
++
++/// \brief The PR2Array interface
++class PR2ArrayIface : public Iface
++{
++ /// \brief Create an interface
++ public: PR2ArrayIface():Iface("pr2array", sizeof(PR2ArrayIface)+sizeof(PR2ArrayData)) {}
++
++ /// \brief Destroy and Interface
++ public: virtual ~PR2ArrayIface() {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 = (PR2ArrayData*)this->mMap;
++ }
++
++ /// \brief Open an existing interface
++ /// \param client Pointer to the client
++ /// \param id Id of the interface
++ publi...
[truncated message content] |
