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