Hi,
I was about to start work on adding actarray support to gazebo when it
suddenly arrived, excellent timing :) I have put together a few extra
features, mainly a wx panel for controlling an actarray. Its not 100%
but it is sufficient to get the basics done, since I will be unable to
put time into it for the next couple of weeks I though I would post the
patch, its attached as a diff and the new file for the panels directory.
Hopefully its of use to someone, possibly there is some swig work to
make it run a bit nicer, I had a bit of a look at it, but Im not that
familiar with swig.
Toby Collett
Index: gui/wx/main.py
===================================================================
RCS file: /cvsroot/playerstage/code/gazebo/gui/wx/main.py,v
retrieving revision 1.17
diff -u -r1.17 main.py
--- gui/wx/main.py 17 Nov 2004 19:01:12 -0000 1.17
+++ gui/wx/main.py 7 Nov 2006 08:04:23 -0000
@@ -30,6 +30,7 @@
# Map from descriptive interface types to panel classes
panelmap = \
{
+ 'actarray' : gzActarray,
'camera' : gzCamera,
'gps' : gzGps,
'guicam' : gzGuicam,
Index: gui/wx/panels/Makefile.am
===================================================================
RCS file: /cvsroot/playerstage/code/gazebo/gui/wx/panels/Makefile.am,v
retrieving revision 1.4
diff -u -r1.4 Makefile.am
--- gui/wx/panels/Makefile.am 17 Nov 2004 16:36:04 -0000 1.4
+++ gui/wx/panels/Makefile.am 7 Nov 2006 08:04:23 -0000
@@ -1,5 +1,5 @@
# Files that go in the distribution
-EXTRA_DIST = __init__.py camera.py gps.py guicam.py laser.py position.py
power.py ptz.py sim.py sonar.py stereo.py
+EXTRA_DIST = __init__.py actarray.py camera.py gps.py guicam.py laser.py
position.py power.py ptz.py sim.py sonar.py stereo.py
Index: gui/wx/panels/__init__.py
===================================================================
RCS file: /cvsroot/playerstage/code/gazebo/gui/wx/panels/__init__.py,v
retrieving revision 1.9
diff -u -r1.9 __init__.py
--- gui/wx/panels/__init__.py 5 Nov 2004 08:02:33 -0000 1.9
+++ gui/wx/panels/__init__.py 7 Nov 2006 08:04:23 -0000
@@ -1,6 +1,7 @@
# All supported panels
+from actarray import *
from camera import *
from guicam import *
from gps import *
Index: libgazebo/gazebo.h
===================================================================
RCS file: /cvsroot/playerstage/code/gazebo/libgazebo/gazebo.h,v
retrieving revision 1.87
diff -u -r1.87 gazebo.h
--- libgazebo/gazebo.h 2 Nov 2006 17:06:31 -0000 1.87
+++ libgazebo/gazebo.h 7 Nov 2006 08:04:26 -0000
@@ -306,6 +306,75 @@
/// @}
+/** @ingroup message_basics
+ * @defgroup utility_structs General-purpose data structures.
+ * These structures often appear inside other structures.
+ * @{ */
+
+/** @brief A point in the plane */
+typedef struct gazebo_point_2d
+{
+ /** X [m] */
+ float px;
+ /** Y [m] */
+ float py;
+} gazebo_point_2d_t;
+
+
+/** @brief A point in 3D space */
+typedef struct gazebo_point_3d
+{
+ /** X [m] */
+ float px;
+ /** Y [m] */
+ float py;
+ /** Z [m] */
+ float pz;
+} gazebo_point_3d_t;
+
+
+/** @brief An angle in 3D space */
+typedef struct gazebo_orientation_3d
+{
+ /** roll [rad] */
+ float proll;
+ /** pitch [rad] */
+ float ppitch;
+ /** yaw [rad] */
+ float pyaw;
+} gazebo_orientation_3d_t;
+
+
+/** @brief A pose in the plane */
+typedef struct gazebo_pose
+{
+ /** X [m] */
+ float px;
+ /** Y [m] */
+ float py;
+ /** yaw [rad] */
+ float pa;
+} gazebo_pose_t;
+
+/** @brief A pose in space */
+typedef struct gazebo_pose3d
+{
+ /** X [m] */
+ float px;
+ /** Y [m] */
+ float py;
+ /** Z [m] */
+ float pz;
+ /** roll [rad] */
+ float proll;
+ /** pitch [rad] */
+ float ppitch;
+ /** yaw [rad] */
+ float pyaw;
+} gazebo_pose3d_t;
+
+/** @} */
+
/***************************************************************************/
/// @addtogroup libgazebo
/// @{
@@ -1333,6 +1402,11 @@
/** Stalled state code */
#define GAZEBO_ACTARRAY_ACTSTATE_STALLED 4
+/** Linear type code */
+#define GAZEBO_ACTARRAY_TYPE_LINEAR 1
+/** Rotary type code */
+#define GAZEBO_ACTARRAY_TYPE_ROTARY 2
+
/** @brief Structure containing a single actuator's information */
typedef struct gazebo_actarray_actuator
@@ -1345,6 +1419,39 @@
uint8_t state;
} gazebo_actarray_actuator_t;
+
+/** @brief Actuator geometry */
+typedef struct gazebo_actarray_actuatorgeom
+{
+ /** The type of the actuator - linear or rotary. */
+ uint8_t type;
+ /** The length of this actuator's link to the next actuator. For linear
+ actuators, this should be its length when at 0 position. */
+ float length;
+ /** The orientation of this actuator when it is in its rest position. When
+ combined with the length of the actuator's link, this will give the position
+ in space of the next actuator in the array in the coordinate space of this
+ actuator (i.e., it is the direction to the next actuator). */
+ gazebo_orientation_3d_t orientation;
+ /** The axis of rotation for this actuator if it is rotary, or axis along
+ which it moves if it is linear. In both cases, it is a vector. */
+ gazebo_point_3d_t axis;
+ /** The range of motion of the actuator, in m or rad depending on the type.
*/
+ float min;
+ /** The range of motion of the actuator, in m or rad depending on the type.
*/
+ float centre;
+ /** The range of motion of the actuator, in m or rad depending on the type.
*/
+ float max;
+ /** The range of motion of the actuator, in m or rad depending on the type.
*/
+ float home;
+ /** The configured speed setting of the actuator - different from current
speed. */
+ float config_speed;
+ /** If the actuator has brakes or not. */
+ uint8_t hasbrakes;
+} gazebo_actarray_actuatorgeom_t;
+
+
+
//TODO: Extend this for the complete interface
/// Actarray interface
typedef struct gz_actarray_data
@@ -1359,14 +1466,14 @@
unsigned int new_cmd;
/** The number of actuators in the array. */
- uint32_t actuators_count;
+ unsigned int actuators_count;
/** The actuator data and geometry. */
gazebo_actarray_actuator_t actuators_data[GAZEBO_ACTARRAY_NUM_ACTUATORS];
- //gazebo_actarray_actuatorgeom_t
actuators_geom[GAZEBO_ACTARRAY_NUM_ACTUATORS];
+ gazebo_actarray_actuatorgeom_t actuators_geom[GAZEBO_ACTARRAY_NUM_ACTUATORS];
/** The position of the base of the actarray. */
- //gazebo_point_3d_t base_pos;
+ gazebo_point_3d_t base_pos;
/** The orientation of the base of the actarray. */
- //gazebo_orientation_3d_t base_orientation;
+ gazebo_orientation_3d_t base_orientation;
float cmd_pos[GAZEBO_ACTARRAY_NUM_ACTUATORS];
float cmd_speed[GAZEBO_ACTARRAY_NUM_ACTUATORS];
float cmd_current[GAZEBO_ACTARRAY_NUM_ACTUATORS];
@@ -1419,6 +1526,12 @@
/// Tell clients that new data is available
extern int gz_actarray_post(gz_actarray_t *self);
+/// Set a cmd pos value
+extern void gz_actarray_set_cmd_pos(gz_actarray_t *self, unsigned int index,
double value);
+
+extern gazebo_actarray_actuatorgeom_t gz_actarray_get_geom(gz_actarray_t
*self, unsigned int index);
+
+extern gazebo_actarray_actuator_t gz_actarray_get_data(gz_actarray_t *self,
unsigned int index);
/** @} */
/// @}
Index: libgazebo/gz_actarray.c
===================================================================
RCS file: /cvsroot/playerstage/code/gazebo/libgazebo/gz_actarray.c,v
retrieving revision 1.1
diff -u -r1.1 gz_actarray.c
--- libgazebo/gz_actarray.c 2 Nov 2006 17:06:31 -0000 1.1
+++ libgazebo/gz_actarray.c 7 Nov 2006 08:04:26 -0000
@@ -116,3 +116,18 @@
{
return gz_iface_post(self->iface);
}
+
+void gz_actarray_set_cmd_pos(gz_actarray_t *self, unsigned int index, double
value)
+{
+ self->data->cmd_pos[index] = value;
+}
+
+gazebo_actarray_actuatorgeom_t gz_actarray_get_geom(gz_actarray_t *self,
unsigned int index)
+{
+ return self->data->actuators_geom[index];
+}
+
+gazebo_actarray_actuator_t gz_actarray_get_data(gz_actarray_t *self, unsigned
int index)
+{
+ return self->data->actuators_data[index];
+}
Index: player/Makefile.am
===================================================================
RCS file: /cvsroot/playerstage/code/gazebo/player/Makefile.am,v
retrieving revision 1.10
diff -u -r1.10 Makefile.am
--- player/Makefile.am 2 Nov 2006 19:52:57 -0000 1.10
+++ player/Makefile.am 7 Nov 2006 08:04:27 -0000
@@ -21,7 +21,7 @@
libgazeboplugin_la_CPPFLAGS = $(AM_CPPFLAGS) @PLAYER_CFLAGS@ -I../libgazebo
#libgazeboplugin_la_LDFLAGS = -L../libgazebo
-libgazeboplugin_la_LIBADD = @PLAYER_LIBS@ -lgazebo
+libgazeboplugin_la_LIBADD = @PLAYER_LIBS@ -L../libgazebo -lgazebo
EXTRA_DIST = gazebo.cfg
endif
# Desc: actarray interface handler
# Author: Andrew Howard
# Date: 19 Sep 2004
# CVS: $Id: position.py,v 1.7 2006/07/07 17:57:37 natepak Exp $
import sys
import math
import time
from wxPython.wx import *
from wxgazebo.panel import gzPanel
from gazebo import *
# Convert radians to degrees
def rtod(a):
return a / math.pi * 180
# IDs
ID_HOME = 0x100
class gzActarray(gzPanel):
"""Mediate actarray interface."""
def __init__(self, parent, client, gzid):
gzPanel.__init__(self, parent, 'actarray [%s]' % gzid, (0, 0))
# Open interface
self.iface = gz_actarray()
if self.iface.open(client, gzid) != 0:
raise gz_error_str()
self.updateTime = 0.0
# Home button
self.stopButton = wxButton(self, ID_HOME, 'Home')
EVT_BUTTON(self, ID_HOME, self.OnHome)
data = self.iface.data
# Layout stuff in window
sizer = wxFlexGridSizer(4, 4, 2, 4)
# Linear speed sliders
self.actSliders = []
self.limits = []
self.values = []
self.homes = []
for i in range(self.iface.data.actuators_count):
geom = self.iface.get_geom(i)
act_data = self.iface.get_data(i)
self.limits.append((geom.min,geom.max))
self.homes.append(geom.home)
self.actSliders.append(wxSlider(self, -1, act_data.position, 0,1000,wxDefaultPosition, (200, -1)))
self.actSliders[i].SetValue(1000*(act_data.position-self.limits[i][0])/(self.limits[i][1] - self.limits[i][0]))
sizer.Add(wxStaticText(self, -1, str(i)))
if (geom.type == GAZEBO_ACTARRAY_TYPE_ROTARY):
sizer.Add(wxStaticText(self, -1, 'R'))
else:
sizer.Add(wxStaticText(self, -1, 'L'))
self.values.append(wxStaticText(self, -1, '%4.2f' % geom.home))
sizer.Add(self.values[i],-1)
sizer.Add(self.actSliders[i], 0, wxEXPAND)
sizer.Add((0, 0))
sizer.Add((self.stopButton), 0, wxEXPAND)
# All done; set frame size
self.SetSizer(sizer)
self.SetAutoLayout(True)
self.Fit()
sizer.SetSizeHints(self)
# We're ready, so show ourselves
self.Show(True)
return
def __del__(self):
# Close the interface
self.iface.close()
return
def OnUpdate(self):
"""Process periodic updates."""
self.iface.lock(1)
data = self.iface.data
## See if we have new data
if data.time != self.updateTime:
self.updateTime = data.time
for i in range(self.iface.data.actuators_count):
act_data = self.iface.get_data(i)
self.values[i].SetLabel("%4.2f" % act_data.position)
## Update velocity commands
for i in range(self.iface.data.actuators_count):
self.iface.set_cmd_pos(i,(self.limits[i][1] - self.limits[i][0])*self.actSliders[i].GetValue()/1000.0 + self.limits[i][0])
self.iface.unlock()
return
def OnHome(self, event):
i = 0
for slider in self.actSliders:
slider.SetValue(1000*(self.homes[i]-self.limits[i][0])/(self.limits[i][1] - self.limits[i][0]))
i+=1
return
-------------------------------------------------------------------------
Using Tomcat but need to do more? Need to support web services, security?
Get stuff done quickly with pre-integrated technology to make your job easier
Download IBM WebSphere Application Server v.1.0.1 based on Apache Geronimo
http://sel.as-us.falkag.net/sel?cmd=lnk&kid=120709&bid=263057&dat=121642
_______________________________________________
Playerstage-gazebo mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-gazebo
