diff -u -r1.26 Makefile.am
--- libgazebo/Makefile.am	31 Aug 2005 23:06:00 -0000	1.26
+++ libgazebo/Makefile.am	26 Sep 2006 12:03:27 -0000
@@ -2,7 +2,7 @@
 
 AM_CPPFLAGS = -Wall -I$(top_srcdir)/replace
 
-libgazebo_a_SOURCES = gazebo.h gz_error.h gz_error.c gz_server.c gz_client.c gz_iface.c gz_sim.c gz_camera.c gz_factory.c gz_fiducial.c gz_gripper.c gz_gps.c gz_guicam.c gz_laser.c gz_position.c gz_power.c gz_ptz.c gz_sonar.c gz_stereo.c gz_truth.c gz_wifi.c gz_joint.c
+libgazebo_a_SOURCES = gazebo.h gz_error.h gz_error.c gz_server.c gz_client.c gz_iface.c gz_sim.c gz_camera.c gz_factory.c gz_fiducial.c gz_gripper.c gz_gps.c gz_guicam.c gz_laser.c gz_position.c gz_power.c gz_ptz.c gz_sonar.c gz_stereo.c gz_truth.c gz_wifi.c gz_joint.c gz_actarray.c
 
 lib_LIBRARIES = libgazebo.a
 include_HEADERS = gazebo.h
Index: libgazebo/gazebo.h
===================================================================
RCS file: /cvsroot/playerstage/code/gazebo/libgazebo/gazebo.h,v
retrieving revision 1.85
diff -u -r1.85 gazebo.h
--- libgazebo/gazebo.h	3 Mar 2006 21:11:04 -0000	1.85
+++ libgazebo/gazebo.h	26 Sep 2006 12:03:27 -0000
@@ -1291,6 +1291,125 @@
 /***************************************************************************/
 /// @addtogroup libgazebo
 /// @{
+/** @defgroup actarray actarray
+
+The actarray interface allows clients to actarray devices like robotic manipulators
+
+
+@{
+*/
+
+#define GAZEBO_ACTARRAY_NUM_ACTUATORS 16
+#define GAZEBO_ACTARRAY_JOINT_POSITION_MODE 0
+#define GAZEBO_ACTARRAY_JOINT_SPEED_MODE 1
+#define GAZEBO_ACTARRAY_JOINT_CURRENT_MODE 2
+
+/** Idle state code */
+#define GAZEBO_ACTARRAY_ACTSTATE_IDLE     1
+/** Moving state code */
+#define GAZEBO_ACTARRAY_ACTSTATE_MOVING   2
+/** Braked state code */
+#define GAZEBO_ACTARRAY_ACTSTATE_BRAKED   3
+/** Stalled state code */
+#define GAZEBO_ACTARRAY_ACTSTATE_STALLED  4
+
+
+/** @brief Structure containing a single actuator's information */
+typedef struct gazebo_actarray_actuator
+{
+  /** The position of the actuator in m or rad depending on the type. */
+  float position;
+  /** The speed of the actuator in m/s or rad/s depending on the type. */
+  float speed;
+  /** The current state of the actuator. */
+  uint8_t state;
+} gazebo_actarray_actuator_t;
+
+//TODO: Extend this for the complete interface
+/// Actarray interface
+typedef struct gz_actarray_data
+{
+  /// Common data structure
+  gz_data_t head;
+  
+  /// Data timestamp
+  double time;
+  
+  // Timestamp for commands;
+  unsigned int new_cmd;
+
+  /** The number of actuators in the array. */
+  uint32_t 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];
+  /** The position of the base of the actarray. */
+  //gazebo_point_3d_t base_pos;
+  /** The orientation of the base of the actarray. */
+  //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];
+   unsigned int joint_mode[GAZEBO_ACTARRAY_NUM_ACTUATORS];  //Position/Speed/Current mode
+
+} gz_actarray_data_t;
+
+
+/// The actarray interface
+typedef struct gz_actarray
+{
+  /// Common data structure
+  gz_data_t head;
+  
+  /// General interface info
+  gz_iface_t *iface;
+
+  /// Pointer to interface data area
+  gz_actarray_data_t *data;
+  
+} gz_actarray_t;
+
+
+/// Create an interface
+extern gz_actarray_t *gz_actarray_alloc();
+
+/// Destroy an interface
+extern void gz_actarray_free(gz_actarray_t *self);
+
+/// @internal Create the interface (used by Gazebo server)
+extern int gz_actarray_create(gz_actarray_t *self, gz_server_t *server, const char *id,
+                         const char *model_type, int model_id, int parent_model_id);
+
+/// @internal Destroy the interface (server)
+extern int gz_actarray_destroy(gz_actarray_t *self);
+
+/// Open an existing interface (used by Gazebo clients)
+extern int gz_actarray_open(gz_actarray_t *self, gz_client_t *client, const char *id);
+
+/// Close the interface (client)
+extern int gz_actarray_close(gz_actarray_t *self);
+
+/// Lock the interface.  Set blocking to 1 if the caller should block
+/// until the lock is acquired.  Returns 0 if the lock is acquired.
+extern int gz_actarray_lock(gz_actarray_t *self, int blocking);
+
+/// Unlock the interface
+extern void gz_actarray_unlock(gz_actarray_t *self);
+
+/// Tell clients that new data is available
+extern int gz_actarray_post(gz_actarray_t *self);
+
+
+/** @} */
+/// @}
+
+
+
+
+
+/***************************************************************************/
+/// @addtogroup libgazebo
+/// @{
 /** @defgroup sonar sonar
 
 The sonar interface allows the positions and range values for sonars
Index: libgazebo/gz_actarray.c
===================================================================
RCS file: libgazebo/gz_actarray.c
diff -N libgazebo/gz_actarray.c
--- /dev/null	1 Jan 1970 00:00:00 -0000
+++ libgazebo/gz_actarray.c	26 Sep 2006 12:03:27 -0000
@@ -0,0 +1,118 @@
+/*
+ *  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: Actarray interface
+ * Author: Alexis Maldonado
+ * Date: 21 September 2006
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+#include "gazebo.h"
+
+
+// Create an interface
+gz_actarray_t *gz_actarray_alloc()
+{
+  gz_actarray_t *self;
+
+  self = calloc(1, sizeof(gz_actarray_t));
+  self->iface = gz_iface_alloc();
+
+  return self;
+}
+
+
+// Destroy an interface
+void gz_actarray_free(gz_actarray_t *self)
+{
+  gz_iface_free(self->iface);
+  free(self);
+  return;
+}
+
+
+// Create the interface
+int gz_actarray_create(gz_actarray_t *self, gz_server_t *server, const char *id,
+                  const char *model_type, int model_id, int parent_model_id)
+{
+  if (gz_iface_create(self->iface, server, "actarray", id, sizeof(gz_actarray_data_t)) != 0)
+    return -1;
+
+  self->data = self->iface->mmap;
+
+  strncpy(self->data->head.model_type, model_type, sizeof(self->data->head.model_type));
+  self->data->head.model_id = (int) model_id;
+  self->data->head.parent_model_id = (int) parent_model_id;
+  
+  return 0;
+}
+
+
+// Destroy the interface (server)
+int gz_actarray_destroy(gz_actarray_t *self)
+{
+  self->data = NULL;
+  return gz_iface_destroy(self->iface);
+}
+
+
+// Open an existing interface
+int gz_actarray_open(gz_actarray_t *self, gz_client_t *client, const char *id)
+{
+  if (gz_iface_open(self->iface, client, "actarray", id, sizeof(gz_actarray_data_t)) != 0)
+    return -1;
+
+  self->data = self->iface->mmap;
+  return 0;
+}  
+
+
+// Close the interface
+int gz_actarray_close(gz_actarray_t *self)
+{
+  self->data = NULL;
+  return gz_iface_close(self->iface);
+}
+
+
+// Lock the interface.
+int gz_actarray_lock(gz_actarray_t *self, int blocking)
+{
+  return gz_iface_lock(self->iface, blocking);
+}
+
+
+// Unlock the interface
+void gz_actarray_unlock(gz_actarray_t *self)
+{
+  return gz_iface_unlock(self->iface);
+}
+
+
+// Tell clients that new data is available
+int gz_actarray_post(gz_actarray_t *self)
+{
+  return gz_iface_post(self->iface);
+}
Index: player/ActarrayInterface.cc
===================================================================
RCS file: player/ActarrayInterface.cc
diff -N player/ActarrayInterface.cc
--- /dev/null	1 Jan 1970 00:00:00 -0000
+++ player/ActarrayInterface.cc	26 Sep 2006 12:03:27 -0000
@@ -0,0 +1,242 @@
+/*
+ *  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: Actarray Interface for Player-Gazebo
+ * Author: Alexis Maldonado
+ * Date: 19 September 2006
+ */
+
+/**
+@addtogroup player
+@par Actarray Interface
+- PLAYER_ACTARRAY_DATA_STATE
+*/
+
+
+#include <math.h>
+
+#include "gazebo.h"
+#include "GazeboDriver.hh"
+#include "ActarrayInterface.hh"
+
+//
+// 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, ...
+//  variables and sets new_cmd to 1. The gazebo model can look at this variable to find out if a new command came
+// Since joints in gazebo only support velocity control, the model has to implement at least a P controller
+//  to implement position commands.
+// The model should support position and velocity commands. 
+// Home commands, brake requests, and power requests are simply translated to a sensible meaning in velocity or position commands.
+// If the models need it, this interface should be extended.
+
+
+///////////////////////////////////////////////////////////////////////////////
+// Constructor
+ActarrayInterface::ActarrayInterface(player_devaddr_t addr,
+                                     GazeboDriver *driver, ConfigFile *cf, int section)
+		: GazeboInterface(addr, driver, cf, section) {
+	// Get the ID of the interface
+	gz_id = (char*) calloc(1024, sizeof(char));
+	strcat(gz_id, GazeboClient::prefixId);
+	strcat(gz_id, cf->ReadString(section, "gz_id", ""));
+
+	// Allocate a Position Interface
+	iface = gz_actarray_alloc();
+
+	datatime = -1;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Destructor
+ActarrayInterface::~ActarrayInterface() {
+	// Release this interface
+	gz_actarray_free(iface);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////
+// Handle all messages. This is called from GazeboDriver
+int ActarrayInterface::ProcessMessage(MessageQueue *respQueue,
+                                      player_msghdr_t *hdr, void *data) {
+
+	//printf("ProcessMessage.\n");
+
+	if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
+	                          PLAYER_ACTARRAY_POS_CMD, device_addr)) {
+
+		assert(hdr->size >= sizeof(player_actarray_position_cmd_t));
+
+		player_actarray_position_cmd_t *cmd;
+		cmd = (player_actarray_position_cmd_t*) data;
+
+		gz_actarray_lock(iface, 1);
+		iface->data->cmd_pos[cmd->joint]=cmd->position;
+		iface->data->joint_mode[cmd->joint]=GAZEBO_ACTARRAY_JOINT_POSITION_MODE;
+		iface->data->new_cmd=1;
+		gz_actarray_unlock(iface);
+
+		return(0);
+
+	} else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
+	                                 PLAYER_ACTARRAY_SPEED_CMD, device_addr) ) {
+
+		assert(hdr->size >= sizeof(player_actarray_speed_cmd_t));
+
+		player_actarray_speed_cmd_t *cmd;
+		cmd = (player_actarray_speed_cmd_t*) data;
+
+		gz_actarray_lock(iface, 1);
+		iface->data->cmd_speed[cmd->joint]=cmd->speed;
+		iface->data->joint_mode[cmd->joint]=GAZEBO_ACTARRAY_JOINT_SPEED_MODE;
+
+		iface->data->new_cmd=1;
+		gz_actarray_unlock(iface);
+		return(0);
+
+
+	} else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
+	                                 PLAYER_ACTARRAY_HOME_CMD, device_addr)) {
+		assert(hdr->size >= sizeof(player_actarray_home_cmd_t));
+		player_actarray_home_cmd_t *cmd;
+		cmd = (player_actarray_home_cmd_t*) data;
+
+		gz_actarray_lock(iface, 1);
+
+		iface->data->cmd_pos[cmd->joint]=0.0;
+		iface->data->joint_mode[cmd->joint]=GAZEBO_ACTARRAY_JOINT_POSITION_MODE;
+		iface->data->new_cmd=1;
+		gz_actarray_unlock(iface);
+		return(0);
+
+	}  else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_ACTARRAY_BRAKES_REQ, device_addr)) {  // Is it a request for actarray geometry?
+
+		assert(hdr->size >= sizeof(player_actarray_brakes_config_t));
+		player_actarray_brakes_config_t *req;
+		req = (player_actarray_brakes_config_t*) data;
+
+		//If brakes=on -> Stop all the joints. If they are off, don't do anything
+		if (req->value == 1) {
+			gz_actarray_lock(iface, 1);
+
+			for (unsigned int i = 0 ; i != GAZEBO_ACTARRAY_NUM_ACTUATORS ; ++i) {
+				iface->data->cmd_speed[i]=0.0;
+				iface->data->joint_mode[i]=GAZEBO_ACTARRAY_JOINT_SPEED_MODE;
+				iface->data->new_cmd=1;
+
+			}
+			gz_actarray_unlock(iface);
+		}
+
+
+		driver->Publish(device_addr, respQueue,
+		                PLAYER_MSGTYPE_RESP_ACK, PLAYER_ACTARRAY_BRAKES_REQ);
+		return(0);
+	} else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_ACTARRAY_POWER_REQ, device_addr)) {  // Is it a request for actarray geometry?
+
+		assert(hdr->size >= sizeof(player_actarray_power_config_t));
+		player_actarray_power_config_t *req;
+		req = (player_actarray_power_config_t*) data;
+
+		//If power=off -> Stop all the joints. If power=on, don't do anything
+		if (req->value == 0) {
+			gz_actarray_lock(iface, 1);
+
+			for (unsigned int i = 0 ; i != GAZEBO_ACTARRAY_NUM_ACTUATORS ; ++i) {
+				iface->data->cmd_speed[i]=0.0;
+				iface->data->joint_mode[i]=GAZEBO_ACTARRAY_JOINT_SPEED_MODE;
+				iface->data->new_cmd=1;
+
+			}
+			gz_actarray_unlock(iface);
+		}
+
+
+		driver->Publish(device_addr, respQueue,
+		                PLAYER_MSGTYPE_RESP_ACK, PLAYER_ACTARRAY_POWER_REQ);
+		return(0);
+	}
+
+
+
+	return -1; //This indicates that the message wasn't processed
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Update this interface, publish new info. This is
+// called from GazeboDriver::Update
+void ActarrayInterface::Update() {
+	player_actarray_data_t data;
+	memset(&data, 0, sizeof(data));
+
+	struct timeval ts;
+
+	gz_actarray_lock(iface, 1);
+
+	// Only Update when new data is present
+	if (iface->data->time > datatime) {
+
+		//Update the local time so we know when new info comes
+		datatime = iface->data->time;
+
+		// The number of actuators in the array.
+		data.actuators_count=iface->data->actuators_count;
+
+		for (unsigned int i=0; i<data.actuators_count ; ++i ) {
+
+			float pos=iface->data->actuators_data[i].position;
+			float speed=iface->data->actuators_data[i].speed;
+			//float current=iface->data->actuators_data[i].current;
+			uint8_t report_state=iface->data->actuators_data[i].state;
+			data.actuators[i].position=pos;
+			data.actuators[i].speed=speed;
+			//data.actuators[i].current=current;
+			data.actuators[i].state=report_state;
+		}
+
+
+		driver->Publish( device_addr, NULL,
+		                 PLAYER_MSGTYPE_DATA,
+		                 PLAYER_ACTARRAY_DATA_STATE,
+		                 (void*)&data, sizeof(data), &datatime );
+
+
+	}
+
+	gz_actarray_unlock(iface);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////
+// Open a SHM interface when a subscription is received. This is called from
+// GazeboDriver::Subscribe
+void ActarrayInterface::Subscribe() {
+	// Open the interface
+	if (gz_actarray_open(iface, GazeboClient::client, gz_id) != 0) {
+		printf("Error Subscribing to Gazebo Actarray Interface\n");
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Close a SHM interface. This is called from GazeboDriver::Unsubscribe
+void ActarrayInterface::Unsubscribe() {
+	gz_actarray_close(iface);
+}
+
+
Index: player/ActarrayInterface.hh
===================================================================
RCS file: player/ActarrayInterface.hh
diff -N player/ActarrayInterface.hh
--- /dev/null	1 Jan 1970 00:00:00 -0000
+++ player/ActarrayInterface.hh	26 Sep 2006 12:03:27 -0000
@@ -0,0 +1,66 @@
+/*
+ *  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: Actarray Interface for Player
+ * Author: Alexis Maldonado
+ * Date: 19 September 2006
+ */
+
+#ifndef PTZINTERFACE_HH
+#define PTXINTERFACE_HH
+
+#include "GazeboInterface.hh"
+
+// Forward declarations
+typedef struct gz_actarray gz_actarray_t;
+
+class ActarrayInterface : public GazeboInterface
+{
+  /// @brief Constructor
+  public: ActarrayInterface(player_devaddr_t addr, GazeboDriver *driver,
+                              ConfigFile *cf, int section);
+
+  /// @brief Destructor
+  public: virtual ~ActarrayInterface();
+
+  /// @brief Handle all messages. This is called from GazeboDriver
+  public: virtual int ProcessMessage(MessageQueue *respQueue,
+                                     player_msghdr_t *hdr, void *data);
+
+  /// @brief Update this interface, publish new info.
+  public: virtual void Update();
+
+  /// @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 \
+  ///        GazeboDriver::Unsubscribe
+  public: virtual void Unsubscribe();
+
+  private: gz_actarray_t *iface;
+
+  /// @brief Gazebo id. This needs to match and ID in a Gazebo WorldFile
+  private: char *gz_id;
+
+  /// @brief Timestamp on last data update
+  private: double datatime;
+};
+#endif
Index: player/GazeboDriver.cc
===================================================================
RCS file: /cvsroot/playerstage/code/gazebo/player/GazeboDriver.cc,v
retrieving revision 1.7
diff -u -r1.7 GazeboDriver.cc
--- player/GazeboDriver.cc	7 Mar 2006 21:11:57 -0000	1.7
+++ player/GazeboDriver.cc	26 Sep 2006 12:03:27 -0000
@@ -47,6 +47,7 @@
 #include "GripperInterface.hh"
 #include "CameraInterface.hh"
 #include "TruthInterface.hh"
+#include "ActarrayInterface.hh"
 
 #include "GazeboDriver.hh"
 
@@ -297,6 +298,12 @@
         if (!player_quiet_startup) printf(" a truth interface.\n");
         ifsrc = new TruthInterface( playerAddr,  this, cf, section );
         break;
+		
+      case PLAYER_ACTARRAY_CODE:
+        if (!player_quiet_startup) printf(" an actarray interface.\n");
+        ifsrc = new ActarrayInterface( playerAddr,  this, cf, section );
+        break;
+			
 
       default:
         printf( "nerror: Gazebo driver doesn't support interface type %d\n",
Index: player/GripperInterface.cc
===================================================================
RCS file: /cvsroot/playerstage/code/gazebo/player/GripperInterface.cc,v
retrieving revision 1.2
diff -u -r1.7 Makefile.am
--- player/Makefile.am	18 Mar 2006 19:47:38 -0000	1.7
+++ player/Makefile.am	26 Sep 2006 12:03:27 -0000
@@ -13,6 +13,7 @@
 														 LaserInterface.cc LaserInterface.hh \
 														 SonarInterface.cc SonarInterface.hh \
 														 PtzInterface.cc PtzInterface.hh \
+														 ActarrayInterface.cc ActarrayInterface.hh \
 														 FiducialInterface.cc FiducialInterface.hh \
 														 GripperInterface.cc GripperInterface.hh \
 														 CameraInterface.cc CameraInterface.hh \
