Revision: 7835
http://playerstage.svn.sourceforge.net/playerstage/?rev=7835&view=rev
Author: rtv
Date: 2009-06-10 19:01:21 +0000 (Wed, 10 Jun 2009)
Log Message:
-----------
added missing files from last commit
Added Paths:
-----------
code/stage/trunk/libstage/model_actuator.cc
code/stage/trunk/libstage/model_lightindicator.cc
code/stage/trunk/libstage/model_loadcell.cc
code/stage/trunk/libstageplugin/p_actarray.cc
code/stage/trunk/libstageplugin/p_aio.cc
code/stage/trunk/libstageplugin/p_dio.cc
Added: code/stage/trunk/libstage/model_actuator.cc
===================================================================
--- code/stage/trunk/libstage/model_actuator.cc (rev 0)
+++ code/stage/trunk/libstage/model_actuator.cc 2009-06-10 19:01:21 UTC (rev
7835)
@@ -0,0 +1,298 @@
+///////////////////////////////////////////////////////////////////////////
+//
+// File: model_laser.c
+// Author: Richard Vaughan
+// Date: 10 June 2004
+//
+// CVS info:
+// $Source:
/home/tcollett/stagecvs/playerstage-cvs/code/stage/libstage/model_position.cc,v
$
+// $Author: rtv $
+// $Revision: 1.5 $
+//
+///////////////////////////////////////////////////////////////////////////
+
+
+#include <sys/time.h>
+#include <math.h>
+#include <stdlib.h>
+
+//#define DEBUG
+#include "stage.hh"
+#include "worldfile.hh"
+
+using namespace Stg;
+
+/**
+...@ingroup model
+...@defgroup model_actuator Actuator model
+
+The actuator model simulates a simple actuator, where child objects can be
displaced in a
+single dimension, or rotated about the Z axis.
+
+API: Stg::ModelActuator
+
+<h2>Worldfile properties</h2>
+
+...@par Summary and default values
+
+...@verbatim
+actuator
+(
+ # actuator properties
+ type ""
+ axis [x y z]
+
+ min_position
+ max_position
+ max_speed
+)
+...@endverbatim
+
+...@par Details
+
+- type "linear" or "rotational"\n
+ the style of actuator. Rotational actuators may only rotate about the z axis
(i.e. 'yaw')
+- axis
+ if a linear actuator the axis that the actuator will move along
+ */
+
+const double STG_ACTUATOR_WATTS_KGMS = 5.0; // cost per kg per meter per second
+const double STG_ACTUATOR_WATTS = 2.0; // base cost of position device
+
+const stg_actuator_control_mode_t ACTUATOR_CONTROL_DEFAULT =
STG_ACTUATOR_CONTROL_VELOCITY;
+const stg_actuator_type_t ACTUATOR_TYPE_DEFAULT = STG_ACTUATOR_TYPE_LINEAR;
+
+
+ModelActuator::ModelActuator( World* world,
+
Model* parent )
+ : Model( world, parent, MODEL_TYPE_ACTUATOR ),
+ goal(0), pos(0), max_speed(1), min_position(0), max_position(1),
+ control_mode(ACTUATOR_CONTROL_DEFAULT),
+ actuator_type(ACTUATOR_TYPE_DEFAULT)
+{
+ // no power consumed until we're subscribed
+ this->SetWatts( 0 );
+
+ // sensible position defaults
+ Velocity vel;
+ memset( &vel, 0, sizeof(vel));
+ this->SetVelocity( vel );
+ this->SetBlobReturn( TRUE );
+
+ memset(&axis,0,sizeof(axis));
+
+}
+
+
+ModelActuator::~ModelActuator( void )
+{
+ // nothing to do
+}
+
+void ModelActuator::Load( void )
+{
+ Model::Load();
+ InitialPose = GetPose();
+
+ // load steering mode
+ if( wf->PropertyExists( wf_entity, "type" ) )
+ {
+ const char* type_str =
+ wf->ReadString( wf_entity, "type", NULL );
+
+ if( type_str )
+ {
+ if( strcmp( type_str, "linear" ) == 0 )
+ actuator_type = STG_ACTUATOR_TYPE_LINEAR;
+ else if( strcmp( type_str, "rotational" ) == 0 )
+ actuator_type = STG_ACTUATOR_TYPE_ROTATIONAL;
+ else
+ {
+ PRINT_ERR1( "invalid actuator type specified:
\"%s\" - should be one of: \"linear\" or \"rotational\". Using \"linear\" as
default.", type_str );
+ }
+ }
+ }
+
+ if (actuator_type == STG_ACTUATOR_TYPE_LINEAR)
+ {
+ // if we are a linear actuator find the axis we operate in
+ if( wf->PropertyExists( wf_entity, "axis" ) )
+ {
+ axis.x = wf->ReadTupleLength( wf_entity, "axis", 0, 0 );
+ axis.y = wf->ReadTupleLength( wf_entity, "axis", 1, 0 );
+ axis.z = wf->ReadTupleLength( wf_entity, "axis", 2, 0 );
+
+ // normalise the axis
+ double length = sqrt(axis.x*axis.x + axis.y*axis.y +
axis.z*axis.z);
+ if (length == 0)
+ {
+ PRINT_ERR( "zero length vector specified for
actuator axis, using (1,0,0) instead" );
+ axis.x = 1;
+ }
+ else
+ {
+ axis.x /= length;
+ axis.y /= length;
+ axis.z /= length;
+ }
+ }
+ }
+
+ if( wf->PropertyExists( wf_entity, "max_speed" ) )
+ {
+ max_speed = wf->ReadFloat ( wf_entity, "max_speed", 1 );
+ }
+
+ if( wf->PropertyExists( wf_entity, "max_position" ) )
+ {
+ max_position = wf->ReadFloat( wf_entity, "max_position", 1 );
+ }
+
+ if( wf->PropertyExists( wf_entity, "min_position" ) )
+ {
+ min_position = wf->ReadFloat( wf_entity, "min_position", 0 );
+ }
+
+}
+
+void ModelActuator::Update( void )
+{
+ PRINT_DEBUG1( "[%lu] actuator update", 0 );
+
+ // stop by default
+ double velocity = 0;
+
+ // update current position
+ Pose CurrentPose = GetPose();
+ // just need to get magnitude difference;
+ Pose PoseDiff;
+ PoseDiff.x = CurrentPose.x - InitialPose.x;
+ PoseDiff.y = CurrentPose.y - InitialPose.y;
+ PoseDiff.z = CurrentPose.z - InitialPose.z;
+ PoseDiff.a = CurrentPose.a - InitialPose.a;
+ switch (actuator_type)
+ {
+ case STG_ACTUATOR_TYPE_LINEAR:
+ {
+ pos = PoseDiff.x * axis.x + PoseDiff.y * axis.y +
PoseDiff.z * axis.z; // Dot product to find distance along axis
+ } break;
+ case STG_ACTUATOR_TYPE_ROTATIONAL:
+ {
+ pos = PoseDiff.a;
+ } break;
+ default:
+ PRINT_ERR1( "unrecognized actuator type %d",
actuator_type );
+ }
+
+
+ if( this->subs ) // no driving if noone is subscribed
+ {
+ switch( control_mode )
+ {
+ case STG_ACTUATOR_CONTROL_VELOCITY :
+ {
+ PRINT_DEBUG( "actuator velocity control
mode" );
+ PRINT_DEBUG2( "model %s command(%.2f)",
+ this->token,
+ this->goal);
+ if ((pos <= min_position && goal < 0)
|| (pos >= max_position && goal > 0))
+ velocity = 0;
+ else
+ velocity = goal;
+ } break;
+
+ case STG_ACTUATOR_CONTROL_POSITION:
+ {
+ PRINT_DEBUG( "actuator position control
mode" );
+
+
+ // limit our axis
+ if (goal < min_position)
+ goal = min_position;
+ else if (goal > max_position)
+ goal = max_position;
+
+ double error = goal - pos;
+ velocity = error;
+
+ PRINT_DEBUG1( "error: %.2f\n", error);
+
+ }
+ break;
+
+ default:
+ PRINT_ERR1( "unrecognized actuator command mode
%d", control_mode );
+ }
+
+ // simple model of power consumption
+ //TODO power consumption
+
+ // speed limits for controller
+ if (velocity < -max_speed)
+ velocity = -max_speed;
+ else if (velocity > max_speed)
+ velocity = max_speed;
+
+ Velocity outvel;
+ switch (actuator_type)
+ {
+ case STG_ACTUATOR_TYPE_LINEAR:
+ {
+ outvel.x = axis.x * velocity;
+ outvel.y = axis.y * velocity;
+ outvel.z = axis.z * velocity;
+ outvel.a = 0;
+ } break;
+ case STG_ACTUATOR_TYPE_ROTATIONAL:
+ {
+ outvel.x = outvel.y = outvel.z = 0;
+ outvel.a = velocity;
+ }break;
+ default:
+ PRINT_ERR1( "unrecognized actuator type %d",
actuator_type );
+ }
+
+ this->SetVelocity( outvel );
+ //this->GPoseDirtyTree();
+ PRINT_DEBUG4( " Set Velocity: [ %.4f %.4f %.4f %.4f ]\n",
+ outvel.x, outvel.y, outvel.z, outvel.a );
+ }
+
+ Model::Update();
+}
+
+void ModelActuator::Startup( void )
+{
+ Model::Startup();
+
+ PRINT_DEBUG( "position startup" );
+
+ this->SetWatts( STG_ACTUATOR_WATTS );
+
+}
+
+void ModelActuator::Shutdown( void )
+{
+ PRINT_DEBUG( "position shutdown" );
+
+ // safety features!
+ goal = 0;
+ bzero( &velocity, sizeof(velocity) );
+
+ this->SetWatts( 0 );
+
+ Model::Shutdown();
+}
+
+void ModelActuator::SetSpeed( double speed)
+{
+ control_mode = STG_ACTUATOR_CONTROL_VELOCITY;
+ goal = speed;
+}
+
+
+void ModelActuator::GoTo( double pos)
+{
+ control_mode = STG_ACTUATOR_CONTROL_POSITION;
+ goal = pos;
+}
Added: code/stage/trunk/libstage/model_lightindicator.cc
===================================================================
--- code/stage/trunk/libstage/model_lightindicator.cc
(rev 0)
+++ code/stage/trunk/libstage/model_lightindicator.cc 2009-06-10 19:01:21 UTC
(rev 7835)
@@ -0,0 +1,45 @@
+#include "stage.hh"
+
+using namespace Stg;
+
+ModelLightIndicator::ModelLightIndicator(World* world, Model* parent)
+ : Model(world, parent, MODEL_TYPE_LIGHTINDICATOR)
+ , m_IsOn(false)
+{
+}
+
+
+ModelLightIndicator::~ModelLightIndicator()
+{
+}
+
+
+void ModelLightIndicator::SetState(bool isOn)
+{
+ m_IsOn = isOn;
+}
+
+
+void ModelLightIndicator::DrawBlocks()
+{
+ if(m_IsOn)
+ {
+ Model::DrawBlocks();
+ }
+ else
+ {
+ stg_color_t currColour = this->GetColor();
+
+ const double scaleFactor = 0.8;
+ double r = 0, g = 0, b = 0, a = 0;
+ stg_color_unpack(currColour, &r, &g, &b, &a);
+ r *= scaleFactor;
+ g *= scaleFactor;
+ b *= scaleFactor;
+
+ this->SetColor(stg_color_pack(r, g, b, a));
+ Model::DrawBlocks();
+
+ this->SetColor(currColour);
+ }
+}
Added: code/stage/trunk/libstage/model_loadcell.cc
===================================================================
--- code/stage/trunk/libstage/model_loadcell.cc (rev 0)
+++ code/stage/trunk/libstage/model_loadcell.cc 2009-06-10 19:01:21 UTC (rev
7835)
@@ -0,0 +1,41 @@
+///////////////////////////////////////////////////////////////////////////
+//
+// File: model_loadcell.c
+// Author: Richard Vaughan
+// Date: 10 June 2004
+//
+// CVS info:
+// $Source:
/home/tcollett/stagecvs/playerstage-cvs/code/stage/libstage/model_loadcell.cc,v
$
+// $Author: rtv $
+// $Revision: 1.7 $
+//
+///////////////////////////////////////////////////////////////////////////
+
+#include <sys/time.h>
+#include "stage.hh"
+
+using namespace Stg;
+
+/**
+...@ingroup model
+...@defgroup model_loadcell Load cell model
+The load cell model simulates a load sensor. It provides a single voltage on
an aio interface which represents the mass of it's child models combined.
+*/
+
+ModelLoadCell::ModelLoadCell( World* world,
+ Model* parent )
+ : Model( world, parent, MODEL_TYPE_LOADCELL )
+{
+}
+
+ModelLoadCell::~ModelLoadCell()
+{
+}
+
+float ModelLoadCell::GetVoltage()
+{
+ if (Parent()->Stalled()) // Assume parent is what moves
+ return 0.0f; // Load has hit something
+ else
+ return float(this->GetTotalMass() - this->mass); // Don't include the mass
of the loadcell itself
+}
Added: code/stage/trunk/libstageplugin/p_actarray.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_actarray.cc
(rev 0)
+++ code/stage/trunk/libstageplugin/p_actarray.cc 2009-06-10 19:01:21 UTC
(rev 7835)
@@ -0,0 +1,138 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2004, 2005 Richard Vaughan
+ *
+ *
+ * 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: A plugin driver for Player that gives access to Stage devices.
+ * Author: Richard Vaughan
+ * Date: 10 December 2004
+ * CVS: $Id: p_position.cc,v 1.2 2008-01-15 01:25:42 rtv Exp $
+ */
+// DOCUMENTATION ------------------------------------------------------------
+
+/** @addtogroup player
+...@par ActArray interface
+- PLAYER_ACTARRAY_REQ_GET_GEOM
+- PLAYER_ACTARRAY_CMD_POS
+- PLAYER_ACTARRAY_CMD_SPEED
+- PLAYER_ACTARRAY_DATA_STATE
+*/
+
+// CODE ----------------------------------------------------------------------
+
+#include "p_driver.h"
+//#include "playerclient.h"
+
+
+InterfaceActArray::InterfaceActArray( player_devaddr_t addr,
+ StgDriver* driver,
+ ConfigFile* cf,
+ int section )
+
+ : InterfaceModel( addr, driver, cf, section, Stg::MODEL_TYPE_ACTUATOR )
+{
+ //puts( "InterfacePosition constructor" );
+}
+
+int InterfaceActArray::ProcessMessage(QueuePointer &resp_queue,
+ player_msghdr_t* hdr,
+ void* data)
+{
+ Stg::ModelActuator* actmod = (Stg::ModelActuator*)this->mod;
+
+ // Is it a new motor command?
+ if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
+ PLAYER_ACTARRAY_REQ_GET_GEOM,
+ this->addr))
+ {
+ Stg::Geom geom = actmod->GetGeom();
+
+ // fill in the geometry data formatted player-like
+ // TODO actually get real data here
+ player_actarray_actuatorgeom_t actuator = {0};
+ actuator.min = actmod->GetMinPosition();
+ actuator.max = actmod->GetMaxPosition();
+ actuator.type = PLAYER_ACTARRAY_TYPE_LINEAR;
+
+ player_actarray_geom_t pgeom = {0};
+ pgeom.base_pos.px = geom.pose.x;
+ pgeom.base_pos.py = geom.pose.y;
+ pgeom.base_pos.pz = geom.pose.z;
+
+ pgeom.base_orientation.pyaw = geom.pose.a;
+
+ pgeom.actuators_count = 1;
+ pgeom.actuators = &actuator;
+
+ this->driver->Publish( this->addr, resp_queue,
+ PLAYER_MSGTYPE_RESP_ACK,
+ PLAYER_ACTARRAY_REQ_GET_GEOM,
+ (void*)&pgeom);
+ return 0;
+ }
+ // Is it a request to reset odometry?
+ else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
+ PLAYER_ACTARRAY_CMD_POS,
+ this->addr))
+ {
+ player_actarray_position_cmd_t &cmd =
*reinterpret_cast<player_actarray_position_cmd_t*> (data);
+ actmod->GoTo(cmd.position);
+ return 0;
+ }
+ // Is it a request to reset odometry?
+ else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
+ PLAYER_ACTARRAY_CMD_SPEED,
+ this->addr))
+ {
+ player_actarray_speed_cmd_t &cmd =
*reinterpret_cast<player_actarray_speed_cmd_t*> (data);
+ actmod->SetSpeed(cmd.speed);
+ return 0;
+ }
+ //else
+
+ // Don't know how to handle this message.
+ PRINT_WARN2( "stg_actuator doesn't support msg with type %d subtype %d",
+ hdr->type, hdr->subtype);
+ return(-1);
+}
+
+void InterfaceActArray::Publish( void )
+{
+ Stg::ModelActuator* actmod = (Stg::ModelActuator*)this->mod;
+
+ // TODO fill in values here
+ player_actarray_actuator_t act = {0};
+ act.position = actmod->GetPosition();
+ act.speed = actmod->GetSpeed();
+ if (act.speed != 0)
+ act.state = PLAYER_ACTARRAY_ACTSTATE_MOVING;
+ else
+ act.state = PLAYER_ACTARRAY_ACTSTATE_IDLE;
+
+ player_actarray_data_t actdata = {0};
+ actdata.actuators_count = 1;
+ actdata.actuators = &act;
+
+
+ // publish this data
+ this->driver->Publish( this->addr,
+ PLAYER_MSGTYPE_DATA, PLAYER_ACTARRAY_DATA_STATE,
+ (void*)&actdata);
+}
Added: code/stage/trunk/libstageplugin/p_aio.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_aio.cc (rev 0)
+++ code/stage/trunk/libstageplugin/p_aio.cc 2009-06-10 19:01:21 UTC (rev
7835)
@@ -0,0 +1,79 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2004, 2005 Richard Vaughan
+ *
+ *
+ * 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: A plugin driver for Player that gives access to Stage devices.
+ * Author: Richard Vaughan
+ * Date: 10 December 2004
+ * CVS: $Id: p_laser.cc,v 1.2 2008-01-15 01:25:42 rtv Exp $
+ */
+
+// DOCUMENTATION ------------------------------------------------------------
+
+/** @addtogroup player
+...@par AIO interface
+- PLAYER_AIO_CMD_STATE
+- PLAYER_AIO_DATA_STATE
+*/
+
+// CODE ----------------------------------------------------------------------
+
+#include "p_driver.h"
+
+InterfaceAio::InterfaceAio( player_devaddr_t addr,
+ StgDriver* driver,
+ ConfigFile* cf,
+ int section )
+ : InterfaceModel( addr, driver, cf, section, Stg::MODEL_TYPE_LOADCELL ) //
TODO: AIO should not always mean a loadcell
+{
+}
+
+void InterfaceAio::Publish( void )
+{
+ Stg::ModelLoadCell* mod = (Stg::ModelLoadCell*)this->mod;
+ float voltage = mod->GetVoltage();
+
+ player_aio_data data;
+ data.voltages_count = 1; // Just one value
+ data.voltages = &voltage; // Might look bad but player does a deep copy so
it should be ok.
+
+ // Write data
+ this->driver->Publish(this->addr,
+ PLAYER_MSGTYPE_DATA,
+ PLAYER_AIO_DATA_STATE,
+ (void*)&data, sizeof(data), NULL);
+}
+
+int InterfaceAio::ProcessMessage(QueuePointer & resp_queue,
+ player_msghdr_t* hdr,
+ void* data)
+{
+ Stg::ModelLoadCell* mod = (Stg::ModelLoadCell*)this->mod;
+
+ // Is it a request to set the voltage?
+ if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
+ PLAYER_AIO_CMD_STATE,
+ this->addr))
+ {
+ player_aio_cmd* cmd = (player_aio_cmd*)data;
+ PRINT_WARN2( "Got request to set AIO voltage %d to %f. Should not
happen, AIO currently only works for load cells!\n", cmd->id, cmd->voltage );
+ }
+}
Added: code/stage/trunk/libstageplugin/p_dio.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_dio.cc (rev 0)
+++ code/stage/trunk/libstageplugin/p_dio.cc 2009-06-10 19:01:21 UTC (rev
7835)
@@ -0,0 +1,37 @@
+#include "p_driver.h"
+
+
+InterfaceDio::InterfaceDio(player_devaddr_t addr, StgDriver* driver,
ConfigFile* cf, int section)
+ : InterfaceModel( addr, driver, cf, section,
Stg::MODEL_TYPE_LIGHTINDICATOR )
+{
+}
+
+
+InterfaceDio::~InterfaceDio()
+{
+}
+
+
+int InterfaceDio::ProcessMessage(QueuePointer & resp_queue, player_msghdr_t*
hdr, void* data)
+{
+ Stg::ModelLightIndicator* mod = (Stg::ModelLightIndicator*) this->mod;
+
+ if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
PLAYER_DIO_CMD_VALUES, this->addr))
+ {
+ player_dio_cmd* cmd = (player_dio_cmd*) data;
+ if(cmd->count != 1)
+ {
+ PRINT_ERR("Invalid light indicator command.");
+ }
+
+ mod->SetState(cmd->digout != 0);
+ return 0;
+ }
+
+ return -1;
+}
+
+void InterfaceDio::Publish()
+{
+
+}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Crystal Reports - New Free Runtime and 30 Day Trial
Check out the new simplified licensing option that enables unlimited
royalty-free distribution of the report engine for externally facing
server and web deployment.
http://p.sf.net/sfu/businessobjects
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
