Revision: 7186
http://playerstage.svn.sourceforge.net/playerstage/?rev=7186&view=rev
Author: thjc
Date: 2008-12-03 01:31:52 +0000 (Wed, 03 Dec 2008)
Log Message:
-----------
applied patch 2286925 DIO interface and indicator model
Modified Paths:
--------------
code/stage/branches/release-2-1-patches/src/Makefile.am
code/stage/branches/release-2-1-patches/src/p_driver.cc
code/stage/branches/release-2-1-patches/src/p_driver.h
code/stage/branches/release-2-1-patches/src/stage.h
code/stage/branches/release-2-1-patches/src/typetable.c
Added Paths:
-----------
code/stage/branches/release-2-1-patches/src/model_indicator.c
code/stage/branches/release-2-1-patches/src/p_dio.cc
Modified: code/stage/branches/release-2-1-patches/src/Makefile.am
===================================================================
--- code/stage/branches/release-2-1-patches/src/Makefile.am 2008-12-02
02:30:04 UTC (rev 7185)
+++ code/stage/branches/release-2-1-patches/src/Makefile.am 2008-12-03
01:31:52 UTC (rev 7186)
@@ -44,6 +44,7 @@
model_wifi.cc \
model_speech.c \
model_audio.c \
+ model_indicator.c \
dijkstra.c \
dijkstra.h \
raytrace.c \
@@ -97,6 +98,7 @@
p_map.cc \
p_simulation.cc \
p_blobfinder.cc \
+ p_dio.cc \
stg_time.cc \
stg_time.h
Added: code/stage/branches/release-2-1-patches/src/model_indicator.c
===================================================================
--- code/stage/branches/release-2-1-patches/src/model_indicator.c
(rev 0)
+++ code/stage/branches/release-2-1-patches/src/model_indicator.c
2008-12-03 01:31:52 UTC (rev 7186)
@@ -0,0 +1,240 @@
+///////////////////////////////////////////////////////////////////////////
+//
+// File: model_indicator.c
+// Author: David Olsen
+// Date: 4 October 2008
+//
+// CVS info:
+// $Source:
/home/tcollett/stagecvs/playerstage-cvs/code/stage/src/model_indicator.c,v $
+// $Author: rtv $
+// $Revision: 1.91 $
+//
+///////////////////////////////////////////////////////////////////////////
+
+
+#include <sys/time.h>
+#include <math.h>
+#include "gui.h"
+
+//#define DEBUG
+
+#include "stage_internal.h"
+
+#define STG_INDICATOR_WATTS 10
+#define STG_DEFAULT_INDICATOR_SIZEX 0.15
+#define STG_DEFAULT_INDICATOR_SIZEY 0.15
+
+/**
[EMAIL PROTECTED] model
[EMAIL PROTECTED] model_indicator Indicator model
+The indicator model simulates a visual indicator, such as a light.
+
+<h2>Worldfile properties</h2>
+
[EMAIL PROTECTED] Summary and default values
+
[EMAIL PROTECTED]
+indicator
+(
+ # indicator properties
+
+ # model properties
+ size [0.15 0.15]
+ color "blue"
+ watts 10
+)
[EMAIL PROTECTED]
+
[EMAIL PROTECTED] Details
+*/
+
+// Returns a copy of an array of polygons. There should really be a built in
function for this.
+stg_polygon_t* indicator_copy_polygons( stg_polygon_t* source, size_t count )
+{
+ stg_polygon_t* copy = stg_polygons_create( count );
+ for ( int i = 0; i < count; i++ )
+ {
+ g_array_append_vals( copy[i].points, source[i].points->data,
source[i].points->len );
+ copy[i].unfilled = source[i].unfilled;
+ copy[i].color = source[i].color;
+ copy[i].bbox = source[i].bbox;
+ }
+ return copy;
+}
+
+void indicator_load( stg_model_t* mod )
+{
+
+ stg_indicator_config_t* cfg = (stg_indicator_config_t*)mod->cfg;
+
+ // Take a copy of the polygon
+ stg_indicator_data_t data;
+ data.numPolys = mod->polygons_count;
+ data.polys = indicator_copy_polygons( mod->polygons, data.numPolys );
+
+ // Set the indicator to off initially
+ data.on = FALSE;
+ stg_model_set_polygons( mod, NULL, 0 );
+
+ // Store the updated data (includes copy of polygon and original state)
+ stg_model_set_data( mod, &data, sizeof(data) );
+
+ model_change( mod, &mod->cfg ); // Notify model has been changed.
+}
+
+int indicator_update( stg_model_t* mod );
+int indicator_startup( stg_model_t* mod );
+int indicator_shutdown( stg_model_t* mod );
+
+int indicator_render_data( stg_model_t* mod, void* userp );
+int indicator_unrender_data( stg_model_t* mod, void* userp );
+//int indicator_render_cfg( stg_model_t* mod, void* userp );
+//int indicator_unrender_cfg( stg_model_t* mod, void* userp );
+
+
+int indicator_init( stg_model_t* mod )
+{
+ // override the default methods
+ mod->f_startup = indicator_startup;
+ mod->f_shutdown = indicator_shutdown;
+ mod->f_update = NULL; // indicator_update is installed startup, removed on
shutdown
+ mod->f_load = indicator_load;
+
+ // sensible indicator defaults
+ stg_geom_t geom;
+ geom.pose.x = 0.0;
+ geom.pose.y = 0.0;
+ geom.pose.a = 0.0;
+ geom.size.x = STG_DEFAULT_INDICATOR_SIZEX;
+ geom.size.y = STG_DEFAULT_INDICATOR_SIZEY;
+ stg_model_set_geom( mod, &geom );
+
+
+ // create a single rectangle body
+ stg_polygon_t* square = stg_unit_polygon_create();
+ stg_model_set_polygons( mod, square, 1 );
+
+
+ // set up an indicator-specific config structure
+ stg_indicator_config_t iconf;
+ memset(&iconf,0,sizeof(iconf));
+ stg_model_set_cfg( mod, &iconf, sizeof(iconf) );
+
+ // sensible starting command
+ stg_dio_cmd_t cmd;
+ cmd.count = 1;
+ cmd.digout = 0;
+ stg_model_set_cmd( mod, &cmd, sizeof(cmd) ) ;
+
+ // Null data till load
+ stg_model_set_data( mod, NULL, 0 );
+
+ // set default color
+ stg_model_set_color( mod, 0xF0F0FF );
+
+ // adds a menu item and associated on-and-off callbacks
+ /*stg_model_add_property_toggles( mod,
+ &mod->data,
+ indicator_render_data, // called when toggled
on
+ NULL,
+ indicator_unrender_data, // called when
toggled off
+ NULL,
+ "indicatordata",
+ "indicator data",
+ TRUE );*/
+
+ return 0;
+}
+
+void stg_indicator_config_print( stg_indicator_config_t* slc )
+{
+ printf( "indicator config" );
+}
+
+int indicator_update( stg_model_t* mod )
+{
+ PRINT_DEBUG2( "[%lu] indicator update (%d subs)",
+ mod->world->sim_time, mod->subs );
+
+ stg_dio_cmd_t* cmd = (stg_dio_cmd_t*)mod->cmd;
+ stg_indicator_data_t* indData = stg_model_get_data(mod, 0);
+
+ if (cmd && indData) // If we have all the appropriate data
+ {
+ // If the command turns indicator on, and the previous state was off
+ if ( (cmd->digout & 1) && !indData->on )
+ {
+ indData->on = TRUE; // Set the state to on
+ // Stage is quite happy to delete any memory given to it, so
make a copy.
+ stg_polygon_t* polysCopy = indicator_copy_polygons(
indData->polys, indData->numPolys );
+ /*stg_polygons_create( indData->numPolys );
+ for ( int i = 0; i < indData->numPolys; i++ )
+ {
+ g_array_append_vals( polysCopy[i].points,
indData->polys[i].points->data, indData->polys[i].points->len );
+ polysCopy[i].unfilled = indData->polys[i].unfilled;
+ polysCopy[i].color = indData->polys[i].color;
+ polysCopy[i].bbox = indData->polys[i].bbox;
+ }*/
+ stg_model_set_polygons( mod, polysCopy, indData->numPolys);
// Show polygons
+ }
+ // If the command turns the indicator off, and the previous state was
on
+ if ( !( cmd->digout & 1 ) && indData->on )
+ {
+ indData->on = FALSE; // Set the state to off
+ stg_model_set_polygons( mod, NULL, 0); // Hide polygons
+ }
+ }
+
+ return 0; //ok
+}
+
+
+int indicator_unrender_data( stg_model_t* mod, void* userp )
+{
+ // TODO: Unrender?
+ //stg_model_fig_clear( mod, "indicator_data_fig" );
+
+ return 1; // callback just runs one time
+}
+
+
+int indicator_render_data( stg_model_t* mod, void* enabled )
+{
+ // TODO: Render?
+
+ return 0; // callback runs until removed
+}
+
+int indicator_startup( stg_model_t* mod )
+{
+ PRINT_DEBUG( "indicator startup" );
+
+ // start consuming power
+ stg_model_set_watts( mod, STG_INDICATOR_WATTS );
+
+ // install the update function
+ mod->f_update = indicator_update;
+
+ return 0; // ok
+}
+
+int indicator_shutdown( stg_model_t* mod )
+{
+ PRINT_DEBUG( "indicator shutdown" );
+
+ // uninstall the update function
+ mod->f_update = NULL;
+
+ // stop consuming power
+ stg_model_set_watts( mod, 0 );
+
+
+ // clear the data - this will unrender it too
+ stg_indicator_data_t* indData = stg_model_get_data(mod, 0);
+ stg_polygons_destroy( indData->polys, indData->numPolys );
+ stg_model_set_data( mod, NULL, 0 );
+
+ return 0; // ok
+}
+
+
Added: code/stage/branches/release-2-1-patches/src/p_dio.cc
===================================================================
--- code/stage/branches/release-2-1-patches/src/p_dio.cc
(rev 0)
+++ code/stage/branches/release-2-1-patches/src/p_dio.cc 2008-12-03
01:31:52 UTC (rev 7186)
@@ -0,0 +1,90 @@
+/*
+ * 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: David Olsen
+ * Date: 25 October 2008
+ * CVS: $Id: p_power.cc,v 1.4 2007-08-23 19:58:49 gerkey Exp $
+ */
+
+
+#include "p_driver.h"
+
+/** @addtogroup player
[EMAIL PROTECTED] Dio interface
+- PLAYER_DIO_DATA_VALUES
+*/
+
+extern "C" {
+ int indicator_init( stg_model_t* mod );
+}
+
+InterfaceDio::InterfaceDio( player_devaddr_t addr,
+ StgDriver* driver,
+ ConfigFile* cf,
+ int section )
+ : InterfaceModel( addr, driver, cf, section, indicator_init )
+{
+}
+
+
+void InterfaceDio::Publish( void )
+{
+ size_t len = mod->data_len;
+ stg_indicator_data_t* data = (stg_indicator_data_t*)mod->data;
+
+ // translate stage data to player data packet
+ player_dio_data_t pdio;
+ pdio.count = 1;
+ pdio.bits = data->on;
+
+ // TODO: Publish?
+ /*this->driver->Publish(this->addr,
+ PLAYER_MSGTYPE_DATA,
+ PLAYER_DIO_DATA_VALUES,
+ (void*)&pdio, sizeof(pdio), NULL);*/
+}
+
+int InterfaceDio::ProcessMessage(QueuePointer &resp_queue,
+ player_msghdr_t* hdr,
+ void* data)
+{
+ if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
+ PLAYER_DIO_CMD_VALUES,
+ this->addr))
+ {
+ player_dio_cmd* pdio = (player_dio_cmd*)data;
+
+ stg_dio_cmd_t cmd;
+ cmd.count = pdio->count;
+ cmd.digout = pdio->digout;
+ stg_model_set_cmd( this->mod, &cmd, sizeof(cmd));
+ return 0;
+ }
+
+ // Don't know how to handle this message.
+ PRINT_WARN2( "stage dio doesn't support message %d:%d.",
+ hdr->type, hdr->subtype);
+ return(-1);
+}
+
+
Modified: code/stage/branches/release-2-1-patches/src/p_driver.cc
===================================================================
--- code/stage/branches/release-2-1-patches/src/p_driver.cc 2008-12-02
02:30:04 UTC (rev 7185)
+++ code/stage/branches/release-2-1-patches/src/p_driver.cc 2008-12-03
01:31:52 UTC (rev 7186)
@@ -413,6 +413,10 @@
case PLAYER_BUMPER_CODE:
ifsrc = new InterfaceBumper( player_addr, this, cf, section );
break;
+
+ case PLAYER_DIO_CODE:
+ ifsrc = new InterfaceDio( player_addr, this, cf, section );
+ break;
default:
PRINT_ERR1( "error: stage driver doesn't support interface type %d\n",
Modified: code/stage/branches/release-2-1-patches/src/p_driver.h
===================================================================
--- code/stage/branches/release-2-1-patches/src/p_driver.h 2008-12-02
02:30:04 UTC (rev 7185)
+++ code/stage/branches/release-2-1-patches/src/p_driver.h 2008-12-03
01:31:52 UTC (rev 7186)
@@ -314,5 +314,18 @@
};
+class InterfaceDio : public InterfaceModel
+{
+ public:
+ InterfaceDio( player_devaddr_t addr, StgDriver* driver, ConfigFile*
cf, int section );
+ virtual ~InterfaceDio( void ){ /* TODO: clean up*/ };
+
+ virtual int ProcessMessage( QueuePointer &resp_queue,
+ player_msghdr * hdr,
+ void * data );
+
+ virtual void Publish( void );
+};
+
#endif
Modified: code/stage/branches/release-2-1-patches/src/stage.h
===================================================================
--- code/stage/branches/release-2-1-patches/src/stage.h 2008-12-02 02:30:04 UTC
(rev 7185)
+++ code/stage/branches/release-2-1-patches/src/stage.h 2008-12-03 01:31:52 UTC
(rev 7186)
@@ -1174,7 +1174,30 @@
/[EMAIL PROTECTED]/
+ // INDICATOR MODEL --------------------------------------------------------
+ /** indicator configuration packet
+ */
+ typedef struct
+ {
+ } stg_indicator_config_t;
+
+ typedef struct {
+ /** the command */
+ uint32_t count;
+ /** output bitfield */
+ uint32_t digout;
+ } stg_dio_cmd_t;
+
+ /** indicator data packet */
+ typedef struct
+ {
+ stg_polygon_t* polys;
+ int numPolys;
+ stg_bool_t on;
+ } stg_indicator_data_t;
+
+
// MACROS ------------------------------------------------------
// Some useful macros
Modified: code/stage/branches/release-2-1-patches/src/typetable.c
===================================================================
--- code/stage/branches/release-2-1-patches/src/typetable.c 2008-12-02
02:30:04 UTC (rev 7185)
+++ code/stage/branches/release-2-1-patches/src/typetable.c 2008-12-03
01:31:52 UTC (rev 7186)
@@ -15,6 +15,7 @@
int speech_init( stg_model_t* mod );
int audio_init( stg_model_t* mod );
int bumper_init( stg_model_t* mod );
+int indicator_init( stg_model_t* mod );
// map worldfile keywords onto initialization functions
stg_type_record_t typetable[] =
@@ -31,6 +32,7 @@
{ "wifi", wifi_init },
{ "speech", speech_init },
{ "audio", audio_init },
+ { "indicator", indicator_init },
{ "bumper", bumper_init },
{ NULL, 0, NULL } // this must be the last entry
};
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
This SF.Net email is sponsored by the Moblin Your Move Developer's challenge
Build the coolest Linux based applications with Moblin SDK & win great prizes
Grand prize is a trip for two to an Open Source event anywhere in the world
http://moblin-contest.org/redirect.php?banner_id=100&url=/
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
